{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Tutorial: Uninformed Search Algorithms and CommonRoad Search\n",
    "\n",
    "This tutorial shows how we can use motion primitives, i.e., short trajectory pieces, in uninformed search algorithms to find a trajectory that connects an **initial state** and a **goal region**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## How to use this tutorial\n",
    "Before you start with this tutorial, make sure that\n",
    "\n",
    "* you have read through the tutorial for [CommonRoad-io](https://commonroad.in.tum.de/static/docs/commonroad-io/index.html). Its tutorial can be found [here](https://commonroad.in.tum.de/tutorials/). The API of CommonRoad-io can be found [here](https://commonroad.in.tum.de/static/docs/commonroad-io/api/index.html)\n",
    "* you have installed all necessary modules for CommonRoad Search according to the installation manual.\n",
    "\n",
    "Let's start with importing the modules we need for setting up the automaton and the CommonRoad scenario."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "import matplotlib.pyplot as plt\n",
    "import sys\n",
    "sys.path.append(\"../../GSMP/motion_automata\")\n",
    "from commonroad.common.file_reader import CommonRoadFileReader\n",
    "from commonroad.visualization.draw_dispatch_cr import draw_object\n",
    "from automata.HelperFunctions import load_scenario, generate_automata\n",
    "from automata.helper_tree_search import *\n",
    "import automata.tree_search as tree_search"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Open and Load the CR Scenario\n",
    "In the next step, we load the scenario and planning problem, which we use in the uninformed search algorithm.\n",
    "If you do not understand the following steps, please go back to the tutorial for [CommonRoad-io](https://commonroad.in.tum.de/tutorials/)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Load scenario ZAM_Urban-3_2\n",
    "scenario_path = '../../scenarios/tutorial/'\n",
    "scenario_id = 'ZAM_Tutorial_Urban-3_2'\n",
    "\n",
    "scenario, planning_problem_set = CommonRoadFileReader(scenario_path+scenario_id+'.xml').open()\n",
    "\n",
    "# Plot scenario and planning problem set\n",
    "\n",
    "plt.figure(figsize=(15, 5))\n",
    "draw_object(scenario)\n",
    "draw_object(planning_problem_set)\n",
    "plt.gca().set_aspect('equal')\n",
    "plt.margins(0, 0)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Generating a Maneuver Automaton\n",
    "\n",
    "In the following, we load the motion primitives from an XML-File and generate a Maneuver Automaton.\n",
    "The maneuver automaton for this tutorial consists of 7 motion primitives and stores the connectivity to other motion primitives. \n",
    "\n",
    "Some additional explanations on the motion primitives:\n",
    "* The motion primitives are generated for the Kinematic Single Track-Model (see [Vehicle Model Documentation](https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/blob/master/vehicleModels_commonRoad.pdf)) and the vehicle parameter are chosen for a BMW320i (vehicle_type_id=2). \n",
    "* We have motion primitives for driving with constant velocity and the steering angle is changed with constant steering angle velocity. We generated motion primitives for all combinations of the steering angles in the initial state and end state for 0 rad and 0.2 rad, i.e., 4 combinations. The three motion primitives for turning left are mirrored for turning right, resulting in total 7 motion primitives. \n",
    "* motion primitives can only be connected if they have matching initial/final velocities and matching initial/final steering angles."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Load motion primitives (mp)\n",
    "mp_path = 'motion_primitives_search_tutorial/'\n",
    "mp_file = 'V_9.0_9.0_Vstep_0_SA_-0.2_0.2_SAstep_0.4_T_0.5_Model_BMW320i.xml'\n",
    "\n",
    "vehicle_type_id = 2\n",
    "automaton = generate_automata(vehicle_type_id, mp_file= mp_path+mp_file, search_tutorial=True)\n",
    "\n",
    "# plot motion primitives\n",
    "\n",
    "plt.figure(figsize=(8,8))\n",
    "\n",
    "for mp in automaton.Primitives:\n",
    "    plot_motion_primitive(mp)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Breadth-First Search (BFS)\n",
    "\n",
    "In the next step, we set-up the BFS with the generated maneuver automaton to obtain a trajectory from the initial state to the goal region. The inital state and the goal region are specified in the planning problem. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "planning_problem = list(planning_problem_set.planning_problem_dict.values())[0]\n",
    "\n",
    "# construct motion planner and set up the initial state for planning problem\n",
    "bfs_planner = tree_search.BreadthFirstSearch(scenario=scenario, planningProblem=planning_problem, \n",
    "                                             automaton=automaton)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Before we run the BFS, you can check the code below to get an idea how BFS works for motion primitives.\n",
    "Please note, we removed the parts for visualization in the code below so it easier to understand the code.\n",
    "\n",
    "```python\n",
    "def search_alg(self):\n",
    "    '''\n",
    "    Implementation of BFS (tree search) using a FIFO queue\n",
    "    '''\n",
    "\n",
    "    # First node\n",
    "    initial_node = Node(path=[[self.initial_state]], primitives=[], tree_depth=0)\n",
    "\n",
    "    # check if we already reached the goal state\n",
    "    if self.reached_goal(initial_node.path[-1]):\n",
    "        return self.remove_states_behind_goal(initial_node.path), initial_node.primitives\n",
    "\n",
    "    # add current node to the frontier\n",
    "    self.frontier.insert(initial_node)\n",
    "\n",
    "    while not self.frontier.empty():\n",
    "        # Pop the shallowest node\n",
    "        current_node: Node = self.frontier.pop()\n",
    "\n",
    "        # Check all possible successor primitives(i.e., actions) for current node\n",
    "        for succ_primitive in current_node.get_successors():\n",
    "\n",
    "            # translate/rotate motion primitive to current position\n",
    "            current_primitive_list = copy.copy(current_node.primitives)\n",
    "            path_translated = self.translate_primitive_to_current_state(succ_primitive, \n",
    "                                                                        current_node.path[-1])\n",
    "\n",
    "            # check for collision, if is not collision free it is skipped\n",
    "            if not self.check_collision_free(path_translated):\n",
    "                continue\n",
    "\n",
    "            current_primitive_list.append(succ_primitive)\n",
    "\n",
    "            # Goal test\n",
    "            if self.reached_goal(path_translated):\n",
    "                path_new = current_node.path + [[current_node.path[-1][-1]] + path_translated]\n",
    "                solution_path = self.remove_states_behind_goal(path_new)\n",
    "                # return solution\n",
    "                return self.remove_states_behind_goal(path_new), current_primitive_list\n",
    "\n",
    "            # Inserting the child to the frontier:\n",
    "            path_new = current_node.path + [[current_node.path[-1][-1]] + path_translated]\n",
    "            child = Node(path=path_new, primitives=current_primitive_list, \n",
    "                         tree_depth=current_node.tree_depth + 1)\n",
    "            self.frontier.insert(child)\n",
    "\n",
    "            if path_translated[-1].time_step > self.desired_time.end:\n",
    "                return None, None\n",
    "\n",
    "    return None, None\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we want to run the algorithm:\n",
    "\n",
    "When executing the following code block, you will see a \"visualize\" button directly beneath the \"iteration\" slider if you are running this notebook for the first time. Otherwise you can always find the button on the bottom.\n",
    "\n",
    "Click the \"visualize\" button and let the search algorithm run through, once it's completed, you can use the slider to see all the iterations step by step."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "scenario_data = (scenario, bfs_planner.initial_state, bfs_planner.egoShape, planning_problem)\n",
    "display_steps(scenario_data=scenario_data, algorithm=bfs_planner.search_alg, \n",
    "              config=bfs_planner.config)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Depth-First Search (DFS)\n",
    "Now we show the same example for the DFS. We use a simple implementation of the DFS which is similar to the BFS implementation but uses a LastInFirstOut(LIFO)-queue. Since the rest of the implementation is the same as the BFS, we directly run the DFS. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# constructing the motion planner\n",
    "dfs_planner = tree_search.DepthFirstSearch(scenario=scenario, planningProblem=planning_problem, \n",
    "                                          automaton=automaton)\n",
    "# run the planner\n",
    "display_steps(scenario_data=scenario_data, algorithm=dfs_planner.search_alg, \n",
    "              config=dfs_planner.config)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this scenario, we were not able to find a solution using DFS, since DFS would append motion primitives for an infinitely long time (infinite state space). This shows that DFS is not complete, i.e., DFS is not guaranteed to find a solution if one exist.\n",
    "\n",
    "To overcome this problem we introduce a depth limit, resulting in Depth-Limited Search (DLS). This search algorithm is introduced in the next section."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Depth-Limited Search (DLS)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Before we run the algorithm, you can have a look at the impementation. We use the recursive implementation as introduced in the lecture. \n",
    "\n",
    "```python\n",
    "def search_alg(self, limit=7):\n",
    "    '''\n",
    "    Recursive implementation of DLS\n",
    "    '''\n",
    "    \n",
    "    # First node\n",
    "    initial_node = Node(path=[[self.initial_state]], primitives=[], tree_depth=0)\n",
    "\n",
    "    result = self.recursive_dls(initial_node, limit)\n",
    "    if result is None:\n",
    "        return None, None\n",
    "\n",
    "    return path, list_primitives\n",
    "\n",
    "def recursive_dls(self, current_node: Node, limit: int):\n",
    "\n",
    "    # Goal test\n",
    "    if self.reached_goal(current_node.path[-1]):\n",
    "        solution_path = self.remove_states_behind_goal(current_node.path)\n",
    "        \n",
    "        # return solution\n",
    "        return solution_path, current_node.primitives\n",
    "\n",
    "    elif limit == 0:\n",
    "        return 'cutoff'\n",
    "\n",
    "    else:\n",
    "        cutoff_occurred = False\n",
    "\n",
    "    for succ_primitive in reversed(current_node.get_successors()):\n",
    "        # translate/rotate motion primitive to current position\n",
    "        current_primitive_list = copy.copy(current_node.primitives)\n",
    "        path_translated = self.translate_primitive_to_current_state(succ_primitive, current_node.path[-1])\n",
    "\n",
    "        # check for collision, if is not collision free it is skipped\n",
    "        if not self.check_collision_free(path_translated):\n",
    "            continue\n",
    "\n",
    "        # Continue search with child node\n",
    "        current_primitive_list.append(succ_primitive)\n",
    "        path_new = current_node.path + [[current_node.path[-1][-1]] + path_translated]\n",
    "        child = Node(path=path_new, primitives=current_primitive_list, \n",
    "                     tree_depth=current_node.tree_depth + 1)\n",
    "\n",
    "        result = self.recursive_dls(current_node=child, limit=limit-1)\n",
    "\n",
    "        if result == 'cutoff':\n",
    "            cutoff_occurred = True\n",
    "\n",
    "        elif result is not None:\n",
    "            return result\n",
    "\n",
    "    return 'cutoff' if cutoff_occurred else None\n",
    "\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now let's run the algorithm and see what changes with the introduced limit. Here we set the limit to 7, as we know from BFS there exists a solution consisting of 7 motion primtives.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "dls_planner = tree_search.DepthLimitedSearch(scenario=scenario, planningProblem=planning_problem, \n",
    "                                             automaton=automaton)\n",
    "limit = 7\n",
    "# run the planner\n",
    "display_steps(scenario_data=scenario_data, algorithm=dls_planner.search_alg, \n",
    "              config=dls_planner.config, limit=7)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, depth-limited search finds a solution."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Uniform-Cost Search\n",
    "\n",
    "Up to now, we looked at all algorithms, which do not consider costs during search. In the following, we look at the uniform-cost search. Uniform-Cost search is optimal for any step costs, as it expands the node with the lowest path cost g(n). In this example our cost is the time to reach the goal. Thus, our cost g(n) is the time it took, to reach our current final state. \n",
    "\n",
    "The Uniform-Cost Search is based on the Best-First Search, which we will also use for Greedy-Best-First Search and A\\* Search. These algorithms only differ in their evaluation function. In Uniform-Cost Search, the evaluation function is f(n) = g(n). \n",
    "\n",
    "Before we run the search, you can have a look at the implementation of the algorithm and the evaluation function. Again, we removed all the visualization parts, so it is easier to understand the code.\n",
    "\n",
    "\n",
    "```python\n",
    "def search_alg(self):\n",
    "    '''\n",
    "    Implementation of Best-First Search (tree search) using a Priority queue\n",
    "    '''\n",
    "    # First node\n",
    "    initial_node = PrioNode(path=[[self.initial_state]], primitives=[], tree_depth=0, current_cost=0)\n",
    "   \n",
    "\n",
    "    # add current node (i.e., current path and primitives) to the frontier\n",
    "    f = self.evaluation_function(initial_node)\n",
    "    self.frontier.insert(item=initial_node, priority=f)\n",
    "\n",
    "    while not self.frontier.empty():\n",
    "        # Pop the shallowest node\n",
    "        current_node: PrioNode = self.frontier.pop()\n",
    "\n",
    "        # Goal test\n",
    "        if self.reached_goal(current_node.path[-1]):\n",
    "            solution_path = self.remove_states_behind_goal(current_node.path)\n",
    "     \n",
    "            # return solution\n",
    "            return solution_path, current_node.primitives\n",
    "\n",
    "        # Check all possible successor primitives(i.e., actions) for current node\n",
    "        for succ_primitive in current_node.get_successors():\n",
    "\n",
    "            # translate/rotate motion primitive to current position\n",
    "            current_primitive_list = copy.copy(current_node.primitives)\n",
    "            path_translated = self.translate_primitive_to_current_state(succ_primitive, \n",
    "                                                                        current_node.path[-1])\n",
    "            # check for collision, if is not collision free it is skipped\n",
    "            if not self.check_collision_free(path_translated):\n",
    "                continue\n",
    "\n",
    "            current_primitive_list.append(succ_primitive)\n",
    "\n",
    "            path_new = current_node.path + [[current_node.path[-1][-1]] + path_translated]\n",
    "            child_node = PrioNode(path=path_new, primitives=current_primitive_list,\n",
    "                                  tree_depth=current_node.tree_depth + 1, \n",
    "                                  current_cost=current_node.current_cost)\n",
    "            f = self.evaluation_function(current_node=child_node)\n",
    "            print(f)\n",
    "\n",
    "            # Inserting the child into the frontier:\n",
    "            self.frontier.insert(item=child_node, priority=f)\n",
    "\n",
    "    return None, None\n",
    "\n",
    "def evaluation_function(self, current_node: PrioNode) -> float:\n",
    "    \"\"\"\n",
    "    Evaluation function of UCS is f(n) = g(n)\n",
    "    \"\"\"\n",
    "    \n",
    "    if self.reached_goal(current_node.path[-1]):\n",
    "        current_node.path = self.remove_states_behind_goal(current_node.path)\n",
    "    # calculate g(n)\n",
    "    current_node.current_cost += len(current_node.path[-1]) * self.scenario.dt\n",
    "\n",
    "    return current_node.current_cost\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ucs_planner = tree_search.UniformCostSearch(scenario=scenario, planningProblem=planning_problem, \n",
    "                                           automaton=automaton)\n",
    "# run the planner\n",
    "display_steps(scenario_data=scenario_data, algorithm=ucs_planner.search_alg, \n",
    "              config=ucs_planner.config)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Congratulations! You finished the tutorial on uninformed search and commonroad search! Next, you can check out the tutorial on informed search and commonroad search."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python(commonroad-py36-dev)",
   "language": "python",
   "name": "commonroad-py36-dev"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}