nass-simscape/src/initializeNanoHexapod.m

function [nano_hexapod] = initializeNanoHexapod(args)
    arguments
        % initializeFramesPositions
        args.H    (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
        args.MO_B (1,1) double {mustBeNumeric} = 175e-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'})} = 'piezo'
        % 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} = 100e-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)
    end

    stewart = initializeFramesPositions('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 strcmp(args.actuator, 'piezo')
        stewart = initializeStrutDynamics(stewart, 'Ki', 1e7*ones(6,1), ...
                                                   'Ci', 1e2*ones(6,1));
    elseif strcmp(args.actuator, 'lorentz')
        stewart = initializeStrutDynamics(stewart, 'Ki', 1e4*ones(6,1), ...
                                                   'Ci', 1e2*ones(6,1));
    else
        error('args.actuator should be piezo or lorentz');
    end

    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);

    [Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);

    nano_hexapod = stewart;

    %% Save
    save('./mat/stages.mat', 'nano_hexapod', '-append');
end