setupCommonOptimalControlInitialGuess.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function sets up the common initial guess for an optimal control
% problem and is used by Tracking Optimization, Verification Optimization,
% and Design Optimization
%
% (struct) -> (struct)
% return a set of setup values common to all optimal control problems


function guess = setupCommonOptimalControlInitialGuess(inputs)
if isfield(inputs.initialGuess, 'state')
    guess.phase.time = scaleToBounds(inputs.initialGuess.time, inputs.maxTime, ...
        inputs.minTime);
    guess.phase.state = scaleToBounds(inputs.initialGuess.state, ...
        inputs.maxState, inputs.minState);
else
    guess.phase.state = scaleToBounds([inputs.experimentalJointAngles ...
        inputs.experimentalJointVelocities ...
        inputs.experimentalJointAccelerations], inputs.maxState, ...
        inputs.minState);
    guess.phase.time = scaleToBounds(inputs.experimentalTime, inputs.maxTime, ...
        inputs.minTime);
end
if strcmp(inputs.controllerType, 'synergy_driven')
    if isfield(inputs.initialGuess, 'control')
        guess.phase.control = scaleToBounds(inputs.initialGuess.control, ...
            inputs.maxControl, inputs.minControl);
    else
        guess.phase.control = scaleToBounds([inputs.experimentalJointJerks ...
            inputs.synergyActivationsGuess], inputs.maxControl, inputs.minControl);
    end
    if isfield(inputs, "optimizeSynergyVectors") && ...
            inputs.optimizeSynergyVectors
        guess.parameter = scaleToBounds(inputs.synergyWeightsGuess, ...
            inputs.maxParameter, inputs.minParameter);
    end
elseif strcmp(inputs.controllerType, 'torque_driven')
    if isfield(inputs.initialGuess, 'control')
        guess.phase.control = scaleToBounds(inputs.initialGuess.control, ...
            inputs.maxControl, inputs.minControl);
    else
        for i = 1:length(inputs.controlTorqueNames)
            indx = find(strcmp(convertCharsToStrings( ...
                inputs.inverseDynamicMomentLabels), ...
                strcat(inputs.controlTorqueNames(i), '_moment')));
            if isempty(indx)
                indx = find(strcmp(convertCharsToStrings( ...
                inputs.inverseDynamicMomentLabels), ...
                strcat(inputs.controlTorqueNames(i), '_force')));
            end
            controlTorquesGuess(:, i) = inputs.experimentalJointMoments(:, indx);
        end
        guess.phase.control = scaleToBounds([inputs.experimentalJointJerks ...
            controlTorquesGuess], inputs.maxControl, inputs.minControl);
    end
end
guess.phase.integral = scaleToBounds(1e1, inputs.maxIntegral, ...
    inputs.minIntegral);
end