calcSynergyBasedModeledValues.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function calculates muscle tendon lengths and velocities along with
% moment arms from a surrogate model. These quantities are used to
% calculate normalized fiber lengths and velocities. Muscle activations are
% calculated from muscles synergies and all of these quantities are used to
% calculate the muscle produced joint moments.
%
% (struct, struct, struct) -> (struct)
% Returns normalized fiber lengths and velocities, muscle activations, and
% muscle joint moments


function modeledValues = calcSynergyBasedModeledValues(values, params, ...
    modeledValues)
if strcmp(params.controllerType, 'synergy_driven')
    [jointAngles, jointVelocities] = getMuscleActuatedDOFs(values, params);
    [params.muscleTendonLength, params.momentArms, ...
        params.muscleTendonVelocity] = calcSurrogateModel(params, ...
        jointAngles, jointVelocities);
    [modeledValues.normalizedFiberLength, modeledValues.normalizedFiberVelocity] = ...
        calcNormalizedMuscleFiberLengthsAndVelocities(params, ...
        ones(1, params.numMuscles), ones(1, params.numMuscles));
    modeledValues.muscleActivations = calcMuscleActivationFromSynergies(values);
    muscleJointMoments = calcMuscleJointMoments(params, ...
        modeledValues.muscleActivations, modeledValues.normalizedFiberLength, ...
        modeledValues.normalizedFiberVelocity);
    modeledValues.muscleJointMoments = muscleJointMoments(:, ...
        params.surrogateModelIndex);
    modeledValues.muscleJointMoments = modeledValues.muscleJointMoments(:, ...
        params.dofsActuatedIndex);
end
end

function muscleActivations = calcMuscleActivationFromSynergies(values)
muscleActivations = values.controlSynergyActivations * values.synergyWeights;
end

function [jointAngles, jointVelocities] = getMuscleActuatedDOFs(values, params)
for i = 1:params.numMuscles
    counter = 1;
    for j = 1:length(params.coordinateNames)
        for k = 1:length(params.surrogateModelLabels{i})
            if strcmp(params.coordinateNames(j), params.surrogateModelLabels{i}{k})
                jointAngles{i}(:,counter) = values.statePositions(:,j);
                jointVelocities{i}(:,counter) = values.stateVelocities(:,j);
                counter = counter + 1;
            end
        end
    end
end
end