143 lines
7.4 KiB
Matlab
143 lines
7.4 KiB
Matlab
function [nano_hexapod] = initializeNanoHexapod(args)
|
|
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'init'})} = 'flexible'
|
|
% initializeFramesPositions
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 95e-3
|
|
args.MO_B (1,1) double {mustBeNumeric} = 170e-3
|
|
% generateGeneralConfiguration
|
|
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
|
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
|
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
|
|
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
|
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
|
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
|
|
% initializeStrutDynamics
|
|
args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz', 'amplified'})} = 'piezo'
|
|
args.k1 (1,1) double {mustBeNumeric} = 1e6
|
|
args.ke (1,1) double {mustBeNumeric} = 5e6
|
|
args.ka (1,1) double {mustBeNumeric} = 60e6
|
|
args.c1 (1,1) double {mustBeNumeric} = 10
|
|
args.F_gain (1,1) double {mustBeNumeric} = 1
|
|
args.k (1,1) double {mustBeNumeric} = -1
|
|
args.c (1,1) double {mustBeNumeric} = -1
|
|
% initializeJointDynamics
|
|
args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'universal'
|
|
args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'spherical'
|
|
args.Ka_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
|
|
args.Ca_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
|
args.Kr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
|
|
args.Cr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
|
args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
|
|
args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
|
args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
|
|
args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
|
args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
|
|
args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
|
args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
|
|
args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
|
args.Ka_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
|
|
args.Ca_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
|
args.Kr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
|
|
args.Cr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
|
% initializeCylindricalPlatforms
|
|
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
|
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
|
|
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
|
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 120e-3
|
|
% initializeCylindricalStruts
|
|
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
|
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
|
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
|
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
|
% inverseKinematics
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
% Equilibrium position of each leg
|
|
args.dLeq (6,1) double {mustBeNumeric} = zeros(6,1)
|
|
% Force that stiffness of each joint should apply at t=0
|
|
args.Foffset logical {mustBeNumericOrLogical} = false
|
|
end
|
|
|
|
stewart = initializeStewartPlatform();
|
|
|
|
stewart = initializeFramesPositions(stewart, 'H', args.H, 'MO_B', args.MO_B);
|
|
|
|
stewart = generateGeneralConfiguration(stewart, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh);
|
|
|
|
stewart = computeJointsPose(stewart);
|
|
|
|
if args.k > 0 && args.c > 0
|
|
stewart = initializeStrutDynamics(stewart, 'type', 'classical', 'K', args.k*ones(6,1), 'C', args.c*ones(6,1));
|
|
elseif args.k > 0
|
|
stewart = initializeStrutDynamics(stewart, 'type', 'classical', 'K', args.k*ones(6,1), 'C', 1.5*sqrt(args.k)*ones(6,1));
|
|
elseif strcmp(args.actuator, 'piezo')
|
|
stewart = initializeStrutDynamics(stewart, 'type', 'classical', 'K', 1e7*ones(6,1), 'C', 1e2*ones(6,1));
|
|
elseif strcmp(args.actuator, 'lorentz')
|
|
stewart = initializeStrutDynamics(stewart, 'type', 'classical', 'K', 1e4*ones(6,1), 'C', 1e2*ones(6,1));
|
|
elseif strcmp(args.actuator, 'amplified')
|
|
stewart = initializeStrutDynamics(stewart, 'type', 'amplified', ...
|
|
'k1', args.k1*ones(6,1), ...
|
|
'c1', args.c1*ones(6,1), ...
|
|
'ka', args.ka*ones(6,1), ...
|
|
'ke', args.ke*ones(6,1), ...
|
|
'F_gain', args.F_gain*ones(6,1));
|
|
else
|
|
error('args.actuator should be piezo, lorentz or amplified');
|
|
end
|
|
|
|
stewart = initializeJointDynamics(stewart, ...
|
|
'type_F', args.type_F, ...
|
|
'type_M', args.type_M, ...
|
|
'Kf_M', args.Kf_M, ...
|
|
'Cf_M', args.Cf_M, ...
|
|
'Kt_M', args.Kt_M, ...
|
|
'Ct_M', args.Ct_M, ...
|
|
'Kf_F', args.Kf_F, ...
|
|
'Cf_F', args.Cf_F, ...
|
|
'Kt_F', args.Kt_F, ...
|
|
'Ct_F', args.Ct_F, ...
|
|
'Ka_F', args.Ka_F, ...
|
|
'Ca_F', args.Ca_F, ...
|
|
'Kr_F', args.Kr_F, ...
|
|
'Cr_F', args.Cr_F, ...
|
|
'Ka_M', args.Ka_M, ...
|
|
'Ca_M', args.Ca_M, ...
|
|
'Kr_M', args.Kr_M, ...
|
|
'Cr_M', args.Cr_M);
|
|
|
|
stewart = initializeCylindricalPlatforms(stewart, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
|
|
|
|
stewart = initializeCylindricalStruts(stewart, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
|
|
|
|
stewart = computeJacobian(stewart);
|
|
|
|
stewart = initializeStewartPose(stewart, 'AP', args.AP, 'ARB', args.ARB);
|
|
|
|
stewart = initializeInertialSensor(stewart, 'type', 'accelerometer');
|
|
|
|
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
|
|
load('mat/Foffset.mat', 'Fnm');
|
|
stewart.actuators.dLeq = -Fnm'./stewart.Ki;
|
|
else
|
|
stewart.actuators.dLeq = args.dLeq;
|
|
end
|
|
|
|
switch args.type
|
|
case 'none'
|
|
stewart.type = 0;
|
|
case 'rigid'
|
|
stewart.type = 1;
|
|
case 'flexible'
|
|
stewart.type = 2;
|
|
case 'init'
|
|
stewart.type = 4;
|
|
end
|
|
|
|
nano_hexapod = stewart;
|
|
save('./mat/stages.mat', 'nano_hexapod', '-append');
|