Desensitized motion planning for underactuated multibody systems