Add index.org file with matlab functions included
This commit is contained in:
@@ -0,0 +1,2 @@
|
|||||||
|
Figures/
|
||||||
|
slprj/
|
||||||
@@ -0,0 +1,362 @@
|
|||||||
|
#+TITLE: Stewart Platform with Simscape
|
||||||
|
|
||||||
|
* Functions
|
||||||
|
:PROPERTIES:
|
||||||
|
:HEADER-ARGS:matlab+: :exports code
|
||||||
|
:HEADER-ARGS:matlab+: :comments no
|
||||||
|
:HEADER-ARGS:matlab+: :mkdir yes
|
||||||
|
:HEADER-ARGS:matlab+: :eval no
|
||||||
|
:END:
|
||||||
|
** getMaxPositions
|
||||||
|
:PROPERTIES:
|
||||||
|
:HEADER-ARGS:matlab+: :tangle src/getMaxPositions.m
|
||||||
|
:END:
|
||||||
|
#+begin_src matlab
|
||||||
|
function [X, Y, Z] = getMaxPositions(stewart)
|
||||||
|
Leg = stewart.Leg;
|
||||||
|
J = stewart.J;
|
||||||
|
theta = linspace(0, 2*pi, 100);
|
||||||
|
phi = linspace(-pi/2 , pi/2, 100);
|
||||||
|
dmax = zeros(length(theta), length(phi));
|
||||||
|
|
||||||
|
for i = 1:length(theta)
|
||||||
|
for j = 1:length(phi)
|
||||||
|
L = J*[cos(phi(j))*cos(theta(i)) cos(phi(j))*sin(theta(i)) sin(phi(j)) 0 0 0]';
|
||||||
|
dmax(i, j) = Leg.stroke/max(abs(L));
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
||||||
|
X = dmax.*cos(repmat(phi,length(theta),1)).*cos(repmat(theta,length(phi),1))';
|
||||||
|
Y = dmax.*cos(repmat(phi,length(theta),1)).*sin(repmat(theta,length(phi),1))';
|
||||||
|
Z = dmax.*sin(repmat(phi,length(theta),1));
|
||||||
|
end
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** getMaxPureDisplacement
|
||||||
|
:PROPERTIES:
|
||||||
|
:HEADER-ARGS:matlab+: :tangle src/getMaxPureDisplacement.m
|
||||||
|
:END:
|
||||||
|
#+begin_src matlab
|
||||||
|
function [max_disp] = getMaxPureDisplacement(Leg, J)
|
||||||
|
max_disp = zeros(6, 1);
|
||||||
|
max_disp(1) = Leg.stroke/max(abs(J*[1 0 0 0 0 0]'));
|
||||||
|
max_disp(2) = Leg.stroke/max(abs(J*[0 1 0 0 0 0]'));
|
||||||
|
max_disp(3) = Leg.stroke/max(abs(J*[0 0 1 0 0 0]'));
|
||||||
|
max_disp(4) = Leg.stroke/max(abs(J*[0 0 0 1 0 0]'));
|
||||||
|
max_disp(5) = Leg.stroke/max(abs(J*[0 0 0 0 1 0]'));
|
||||||
|
max_disp(6) = Leg.stroke/max(abs(J*[0 0 0 0 0 1]'));
|
||||||
|
end
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** getStiffnessMatrix
|
||||||
|
:PROPERTIES:
|
||||||
|
:HEADER-ARGS:matlab+: :tangle src/getStiffnessMatrix.m
|
||||||
|
:END:
|
||||||
|
#+begin_src matlab
|
||||||
|
function [K] = getStiffnessMatrix(k, J)
|
||||||
|
% k - leg stiffness
|
||||||
|
% J - Jacobian matrix
|
||||||
|
K = k*(J'*J);
|
||||||
|
end
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** identifyPlant
|
||||||
|
:PROPERTIES:
|
||||||
|
:HEADER-ARGS:matlab+: :tangle src/identifyPlant.m
|
||||||
|
:END:
|
||||||
|
#+begin_src matlab
|
||||||
|
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
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** initializeHexapod
|
||||||
|
:PROPERTIES:
|
||||||
|
:HEADER-ARGS:matlab+: :tangle src/initializeHexapod.m
|
||||||
|
:END:
|
||||||
|
#+begin_src matlab
|
||||||
|
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
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** initializeSample
|
||||||
|
:PROPERTIES:
|
||||||
|
:HEADER-ARGS:matlab+: :tangle src/initializeSample.m
|
||||||
|
:END:
|
||||||
|
#+begin_src matlab
|
||||||
|
function [] = initializeSample(opts_param)
|
||||||
|
%% Default values for opts
|
||||||
|
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] ...
|
||||||
|
);
|
||||||
|
|
||||||
|
%% 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
|
||||||
|
#+end_src
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user