146 lines
8.6 KiB
Mathematica
146 lines
8.6 KiB
Mathematica
|
function [nano_hexapod] = initializeSimplifiedNanoHexapod(args)
|
||
|
|
|
||
|
|
arguments
|
||
|
|
%% initializeFramesPositions
|
||
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 95e-3 % Height of the nano-hexapod [m]
|
||
|
|
args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]
|
||
|
|
%% generateGeneralConfiguration
|
||
|
|
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 % Height of fixed joints [m]
|
||
|
|
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 120e-3 % Radius of fixed joints [m]
|
||
|
|
args.FTh (6,1) double {mustBeNumeric} = [220, 320, 340, 80, 100, 200]*(pi/180) % Angles of fixed joints [rad]
|
||
|
|
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 % Height of mobile joints [m]
|
||
|
|
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius of mobile joints [m]
|
||
|
|
args.MTh (6,1) double {mustBeNumeric} = [255, 285, 15, 45, 135, 165]*(pi/180) % Angles of fixed joints [rad]
|
||
|
|
%% Actuators
|
||
|
|
args.actuator_type char {mustBeMember(args.actuator_type,{'1dof', '2dof', 'flexible'})} = '1dof'
|
||
|
|
args.actuator_k (1,1) double {mustBeNumeric, mustBePositive} = 1e6
|
||
|
|
args.actuator_kp (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e4
|
||
|
|
args.actuator_ke (1,1) double {mustBeNumeric, mustBePositive} = 4952605
|
||
|
|
args.actuator_ka (1,1) double {mustBeNumeric, mustBePositive} = 2476302
|
||
|
|
args.actuator_c (1,1) double {mustBeNumeric, mustBePositive} = 50
|
||
|
|
args.actuator_cp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
args.actuator_ce (1,1) double {mustBeNumeric, mustBePositive} = 100
|
||
|
|
args.actuator_ca (1,1) double {mustBeNumeric, mustBePositive} = 50
|
||
|
|
%% initializeCylindricalPlatforms
|
||
|
|
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the fixed plate [kg]
|
||
|
|
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the fixed plate [m]
|
||
|
|
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the fixed plate [m]
|
||
|
|
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the mobile plate [kg]
|
||
|
|
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the mobile plate [m]
|
||
|
|
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the mobile plate [m]
|
||
|
|
%% initializeCylindricalStruts
|
||
|
|
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the fixed part of the strut [kg]
|
||
|
|
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the fixed part of the struts [m]
|
||
|
|
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m]
|
||
|
|
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the mobile part of the strut [kg]
|
||
|
|
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the mobile part of the struts [m]
|
||
|
|
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m]
|
||
|
|
%% Bottom and Top Flexible Joints
|
||
|
|
args.flex_type_F char {mustBeMember(args.flex_type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof'
|
||
|
|
args.flex_type_M char {mustBeMember(args.flex_type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof'
|
||
|
|
args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||
|
|
%% inverseKinematics
|
||
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||
|
|
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);
|
||
|
|
|
||
|
|
stewart = initializeStrutDynamics(stewart, ...
|
||
|
|
'type', args.actuator_type, ...
|
||
|
|
'k', args.actuator_k, ...
|
||
|
|
'kp', args.actuator_kp, ...
|
||
|
|
'ke', args.actuator_ke, ...
|
||
|
|
'ka', args.actuator_ka, ...
|
||
|
|
'c', args.actuator_c, ...
|
||
|
|
'cp', args.actuator_cp, ...
|
||
|
|
'ce', args.actuator_ce, ...
|
||
|
|
'ca', args.actuator_ca);
|
||
|
|
|
||
|
|
stewart = initializeJointDynamics(stewart, ...
|
||
|
|
'type_F', args.flex_type_F, ...
|
||
|
|
'type_M', args.flex_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);
|
||
|
|
|
||
|
|
nano_hexapod = stewart;
|
||
|
|
if exist('./mat', 'dir')
|
||
|
|
if exist('./mat/nass_model_stages.mat', 'file')
|
||
|
|
save('mat/nass_model_stages.mat', 'nano_hexapod', '-append');
|
||
|
|
else
|
||
|
|
save('mat/nass_model_stages.mat', 'nano_hexapod');
|
||
|
|
end
|
||
|
|
elseif exist('./matlab', 'dir')
|
||
|
|
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
||
|
|
save('matlab/mat/nass_model_stages.mat', 'nano_hexapod', '-append');
|
||
|
|
else
|
||
|
|
save('matlab/mat/nass_model_stages.mat', 'nano_hexapod');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
end
|