calcMetabolicCost.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function calculates the total metabolic cost based of the ProbeSet
% indicated in the osim model. The muscles, metabolic cost model, and
% metabolic cost elements used are solely dependent on the settings
% specified in the ProbeSet. 
%
% (Array of number, 2D matrix, 2D matrix, struct) -> (Array of number)
% Calculates metabolic cost using a model's ProbeSet.


function metabolicCost = calcMetabolicCost(time, statePositions, ...
    muscleActivations, params)
metabolicCost = [];
for indx = 1 : numel(params.costTerms)
    if strcmpi(params.costTerms{indx}.type, 'metabolic_cost_minimization')
        import org.opensim.modeling.*
        model = Model(params.model);
        for i = 1 : params.numMuscles
            controller = PrescribedController();
            controller.addActuator(model.getMuscles().get(params.muscleNames{i}));
            controlFunction = PiecewiseLinearFunction();
            for j = 1:size(muscleActivations, 1)
                controlFunction.addPoint(time(j), muscleActivations(j, i));
            end
            controller.prescribeControlForActuator(params.muscleNames{i}, ...
                controlFunction);
            model.addComponent(controller);
        end
        
        state = model.initSystem();
        for i = 1:size(muscleActivations, 1)
            for j = 1 : size(params.coordinateNames, 2)
                if ~model.getCoordinateSet.get(params.coordinateNames(j)). ....
                        get_locked
                    model.getCoordinateSet.get(params.coordinateNames(j)). ...
                        setValue(state, statePositions(i, j));
                end
            end
            state.setTime(time(i));
            model.realizeDynamics(state);
            model.equilibrateMuscles(state);
            tempTotalCost = model.getProbeSet().get(0).getProbeOutputs(state);
            metabolicCost(i, :) = tempTotalCost.get(0);
        end
    end
end
end