Title: Motion planning for rigid body robots