Finding and Following Optimal Trajectories for an Overactuated Floating Robotic Platform