calcMaximizingStepLengthIntegrand.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function maximizes the step length for the specified foot.
%
% (struct, struct, struct, struct) -> (Array of number)
%


function cost = calcMaximizingStepLengthIntegrand(values, modeledValues,...
    params, costTerm)

for i = 1:length(params.contactSurfaces)
    if params.contactSurfaces{i}.isLeftFoot == costTerm.is_left_foot
        if i == 1
            stepLength = calcStepLength(modeledValues. ...
                groundReactionsLab.forces{i}(:, 2), ...
                [modeledValues.bodyLocations.parent{i}(:, 1) ...
                modeledValues.bodyLocations.parent{i+1}(:, 1)]);
        else
            stepLength = calcStepLength(modeledValues. ...
                groundReactionsLab.forces{i}(:,2), ...
                [modeledValues.bodyLocations.parent{i}(:, 1) ...
                modeledValues.bodyLocations.parent{i-1}(:, 1)]);
        end
    end
end
cost = calcMaximizingCostArrayTerm(stepLength * ...
    ones(length(values.time), 1));
end