getCoordinatesFromBodies.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function takes a model and an array of body names and returns an
% array of strings of coordinates directly connected to the bodies.
%
% (Model, Array of string) -> (Array of string)
% Create and return an array of coordinate names associated with the bodies


function coordinates = getCoordinatesFromBodies(model, bodies)
coordinates = {};
for i = 0 : model.getJointSet().getSize()-1
    try
        childName = model.findComponent(model.getJointSet().get(i) ...
            .getChildFrame().getSocket('parent').getConnecteePath()) ...
            .getName().toCharArray';
        if(any(strcmp(bodies, childName)))
            for j=0:model.getJointSet().get(i).numCoordinates()-1
                coordinates{end+1} = model.getJointSet().get(i) ...
                    .get_coordinates(j);
            end
        end
    catch
    end
end
coordinates = string(coordinates);
end