Add functions and documentation to initialize stewart platforms
This commit is contained in:
45
src/computeJointsPose.m
Normal file
45
src/computeJointsPose.m
Normal file
@@ -0,0 +1,45 @@
|
||||
function [stewart] = computeJointsPose(stewart)
|
||||
% computeJointsPose -
|
||||
%
|
||||
% Syntax: [stewart] = computeJointsPose(stewart, opts_param)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - FO_A [3x1] - Position of {A} with respect to {F}
|
||||
% - MO_B [3x1] - Position of {B} with respect to {M}
|
||||
% - FO_M [3x1] - Position of {M} with respect to {F}
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following added fields
|
||||
% - Aa [3x6] - The i'th column is the position of ai with respect to {A}
|
||||
% - Ab [3x6] - The i'th column is the position of bi with respect to {A}
|
||||
% - Ba [3x6] - The i'th column is the position of ai with respect to {B}
|
||||
% - Bb [3x6] - The i'th column is the position of bi with respect to {B}
|
||||
% - l [6x1] - The i'th element is the initial length of strut i
|
||||
% - As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
|
||||
% - Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
|
||||
% - FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
|
||||
% - MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
|
||||
|
||||
stewart.Aa = stewart.Fa - repmat(stewart.FO_A, [1, 6]);
|
||||
stewart.Bb = stewart.Mb - repmat(stewart.MO_B, [1, 6]);
|
||||
|
||||
stewart.Ab = stewart.Bb - repmat(-stewart.MO_B-stewart.FO_M+stewart.FO_A, [1, 6]);
|
||||
stewart.Ba = stewart.Aa - repmat( stewart.MO_B+stewart.FO_M-stewart.FO_A, [1, 6]);
|
||||
|
||||
stewart.As = (stewart.Ab - stewart.Aa)./vecnorm(stewart.Ab - stewart.Aa); % As_i is the i'th vector of As
|
||||
|
||||
stewart.l = vecnorm(stewart.Ab - stewart.Aa)';
|
||||
|
||||
stewart.Bs = (stewart.Bb - stewart.Ba)./vecnorm(stewart.Bb - stewart.Ba);
|
||||
|
||||
stewart.FRa = zeros(3,3,6);
|
||||
stewart.MRb = zeros(3,3,6);
|
||||
|
||||
for i = 1:6
|
||||
stewart.FRa(:,:,i) = [cross([0;1;0], stewart.As(:,i)) , cross(stewart.As(:,i), cross([0;1;0], stewart.As(:,i))) , stewart.As(:,i)];
|
||||
stewart.FRa(:,:,i) = stewart.FRa(:,:,i)./vecnorm(stewart.FRa(:,:,i));
|
||||
|
||||
stewart.MRb(:,:,i) = [cross([0;1;0], stewart.Bs(:,i)) , cross(stewart.Bs(:,i), cross([0;1;0], stewart.Bs(:,i))) , stewart.Bs(:,i)];
|
||||
stewart.MRb(:,:,i) = stewart.MRb(:,:,i)./vecnorm(stewart.MRb(:,:,i));
|
||||
end
|
||||
48
src/generateCubicConfiguration.m
Normal file
48
src/generateCubicConfiguration.m
Normal file
@@ -0,0 +1,48 @@
|
||||
function [stewart] = generateCubicConfiguration(stewart, opts_param)
|
||||
% generateCubicConfiguration -
|
||||
%
|
||||
% Syntax: [stewart] = generateCubicConfiguration(stewart, opts_param)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - the Stewart struct should have a parameter "H" corresponding to the total height of the platform
|
||||
% - opts_param - Structure with the following parameters
|
||||
% - Hc [1x1] - Height of the "useful" part of the cube [m]
|
||||
% - FOc [1x1] - Height of the center of the cute with respect to {F} [m]
|
||||
% - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m]
|
||||
% - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added parameters:
|
||||
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
|
||||
opts = struct( ...
|
||||
'Hc', 60e-3, ... % [m]
|
||||
'FOc', 50e-3, ... % [m]
|
||||
'FHa', 15e-3, ... % [m]
|
||||
'MHb', 15e-3 ... % [m]
|
||||
);
|
||||
|
||||
if exist('opts_param','var')
|
||||
for opt = fieldnames(opts_param)'
|
||||
opts.(opt{1}) = opts_param.(opt{1});
|
||||
end
|
||||
end
|
||||
|
||||
sx = [ 2; -1; -1];
|
||||
sy = [ 0; 1; -1];
|
||||
sz = [ 1; 1; 1];
|
||||
|
||||
R = [sx, sy, sz]./vecnorm([sx, sy, sz]);
|
||||
|
||||
L = opts.Hc*sqrt(3);
|
||||
|
||||
Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*opts.Hc];
|
||||
|
||||
CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg
|
||||
CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg
|
||||
|
||||
CSi = (CCm - CCf)./vecnorm(CCm - CCf);
|
||||
|
||||
stewart.Fa = CCf + [0; 0; opts.FOc] + ((opts.FHa-(opts.FOc-opts.Hc/2))./CSi(3,:)).*CSi;
|
||||
stewart.Mb = CCf + [0; 0; opts.FOc-stewart.H] + ((stewart.H-opts.MHb-(opts.FOc-opts.Hc/2))./CSi(3,:)).*CSi;
|
||||
37
src/initializeFramesPositions.m
Normal file
37
src/initializeFramesPositions.m
Normal file
@@ -0,0 +1,37 @@
|
||||
function [stewart] = initializeFramesPositions(opts_param)
|
||||
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
|
||||
%
|
||||
% Syntax: [stewart] = initializeFramesPositions(H, MO_B)
|
||||
%
|
||||
% Inputs:
|
||||
% - opts_param - Structure with the following fields:
|
||||
% - H [1x1] - Total Height of the Stewart Platform [m]
|
||||
% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following fields:
|
||||
% - H [1x1] - Total Height of the Stewart Platform [m]
|
||||
% - FO_M [3x1] - Position of {M} with respect to {F} [m]
|
||||
% - MO_B [3x1] - Position of {B} with respect to {M} [m]
|
||||
% - FO_A [3x1] - Position of {A} with respect to {F} [m]
|
||||
|
||||
opts = struct( ...
|
||||
'H', 90e-3, ... % [m]
|
||||
'MO_B', 50e-3 ... % [m]
|
||||
);
|
||||
|
||||
if exist('opts_param','var')
|
||||
for opt = fieldnames(opts_param)'
|
||||
opts.(opt{1}) = opts_param.(opt{1});
|
||||
end
|
||||
end
|
||||
|
||||
stewart = struct();
|
||||
|
||||
stewart.H = opts.H; % Total Height of the Stewart Platform [m]
|
||||
|
||||
stewart.FO_M = [0; 0; stewart.H]; % Position of {M} with respect to {F} [m]
|
||||
|
||||
stewart.MO_B = [0; 0; opts.MO_B]; % Position of {B} with respect to {M} [m]
|
||||
|
||||
stewart.FO_A = stewart.MO_B + stewart.FO_M; % Position of {A} with respect to {F} [m]
|
||||
28
src/initializeStrutDynamics.m
Normal file
28
src/initializeStrutDynamics.m
Normal file
@@ -0,0 +1,28 @@
|
||||
function [stewart] = initializeStrutDynamics(stewart, opts_param)
|
||||
% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
|
||||
%
|
||||
% Syntax: [stewart] = initializeStrutDynamics(opts_param)
|
||||
%
|
||||
% Inputs:
|
||||
% - opts_param - Structure with the following fields:
|
||||
% - Ki [6x1] - Stiffness of each strut [N/m]
|
||||
% - Ci [6x1] - Damping of each strut [N/(m/s)]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - Ki [6x1] - Stiffness of each strut [N/m]
|
||||
% - Ci [6x1] - Damping of each strut [N/(m/s)]
|
||||
|
||||
opts = struct( ...
|
||||
'Ki', 1e6*ones(6,1), ... % [N/m]
|
||||
'Ci', 1e2*ones(6,1) ... % [N/(m/s)]
|
||||
);
|
||||
|
||||
if exist('opts_param','var')
|
||||
for opt = fieldnames(opts_param)'
|
||||
opts.(opt{1}) = opts_param.(opt{1});
|
||||
end
|
||||
end
|
||||
|
||||
stewart.Ki = opts.Ki;
|
||||
stewart.Ci = opts.Ci;
|
||||
Reference in New Issue
Block a user