67 lines
2.8 KiB
Matlab
67 lines
2.8 KiB
Matlab
function [stewart] = generateDeltaRobot(stewart, args)
|
|
% generateDeltaRobot - Generate a Delta Robot Configuration
|
|
%
|
|
% Syntax: [stewart] = generateDeltaRobot(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Can have the following fields:
|
|
% - d [1x1] - Size of the cube [m]
|
|
% - b [1x1] - Distance between top joints bi and cube's vertices [m]
|
|
% - L [1x1] - Length of the struts (i.e. distances between ai and bi) [m]
|
|
|
|
arguments
|
|
stewart
|
|
args.d (1,1) double {mustBeNumeric, mustBePositive} = 70e-3
|
|
args.b (1,1) double {mustBeNumeric, mustBeNonnegative} = 50e-3
|
|
args.L (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
|
args.m_top (1,1) double {mustBeNumeric, mustBePositive} = 0.3 % Mass of the top platform
|
|
end
|
|
|
|
% We define the useful vertices of the cube with respect to the Cube's center
|
|
% ${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is located at the center of the cube and aligned with {F} and {M}
|
|
|
|
sx = [ 2; -1; -1];
|
|
sy = [ 0; 1; -1];
|
|
sz = [ 1; 1; 1];
|
|
|
|
R = [sx, sy, sz]./vecnorm([sx, sy, sz]);
|
|
|
|
% Coordinates of the cube's vertices
|
|
Cc = R'*[[args.d/2;-args.d/2;-args.d/2],[args.d/2;args.d/2;-args.d/2],[-args.d/2;args.d/2;-args.d/2],[-args.d/2;args.d/2;args.d/2],[-args.d/2;-args.d/2;args.d/2],[args.d/2;-args.d/2;args.d/2]];
|
|
|
|
% Coordinates of the cube's vertices corresponding to the six struts, expresssed with respect to the cube's center
|
|
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(:,6), Cc(:,4), Cc(:,2), Cc(:,6), Cc(:,4), Cc(:,2)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg
|
|
|
|
% We can compute the vector of each leg si
|
|
CSi = (CCm - CCf)./vecnorm(CCm - CCf);
|
|
|
|
% We now compute the position of the joints ai and bi with respect to the cube's center
|
|
Ca = CCf - (args.b + args.L).*CSi;
|
|
Cb = CCf - args.b.*CSi;
|
|
|
|
%% Computes relative position of frames
|
|
FO_M = [0; 0; args.L*CSi(3,1)]; % Position of {M} with respect to {F} [m]
|
|
|
|
MO_B = [0; 0; (args.b+args.d/2)*CSi(3,1)]; % Position of {B} with respect to {M} [m]
|
|
|
|
FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
|
|
|
|
stewart.geometry.d = args.d; % Size of the cube
|
|
stewart.geometry.H = args.L*CSi(3,1); % Height between {M} and {F} frames
|
|
stewart.geometry.FO_M = FO_M;
|
|
stewart.platform_M.MO_B = MO_B;
|
|
stewart.platform_F.FO_A = FO_A;
|
|
|
|
%% Now ai and bi can be expressed with respect to the {F} and {M} frames
|
|
Fa = Ca + FO_A;
|
|
Mb = Cb + MO_B;
|
|
|
|
%%
|
|
stewart.platform_F.Ca = Ca; % Position of bottom joints ai with respect to {F}
|
|
stewart.platform_F.Fa = Fa; % Position of bottom joints ai with respect to {F}
|
|
stewart.platform_M.Mb = Mb; % Position of top joints bi with respect to {M}
|
|
stewart.platform_M.Cb = Cb; % Position of top joints bi with respect to {M}
|
|
|
|
stewart.platform_M.m = args.m_top;
|