Motion and task planning