plotGroundReactionQuantities.m

% This function is part of the NMSM Pipeline, see file for full license.
%
%
%
% (struct, struct, double, double) -> (None)
% Plot optimized ground reaction quantities from GCP from workspace data.


function plotGroundReactionQuantities(inputs, params, task, foot)

plotParams = params;
% plotParams.tasks{task}.costTerms.verticalGrfError.isEnabled = true;
% plotParams.tasks{task}.costTerms.horizontalGrfError.isEnabled = true;
% plotParams.tasks{task}.costTerms.groundReactionMomentError.isEnabled = true;

for i = 1:length(inputs.surfaces)
    models.("model_" + i) = Model(inputs.surfaces{foot}.model);
end
[modeledJointPositions, modeledJointVelocities] = ...
    calcGCPJointKinematics(inputs.surfaces{foot}.experimentalJointPositions, ...
    inputs.surfaces{foot}.jointKinematicsBSplines, inputs.surfaces{foot}.bSplineCoefficients);
modeledValues = calcGCPModeledValues(inputs, inputs, ...
    modeledJointPositions, modeledJointVelocities, plotParams, task, ...
    foot, models);
modeledValues.jointPositions = modeledJointPositions;
modeledValues.jointVelocities = modeledJointVelocities;

groundReactions = ["verticalGrf", "anteriorGrf", "lateralGrf", ...
    "xGrfMoment", "yGrfMoment", "zGrfMoment"];
subplot(2, 3, 1)
plot(inputs.surfaces{foot}.time, ...
    inputs.surfaces{foot}.experimentalGroundReactionForces(2, :), "red", "LineWidth", 2)
hold on
plot(inputs.surfaces{foot}.time, modeledValues.verticalGrf, "blue", "LineWidth", 2)
error = rms(inputs.surfaces{foot}.experimentalGroundReactionForces(2, :) - ...
    modeledValues.verticalGrf);
title(groundReactions(1) + newline + " RMSE: " + error)
xlabel('Time')
ylabel('Force (N)')
hold off

for i = 2:6
    subplot(2, 3, i)
    if (i == 2)
        plot(inputs.surfaces{foot}.time, ...
            inputs.surfaces{foot}.experimentalGroundReactionForces(1, :), "red", ...
            "LineWidth", 2)
        hold on
        plot(inputs.surfaces{foot}.time, modeledValues.(groundReactions(i)), ...
            "blue", "LineWidth", 2)
        error = rms(inputs.surfaces{foot}.experimentalGroundReactionForces(1, :) - ...
            modeledValues.(groundReactions(i)));
        title(groundReactions(i) + newline + " RMSE: " + error)
        xlabel('Time')
        hold off
    elseif (i == 3)
        plot(inputs.surfaces{foot}.time, ...
            inputs.surfaces{foot}.experimentalGroundReactionForces(i, :), "red", ...
            "LineWidth", 2)
        hold on
        plot(inputs.surfaces{foot}.time, modeledValues.(groundReactions(i)), ...
            "blue", "LineWidth", 2)
        error = rms(inputs.surfaces{foot}.experimentalGroundReactionForces(i, :) - ...
            modeledValues.(groundReactions(i)));
        title(groundReactions(i) + newline + " RMSE: " + error)
        xlabel('Time')
        hold off
    else
        plot(inputs.surfaces{foot}.time, ...
            inputs.surfaces{foot}.experimentalGroundReactionMoments(i - 3, :), ...
            "red", "LineWidth", 2)
        hold on
        plot(inputs.surfaces{foot}.time, modeledValues.(groundReactions(i)), ...
            "blue", "LineWidth", 2)
        error = rms(inputs.surfaces{foot}.experimentalGroundReactionMoments(i - 3, :) - ...
            modeledValues.(groundReactions(i)));
        title(groundReactions(i) + newline + " RMSE: " + error)
        xlabel('Time')
        if i == 4
            ylabel('Moment (N*m)')
        end
        hold off
    end
end

end