findGCPFreeCoordinates.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function returns an array of names of coordinates that connect the
% ground to the contact bodies. These coordinates all will be modeled in
% the kinematic model for GCP.
%
% (Model, Array of string) -> (Array of string)
% Create an array of coordinate names connecting the bodies to ground


function coordinatesOfInterest = findGCPFreeCoordinates(model, ...
    contactBodies)
model = Model(model);
bodyStructure = makeBodyTree(model);

bodiesOfInterest = string([]);
for i=1:length(contactBodies)
    [~, path] = getFieldByName(bodyStructure, contactBodies(i));
    bodiesOfInterest = unique([bodiesOfInterest, string(path)]);
end

coordinatesOfInterest = getCoordinatesFromBodies(model, bodiesOfInterest);
end