GetCentroid added RigidBodySimulator.hpp

-also defined, returns centroid of robot using INITIAL vertices