Update RigidBodySimulator.hpp

-added vector<double> dist_to_goal; to Robot struct