Add sample on top of hexapod. Add function to initialize hexapod.

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;
+clear; close all; clc;
-close all;
-clc
 
-%% Define options for bode plots
+%%
-bode_opts = bodeoptions;
+initializeNanoHexapod();
 
-bode_opts.Title.FontSize  = 12;
+%%
-bode_opts.XLabel.FontSize = 12;
+initializeSample(struct('mass', 0));
-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';
 
-%% Options for Linearized
+G_cart_0 = getPlantCart();
-options = linearizeOptions;
-options.SampleTime = 0;
 
-%% Name of the Simulink File
+%%
-mdl = 'stewart_simscape';
+initializeSample(struct('mass', 10));
 
-%% Centralized control (Cartesian coordinates)
+G_cart_10 = getPlantCart();
-% Input/Output definition
-io(1) = linio([mdl, '/F_cart'],1,'input');
-io(2) = linio([mdl, '/Stewart_Platform'],1,'output');
 
-% Run the linearization
+%%
-G_cart = linearize(mdl,io, 0);
+initializeSample(struct('mass', 50));
 
-% Input/Output names
+G_cart_50 = getPlantCart();
-G_cart.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
-G_cart.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
 
+%%
+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');

init_simulink.m

@@ -1,7 +1,2 @@
-params_micro_hexapod;
+load('./mat/sample.mat', 'sample')
-micro_hexapod = stewart;
+load('./mat/stewart.mat', 'stewart')
-
-params_nano_hexapod;
-nano_hexapod = stewart;
-
-clear stewart;

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.k.ax       = 5e7;   % Stiffness of each leg [N/m]
-Leg.ksi.ax     = 3; % Maximum amplification at resonance []
+Leg.ksi.ax     = 3;     % Maximum amplification at resonance []
-Leg.rad.bottom = 25; % Radius of the cylinder of the bottom part [mm]
+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.rad.top    = 17;    % Radius of the cylinder of the top part [mm]
-Leg.density    = 8000; % Density of the material [kg/m^3]
+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]
 

params_nano_hexapod.m

@@ -1,17 +1,17 @@
 %% Stewart Object
 stewart = struct();
-stewart.h        = 90; % Total height of the platform [mm]
+stewart.h        = 90;    % Total height of the platform [mm]
-stewart.jacobian = 174.5;   % Point where the Jacobian is computed => Center of rotation [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.rad.ext   = 100; % Internal Radius [mm]
-TP.thickness = 10;   % Thickness [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.leg.ang   = 5;   % Angle Offset [deg]
-TP.density   = 8000; % Density of the material [kg/m^3]
+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.k.ax       = 5e7;   % Stiffness of each leg [N/m]
-Leg.ksi.ax     = 10; % Maximum amplification at resonance []
+Leg.ksi.ax     = 10;    % Maximum amplification at resonance []
-Leg.rad.bottom = 12; % Radius of the cylinder of the bottom part [mm]
+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.rad.top    = 10;    % Radius of the cylinder of the top part [mm]
-Leg.density    = 8000; % Density of the material [kg/m^3]
+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]
 

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
+

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

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

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