Collision Free Motion Planning