replanning_dash_shapely.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 96,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": "'/home/jhonas/dev/mpnet'"
     },
     "execution_count": 96,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import os\n",
    "from copy import deepcopy\n",
    "\n",
    "import dash\n",
    "import dash_core_components as dcc\n",
    "import dash_html_components as html\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "import plotly.express as px\n",
    "import torch\n",
    "from dash.dependencies import Input, MATCH, Output, State\n",
    "from jupyter_dash import JupyterDash as Dash\n",
    "from plotly import graph_objects as go\n",
    "from shapely.geometry import MultiPolygon, Point, LineString, Polygon\n",
    "\n",
    "from MPNet.enet import data_loader as ae_dl\n",
    "from MPNet.enet.CAE import ContractiveAutoEncoder\n",
    "from MPNet.neuralplanner import feasibility_check, lvc\n",
    "from MPNet.pnet.data_loader import loader\n",
    "from MPNet.pnet.model import PNet\n",
    "\n",
    "os.getcwd()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 97,
   "metadata": {},
   "outputs": [],
   "source": [
    "external_stylesheets = ['https://codepen.io/chriddyp/pen/bWLwgP.css']\n",
    "app = Dash(\"Replanning\", external_stylesheets=external_stylesheets)\n",
    "\n",
    "stage_figures = {0: [go.Figure(layout=dict(title=\"Bidirectional Planning\"))],\n",
    "                 1: [go.Figure(layout=dict(title=\"States Connection\"))],\n",
    "                 2: [go.Figure(layout=dict(title=\"LVC\"))],\n",
    "                 3: [go.Figure(layout=dict(title=\"Replanning\"))],\n",
    "                 4: [go.Figure(layout=dict(title=\"Final Path\"))]}\n",
    "\n",
    "graphs = {i: dcc.Graph(id={\"type\": \"graph\", \"index\": i}, animate=i not in (2, 3, 4)) for i in stage_figures.keys()}\n",
    "\n",
    "manual_update = dcc.Interval(id=\"update_interval\", disabled=True, n_intervals=0, interval=1000, max_intervals=1)\n",
    "slide_update = dcc.Interval(id=\"slide_interval\", disabled=True, n_intervals=0, interval=1200, max_intervals=-1)\n",
    "pause_button = html.Button(\"PAUSE\", id=\"pause_but\", n_clicks=0,\n",
    "                           style={\"width\"     : \"100px\", \"textAlign\": \"center\", \"marginBottom\": \"20px\",\n",
    "                                  \"marginLeft\": '20px'})\n",
    "feasible_buttons = {i: html.Button(\"FEASIBLE\", id={\"type\": \"feasible_but\", \"index\": i}, n_clicks=0, disabled=True,\n",
    "                                   style={\"textAlign\": \"center\", \"padding\": \"0 15px\", \"font-size\": \"14px\", 'color':\n",
    "                                       \"white\", \"background\": \"red\", \"cursor\": \"default\", \"font-weight\": \"bold\"}) for i\n",
    "                    in stage_figures.keys()}\n",
    "\n",
    "app.layout = html.Div([\n",
    "        html.H2(\"MPNet Execution Step by Step\"),\n",
    "        html.Div([\n",
    "                html.Button(\"PLAY\", id=\"play_but\", n_clicks=0,\n",
    "                            style={\"width\": \"100px\", \"textAlign\": \"center\", \"marginBottom\": \"20px\"}),\n",
    "                pause_button,\n",
    "                html.Div(id=\"feasible_div\",\n",
    "                         style={\"width\"  : \"125px\", \"marginBottom\": \"20px\", \"float\": \"right\", \"display\": \"flex\",\n",
    "                                \"padding\": \"0 15px\", \"justify-content\": \"flex-end\"}),\n",
    "                dcc.Slider(id=\"step_slider\", value=0, min=0, max=4, step=1, marks={\n",
    "                        0: \"Bidirectional\",\n",
    "                        1: \"States Connection\",\n",
    "                        2: \"LVC\",\n",
    "                        3: \"Replanning\",\n",
    "                        4: \"Final Path\"\n",
    "                        })], style={\"margin-bottom\": \"25px\", \"margin-left\": \"40px\", \"margin-right\": \"40px\"}),\n",
    "        html.Div(id=\"hidden_slider\"),\n",
    "        html.Div(id=\"graph_div\", style={\"display\": \"flex\", \"justify-content\": \"center\"}),\n",
    "        slide_update,\n",
    "        manual_update,\n",
    "        ])\n",
    "\n",
    "frame_sliders = {0: dcc.Slider(id={\"type\": \"dynamic_slider\", \"index\": 0}, min=1, max=2, value=1, step=1),\n",
    "                 1: dcc.Slider(id={\"type\": \"dynamic_slider\", \"index\": 1}, min=1, max=2, value=1, step=1),\n",
    "                 2: dcc.Slider(id={\"type\": \"dynamic_slider\", \"index\": 2}, min=1, max=2, value=1, step=1),\n",
    "                 3: dcc.Slider(id={\"type\": \"dynamic_slider\", \"index\": 3}, min=1, max=2, value=1, step=1),\n",
    "                 4: dcc.Slider(id={\"type\": \"dynamic_slider\", \"index\": 4}, min=1, max=2, value=1, step=1)}\n",
    "\n",
    "\n",
    "@app.callback(Output({\"type\": \"graph\", \"index\": MATCH}, \"figure\"),\n",
    "              Input({\"type\": \"dynamic_slider\", \"index\": MATCH}, \"value\"), Input(\"step_slider\", \"value\"))\n",
    "def master_slide_cb(value, master_value):\n",
    "    #     return dash.no_update, f'{value} and {master_value}'\n",
    "    try:\n",
    "        return stage_figures[master_value][value - 1]\n",
    "    except IndexError:\n",
    "        return dash.no_update\n",
    "\n",
    "\n",
    "@app.callback(Output({\"type\": \"dynamic_slider\", \"index\": MATCH}, \"value\"), Input(\"slide_interval\", \"n_intervals\"),\n",
    "              State(\"hidden_slider\", \"children\"))\n",
    "def interval_update(value, div_slider_children):\n",
    "    max_steps = div_slider_children[0]['props']['max']\n",
    "    if value == max_steps - 1:\n",
    "        slide_update.n_intervals = 0\n",
    "    return int(value % max_steps + 1)\n",
    "\n",
    "\n",
    "@app.callback(Output(\"slide_interval\", \"disabled\"), Input(\"play_but\", \"n_clicks\"), Input(\"pause_but\", \"n_clicks\"),\n",
    "              Input(\"slide_interval\", \"n_intervals\"), State(\"hidden_slider\", \"children\"), prevent_initial_call=True)\n",
    "def set_slide_interval_state(p_but, pause_but, value, children):\n",
    "    changed_id = [p['prop_id'] for p in dash.callback_context.triggered][0]\n",
    "    if 'play' in changed_id:\n",
    "        return False\n",
    "    elif 'pause' in changed_id:\n",
    "        return True\n",
    "    elif 'slide' in changed_id:\n",
    "        max_interval = children[0]['props']['max']\n",
    "        if value % max_interval == max_interval - 1:\n",
    "            return True\n",
    "        else:\n",
    "            return dash.no_update\n",
    "\n",
    "\n",
    "@app.callback(Output({\"type\": \"feasible_but\", \"index\": MATCH}, \"style\"),\n",
    "              Input({\"type\": \"feasible_but\", \"index\": MATCH}, \"n_clicks\"))\n",
    "def change_feasible_button_style(value):\n",
    "    mapping = {False: \"red\", True: \"green\"}\n",
    "    return {\"width\" : \"125px\", \"textAlign\": \"center\", \"color\": \"white\", \"background\": mapping[bool(value)],\n",
    "            \"cursor\": \"default\", \"font-weight\": \"bold\", \"font-size\": \"14px\", \"padding\": \"0 15px\"}\n",
    "\n",
    "\n",
    "@app.callback([Output(\"hidden_slider\", \"children\"), Output(\"hidden_slider\", \"style\"), Output(\"graph_div\", \"children\"),\n",
    "               Output(\"feasible_div\", \"children\")],\n",
    "              [Input(\"step_slider\", \"value\"), Input(\"update_interval\", \"n_intervals\")])\n",
    "def show_secondary_slider(value, enabled):\n",
    "    return [frame_sliders[value]], {\"marginBottom\": \"15px\", \"marginLeft\": \"40px\", \"marginRight\": \"40px\"}, [\n",
    "            graphs[value]], feasible_buttons[value]\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 98,
   "metadata": {},
   "outputs": [],
   "source": [
    "def update_sliders(figure_idx, markers={}):\n",
    "    frame_sliders[figure_idx].marks = markers\n",
    "    frame_sliders[figure_idx].max = len(markers)\n",
    "\n",
    "\n",
    "def update_figures(stage_idx, figures=[]):\n",
    "    stage_figures[stage_idx] = figures\n",
    "\n",
    "\n",
    "def update_feasible(analised_path, env, stage_idx):\n",
    "    feasible = feasibility_check(analised_path, env)\n",
    "    feasible_buttons[stage_idx].n_clicks = int(feasible)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "Initialize both modules from previously trained checkpoints"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 99,
   "metadata": {
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "pnet = PNet.load_from_checkpoint('models/pnet_3_cae.ckpt')\n",
    "enet_ckpt = pnet.training_config['enet']\n",
    "enet = ContractiveAutoEncoder.load_from_checkpoint(enet_ckpt)\n",
    "pnet.freeze()\n",
    "enet.freeze()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "Load the environment and trajectory data.\n",
    "The `loader` funcion returns a `torch.Dataset` instead of a `torch.DataLoader` when used with the get_dataset option\n",
    "set to True.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 100,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "127.0.0.1 - - [16/May/2021 20:00:25] \"POST /_dash-update-component HTTP/1.1\" 200 -\n",
      "127.0.0.1 - - [16/May/2021 20:00:25] \"POST /_dash-update-component HTTP/1.1\" 204 -\n",
      "127.0.0.1 - - [16/May/2021 20:00:25] \"POST /_dash-update-component HTTP/1.1\" 204 -\n",
      "127.0.0.1 - - [16/May/2021 20:00:26] \"POST /_dash-update-component HTTP/1.1\" 200 -\n",
      "127.0.0.1 - - [16/May/2021 20:00:26] \"POST /_dash-update-component HTTP/1.1\" 200 -\n",
      "127.0.0.1 - - [16/May/2021 20:00:26] \"POST /_dash-update-component HTTP/1.1\" 204 -\n"
     ]
    }
   ],
   "source": [
    "environments = ae_dl.load_perms(110, 0)\n",
    "\n",
    "# This may take a while\n",
    "data = loader(enet, \"valEnv/\", 110, 0, 1, get_dataset=True)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "\n",
    "When acessing the dataset with a tuple where the second element is a boolean set to True, the return value is a\n",
    "tuple with ` (input, target, path, embed_idx)`, where input is the processed input of the pnet module (where the\n",
    "environment has already been processed by the enet module), target is the target to be used when training, path is\n",
    "the reference path where the trajectory path comes from (only used for ilustration) and embed_idx is the index of\n",
    "the environment in the environments list."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 101,
   "metadata": {
    "pycharm": {
     "name": "#%%\n"
    },
    "tags": [
     "path"
    ]
   },
   "outputs": [
    {
     "data": {
      "text/plain": "((tensor([ 1.6291,  1.0762,  2.7489,  1.5210, -0.0190, -0.0365, -0.3569,  2.7006,\n           3.2458, -0.3456,  0.3670,  2.4598, -0.5975,  0.7302,  1.5588,  0.8238,\n           1.3224,  1.6739,  0.2004,  0.9638,  1.0873, -0.6527, -0.4027,  2.1710,\n           0.9183, -1.4306,  0.6946,  0.7743,  4.8255, -6.0217,  8.5704, -6.0862]),\n  tensor([ 5.8086, -6.0407]),\n  array([[-16.31999969,  -1.24000001],\n         [-12.73313221,  -2.2647926 ],\n         [-11.00150608,  -2.77480362],\n         [ -8.91285181,  -3.32947919],\n         [ -6.84361193,  -3.88879029],\n         [ -5.34212522,  -4.26410723],\n         [ -1.57698667,  -5.38536983],\n         [ -0.49412161,  -5.68837728],\n         [  2.78100487,  -6.0127461 ],\n         [  4.82552458,  -6.02169821],\n         [  5.80856036,  -6.04067165],\n         [  8.57040909,  -6.08615535]]),\n  100),\n 104160)"
     },
     "execution_count": 101,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Choose a path to be processed from available data\n",
    "path_idx = np.random.choice(len(data), 1)[0]\n",
    "path_idx = 104160\n",
    "model_input, _, reference_path, env_idx = data[path_idx, True]\n",
    "data[path_idx, True], path_idx"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "Since the `model_input` variable refers to a single step in a trajectory, it is necessary to change the elements that\n",
    "refer to the current state to the origin state of said trajectory.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 102,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "model_input[-4:-2] = torch.from_numpy(reference_path[0])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "Using the `env_idx` variable, it is possible to retrieve the environment data from the `environments` list.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [
    "def draw_obstacles(fig, perm, **kwargs):\n",
    "    obstacles = []\n",
    "    for obstacle in perm:\n",
    "        x, y = obstacle\n",
    "        obstacles.extend([[x - 2.5, y - 2.5], [x - 2.5, y + 2.5], [x + 2.5, y + 2.5], [x + 2.5, y - 2.5],\n",
    "                          [x - 2.5, y - 2.5], [None, None]])\n",
    "    obstacles = np.array(obstacles)\n",
    "    x = obstacles[:, 0]\n",
    "    y = obstacles[:, 1]\n",
    "    fig.add_trace(go.Scatter(x=x, y=y, fill=\"toself\", fillcolor=\"black\", name=\"obstacle\"), **kwargs)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 103,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "environment = environments[env_idx]\n",
    "obstacles_trace, obstacles = draw_obstacles(None, environment, return_trace=True, marker=dict(color='black'))\n",
    "obstacle = []\n",
    "obstacles_polygon = []\n",
    "for point in obstacles:\n",
    "    if None in point:\n",
    "        polygon = Polygon(obstacle)\n",
    "        obstacles_polygon.append(polygon)\n",
    "        obstacle = []\n",
    "    else:\n",
    "        obstacle.append(tuple(point))\n",
    "env_polygons = MultiPolygon(obstacles_polygon)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 104,
   "outputs": [],
   "source": [
    "def is_in_collision(x, env):\n",
    "    x = Point(x)\n",
    "    for obstacle in env:\n",
    "        if obstacle.contains(x):\n",
    "            return True\n",
    "    return False\n",
    "\n",
    "\n",
    "def steer_to(start, end, env):\n",
    "    start = Point(start)\n",
    "    end = Point(end)\n",
    "    line = LineString([start, end])\n",
    "    for polygon in env:\n",
    "        if polygon.intersects(line):\n",
    "            return False\n",
    "    else:\n",
    "        return True\n",
    "\n",
    "\n",
    "def feasibility_check(path, env) -> bool:\n",
    "    for i in range(0, len(path[:-1])):\n",
    "        ind = steer_to(path[i], path[i + 1], env)\n",
    "        if not ind:\n",
    "            return False\n",
    "    return True\n",
    "\n",
    "\n",
    "def lvc(path, env):\n",
    "    # Iterate from the first beacon state to the second to last.\n",
    "    for i in range(0, len(path) - 1):\n",
    "        # Iterate from the last beacon state to the ith.\n",
    "        for j in range(len(path) - 1, i + 1, -1):\n",
    "            ind = steer_to(path[i], path[j], env)\n",
    "            if ind:\n",
    "                pc = []\n",
    "                for k in range(0, i + 1):\n",
    "                    pc.append(path[k])\n",
    "                for k in range(j, len(path)):\n",
    "                    pc.append(path[k])\n",
    "\n",
    "                return lvc(pc, env)\n",
    "\n",
    "    return path\n",
    "\n",
    "\n",
    "def h_lvc(path, env):\n",
    "    p_1 = path[0]\n",
    "    if is_in_collision(p_1, env):\n",
    "        return False, None\n",
    "    optimized_path = [p_1]\n",
    "    for j, p_2 in enumerate(path[::-1][:-1]):\n",
    "        steerable = steer_to(p_1, p_2, env)\n",
    "        temp_path = []\n",
    "        if steerable:\n",
    "            feasible, states = h_lvc(path[len(path) - j - 1:], env)\n",
    "            if feasible:\n",
    "                optimized_path.extend(states)\n",
    "                return True, optimized_path\n",
    "    return False, None\n",
    "\n",
    "\n",
    "\n"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "Now we define the planning function and some auxiliary functions.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 105,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "def bidirectional_planning(pnet, origin, goal, env):\n",
    "    result_1 = deepcopy(origin[-4:-2])\n",
    "    result_2 = deepcopy(goal[-4:-2])\n",
    "    path_1, path_2 = [result_1.numpy()], [result_2.numpy()]\n",
    "    tree, target_reached = False, False\n",
    "    step = 0\n",
    "    while not target_reached and step < 150:\n",
    "        step += 1\n",
    "        if not tree:\n",
    "            result_1 = pnet(origin)\n",
    "            result_1 = result_1.data.detach()\n",
    "            path_1.append(result_1.numpy())\n",
    "            origin[-4:-2] = result_1\n",
    "            goal[-2:] = result_1\n",
    "        else:\n",
    "            result_2 = pnet(goal)\n",
    "            result_2 = result_2.data.detach()\n",
    "            path_2.append(result_2.numpy())\n",
    "            goal[-4:-2] = result_2\n",
    "            origin[-2:] = result_2\n",
    "        tree = not tree\n",
    "        target_reached = steer_to(result_1.numpy(), result_2.numpy(), env)\n",
    "    return target_reached, path_1, path_2\n",
    "\n",
    "\n",
    "def plan(pnet, env, model_input):\n",
    "    origin = deepcopy(model_input)\n",
    "    goal = deepcopy(origin)\n",
    "    goal[-4:] = goal[[-2, -1, -4, -3]]\n",
    "    target_reached, path_1, path_2 = bidirectional_planning(pnet, origin, goal, env)\n",
    "    return target_reached, path_1, path_2\n",
    "\n",
    "\n",
    "# Checks if it's necessary to replan this section\n",
    "def bidirectional_replan_check(pnet, env, start_point, goal_point, model_input):\n",
    "    start = deepcopy(model_input)\n",
    "    start[-4:] = torch.as_tensor([*start_point, *goal_point])\n",
    "    goal = deepcopy(start)\n",
    "    goal[-4:] = goal[[-2, -1, -4, -3]]\n",
    "    steerable = steer_to(start_point, goal_point, env)\n",
    "    return steerable, start, goal"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 106,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "target_reached, path_1, path_2 = plan(pnet, env_polygons, model_input)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "Animation of all the states processed by the bidirectional planning step."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 106,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 107,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "frames = pd.DataFrame(columns=[\"Frame\", \"x\", \"y\"])\n",
    "obstacles_df = pd.DataFrame(data=obstacles, columns=['x', 'y'])\n",
    "\n",
    "path_1 = np.array(path_1)\n",
    "path_2 = np.array(path_2)\n",
    "\n",
    "points_1x, points_1y = [], []\n",
    "points_2x, points_2y = [], []\n",
    "\n",
    "for n in range(len(path_1)):\n",
    "    frames = pd.concat(\n",
    "            [frames, pd.DataFrame([[n, *p_1, '1', 'Bidirectional'] for p_1 in path_1[:n + 1]],\n",
    "                                  columns=['Frame', 'x', 'y', 'Planner', 'Step'])], ignore_index=True)\n",
    "    frames = pd.concat(\n",
    "            [frames, pd.DataFrame([[n, *p_2, '2', 'Bidirectional'] for p_2 in path_2[:n + 1]],\n",
    "                                  columns=['Frame', 'x', 'y', 'Planner', 'Step'])], ignore_index=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 108,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "def add_obstacles_trace(figure):\n",
    "    trace = figure.select_traces(selector=dict(name='obstacle'))\n",
    "    if not list(trace):\n",
    "        figure.add_trace(obstacles_trace)\n",
    "    traces = figure.data[:-1]\n",
    "    obstacles = figure.data[-1]\n",
    "    figure.data = [obstacles, *traces]\n",
    "\n",
    "\n",
    "def update_fig(figure):\n",
    "    figure.update_layout(dict(width=1280, height=720, legend=dict(yanchor=\"bottom\", xanchor=\"left\", y=-0.1, x=-0.2)))\n",
    "    figure.update_traces(marker=dict(size=15),\n",
    "                         selector=dict(mode='markers'))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 109,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "unique_frames = frames[frames['Step'] == 'Bidirectional'].Frame.unique()\n",
    "figure_0 = [px.scatter(frames[(frames.Step == 'Bidirectional') & (frames.Frame == f)], x=\"x\", y=\"y\", color='Planner',\n",
    "                       range_x=[-20, 20], range_y=[-20, 20]) for f in unique_frames]\n",
    "for fig in figure_0:\n",
    "    add_obstacles_trace(fig)\n",
    "    update_fig(fig)\n",
    "\n",
    "update_sliders(0, {i + 1: f\"{i + 1}\" for i, _ in enumerate(figure_0)})\n",
    "update_figures(0, figure_0)\n",
    "manual_update.disabled = False\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "Connecting the beacon states."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 110,
   "metadata": {},
   "outputs": [],
   "source": [
    "path = np.concatenate([path_1, path_2[::-1]])\n",
    "\n",
    "frames = pd.concat(\n",
    "        [frames,\n",
    "         pd.DataFrame([[0, *p, \"\", \"Connection\"] for p in path], columns=['Frame', 'x', 'y', 'Planner', 'Step']),\n",
    "         pd.DataFrame([[1, *p, \"\", \"Connection\"] for p in path], columns=['Frame', 'x', 'y', 'Planner', 'Step'])],\n",
    "        ignore_index=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 111,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "unique_frames = frames[frames['Step'] == 'Connection'].Frame.unique()\n",
    "figure_1 = [px.scatter(frames[(frames.Step == 'Connection') & (frames.Frame == f)], x=\"x\", y=\"y\", color='Planner',\n",
    "                       range_x=[-20, 20], range_y=[-20, 20]) for f in unique_frames]\n",
    "for fig in figure_1:\n",
    "    add_obstacles_trace(fig)\n",
    "    update_fig(fig)\n",
    "\n",
    "figure_1[-1].update_traces(dict(mode='lines+markers', marker=dict(size=15)), selector=dict(mode='markers'))\n",
    "update_sliders(1, {i + 1: f\"{i + 1}\" for i, _ in enumerate(figure_1)})\n",
    "update_figures(1, figure_1)\n",
    "update_feasible(path, env_polygons, 1)\n",
    "manual_update.disabled = False\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "Running the Lazy Vertex Contraction algorithm to resume the obtained trajectory."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 112,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "\n",
    "frames = pd.concat(\n",
    "        [frames,\n",
    "         pd.DataFrame([[0, *p, \"\", \"lvc\"] for p in path], columns=['Frame', 'x', 'y', 'Planner', 'Step'])],\n",
    "        ignore_index=True)\n",
    "\n",
    "path = np.array(lvc(path, env_polygons))\n",
    "\n",
    "frames = pd.concat(\n",
    "        [frames,\n",
    "         pd.DataFrame([[1, *p, \"\", \"lvc\"] for p in path], columns=['Frame', 'x', 'y', 'Planner', 'Step'])],\n",
    "        ignore_index=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 113,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "unique_frames = frames[frames['Step'] == 'lvc'].Frame.unique()\n",
    "figure_2 = [px.scatter(frames[(frames.Step == 'lvc') & (frames.Frame == f)], x=\"x\", y=\"y\", color='Planner',\n",
    "                       range_x=[-20, 20], range_y=[-20, 20]) for f in unique_frames]\n",
    "for fig in figure_2:\n",
    "    add_obstacles_trace(fig)\n",
    "    update_fig(fig)\n",
    "    fig.update_traces(dict(mode='lines+markers', marker=dict(size=15)), selector=dict(mode='markers'))\n",
    "\n",
    "update_sliders(2, {i + 1: f\"{i + 1}\" for i, _ in enumerate(figure_2)})\n",
    "update_figures(2, figure_2)\n",
    "update_feasible(path, env_polygons, 2)\n",
    "manual_update.disabled = False"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "Remove beacon states that colide with an obstacle."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 114,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "def remove_invalid_beacon_states(path):\n",
    "    updated_index = []\n",
    "    new_path = []\n",
    "    for state in path:\n",
    "        if not is_in_collision(state, env_polygons):\n",
    "            new_path.append(state)\n",
    "        else:\n",
    "            new_path[-1] = np.mean([new_path[-1], new_path[-2]], axis=0)\n",
    "    for n, state in enumerate(new_path[::-1]):\n",
    "        if is_in_collision(state, env_polygons):\n",
    "            try:\n",
    "                new_path[n - 1] = np.mean([new_path[n - 1], new_path[n - 2]], axis=0)\n",
    "            except IndexError:\n",
    "                pass\n",
    "    new_path = np.array(new_path)\n",
    "    return new_path\n",
    "\n",
    "\n",
    "new_path = remove_invalid_beacon_states(path)\n",
    "\n",
    "# For illustration purposes only\n",
    "connectable_paths = []\n",
    "temp_list = [path[0]]\n",
    "for i in range(1, len(path)):\n",
    "    try:\n",
    "        if steer_to(path[i], path[i - 1], env_polygons):\n",
    "            temp_list.append(path[i])\n",
    "        elif steer_to(path[i], path[i + 1], env_polygons):\n",
    "            connectable_paths.append(temp_list)\n",
    "            temp_list = [path[i]]\n",
    "        else:\n",
    "            if temp_list:\n",
    "                connectable_paths.append(temp_list)\n",
    "                temp_list = []\n",
    "    except IndexError:\n",
    "        if temp_list:\n",
    "            connectable_paths.append(temp_list)\n",
    "            temp_list = []\n",
    "else:\n",
    "    if not temp_list:\n",
    "        temp_list = [path[-1]]\n",
    "    connectable_paths.append(temp_list)\n",
    "\n",
    "frames = pd.concat([frames,\n",
    "                    pd.DataFrame([[\"Previous Path\", *p, \"Optimized Path\", \"Replan\"] for p in path],\n",
    "                                 columns=['Frame', 'x', 'y', 'Planner', 'Step']),\n",
    "                    *[pd.DataFrame([[\"Valid Beacon States\", *p, f\"Valid Subtrajectory {n}\", \"Replan\"] for p in stage],\n",
    "                                   columns=[\"Frame\", \"x\", \"y\", \"Planner\", \"Step\"]) for n, stage in\n",
    "                      enumerate(connectable_paths)]], ignore_index=True)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 115,
   "outputs": [],
   "source": [
    "replanned_path = path\n",
    "tries = 0\n",
    "feasible = feasibility_check(new_path, env_polygons)\n",
    "while not feasible and tries < 20:\n",
    "    tries += 1\n",
    "    new_path = remove_invalid_beacon_states(new_path)\n",
    "    replanned_path = [new_path[0]]\n",
    "    for i in range(len(new_path) - 1):\n",
    "        steerable, start, goal = bidirectional_replan_check(pnet, env_polygons, new_path[i], new_path[i + 1],\n",
    "                                                            model_input)\n",
    "        if steerable:\n",
    "            replanned_path.append(new_path[i + 1])\n",
    "        else:\n",
    "            target_reach, rpath_1, rpath_2 = bidirectional_planning(pnet, start, goal, env_polygons)\n",
    "            replanned_path = list(np.concatenate([replanned_path, rpath_1, rpath_2[::-1]]))\n",
    "\n",
    "    replanned_path = list(np.unique(replanned_path, axis=0))\n",
    "    lvc_replanned_path = lvc(replanned_path, env_polygons)\n",
    "    lvc_replanned_path = np.array(lvc_replanned_path)\n",
    "    feasible = feasibility_check(lvc_replanned_path, env_polygons)\n",
    "    if feasible:\n",
    "        new_path = lvc_replanned_path\n",
    "        break\n",
    "    else:\n",
    "        new_path = np.array(replanned_path)\n",
    "    frames = pd.concat(\n",
    "            [frames, pd.DataFrame(\n",
    "                    [[f\"Try {tries}\", *beacon_state, f\"Replanned Path {tries}\", \"Replan\"] for beacon_state in\n",
    "                     new_path],\n",
    "                    columns=[\"Frame\", \"x\", \"y\", \"Planner\", \"Step\"])], ignore_index=True)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 116,
   "outputs": [],
   "source": [
    "\n",
    "new_path = np.array(lvc(new_path, env_polygons))\n",
    "frames = pd.concat([frames,\n",
    "                    pd.DataFrame(\n",
    "                            [[\"Bidirectional Replanning\", *beacon_state, \"Replanned Path\", \"Replan\"] for beacon_state in\n",
    "                             new_path], columns=[\"Frame\", \"x\", \"y\", \"Planner\", \"Step\"])], ignore_index=True)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 117,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "unique_frames = frames[frames['Step'] == 'Replan'].Frame.unique()\n",
    "figure_3 = [px.scatter(frames[(frames.Step == 'Replan') & (frames.Frame == f)], x=\"x\", y=\"y\", color='Planner',\n",
    "                       range_x=[-20, 20], range_y=[-20, 20]) for f in unique_frames]\n",
    "for fig in figure_3:\n",
    "    add_obstacles_trace(fig)\n",
    "    update_fig(fig)\n",
    "    fig.update_traces(dict(mode='lines+markers', marker=dict(size=15)), selector=dict(mode='markers'))\n",
    "\n",
    "update_sliders(3, {i + 1: {'label': f} for i, f in enumerate(unique_frames)})\n",
    "update_figures(3, figure_3)\n",
    "update_feasible(new_path, env_polygons, 3)\n",
    "manual_update.disabled = False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 118,
   "outputs": [],
   "source": [
    "figure_4 = []\n",
    "for k, v in stage_figures.items():\n",
    "    if k != 4:\n",
    "        figure_4.append(v[-1])\n",
    "\n",
    "unique_steps = frames.Step.unique()\n",
    "\n",
    "update_sliders(4, {n + 1: f for n, f in enumerate(unique_steps)})\n",
    "update_figures(4, figure_4)\n",
    "update_feasible(new_path, env_polygons, 4)\n",
    "manual_update.disabled = False"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 119,
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/home/jhonas/dev/mpr/.venv/lib/python3.8/site-packages/jupyter_dash/jupyter_app.py:139: UserWarning:\n",
      "\n",
      "The 'environ['werkzeug.server.shutdown']' function is deprecated and will be removed in Werkzeug 2.1.\n",
      "\n",
      "127.0.0.1 - - [16/May/2021 20:00:30] \"GET /_shutdown_b4450afd-37b7-4cd9-bf50-51f5ac47dd27 HTTP/1.1\" 200 -\n",
      " * Running on http://127.0.0.1:8050/ (Press CTRL+C to quit)\n",
      "127.0.0.1 - - [16/May/2021 20:00:31] \"GET /_alive_b4450afd-37b7-4cd9-bf50-51f5ac47dd27 HTTP/1.1\" 200 -\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Dash app running on http://127.0.0.1:8050/\n"
     ]
    }
   ],
   "source": [
    "app.run_server(mode='external', debug=False)\n"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 119,
   "outputs": [],
   "source": [],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "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.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}