diff --git a/identification_control.m b/identification_control.m index 908c321..a7b6f78 100644 --- a/identification_control.m +++ b/identification_control.m @@ -1,65 +1,69 @@ %% Script Description -% Script used to identify the transfer functions of the +% Script used to identify the transfer functions of the % Stewart platform (from actuator to displacement) %% -clear; -close all; -clc +clear; close all; clc; -%% Define options for bode plots -bode_opts = bodeoptions; +%% +initializeNanoHexapod(); -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'; +%% +initializeSample(struct('mass', 0)); -%% Options for Linearized -options = linearizeOptions; -options.SampleTime = 0; +G_cart_0 = getPlantCart(); -%% Name of the Simulink File -mdl = 'stewart_simscape'; +%% +initializeSample(struct('mass', 10)); -%% Centralized control (Cartesian coordinates) -% Input/Output definition -io(1) = linio([mdl, '/F_cart'],1,'input'); -io(2) = linio([mdl, '/Stewart_Platform'],1,'output'); +G_cart_10 = getPlantCart(); -% Run the linearization -G_cart = linearize(mdl,io, 0); +%% +initializeSample(struct('mass', 50)); -% Input/Output names -G_cart.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; -G_cart.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; +G_cart_50 = getPlantCart(); +%% +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(1, 1), G_cart(2, 2), G_cart(3, 3)}, freqs, struct('phase', true)) +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(4, 4), G_cart(5, 5), G_cart(6, 6)}, freqs, struct('phase', true)) +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(1, 1), G_cart(2, 1), G_cart(3, 1)}, freqs, struct('phase', true)) +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'); + %% 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 = linearize(mdl,io, 0); +G_legs_raw = linearize(mdl,io, 0); + +G_legs = preprocessIdTf(G_legs_raw, 10, 10000); % Input/Output names G_legs.InputName = {'F1', 'F2', 'F3', 'M4', 'M5', 'M6'}; @@ -75,3 +79,5 @@ exportFig('hexapod_legs', 'normal-normal') bodeFig({G_legs(1, 1), G_legs(2, 1)}, freqs, struct('phase', true)) legend({'$F_i \rightarrow D_i$', '$F_i \rightarrow D_j$'}) exportFig('hexapod_legs_coupling', 'normal-normal') + +save('mat/G_legs.mat', 'G_legs'); diff --git a/init_simulink.m b/init_simulink.m index eca79df..60c9efc 100644 --- a/init_simulink.m +++ b/init_simulink.m @@ -1,7 +1,2 @@ -params_micro_hexapod; -micro_hexapod = stewart; - -params_nano_hexapod; -nano_hexapod = stewart; - -clear stewart; +load('./mat/sample.mat', 'sample') +load('./mat/stewart.mat', 'stewart') diff --git a/params_micro_hexapod.m b/params_micro_hexapod.m index 9be175d..83db096 100644 --- a/params_micro_hexapod.m +++ b/params_micro_hexapod.m @@ -31,11 +31,11 @@ TP.shape = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext 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.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] diff --git a/params_nano_hexapod.m b/params_nano_hexapod.m index 105bd11..b6ac2c4 100644 --- a/params_nano_hexapod.m +++ b/params_nano_hexapod.m @@ -1,17 +1,17 @@ %% 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] +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.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.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]; @@ -19,11 +19,11 @@ BP.shape = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext TP = struct(); TP.rad.int = 0; % Internal Radius [mm] -TP.rad.ext = 100; % Internal Radius [mm] -TP.thickness = 10; % Thickness [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.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]; @@ -31,11 +31,11 @@ TP.shape = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext 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.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] diff --git a/params_sample.m b/params_sample.m new file mode 100644 index 0000000..e69de29 diff --git a/src/getPlantCart.m b/src/getPlantCart.m new file mode 100644 index 0000000..9994def --- /dev/null +++ b/src/getPlantCart.m @@ -0,0 +1,35 @@ +function [G_cart, G_cart_raw] = getPlantCart() + %% 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 + diff --git a/src/initializeMicroHexapod.m b/src/initializeMicroHexapod.m new file mode 100644 index 0000000..5f1144d --- /dev/null +++ b/src/initializeMicroHexapod.m @@ -0,0 +1,92 @@ +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 diff --git a/src/initializeNanoHexapod.m b/src/initializeNanoHexapod.m new file mode 100644 index 0000000..4a63041 --- /dev/null +++ b/src/initializeNanoHexapod.m @@ -0,0 +1,93 @@ +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 diff --git a/src/initializeSample.m b/src/initializeSample.m new file mode 100644 index 0000000..ae9d8e7 --- /dev/null +++ b/src/initializeSample.m @@ -0,0 +1,19 @@ +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 + save('./mat/sample.mat', 'sample'); +end diff --git a/stewart_displacement.slx b/stewart_displacement.slx index 2b06f07..a914947 100644 Binary files a/stewart_displacement.slx and b/stewart_displacement.slx differ diff --git a/stewart_simscape.slx b/stewart_simscape.slx index b4cf824..85f43d1 100644 Binary files a/stewart_simscape.slx and b/stewart_simscape.slx differ