nass-simscape/src/initializeMicroHexapod.m

function [micro_hexapod] = initializeMicroHexapod(args)

arguments
    args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init', 'compliance'})} = 'flexible'
    % initializeFramesPositions
    args.H    (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
    args.MO_B (1,1) double {mustBeNumeric} = 270e-3
    % generateGeneralConfiguration
    args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
    args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-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} = 45e-3
    args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 118e-3
    args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
    % initializeStrutDynamics
    args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1)
    args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1)
    % initializeCylindricalPlatforms
    args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
    args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
    args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3
    args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
    args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
    args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
    % initializeCylindricalStruts
    args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
    args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
    args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
    args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
    args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
    args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
    % inverseKinematics
    args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
    args.ARB (3,3) double {mustBeNumeric} = eye(3)
    % 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);

stewart = initializeStrutDynamics(stewart, ...
                                  'K', args.Ki, ...
                                  'C', args.Ci);

stewart = initializeJointDynamics(stewart, ...
                                  'type_F', 'universal_p', ...
                                  'type_M', 'spherical_p');

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', 'none');

if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
  load('mat/Foffset.mat', 'Fhm');
  stewart.actuators.dLeq = -Fhm'./args.Ki;
else
  stewart.actuators.dLeq = zeros(6,1);
end

switch args.type
  case 'none'
    stewart.type = 0;
  case 'rigid'
    stewart.type = 1;
  case 'flexible'
    stewart.type = 2;
  case 'modal-analysis'
    stewart.type = 3;
  case 'init'
    stewart.type = 4;
  case 'compliance'
    stewart.type = 5;
end

micro_hexapod = stewart;
save('./mat/stages.mat', 'micro_hexapod', '-append');