plotCoordinates.m

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


function plotCoordinates(inputs)
[modeledJointPositions, ~] = calcGCPJointKinematics( ...
    inputs.experimentalJointPositions, inputs.jointKinematicsBSplines, ...
    inputs.bSplineCoefficients);
coordinates = ["Toe Angle", "Y Rotation", "X Rotation", "Z Rotation", ...
    "X Translation", "Y Translation", "Z Translation"];
for i = 1:7
    subplot(2,4,i)
    
    if i <= 4
        experimental = rad2deg(inputs.experimentalJointPositions(i, :));
        model = rad2deg(modeledJointPositions(i, :));
    else
        experimental = inputs.experimentalJointPositions(i, :);
        model = modeledJointPositions(i, :);
    end

    plot(inputs.time, experimental, "red", "LineWidth", 2)
    hold on
    plot(inputs.time, model, "blue", "LineWidth", 2)
    error = rms(experimental - model);
    title(coordinates(i) + newline + " RMSE: " + error)
    xlabel('Time')
    if i == 1
        ylabel('Angle (deg)')
    elseif i == 5
        ylabel('Translation (m)')
    end
    hold off
end
end