calcTrackingInverseDynamicLoadsIntegrand.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function calculates the difference between the experimental and
% predicted inverse dynamic moments for the specified coordinate.
%
% (struct, Array of number, 2D matrix, Array of string) -> (Array of number)
%


function cost = calcTrackingInverseDynamicLoadsIntegrand(params, time, ...
    inverseDynamicMoments, loadName)

loadName = erase(loadName, '_moment');
loadName = erase(loadName, '_force');
indx = find(strcmp(convertCharsToStrings(params.coordinateNames), ...
    loadName));

if params.splineJointMoments.dim > 1
    experimentalJointMoments = fnval(params.splineJointMoments, time)';
else
    experimentalJointMoments = fnval(params.splineJointMoments, time);
end

momentLabelsNoSuffix = erase(params.inverseDynamicMomentLabels, '_moment');
momentLabelsNoSuffix = erase(momentLabelsNoSuffix, '_force');
includedJointMomentCols = ismember(momentLabelsNoSuffix, convertCharsToStrings(params.coordinateNames));
if size(inverseDynamicMoments, 2) ~= size(experimentalJointMoments, 2)
    experimentalJointMoments = experimentalJointMoments(:, includedJointMomentCols);
end
cost = calcTrackingCostArrayTerm(experimentalJointMoments, ...
    inverseDynamicMoments, indx);
end