% This function is part of the NMSM Pipeline, see file for full license.
%
% This function calculates kinematic symmetry, or the difference between
% two joint angles. Kinematic symmetry is calculated as the difference
% between one joint angle and the second joint angle with a 50% phase
% shift. To use this term, an even number is encouraged for the
% <setup_mesh_phase_intervals> tag in the optimal control settings.
% Additionally, two coordinate names are required to calculate the
% difference.
%
% (2D matrix, struct, struct) -> (Array of number)
%
function cost = calcKinematicSymmetryIntegrand(statePositions, auxdata, ...
costTerm)
halfWayFrame = size(statePositions, 1)/2;
indx1 = find(strcmp(convertCharsToStrings(auxdata.coordinateNames), ...
costTerm.coordinate1));
indx2 = find(strcmp(convertCharsToStrings(auxdata.coordinateNames), ...
costTerm.coordinate2));
cost = calcTrackingCostArrayTerm(statePositions(:,indx1), ...
[statePositions(1 + round(halfWayFrame):end, indx2); ...
statePositions(1:round(halfWayFrame), indx2)], 1);
end