nass-simscape/src/initializeMicroHexapod.m

function [micro_hexapod] = initializeMicroHexapodNew(args)

arguments
    % 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)
    % Equilibrium position of each leg
    args.dLeq (6,1) double {mustBeNumeric} = zeros(6,1)
end

micro_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
micro_hexapod = generateGeneralConfiguration(micro_hexapod, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh);
micro_hexapod = computeJointsPose(micro_hexapod);
micro_hexapod = initializeStrutDynamics(micro_hexapod, 'Ki', args.Ki, 'Ci', args.Ci);
micro_hexapod = initializeCylindricalPlatforms(micro_hexapod, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
micro_hexapod = initializeCylindricalStruts(micro_hexapod, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
micro_hexapod = computeJacobian(micro_hexapod);
[Li, dLi] = inverseKinematics(micro_hexapod, 'AP', args.AP, 'ARB', args.ARB);
micro_hexapod.Li = Li;
micro_hexapod.dLi = dLi;

micro_hexapod.dLeq = args.dLeq;

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