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

Update all project: add folders with scripts

Browse Source
This commit is contained in:
Thomas Dehaeze 2019-03-20 09:23:10 +01:00
parent f49e38f312
commit 482f8acd3f
43 changed files with 1009 additions and 497 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
.gitignore vendored
View File

@ -1,29 +0,0 @@
# Windows default autosave extension
*.asv
# OSX / *nix default autosave extension
*.m~
# Compiled MEX binaries (all platforms)
*.mex*
# Packaged app and toolbox files
*.mlappinstall
*.mltbx
# Generated helpsearch folders
helpsearch*/
# Simulink code generation folders
slprj/
sccprj/
# Simulink autosave extension
*.autosave
# Octave session info
octave-workspace
# Custom
stewart_displacement_grt_rtw/
Figures/

2
Stewartsimscape.prj Normal file
View File

@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<SimulinkProject xmlns="http://www.mathworks.com/SimulinkProjectFile" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" version="1.0"/>

36
active_damping/act_damp_iff_generate.m Normal file
View File

@ -0,0 +1,36 @@
%%
clear; close all; clc;
%% Load the transfer functions
load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%%
s = tf('s');
%% Light Voice Coil
%sisotool(-G_light_vc.G_iff('Fm1', 'F1')/s);
K_iff_light_vc = 105/s*tf(eye(6));
%% Light Piezo
%sisotool(-G_light_pz.G_iff('Fm1', 'F1')/s);
K_iff_light_pz = 3300/s*tf(eye(6));
%% Heavy Voice Coil
%sisotool(-G_heavy_vc.G_iff('Fm1', 'F1')/s);
K_iff_heavy_vc = 22.7/s*tf(eye(6));
%% Heavy Piezo
%sisotool(-G_heavy_pz.G_iff('Fm1', 'F1')/s);
K_iff_heavy_pz = 720/s*tf(eye(6));
%% Save Controllers
save('./mat/K_iff_sisotool.mat', ...
'K_iff_light_vc', 'K_iff_light_pz', ...
'K_iff_heavy_vc', 'K_iff_heavy_pz');

45
active_damping/act_damp_iff_id.m Normal file
View File

@ -0,0 +1,45 @@
%%
clear; close all; clc;
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%% Load controllers
load('./mat/K_iff_sisotool.mat', ...
'K_iff_light_vc', 'K_iff_light_pz', ...
'K_iff_heavy_vc', 'K_iff_heavy_pz');
%%
initializeSample(struct('mass', 1));
initializeHexapod(struct('actuator', 'lorentz'));
K_iff = K_iff_light_vc; %#ok
save('./mat/controllers.mat', 'K_iff', '-append');
G_light_vc_iff = identifyPlant();
initializeHexapod(struct('actuator', 'piezo'));
K_iff = K_iff_light_pz; %#ok
save('./mat/controllers.mat', 'K_iff', '-append');
G_light_pz_iff = identifyPlant();
%%
initializeSample(struct('mass', 50));
initializeHexapod(struct('actuator', 'lorentz'));
K_iff = K_iff_heavy_vc; %#ok
save('./mat/controllers.mat', 'K_iff', '-append');
G_heavy_vc_iff = identifyPlant();
initializeHexapod(struct('actuator', 'piezo'));
K_iff = K_iff_heavy_pz;
save('./mat/controllers.mat', 'K_iff', '-append');
G_heavy_pz_iff = identifyPlant();
%% Save the obtained transfer functions
save('./mat/G_iff.mat', ...
'G_light_vc_iff', 'G_light_pz_iff', ...
'G_heavy_vc_iff', 'G_heavy_pz_iff');

133
active_damping/act_damp_iff_plots.m Normal file
View File

@ -0,0 +1,133 @@
%%
clear; close all; clc;
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%% Load
load('./mat/G_iff.mat', 'G_light_vc_iff', 'G_light_pz_iff', 'G_heavy_vc_iff', 'G_heavy_pz_iff');
load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
%% New Damped Plant - Horizontal Direction
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fx'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_light_vc_iff.G_cart('Dx', 'Fx'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_light_pz_iff.G_cart('Dx', 'Fx'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc_iff.G_cart('Dx', 'Fx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz_iff.G_cart('Dx', 'Fx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;
linkaxes([ax1,ax2],'x');
if save_fig; exportFig('G_hori_iff', 'normal-normal', struct('path', 'active_damping')); end
%% New Damped Plant - Vertical Direction
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fz'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_light_vc_iff.G_cart('Dz', 'Fz'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_light_pz_iff.G_cart('Dz', 'Fz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc_iff.G_cart('Dz', 'Fz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz_iff.G_cart('Dz', 'Fz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;
linkaxes([ax1,ax2],'x');
if save_fig; exportFig('G_vert_iff', 'normal-normal', struct('path', 'active_damping')); end
%% Ground motion Transmissibility - Horizontal Direction
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_db('Dx', 'Dbx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_db('Dx', 'Dbx'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_light_vc_iff.G_db('Dx', 'Dbx'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_light_pz_iff.G_db('Dx', 'Dbx'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
hold off;
if save_fig; exportFig('G_db_hori_iff', 'normal-normal', struct('path', 'active_damping')); end
%% Ground motion Transmissibility - Vertical Direction
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_db('Dz', 'Dbz'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_db('Dz', 'Dbz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_light_vc_iff.G_db('Dz', 'Dbz'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_light_pz_iff.G_db('Dz', 'Dbz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
hold off;
if save_fig; exportFig('G_db_vert_iff', 'normal-normal', struct('path', 'active_damping')); end
%% Direct Forces Compliance - Horizontal Direction
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fi('Dx', 'Fix'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fi('Dx', 'Fix'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_light_vc_iff.G_fi('Dx', 'Fix'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_light_pz_iff.G_fi('Dx', 'Fix'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
hold off;
if save_fig; exportFig('G_fi_hori_iff', 'normal-normal', struct('path', 'active_damping')); end
%% Direct Forces Compliance - Vertical Direction
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fi('Dz', 'Fiz'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fi('Dz', 'Fiz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_light_vc_iff.G_fi('Dz', 'Fiz'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_light_pz_iff.G_fi('Dz', 'Fiz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
hold off;
if save_fig; exportFig('G_fi_vert_iff', 'normal-normal', struct('path', 'active_damping')); end

8
active_damping/act_damp_main.m Normal file
View File

@ -0,0 +1,8 @@
% Generate the IFF controls
run act_damp_iff_generate.m
% Identification of the damped plant
run act_damp_iff_id.m
% Plot new transfer functions
run act_damp_iff_plots.m

14
analysis/analysis_stiffness.m Normal file
View File

@ -0,0 +1,14 @@
%%
clear; close all; clc;
%%
sys_0 = initializeHexapod(struct('actuator', 'piezo', 'jacobian', 0));
sys_100 = initializeHexapod(struct('actuator', 'piezo', 'jacobian', 100));
sys_200 = initializeHexapod(struct('actuator', 'piezo', 'jacobian', 1000000));
%%
K_0 = getStiffnessMatrix(sys_0.Leg.k.ax, sys_0.J );
K_100 = getStiffnessMatrix(sys_100.Leg.k.ax, sys_100.J);
K_200 = getStiffnessMatrix(sys_200.Leg.k.ax, sys_200.J);
%%

2
analyze_jacobian.m
View File

@ -1,6 +1,7 @@
%% Script Description
%
%%
figure;
plot(d_meas.Time, d.Data-d_meas.Data)
@ -46,4 +47,3 @@ plot(jacobian.Time, squeeze(K_change(5, 5, :)));
plot(jacobian.Time, squeeze(K_change(6, 6, :)));
legend({'Kxx', 'Kyy', 'Kzz', 'Kmx', 'Kmy', 'Kmz'})
hold off;

12
config.m Normal file
View File

@ -0,0 +1,12 @@
%%
addpath('active_damping');
addpath('analysis');
addpath('identification');
addpath('library');
addpath('studies');
addpath('src');
%%
freqs = logspace(-1, 3, 1000);
save_fig = false;
save('./mat/config.mat', 'freqs', 'save_fig');

20
identification/id_G.m Normal file
View File

@ -0,0 +1,20 @@
%% Script Description
% Script used to identify various transfer functions
% of the Stewart platform
%%
clear; close all; clc;
%%
K_iff = tf(zeros(6));
save('./mat/controllers.mat', 'K_iff', '-append');
%% Initialize System
initializeSample(struct('mass', 50));
initializeHexapod(struct('actuator', 'piezo'));
%% Identification
G = identifyPlant();
%% Save
save('./mat/G.mat', 'G');

38
identification/id_jacobian.m Normal file
View File

@ -0,0 +1,38 @@
%% Script Description
%%
clear; close all; clc;
%%
K_iff = tf(zeros(6));
save('./mat/controllers.mat', 'K_iff', '-append');
%% System - perfectly aligned
initializeHexapod(struct('actuator', 'piezo', 'jacobian', 1, 'density', 0.1));
initializeSample(struct('mass', 50, 'height', 1, 'measheight', 1, 'offset', [0, 0, -25.5]));
% Identification
G_center = identifyPlant();
%% System - Jacobian is too high
initializeHexapod(struct('actuator', 'piezo', 'jacobian', 160));
initializeSample(struct('mass', 50, 'height', 300, 'measheight', 150));
% Identification
G_Jac_offset = identifyPlant();
%% System - CoM is too low
initializeHexapod(struct('actuator', 'piezo', 'jacobian', 150));
initializeSample(struct('mass', 50, 'height', 280, 'measheight', 150));
% Identification
G_CoM_offset = identifyPlant();
%% System - Meas point is too high
initializeHexapod(struct('actuator', 'piezo', 'jacobian', 150));
initializeSample(struct('mass', 50, 'height', 300, 'measheight', 160));
% Identification
G_Meas_offset = identifyPlant();
%% Save
save('./mat/G_jacobian.mat', 'G_center', 'G_Jac_offset', 'G_CoM_offset', 'G_Meas_offset');

43
identification/id_jacobian_plots.m Normal file
View File

@ -0,0 +1,43 @@
%% Script Description
%
%%
clear; close all; clc;
%%
load('./mat/G_jacobian.mat');
%%
freqs = logspace(0, 3, 2000);
%%
bode_opts = bodeoptions;
bode_opts.FreqUnits = 'Hz';
bode_opts.MagUnits = 'abs';
bode_opts.MagScale = 'log';
bode_opts.PhaseVisible = 'off';
%% Compare when the Jac is above Meas. point and CoM
% =>
figure;
bode(G_center.G_cart, G_Jac_offset.G_cart, 2*pi*freqs, bode_opts);
%% Compare when the CoM is bellow the Meas. point and Jac
% => This make the tilt resonance frequency a little bit higher.
figure;
bode(G_center.G_cart, G_CoM_offset.G_cart, 2*pi*freqs, bode_opts);
%% Compare when the measurement point is higher than CoM and Jac
% =>
figure;
bode(G_center.G_cart, G_Meas_offset.G_cart, 2*pi*freqs, bode_opts);
%% Compare direct forces and forces applied by actuators on the same point
% => This should be the same is the support is rigid.
% => Looks like it's close but not equal
figure;
bode(G_center.G_cart, G_center.G_comp, 2*pi*freqs, bode_opts);
%% Compare relative sensor and absolute sensor
% => This should be the same as the support is rigid
figure;
bode(G_center.G_iner, G_center.G_comp, 2*pi*freqs, bode_opts);

14
identification/id_main.m Normal file
View File

@ -0,0 +1,14 @@
%%
clear; close all; clc;
%% Jacobian Study
%% Identification of the system
run id_G.m
%% Plots of the identifications
run id_plot_cart.m
run id_plot_legs.m
run id_plot_iff.m
run id_plot_db.m

96
identification/id_plot_cart.m Normal file
View File

@ -0,0 +1,96 @@
%%
clear; close all; clc;
%% Load the transfer functions
load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%% Plant in the X direction
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fx'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_cart('Dx', 'Fx'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_cart('Dx', 'Fx'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_vc.G_cart('Dx', 'Fx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_pz.G_cart('Dx', 'Fx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;
linkaxes([ax1,ax2],'x');
if save_fig; exportFig('G_hori', 'normal-normal', struct('path', 'identification')); end
%% Plant in the Z direction
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fz'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_cart('Dz', 'Fz'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_cart('Dz', 'Fz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_vc.G_cart('Dz', 'Fz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_pz.G_cart('Dz', 'Fz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;
linkaxes([ax1,ax2],'x');
if save_fig; exportFig('G_vert', 'normal-normal', struct('path', 'identification')); end
%% Coupling
figure;
for i_input = 1:3
for i_output = 1:3
subaxis(3,3,3*(i_input-1)+i_output);
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart(i_output, i_input), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart(i_output, i_input), freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlim([freqs(1) freqs(end)]); ylim([1e-22, 1e-2]);
yticks([1e-20, 1e-15, 1e-10, 1e-5]); xticks([0.1 1 10 100 1000]);
if i_output > 1; set(gca,'yticklabel',[]); end
if i_input < 3; set(gca,'xticklabel',[]); end
hold off;
end
end
if save_fig; exportFig('G_coupling', 'wide-tall', struct('path', 'identification')); end

40
identification/id_plot_db.m Normal file
View File

@ -0,0 +1,40 @@
%%
clear; close all; clc;
%% Load the transfer functions
load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%%
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_db('Dx', 'Dbx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_db('Dx', 'Dbx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_db('Dx', 'Dbx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_db('Dx', 'Dbx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
hold off;
legend('Location', 'southeast');
if save_fig; exportFig('G_db_hori', 'normal-normal', struct('path', 'identification')); end
%%
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_db('Dz', 'Dbz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_db('Dz', 'Dbz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_db('Dz', 'Dbz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_db('Dz', 'Dbz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
hold off;
legend('Location', 'southeast');
if save_fig; exportFig('G_db_vert', 'normal-normal', struct('path', 'identification')); end
%%

57
identification/id_plot_iff.m Normal file
View File

@ -0,0 +1,57 @@
%%
clear; close all; clc;
%% Load the transfer functions
load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%%
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_iff('Fm1', 'F1'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_iff('Fm1', 'F1'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_iff('Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_pz.G_iff('Fm1', 'F1'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;
linkaxes([ax1,ax2],'x');
if save_fig; exportFig('G_iff', 'normal-normal', struct('path', 'identification')); end
%% Coupling
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))), 'k-');
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm2', 'F1'), freqs, 'Hz'))), 'k--');
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm3', 'F1'), freqs, 'Hz'))), 'k--');
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm4', 'F1'), freqs, 'Hz'))), 'k--');
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm5', 'F1'), freqs, 'Hz'))), 'k--');
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm6', 'F1'), freqs, 'Hz'))), 'k--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
hold off;
if save_fig; exportFig('G_iff_coupling', 'normal-normal', struct('path', 'identification')); end

57
identification/id_plot_legs.m Normal file
View File

@ -0,0 +1,57 @@
%%
clear; close all; clc;
%% Load the transfer functions
load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%%
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_legs('D1', 'F1'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_legs('D1', 'F1'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_legs('D1', 'F1'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_legs('D1', 'F1'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_legs('D1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_legs('D1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_vc.G_legs('D1', 'F1'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_pz.G_legs('D1', 'F1'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;
linkaxes([ax1,ax2],'x');
if save_fig; exportFig('G_legs', 'normal-normal', struct('path', 'identification')); end
%% Coupling
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_legs('D1', 'F1'), freqs, 'Hz'))), 'k-');
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_legs('D2', 'F1'), freqs, 'Hz'))), 'k--');
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_legs('D3', 'F1'), freqs, 'Hz'))), 'k--');
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_legs('D4', 'F1'), freqs, 'Hz'))), 'k--');
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_legs('D5', 'F1'), freqs, 'Hz'))), 'k--');
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_legs('D6', 'F1'), freqs, 'Hz'))), 'k--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
hold off;
if save_fig; exportFig('G_legs_coupling', 'normal-normal', struct('path', 'identification')); end

56
identification_cart.m
View File

@ -1,56 +0,0 @@
%% Script Description
% Script used to identify the transfer functions of the
% Stewart platform (from actuator to displacement)
%%
clear; close all; clc;
%%
initializeNanoHexapod();
%%
initializeSample(struct('mass', 0));
G_cart_0 = identifyPlantCart();
%%
initializeSample(struct('mass', 10));
G_cart_10 = identifyPlantCart();
%%
initializeSample(struct('mass', 50));
G_cart_50 = identifyPlantCart();
%%
freqs = logspace(1, 4, 1000);
bodeFig({G_cart_0(1, 1), G_cart_10(1, 1), G_cart_50(1, 1)}, freqs, struct('phase', true))
legend({'$F_x \rightarrow D_x$ - $M = 0Kg$', '$F_x \rightarrow D_x$ - $M = 10Kg$', '$F_x \rightarrow D_x$ - $M = 50Kg$'})
legend('location', 'southwest')
exportFig('hexapod_cart_mass_x', 'normal-tall')
bodeFig({G_cart_0(3, 3), G_cart_10(3, 3), G_cart_50(3, 3)}, freqs, struct('phase', true))
legend({'$F_z \rightarrow D_z$ - $M = 0Kg$', '$F_z \rightarrow D_z$ - $M = 10Kg$', '$F_z \rightarrow D_z$ - $M = 50Kg$'})
legend('location', 'southwest')
exportFig('hexapod_cart_mass_z', 'normal-tall')
%%
% Bode Plot of the linearized function
freqs = logspace(2, 4, 1000);
bodeFig({G_cart_0(1, 1), G_cart_0(2, 2), G_cart_0(3, 3)}, freqs, struct('phase', true))
legend({'$F_x \rightarrow D_x$', '$F_y \rightarrow D_y$', '$F_z \rightarrow D_z$'})
exportFig('hexapod_cart_trans', 'normal-normal')
bodeFig({G_cart_0(4, 4), G_cart_0(5, 5), G_cart_0(6, 6)}, freqs, struct('phase', true))
legend({'$M_x \rightarrow R_x$', '$M_y \rightarrow R_y$', '$M_z \rightarrow R_z$'})
exportFig('hexapod_cart_rot', 'normal-normal')
bodeFig({G_cart_0(1, 1), G_cart_0(2, 1), G_cart_0(3, 1)}, freqs, struct('phase', true))
legend({'$F_x \rightarrow D_x$', '$F_x \rightarrow D_y$', '$F_x \rightarrow D_z$'})
exportFig('hexapod_cart_coupling', 'normal-normal')
%% Save identify transfer functions
save('./mat/G_cart.mat', 'G_cart_0', 'G_cart_10', 'G_cart_50');

45
identification_legs.m
View File

@ -1,45 +0,0 @@
%% Script Description
% Script used to identify the transfer functions of the
% Stewart platform (from actuator to displacement)
%%
clear; close all; clc;
%%
initializeNanoHexapod();
%%
initializeSample(struct('mass', 0));
G_legs_0 = identifyPlantLegs();
%%
initializeSample(struct('mass', 10));
G_legs_10 = identifyPlantLegs();
%%
initializeSample(struct('mass', 50));
G_legs_50 = identifyPlantLegs();
%%
freqs = logspace(1, 4, 1000);
bodeFig({G_legs_0(1, 1), G_legs_10(1, 1), G_legs_50(1, 1)}, freqs, struct('phase', true))
legend({'$F_i \rightarrow D_i$ - $M = 0Kg$', '$F_i \rightarrow D_i$ - $M = 10Kg$', '$F_i \rightarrow D_i$ - $M = 50Kg$'})
legend('location', 'southwest')
exportFig('hexapod_legs_mass', 'normal-tall')
%%
freqs = logspace(1, 4, 1000);
bodeFig({G_legs_0(1, 2), G_legs_10(1, 2), G_legs_50(1, 2)}, freqs, struct('phase', true))
legend({'$F_i \rightarrow D_j$ - $M = 0Kg$', '$F_i \rightarrow D_j$ - $M = 10Kg$', '$F_i \rightarrow D_j$ - $M = 50Kg$'})
legend('location', 'southwest')
exportFig('hexapod_legs_coupling_mass', 'normal-tall')
%% Save identify transfer functions
save('./mat/G_legs.mat', 'G_legs_0', 'G_legs_10', 'G_legs_50');

6
init_simulink.m
View File

@ -1,6 +0,0 @@
%% Script Description
%
%% Load the sample and the stewart platform
load('./mat/sample.mat', 'sample')
load('./mat/stewart.mat', 'stewart')

BIN
library/QuaternionToAngles.slx Normal file
View File

Binary file not shown.

BIN
library/compute_jacobian.slx Normal file
View File

Binary file not shown.

11
main.m Normal file
View File

@ -0,0 +1,11 @@
%%
clear; close all; clc;
%% Configuration File
run config.m
%% Identification
open id_main.m
%% Active Damping
open act_damp_main.m

6
readme.org
View File

@ -1,6 +0,0 @@
#+TITLE: Stewart Platform using Simscape
* TODOS
** TODO Add functions to identify transmissibility and sensitivity
** TODO Rewrite the script to study the effect of various parameters on the stiffness/stroke/...
** TODO Rewrite the function to study the jacobian, or delete it.

7
src/getJacobianMatrix.m
View File

@ -1,7 +0,0 @@
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

6
src/getStiffnessMatrix.m
View File

@ -1,3 +1,5 @@
function [K] = getStiffnessMatrix(leg, J)
K = leg.k.ax*(J'*J);
function [K] = getStiffnessMatrix(k, J)
% k - leg stiffness
% J - Jacobian matrix
K = k*(J'*J);
end

51
src/identifyPlant.m Normal file
View File

@ -0,0 +1,51 @@
function [sys] = identifyPlant(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
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_identification';
%% Input/Output definition
io(1) = linio([mdl, '/F'], 1, 'input'); % Cartesian forces
io(2) = linio([mdl, '/Fl'], 1, 'input'); % Leg forces
io(3) = linio([mdl, '/Fd'], 1, 'input'); % Direct forces
io(4) = linio([mdl, '/Dw'], 1, 'input'); % Base motion
io(5) = linio([mdl, '/Dm'], 1, 'output'); % Relative Motion
io(6) = linio([mdl, '/Dlm'], 1, 'output'); % Displacement of each leg
io(7) = linio([mdl, '/Flm'], 1, 'output'); % Force sensor in each leg
io(8) = linio([mdl, '/Xm'], 1, 'output'); % Absolute motion of platform
%% Run the linearization
G = linearize(mdl, io, 0);
%% Input/Output names
G.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz', ...
'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ...
'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz', ...
'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz'};
G.OutputName = {'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm', ...
'D1m', 'D2m', 'D3m', 'D4m', 'D5m', 'D6m', ...
'F1m', 'F2m', 'F3m', 'F4m', 'F5m', 'F6m', ...
'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'};
%% Cut into sub transfer functions
sys.G_cart = minreal(G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}));
sys.G_forc = minreal(G({'F1m', 'F2m', 'F3m', 'F4m', 'F5m', 'F6m'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
sys.G_legs = G({'D1m', 'D2m', 'D3m', 'D4m', 'D5m', 'D6m'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'});
sys.G_tran = minreal(G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz'}));
sys.G_comp = minreal(G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}));
sys.G_iner = minreal(G({'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'}, {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}));
sys.G_all = minreal(G);
end

35
src/identifyPlantCart.m
View File

@ -1,35 +0,0 @@
function [G_cart, G_cart_raw] = identifyPlantCart()
%% 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 = 'stewart_simscape';
%% Centralized control (Cartesian coordinates)
% Input/Output definition
io(1) = linio([mdl, '/F_cart'],1,'input');
io(2) = linio([mdl, '/Stewart_Platform'],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 = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_cart.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
end

35
src/identifyPlantLegs.m
View File

@ -1,35 +0,0 @@
function [G_legs, G_legs_raw] = identifyPlantLegs()
%% 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 = 'stewart_simscape';
%% Centralized control (Cartesian coordinates)
% Input/Output definition
io(1) = linio([mdl, '/F_legs'], 1,'input');
io(2) = linio([mdl, '/Stewart_Platform'],2,'output');
% Run the linearization
G_legs_raw = linearize(mdl,io, 0);
G_legs = preprocessIdTf(G_legs_raw, opts.f_low, opts.f_high);
% Input/Output names
G_legs.InputName = {'F1', 'F2', 'F3', 'M4', 'M5', 'M6'};
G_legs.OutputName = {'D1', 'D2', 'D3', 'R4', 'R5', 'R6'};
end

208
src/initializeHexapod.m Normal file
View File

@ -0,0 +1,208 @@
function [stewart] = initializeHexapod(opts_param)
%% Default values for opts
opts = struct(...
'height', 90, ... % Height of the platform [mm]
'jacobian', 150, ... % Jacobian offset [mm]
'density', 8000, ... % Density of hexapod [mm]
'name', 'stewart' ... % Name of the file
);
%% 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
stewart = struct();
stewart.h = opts.height; % Total height of the platform [mm]
stewart.jacobian = opts.jacobian; % distance from the center of the top platform
% where the jacobian is computed [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 = opts.density; % Density of the material [kg/m3]
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 = opts.density; % Density of the material [kg/m3]
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]
if strcmp(opts.actuator, 'piezo')
Leg.k.ax = 1e7; % Stiffness of each leg [N/m]
Leg.c.ax = 500; % [N/(m/s)]
elseif strcmp(opts.actuator, 'lorentz')
Leg.k.ax = 1e4; % Stiffness of each leg [N/m]
Leg.c.ax = 200; % [N/(m/s)]
elseif isnumeric(opts.actuator)
Leg.k.ax = opts.actuator; % Stiffness of each leg [N/m]
Leg.c.ax = 100; % [N/(m/s)]
else
error('opts.actuator should be piezo or lorentz or numeric value');
end
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 = opts.density; % Density of the material [kg/m3]
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]
%% Sphere
SP = struct();
SP.height.bottom = 15; % [mm]
SP.height.top = 15; % [mm]
SP.density.bottom = opts.density; % [kg/m^3]
SP.density.top = opts.density; % [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.c.ax = 0; % [N*m/deg]
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]
%%
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];
%%
stewart.BP = BP;
stewart.TP = TP;
stewart.Leg = Leg;
stewart.SP = SP;
%%
stewart = initializeParameters(stewart);
%%
save('./mat/stewart.mat', 'stewart')
%% ==============================================================
% Additional Functions
% ===============================================================
%% Initialize Parameters
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 [m] TODO
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;
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
% TODO
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
% TODO
% aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
bb = stewart.pos_top_tranform - (stewart.TP.thickness + stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
bb = bb - stewart.jacobian*1e-3*[zeros(6, 2),ones(6, 1)];
stewart.J = getJacobianMatrix(leg_vectors', bb');
stewart.K = stewart.Leg.k.ax*stewart.J'*stewart.J;
end
%% Compute the Jacobian Matrix
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

92
src/initializeMicroHexapod.m
View File

@ -1,92 +0,0 @@
function [stewart] = initializeMicroHexapod()
%% Stewart Object
stewart = struct();
stewart.h = 350; % Total height of the platform [mm]
stewart.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];
%%
stewart.BP = BP;
stewart.TP = TP;
stewart.Leg = Leg;
stewart.SP = SP;
%%
stewart = initializeParameters(stewart);
%%
save('./mat/hexapod.mat', 'stewart');
%%
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
end

93
src/initializeNanoHexapod.m
View File

@ -1,93 +0,0 @@
function [stewart] = initializeNanoHexapod()
%% Stewart Object
stewart = struct();
stewart.h = 90; % Total height of the platform [mm]
stewart.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];
%%
stewart.BP = BP;
stewart.TP = TP;
stewart.Leg = Leg;
stewart.SP = SP;
%%
stewart = initializeParameters(stewart);
%%
save('./mat/stewart.mat', 'stewart')
%%
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
end

80
src/initializeParameters.m
View File

@ -1,80 +0,0 @@
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

10
src/initializeSample.m
View File

@ -1,9 +1,11 @@
function [] = initializeSample(opts_param)
%% Default values for opts
sample = struct('radius', 100,...
'height', 300,...
'mass', 50,...
'offset', 0,...
sample = struct( ...
'radius', 100, ... % radius of the cylinder [mm]
'height', 300, ... % height of the cylinder [mm]
'mass', 50, ... % mass of the cylinder [kg]
'measheight', 150, ... % measurement point z-offset [mm]
'offset', [0, 0, 0], ... % offset position of the sample [mm]
'color', [0.9 0.1 0.1] ...
);

BIN
stewart_displacement.slx
View File

Binary file not shown.

BIN
stewart_identification.slx Normal file
View File

Binary file not shown.

BIN
stewart_kinematics.slx Normal file
View File

Binary file not shown.

BIN
stewart_simscape.slx
View File

Binary file not shown.

BIN
stewart_without_sensing.slx
View File

Binary file not shown.

44
studies/com.m Normal file
View File

@ -0,0 +1,44 @@
clear; close all; clc;
%% System - center of mass in the plane of joints
initializeHexapod(struct('actuator', 'piezo', 'jacobian', 150, 'density', 0.1));
initializeSample(struct('mass', 50, 'height', 300, 'measheight', 150));
%% Identification
G_aligned = identifyPlant();
%%
initializeHexapod(struct('actuator', 'piezo', 'jacobian', 160, 'density', 0.1));
initializeSample(struct('mass', 50, 'height', 300, 'measheight', 160));
%% Identification
G_com = identifyPlant();
%%
freqs = logspace(0, 3, 2000);
%%
bode_opts = bodeoptions;
bode_opts.FreqUnits = 'Hz';
bode_opts.MagUnits = 'abs';
bode_opts.MagScale = 'log';
bode_opts.PhaseVisible = 'off';
%%
figure;
bode(G_aligned.G_cart, G_com.G_cart, 2*pi*freqs, bode_opts);
exportFig('G_com', 'wide-tall', struct('path', 'studies'));
%%
initializeHexapod(struct('actuator', 'piezo', 'jacobian', 150, 'density', 0.1));
initializeSample(struct('mass', 1, 'height', 300, 'measheight', 150));
%% Identification
G_massless = identifyPlant();
%%
figure;
bode(G_aligned.G_cart, G_massless.G_cart, 2*pi*freqs, bode_opts);

41
studies/coupling.m Normal file
View File

@ -0,0 +1,41 @@
clear; close all; clc;
%% System - center of mass in the plane of joints
initializeHexapod(struct('actuator', 'piezo', 'jacobian', -25, 'density', 0.1));
initializeSample(struct('mass', 50, 'height', 1, 'measheight', -25, 'offset', [0, 0, -25.5]));
%% Identification
G_aligned = identifyPlant();
%%
freqs = logspace(0, 3, 2000);
%%
bode_opts = bodeoptions;
bode_opts.FreqUnits = 'Hz';
bode_opts.MagUnits = 'abs';
bode_opts.MagScale = 'log';
bode_opts.PhaseVisible = 'off';
%%
figure;
bode(G_aligned.G_legs, 2*pi*freqs, bode_opts);
%% Change height of stewart platform
for height = [50, 70, 90, 110, 130]
initializeHexapod(struct('actuator', 'piezo', 'jacobian', -25, 'density', 0.1, 'height', height));
G.(['h_' num2str(height)]) = identifyPlant();
end
%%
figure;
bode( ...
G.h_50.G_legs, ...
G.h_70.G_legs, ...
G.h_90.G_legs, ...
G.h_110.G_legs, ...
G.h_130.G_legs, ...
2*pi*freqs, bode_opts);
% legend({'60', '80', '100', '120', '140'})

26
studies/stiffness_matrix.m Normal file
View File

@ -0,0 +1,26 @@
%%
clear; close all; clc;
%%
K_iff = tf(zeros(6));
save('./mat/controllers.mat', 'K_iff', '-append');
%% Initialize System
hexapod = initializeHexapod(struct('actuator', 'piezo', 'jacobian', 150));
initializeSample(struct('mass', 50, 'height', 300, 'measheight', 150));
%% Identify transfer functions
G = identifyPlant();
%% Run to obtain the computed Jacobian
sim stewart_identification
%% Compare the two Jacobian matrices
J_rel = (J.data(:, :, 1)-hexapod.J)./hexapod.J;
%% Compute the Stiffness Matrix
K = hexapod.Leg.k.ax*hexapod.J'*hexapod.J;
K_id = pinv(freqresp(G.G_cart, 0));
K_rel = (K-K_id)./K;

4
study_architecture.m
View File

@ -1,12 +1,8 @@
%% Script Description
%
%%
clear; close all; clc;
%%
run stewart_parameters.m
format shortE
%% Study the effect of the radius of the top platform position of the legs
leg_radius = 50:1:120;