plotSpringConstants.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% 
%
% (struct, struct) -> (None)
% Plot optimized spring constants from GCP from workspace data.


function plotSpringConstants(inputs, sharedInputs)
toesBodyName = inputs.toesBodyName;
hindfootBodyName = inputs.hindfootBodyName;

[footModel, state] = Model(inputs.model);
calcnToToes = footModel.getBodySet().get(toesBodyName) ...
    .findTransformBetween(state, footModel.getBodySet() ...
    .get(hindfootBodyName));
springX = zeros(1, length(sharedInputs.springConstants));
springZ = zeros(1, length(sharedInputs.springConstants));
for i=1:length(sharedInputs.springConstants)
    markerPositionOnFoot = footModel.getMarkerSet().get(...
        "spring_marker_" + i).getPropertyByName("location").toString(...
        ).toCharArray';
    markerPositionOnFoot = split(markerPositionOnFoot(2:end-1));
    springX(i) = str2double(markerPositionOnFoot{1});
    springZ(i) = str2double(markerPositionOnFoot{3});
    if strcmp(getMarkerBodyName(footModel, "spring_marker_" + i), ...
            toesBodyName)
        springX(i) = springX(i) + calcnToToes.T.get(0);
        springZ(i) = springZ(i) + calcnToToes.T.get(2);
    end
end
scatter(springZ, springX, 200, sharedInputs.springConstants, "filled")
title("Spring constants")
xlabel("Z location on foot (m)")
ylabel("X location on foot (m)")
colormap jet
colorbar
end