calcMinimizingJointPowerIntegrand.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function calculates and minimizes the joint power for the specified
% coordinate. 
%
% (2D matrix, 2D matrix, struct, Array of string) -> (Array of number)


function cost = calcMinimizingJointPowerIntegrand(jointVelocity, ...
    jointMoment, params, loadName)

loadName = erase(loadName, '_moment');
loadName = erase(loadName, '_force');
indx = find(strcmp(convertCharsToStrings(params.coordinateNames), ...
    loadName));
momentLabelsNoSuffix = erase(params.inverseDynamicMomentLabels, '_moment');
momentLabelsNoSuffix = erase(momentLabelsNoSuffix, '_force');
includedJointMomentCols = ismember(momentLabelsNoSuffix, ...
    convertCharsToStrings(params.coordinateNames));
if isequal(mexext, 'mexw64')
    jointMoment = jointMoment(:, includedJointMomentCols);
end
jointPower = jointMoment(:, indx) .* jointVelocity(:, indx);
cost = calcMinimizingCostArrayTerm(jointPower);
end