writeOptimizedGroundReactionsToSto.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% 
%
% (struct, struct, string, string) -> (None)
% Write modeled ground reactions to an OpenSim Storage file.


function writeOptimizedGroundReactionsToSto(inputs, params, ...
    resultsDirectory, modelName)
columnLabels = ["Fx" "Fy" "Fz" "Mx" "My" "Mz" "ECx" "ECy" "ECz"];
for foot = 1:length(inputs.surfaces)
    models.("model_" + foot) = Model(inputs.surfaces{foot}.model);
end
for foot = 1:length(inputs.surfaces)
    [modeledJointPositions, modeledJointVelocities] = ...
        calcGCPJointKinematics(inputs.surfaces{foot} ...
        .experimentalJointPositions, inputs.surfaces{foot} ...
        .jointKinematicsBSplines, inputs.surfaces{foot}.bSplineCoefficients);
    modeledValues = calcGCPModeledValues(inputs, inputs, ...
        modeledJointPositions, modeledJointVelocities, params, ...
        length(params.tasks), foot, models);
    center = inputs.surfaces{foot}.midfootSuperiorPosition;
    center(2, :) = inputs.restingSpringLength;
    data = [modeledValues.anteriorGrf' modeledValues.verticalGrf' ...
        modeledValues.lateralGrf' modeledValues.xGrfMoment' ...
        modeledValues.yGrfMoment' modeledValues.zGrfMoment' center'];
    writeToSto(columnLabels, inputs.surfaces{foot}.time, data, ...
        fullfile(resultsDirectory, strcat(modelName, "_Foot_", ...
            num2str(foot), "_optimizedGroundReactions.sto")));
end
end