calcTrackingOptimizationTerminalConstraint.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function calculates the terminal constraint for tracking
% optimization
%
% (struct, struct, struct) -> (Array of number)
% Returns terminal constraint


function event = calcTrackingOptimizationTerminalConstraint(inputs, params)

inputs.phase.state = [inputs.phase.initialstate; inputs.phase.finalstate];
inputs.phase.time = [inputs.phase.initialtime; inputs.phase.finaltime];
inputs.phase.control = ones(size(inputs.phase.time,1),length(params.minControl));
if inputs.auxdata.optimizeSynergyVectors
    inputs.phase.parameter = inputs.parameter;
end
values = getTrackingOptimizationValueStruct(inputs.phase, params);
modeledValues = calcTorqueBasedModeledValues(values, params);

event = [];
for i = 1:length(params.terminal)
    constraintTerm = params.terminal{i};
    if constraintTerm.isEnabled
        switch constraintTerm.type
            case "state_position_periodicity"
                event = cat(2, event, ...
                    calcStatePositionPeriodicity(values.statePositions, ...
                    params.coordinateNames, ...
                    constraintTerm.coordinate));
            case "state_velocity_periodicity"
                event = cat(2, event, ...
                    calcStateVelocityPeriodicity(values.stateVelocities, ...
                    params.coordinateNames, ...
                    constraintTerm.coordinate));
            case "root_segment_residual_load_periodicity"
                event = cat(2, event, ...
                    calcRootSegmentResidualsPeriodicity(... 
                    modeledValues.inverseDynamicMoments, ...
                    params.inverseDynamicMomentLabels, ...
                    constraintTerm.load));    
            case "external_force_tracking_periodicity"
                event = cat(2, event, ...
                    calcExternalForcesPeriodicity(... 
                    modeledValues.groundReactionsLab.forces, ...
                    params.contactSurfaces, ...
                    constraintTerm.force));  
            case "external_moment_tracking_periodicity"
                event = cat(2, event, ...
                    calcExternalMomentsPeriodicity(... 
                    modeledValues.groundReactionsLab.moments, ...
                    params.contactSurfaces, ...
                    constraintTerm.moment));  
            case "synergy_weight_sum"
                event = cat(2, event, ...
                    calcSynergyWeightsSum(... 
                    values.synergyWeights, ...
                    params.synergyGroups, ...
                    constraintTerm.synergy_group)); 
            otherwise
                throw(MException('', ['Constraint term type ' ...
                    constraintTerm.type ' does not exist for this tool.']))    
        end
    end
end
end