Real-time motion planning for multibody systems