calcModeledVerticalGroundReactionForce.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function uses a spring model with damping to calculate the modeled 
% vertical GRF force as the summation of the forces applied by each
% individual spring
%
% (Array of double, double, double, struct, Array of double) 
% -> (double, Array of double)
% Returns the sum of the modeled vertical GRF forces at the given state


function [modeledVerticalGrf, springForces] = ...
    calcModeledVerticalGroundReactionForce(springConstants, ...
    dampingFactor, springRestingLength, ...
    markerKinematics, springForces)
modeledVerticalGrf = 0;
for i=1:length(springConstants)
    height = markerKinematics.height(i);
    verticalVelocity = markerKinematics.yVelocity(i);
    % The freglyVerticalGrf model closely approximates a linear spring
    % during contact while allowing a small force with a small slope to
    % exist for spring markers out of contact. This can help the
    % optimization algorithm find a better search direction when springs
    % are incorrectly out of contact. 
    klow = 1e-1;
    h = 1e-3;
    c = 5e-4;
    ymax = 1e-2;
    Kval = springConstants(i);
    height = height - springRestingLength;
    numFrames = length(height);
    v = ones(numFrames, 1)' .* ((Kval + klow) ./ (Kval - klow));
    s = ones(numFrames, 1)' .* ((Kval - klow) ./ 2);
    constant = -s .* (v .* ymax - c .* log(cosh((ymax + h) ./ c)));
    freglyVerticalGrf = -s .* (v .* height - c .* ...
        log(cosh((height + h) ./ c))) - constant;
    % Account for potential errors in force model
    freglyVerticalGrf(isnan(freglyVerticalGrf)) = ...
        min(min(freglyVerticalGrf));
    freglyVerticalGrf(isinf(freglyVerticalGrf)) = ...
        min(min(freglyVerticalGrf));
    springForces(2, i) = freglyVerticalGrf * (1 + dampingFactor * ...
        verticalVelocity);
    modeledVerticalGrf = modeledVerticalGrf + springForces(2, i);
end
end