Motion and path planning