optimizeGroundContactPersonalizationTask.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% 
%
% (struct, struct, double) -> (struct)
% Optimize ground contact parameters according to Jackson et al. (2016)


function inputs = optimizeGroundContactPersonalizationTask(inputs, ...
    params, task)
[initialValues, fieldNameOrder] = makeInitialValues(inputs, ...
    params, task);
[lowerBounds, upperBounds] = makeBounds(inputs, params, task);
optimizerOptions = prepareOptimizerOptions(params);
clear calcGroundContactPersonalizationTaskCost
results = lsqnonlin(@(values) calcGroundContactPersonalizationTaskCost( ...
    values, fieldNameOrder, inputs, params, task), initialValues, ...
    lowerBounds, upperBounds, optimizerOptions);
inputs = mergeGroundContactPersonalizationRoundResults(inputs, results, ...
    params, task);
end

% (struct, struct) -> (Array of double, Array of string)
% Generate initial values to be optimized from inputs and params. The
% fieldNameOrder allows tracking of included design variables for
% rebuilding a struct of design variables inside the cost function. 
function [initialValues, fieldNameOrder] = makeInitialValues( ...
    inputs, params, task)
initialValues = [];
fieldNameOrder = [];
if (params.tasks{task}.designVariables(1))
    initialValues = [initialValues 0.001 * inputs.springConstants];
    fieldNameOrder = [fieldNameOrder "springConstants"];
end
if (params.tasks{task}.designVariables(2))
    initialValues = [initialValues inputs.dampingFactor];
    fieldNameOrder = [fieldNameOrder "dampingFactor"];
end
if (params.tasks{task}.designVariables(3))
    initialValues = [initialValues inputs.dynamicFrictionCoefficient];
    fieldNameOrder = [fieldNameOrder "dynamicFrictionCoefficient"];
end
if (params.tasks{task}.designVariables(4))
    initialValues = [initialValues inputs.viscousFrictionCoefficient];
    fieldNameOrder = [fieldNameOrder "viscousFrictionCoefficient"];
end
if (params.tasks{task}.designVariables(5))
    initialValues = [initialValues inputs.restingSpringLength];
    fieldNameOrder = [fieldNameOrder "restingSpringLength"];
end
if (params.tasks{task}.designVariables(6))
    for foot = 1:length(inputs.surfaces)
        initialValues = [initialValues ...
            reshape(inputs.surfaces{foot}.bSplineCoefficients, 1, [])];
        fieldNameOrder = [fieldNameOrder ("bSplineCoefficients" + foot)];
    end
end
end

% (struct) -> (Array of double, Array of double)
% Generate lower and upper bounds for design variables from inputs
function [lowerBounds, upperBounds] = makeBounds(inputs, params, task)
lowerBounds = [];
upperBounds = [];
if (params.tasks{task}.designVariables(1))
    lowerBounds = [lowerBounds zeros(1, length(inputs.springConstants))];
    upperBounds = [upperBounds Inf(1, length(inputs.springConstants))];
end
if (params.tasks{task}.designVariables(2))
    lowerBounds = [lowerBounds 0];
    upperBounds = [upperBounds Inf];
end
if (params.tasks{task}.designVariables(3))
    lowerBounds = [lowerBounds 0];
    upperBounds = [upperBounds Inf];
end
if (params.tasks{task}.designVariables(4))
    lowerBounds = [lowerBounds 0];
    upperBounds = [upperBounds Inf];
end
if (params.tasks{task}.designVariables(5))
    lowerBounds = [lowerBounds -Inf];
    upperBounds = [upperBounds Inf];
end
if (params.tasks{task}.designVariables(6))
    for foot = 1:length(inputs.surfaces)
        lowerBounds = [lowerBounds -Inf(1, length(reshape(...
            inputs.surfaces{foot}.bSplineCoefficients, 1, [])))];
        upperBounds = [upperBounds Inf(1, length(reshape(...
            inputs.surfaces{foot}.bSplineCoefficients, 1, [])))];
    end
end
end

% (struct) -> (struct)
% Prepare optimizer options for lsqnonlin. 
function output = prepareOptimizerOptions(params)
output = optimoptions('lsqnonlin', 'UseParallel', true);
output.DiffMinChange = valueOrAlternate(params, 'diffMinChange', 1e-4);
output.OptimalityTolerance = valueOrAlternate(params, ...
    'optimalityTolerance', 1e-6);
output.FunctionTolerance = valueOrAlternate(params, ...
    'functionTolerance', 1e-6);
output.StepTolerance = valueOrAlternate(params, ...
    'stepTolerance', 1e-6);
output.MaxFunctionEvaluations = valueOrAlternate(params, ...
    'maxFunctionEvaluations', 3e6);
output.MaxIterations = valueOrAlternate(params, 'maxIterations', 400);
output.Display = valueOrAlternate(params, ...
    'display','iter');
end