MotionToGoal implemented