Algorithm_Testbench.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Algorithm Test Bench\n",
    "Replace the `Search Map` block with any RRT variant to test it against obstacles generated randomly"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cv2\n",
    "import heapq\n",
    "import time\n",
    "import math"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Define robot parameters"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "ROBOT_RADIUS = 50#/1000 # 220mm\n",
    "clearance = 5\n",
    "clearance += ROBOT_RADIUS"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Generate a random map \n",
    "Generate a random map of obstacles of different types"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "clearance_color = (0, 255, 255)\n",
    "obstacle_color = (0, 0, 0)\n",
    "robot_radius_color = (254, 105, 180)\n",
    "free_space = (255, 255, 255)\n",
    "start_color = (254, 0, 0)\n",
    "goal_color = (0, 0, 254)\n",
    "\n",
    "# Define the map size\n",
    "width = 1000\n",
    "height = 1000\n",
    "scale = 1\n",
    "\n",
    "width_resized = width//scale\n",
    "height_resized = height//scale\n",
    "\n",
    "\n",
    "# Define a function to generate random obstacles\n",
    "def obstacles():\n",
    "\n",
    "    # Create a black canvas \n",
    "    canvas = np.zeros((height, width, 3), dtype=\"uint8\")\n",
    "    # Create a white rectangle\n",
    "    canvas = cv2.rectangle(canvas, (clearance, clearance), (width-clearance, height-clearance), free_space, -1)\n",
    "\n",
    "    sq_size = 25\n",
    "    num_obstacles = 100\n",
    "\n",
    "    for i in range(num_obstacles):\n",
    "\n",
    "        # Randomly generate the obstacle position\n",
    "        x = np.random.randint(clearance, width-clearance)\n",
    "        y = np.random.randint(clearance, height-clearance)\n",
    "\n",
    "        if i%2 == 0:\n",
    "            # Generate a square at the random position\n",
    "            canvas = cv2.rectangle(canvas, (x-sq_size, y-sq_size), (x+sq_size, y+sq_size), obstacle_color, -1)\n",
    "        else:\n",
    "            # Generate a rectangle of sq_size + random length at the random position\n",
    "            x_len = np.random.randint(15, 40)\n",
    "            y_len = np.random.randint(15, 40)\n",
    "            canvas = cv2.rectangle(canvas, (x-x_len, y-y_len), (x+x_len, y+y_len), obstacle_color, -1)\n",
    "    \n",
    "    # Sample a start from 0 to 100 for x and y, and make sure it is not in the obstacle\n",
    "    start = (np.random.randint(clearance, clearance+100), np.random.randint(height-clearance, height))\n",
    "    while canvas[start[1], start[0], 0] == 0:\n",
    "        start = (np.random.randint(clearance, clearance+100), np.random.randint(height-clearance, height))\n",
    "\n",
    "    # Sample a goal from 100 off the top right corner for x and y, and make sure it is not in the obstacle\n",
    "    goal = (np.random.randint(width-100, width-clearance), np.random.randint(clearance, 100))\n",
    "    while canvas[goal[1], goal[0], 0] == 0:\n",
    "        goal = (np.random.randint(width-100, width-clearance), np.random.randint(clearance, 100))\n",
    "\n",
    "    # Draw the start and goal on the canvas\n",
    "    canvas = cv2.circle(canvas, start, 5, start_color, 5)\n",
    "    canvas = cv2.circle(canvas, goal, 5, goal_color, 5)\n",
    "\n",
    "    # Scale the canvas down to the desired size by scaling the image\n",
    "    canvas = cv2.resize(canvas, (width_resized, height_resized), interpolation=cv2.INTER_NEAREST)\n",
    "    return canvas, start, goal"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Display the generated map"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 44,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Get a random obstacle map\n",
    "map, start_node, goal_node = obstacles()\n",
    "canvas = map.copy()\n",
    "\n",
    "cv2.imshow(\"Obstacles\", canvas)\n",
    "cv2.waitKey(0)\n",
    "cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Implement a search algorithm"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### RRT*"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 45,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Goal reached:  3.1297643184661865 seconds\n",
      "Current Cost:  1339.046470530676\n",
      "Goal reached:  5.329076528549194 seconds\n",
      "Current Cost:  1334.3852068832075\n",
      "Goal reached:  6.976219415664673 seconds\n",
      "Current Cost:  1328.5808539622706\n",
      "Goal reached:  7.0925986766815186 seconds\n",
      "Current Cost:  1331.4669319713148\n",
      "Final cost:  1275.803808491849\n"
     ]
    }
   ],
   "source": [
    "canvas = map.copy()\n",
    "x_start, y_start = start_node\n",
    "x_goal, y_goal = goal_node\n",
    "\n",
    "step_size = 50\n",
    "distance_threshold = 1\n",
    "search_radius = 70\n",
    "\n",
    "# Make a lambda function to adjust the value of x to the visited space\n",
    "adjust = lambda x, threshold: int(int(round(x*2)/2)/threshold)\n",
    "\n",
    "# Dictionary to store visited nodes\n",
    "visited = {(adjust(x_start, distance_threshold),\n",
    "            adjust(y_start, distance_threshold)): 1}\n",
    "\n",
    "# Dictionary to store the parent of each node\n",
    "parent = {(x_start, y_start): (x_start, y_start)}\n",
    "\n",
    "# Dictionary to store the children of each node\n",
    "children = {(x_start, y_start): []}\n",
    "\n",
    "# Dictionary to store the cost of each node\n",
    "cost = {(x_start, y_start): 0}\n",
    "\n",
    "# Function to check if the node is valid\n",
    "def valid_node(x, y):\n",
    "    # Adjust the value for visited nodes\n",
    "    x_vis = adjust(x, distance_threshold)\n",
    "    y_vis = adjust(y, distance_threshold)\n",
    "    # Adjust the values for canvas\n",
    "    x_cvs = int(round(x*2)/2)\n",
    "    y_cvs = int(round(y*2)/2)\n",
    "\n",
    "    # Check if the node is not in the obstacle and not visited\n",
    "    if canvas[y_cvs, x_cvs, 0] != 0 and (x_vis, y_vis) not in visited:\n",
    "        return True\n",
    "    return False\n",
    "\n",
    "# Function to check if the child node and parent node are not intersecting with the obstacle\n",
    "def valid_edge(x_parent, y_parent, x_child, y_child):\n",
    "    # Sample 3 points on the line joining the parent and child nodes, \n",
    "    # not including the parent and child nodes\n",
    "    x_intermediate = np.linspace(x_parent, x_child, 5)[1:-1]\n",
    "    y_intermediate = np.linspace(y_parent, y_child, 5)[1:-1]\n",
    "\n",
    "    # Adjust the values for canvas\n",
    "    x_intermediate = [int(round(x*2)/2) for x in x_intermediate]\n",
    "    y_intermediate = [int(round(y*2)/2) for y in y_intermediate]\n",
    "\n",
    "    # Check if any of the intermediate points are in the obstacle\n",
    "    for x, y in zip(x_intermediate, y_intermediate):\n",
    "        if canvas[y, x, 0] == 0:\n",
    "            return False\n",
    "    return True\n",
    "\n",
    "\n",
    "reached = False\n",
    "iterations = 0\n",
    "c_best = float('inf')\n",
    "start = time.time()\n",
    "\n",
    "while iterations < 1000:\n",
    "    \n",
    "    # Get a new node\n",
    "    x_node = np.random.randint(clearance, width-clearance)\n",
    "    y_node = np.random.randint(clearance, height-clearance)    \n",
    "\n",
    "    # If the node is valid\n",
    "    if valid_node(x_node, y_node):\n",
    "\n",
    "        # Find the nearsest node in the tree to get the projected node\n",
    "        min_dist = float('inf')\n",
    "\n",
    "        for x, y in parent:\n",
    "            dist = np.sqrt((x-x_node)**2 + (y-y_node)**2)\n",
    "            if dist < min_dist:\n",
    "                min_dist = dist\n",
    "                x_parent, y_parent = x, y\n",
    "\n",
    "        # Get the angle of the new node\n",
    "        theta = np.arctan2(y_node-y_parent, x_node-x_parent)\n",
    "\n",
    "        # Calculate the new node\n",
    "        x_new = int(x_parent + step_size*np.cos(theta))\n",
    "        y_new = int(y_parent + step_size*np.sin(theta))\n",
    "        \n",
    "        if reached:\n",
    "            iterations += 1\n",
    "\n",
    "        # Check if the new node is valid\n",
    "        if valid_node(x_new, y_new):\n",
    "\n",
    "            # Collect all the nodes in the search radius\n",
    "            neighbours = []\n",
    "            for x, y in parent:\n",
    "                if np.sqrt((x-x_new)**2 + (y-y_new)**2) < search_radius:\n",
    "                    neighbours.append((x, y))\n",
    "\n",
    "            # Find the node with the minimum cost to get new node\n",
    "            min_cost = cost[(x_parent, y_parent)] + step_size\n",
    "            for x, y in neighbours:\n",
    "                new_cost = cost[(x, y)] + np.sqrt((x-x_new)**2 + (y-y_new)**2)\n",
    "                if new_cost < min_cost:\n",
    "                    min_cost = new_cost\n",
    "                    x_parent, y_parent = x, y\n",
    "\n",
    "            # Check if the edge between the parent and child nodes is valid\n",
    "            if not valid_edge(x_parent, y_parent, x_new, y_new):\n",
    "                continue\n",
    "            \n",
    "            # Add the cost of the new node\n",
    "            cost[(x_new, y_new)] = min_cost\n",
    "\n",
    "            # Check if rewiring to the newly added node will reduce the cost of the neighbours\n",
    "            for x, y in neighbours:\n",
    "                new_cost = cost[(x_new, y_new)] + np.sqrt((x-x_new)**2 + (y-y_new)**2)\n",
    "                if new_cost < cost[(x, y)]:\n",
    "\n",
    "                    # Check if the edge between the parent and child nodes is valid\n",
    "                    if not valid_edge(x, y, x_new, y_new):\n",
    "                        continue\n",
    "\n",
    "                    previous_cost = cost[(x, y)]\n",
    "                    cost[(x, y)] = new_cost\n",
    "                    \n",
    "                    # Remove this node from the children of its current parent\n",
    "                    x_parent_neighbour, y_parent_neighbour = parent[(x, y)]\n",
    "                    children[(x_parent_neighbour, y_parent_neighbour)].remove((x, y))\n",
    "\n",
    "                    # Add the new node to the parent dictionary\n",
    "                    parent[(x, y)] = (x_new, y_new)\n",
    "\n",
    "                    # Add the new node to the children dictionary\n",
    "                    children.setdefault((x_new, y_new), []).append((x, y))\n",
    "                    \n",
    "                    # If the cost of this node is reduced, we must update the cost of its children,\n",
    "                    # and their children, and so on\n",
    "                    cost_reduction = previous_cost - new_cost\n",
    "                    queue = []\n",
    "                    heapq.heappush(queue, ((x, y)))\n",
    "                    while queue:\n",
    "                        x_, y_ = heapq.heappop(queue)\n",
    "                        # Check if the node has children\n",
    "                        if (x_, y_) in children:\n",
    "                            for child in children[(x_, y_)]:\n",
    "                                    cost[child] -= cost_reduction\n",
    "                                    heapq.heappush(queue, child)\n",
    "                                    \n",
    "            x_achieved, y_achieved = x_new, y_new\n",
    "\n",
    "            x_adjusted = adjust(x_new, distance_threshold)\n",
    "            y_adjusted = adjust(y_new, distance_threshold)\n",
    "            \n",
    "            # Add the new node to the visited nodes\n",
    "            visited[(x_adjusted, y_adjusted)] = 1\n",
    "\n",
    "            # Add the new node to the parent dictionary\n",
    "            parent[(x_new, y_new)] = (x_parent, y_parent)\n",
    "\n",
    "            # Add the new node to the children dictionary\n",
    "            children.setdefault((x_parent, y_parent), []).append((x_new, y_new))\n",
    "\n",
    "\n",
    "                # Check if the new node is close to the goal and the cost is less than the best cost\n",
    "            if np.sqrt((x_new-x_goal)**2 + (y_new-y_goal)**2) < 50 and cost[(x_new, y_new)] < c_best:\n",
    "                # if np.sqrt((x_new-x_goal)**2 + (y_new-y_goal)**2) < 50:\n",
    "                    end = time.time()\n",
    "                    print(\"Goal reached: \", end-start, \"seconds\")\n",
    "\n",
    "                    # Update cost of c_best\n",
    "                    c_best = cost[(x_new, y_new)]\n",
    "\n",
    "                    # Add the final node as a child of the current node to complete the path\n",
    "                    children.setdefault((x_new, y_new), []).append((x_goal, y_goal))\n",
    "\n",
    "                    # Add the final node to the parent dictionary\n",
    "                    parent[(x_goal, y_goal)] = (x_new, y_new)\n",
    "\n",
    "                    # Save the final node\n",
    "                    x_final, y_final = x_goal, y_goal\n",
    "\n",
    "                    # Add the cost of the final node\n",
    "                    cost[(x_goal, y_goal)] = cost[(x_new, y_new)] + np.sqrt((x_goal-x_new)**2 + (y_goal-y_new)**2)\n",
    "\n",
    "                    print(\"Current Cost: \", cost[(x_goal, y_goal)])\n",
    "\n",
    "                    # Set the reached flag to True\n",
    "                    reached = True\n",
    "        \n",
    "print(\"Final cost: \", cost[(x_final, y_final)]) "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### RRT*-SMART"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 49,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Goal reached:  2.003403663635254 seconds\n",
      "Current Cost:  1265.2955393004033\n",
      "Goal reached:  4.2163918018341064 seconds\n",
      "Current Cost:  1255.2964146952945\n",
      "Goal reached:  4.966987371444702 seconds\n",
      "Current Cost:  1246.524917404515\n",
      "Goal reached:  6.7037060260772705 seconds\n",
      "Current Cost:  1245.765642610983\n",
      "Goal reached:  7.337255239486694 seconds\n",
      "Current Cost:  1243.3487929837675\n",
      "Final cost:  1168.6829812097849\n"
     ]
    }
   ],
   "source": [
    "canvas = map.copy()\n",
    "x_start, y_start = start_node\n",
    "x_goal, y_goal = goal_node\n",
    "\n",
    "step_size = 50\n",
    "distance_threshold = 1\n",
    "search_radius = 70\n",
    "\n",
    "# Make a lambda function to adjust the value of x to the visited space\n",
    "adjust = lambda x, threshold: int(int(round(x*2)/2)/threshold)\n",
    "\n",
    "# Dictionary to store visited nodes\n",
    "visited = {(adjust(x_start, distance_threshold),\n",
    "            adjust(y_start, distance_threshold)): 1}\n",
    "\n",
    "# Dictionary to store the parent of each node\n",
    "parent = {(x_start, y_start): (x_start, y_start)}\n",
    "\n",
    "# Dictionary to store the children of each node\n",
    "children = {(x_start, y_start): []}\n",
    "\n",
    "# Dictionary to store the cost of each node\n",
    "cost = {(x_start, y_start): 0}\n",
    "\n",
    "# Function to check if the node is valid\n",
    "def valid_node(x, y):\n",
    "    # Adjust the value for visited nodes\n",
    "    x_vis = adjust(x, distance_threshold)\n",
    "    y_vis = adjust(y, distance_threshold)\n",
    "\n",
    "    # Adjust the values for canvas\n",
    "    x_cvs = int(round(x*2)/2)\n",
    "    y_cvs = int(round(y*2)/2)\n",
    "\n",
    "    # Check if the node is not in the obstacle and not visited\n",
    "    if canvas[y_cvs, x_cvs, 0] !=0 and (x_vis, y_vis) not in visited:\n",
    "            return True\n",
    "    return False\n",
    "\n",
    "# Function to check if the child node and parent node are not intersecting with the obstacle\n",
    "def valid_edge(x_parent, y_parent, x_child, y_child):\n",
    "    # Sample n_sample points on the line joining the parent and child nodes, \n",
    "    # not including the parent and child nodes\n",
    "    n_sample = int(np.sqrt((x_child-x_parent)**2 + (y_child-y_parent)**2)/distance_threshold)\n",
    "    x_intermediate = np.linspace(x_parent, x_child, n_sample)[1:-1]\n",
    "    y_intermediate = np.linspace(y_parent, y_child, n_sample)[1:-1]\n",
    "\n",
    "    # Adjust the values for canvas\n",
    "    x_intermediate = [int(round(x*2)/2) for x in x_intermediate]\n",
    "    y_intermediate = [int(round(y*2)/2) for y in y_intermediate]\n",
    "\n",
    "    # Check if any of the intermediate points are in the obstacle\n",
    "    for x, y in zip(x_intermediate, y_intermediate):\n",
    "        if canvas[y, x, 0] == 0:\n",
    "            return False\n",
    "    return True\n",
    "\n",
    "def compute_path(x_start, y_start, x_goal, y_goal):\n",
    "    # Get the path from the parent dictionary\n",
    "    path = []\n",
    "    x, y = x_final, y_final\n",
    "    while (x, y) != (x_start, y_start):\n",
    "        # print(x, y)\n",
    "        path.append((x, y))\n",
    "        x, y = parent[(x, y)]\n",
    "    path.append((x, y))\n",
    "    path.reverse()\n",
    "    return path\n",
    "\n",
    "def sample():\n",
    "    # If the goal is reached, sample heuristic is satisfied and beacons are not empty\n",
    "    if reached and iterations%2==0 and beacons:\n",
    "        # Randomly pick a point from the beacons\n",
    "        x_beacon, y_beacon = beacons[np.random.randint(0, len(beacons))]\n",
    "\n",
    "        # Sample a point around the beacon of radius r_beacon\n",
    "        x_node = np.random.randint(x_beacon-r_beacon, x_beacon+r_beacon)\n",
    "        y_node = np.random.randint(y_beacon-r_beacon, y_beacon+r_beacon)\n",
    "        \n",
    "    else:\n",
    "        x_node = np.random.randint(clearance, width-clearance)\n",
    "        y_node = np.random.randint(clearance, height-clearance)  \n",
    "    return x_node, y_node  \n",
    "\n",
    "\n",
    "reached = False\n",
    "iterations = 0\n",
    "c_best = float('inf') \n",
    "n = -1 # Iteration at which the goal is reached\n",
    "N = 1000/1 # Number of iterations\n",
    "b = 4 # Constant that determines number of times to sample around beacons\n",
    "r_beacon = 30 # Radius around the beacon to sample\n",
    "start = time.time()\n",
    "\n",
    "while iterations < N:\n",
    "    \n",
    "    # Get a new node\n",
    "    # x_node = np.random.randint(clearance, width-clearance)\n",
    "    # y_node = np.random.randint(clearance, height-clearance) \n",
    "\n",
    "    x_node, y_node = sample()   \n",
    "\n",
    "    # If the node is valid\n",
    "    if valid_node(x_node, y_node):\n",
    "\n",
    "        # Find the nearsest node in the tree to get the projected node\n",
    "        min_dist = float('inf')\n",
    "\n",
    "        for x, y in parent:\n",
    "            dist = np.sqrt((x-x_node)**2 + (y-y_node)**2)\n",
    "            if dist < min_dist:\n",
    "                min_dist = dist\n",
    "                x_parent, y_parent = x, y\n",
    "\n",
    "        # Get the angle of the new node\n",
    "        theta = np.arctan2(y_node-y_parent, x_node-x_parent)\n",
    "\n",
    "        # Calculate the new node\n",
    "        x_new = int(x_parent + step_size*np.cos(theta))\n",
    "        y_new = int(y_parent + step_size*np.sin(theta))\n",
    "        \n",
    "        if reached:\n",
    "            iterations += 1\n",
    "\n",
    "        # Check if the new node is valid\n",
    "        if valid_node(x_new, y_new):\n",
    "\n",
    "            # Collect all the nodes in the search radius\n",
    "            neighbours = []\n",
    "            for x, y in parent:\n",
    "                if np.sqrt((x-x_new)**2 + (y-y_new)**2) < search_radius:\n",
    "                    neighbours.append((x, y))\n",
    "\n",
    "            # Find the node with the minimum cost to get new node\n",
    "            min_cost = cost[(x_parent, y_parent)] + step_size\n",
    "            for x, y in neighbours:\n",
    "                new_cost = cost[(x, y)] + np.sqrt((x-x_new)**2 + (y-y_new)**2)\n",
    "                if new_cost < min_cost:\n",
    "                    min_cost = new_cost\n",
    "                    x_parent, y_parent = x, y\n",
    "\n",
    "            # Check if the edge between the parent and child nodes is valid\n",
    "            if not valid_edge(x_parent, y_parent, x_new, y_new):\n",
    "                continue\n",
    "            \n",
    "            # Add the cost of the new node\n",
    "            cost[(x_new, y_new)] = min_cost\n",
    "\n",
    "            # Check if rewiring to the newly added node will reduce the cost of the neighbours\n",
    "            for x, y in neighbours:\n",
    "                new_cost = cost[(x_new, y_new)] + np.sqrt((x-x_new)**2 + (y-y_new)**2)\n",
    "                if new_cost < cost[(x, y)]:\n",
    "\n",
    "                    # Check if the edge between the parent and child nodes is valid\n",
    "                    if not valid_edge(x, y, x_new, y_new):\n",
    "                        continue\n",
    "\n",
    "                    previous_cost = cost[(x, y)]\n",
    "                    cost[(x, y)] = new_cost\n",
    "                    \n",
    "                    # Remove this node from the children of its current parent\n",
    "                    x_parent_neighbour, y_parent_neighbour = parent[(x, y)]\n",
    "                    children[(x_parent_neighbour, y_parent_neighbour)].remove((x, y))\n",
    "\n",
    "                    # Add the new node to the parent dictionary\n",
    "                    parent[(x, y)] = (x_new, y_new)\n",
    "\n",
    "                    # Add the new node to the children dictionary\n",
    "                    children.setdefault((x_new, y_new), []).append((x, y))\n",
    "                    \n",
    "                    # If the cost of this node is reduced, we must update the cost of its children,\n",
    "                    # and their children, and so on\n",
    "                    cost_reduction = previous_cost - new_cost\n",
    "                    queue = []\n",
    "                    heapq.heappush(queue, (x, y))\n",
    "                    while queue:\n",
    "                        x_, y_ = heapq.heappop(queue)\n",
    "                        # Check if the node has children\n",
    "                        if (x_, y_) in children:\n",
    "                            for child in children[(x_, y_)]:\n",
    "                                    cost[child] -= cost_reduction\n",
    "                                    heapq.heappush(queue, child)\n",
    "                                    \n",
    "            x_achieved, y_achieved = x_new, y_new\n",
    "\n",
    "            x_adjusted = adjust(x_new, distance_threshold)\n",
    "            y_adjusted = adjust(y_new, distance_threshold)\n",
    "            \n",
    "            # Add the new node to the visited nodes\n",
    "            visited[(x_adjusted, y_adjusted)] = 1\n",
    "\n",
    "            # Add the new node to the parent dictionary\n",
    "            parent[(x_new, y_new)] = (x_parent, y_parent)\n",
    "\n",
    "            # Add the new node to the children dictionary\n",
    "            children.setdefault((x_parent, y_parent), []).append((x_new, y_new))\n",
    "\n",
    "            # Path Optimization: Check if the goal is reached, then check if a node on the path can be\n",
    "            # connected to its grandparent node\n",
    "            if reached:\n",
    "                path = compute_path(x_start, y_start, x_final, y_final)\n",
    "\n",
    "                # Start from the goal node\n",
    "                node = path[-1]\n",
    "                parent_node = parent[node]\n",
    "                grandparent_node = parent[parent_node]\n",
    "\n",
    "                while node!= (x_start, y_start):\n",
    "                    # Check if there is a valid edge between the node and its grandparent\n",
    "                    if valid_edge(grandparent_node[0], grandparent_node[1], node[0], node[1]):\n",
    "                        \n",
    "                        # print(\"Rewiring the tree\")\n",
    "                        # Rewire the tree\n",
    "                        # Remove the node from the children of its parent\n",
    "                        children[parent_node].remove(node)\n",
    "\n",
    "                        # Add the node to the parent dictionary of the grandparent\n",
    "                        parent[node] = grandparent_node\n",
    "\n",
    "                        # Add the node to the children dictionary of the grandparent\n",
    "                        children.setdefault(grandparent_node, []).append(node)\n",
    "\n",
    "                        # Update the cost of the node\n",
    "                        previous_cost = cost[node]\n",
    "                        cost[node] = cost[grandparent_node] + np.sqrt((node[0]-grandparent_node[0])**2 + (node[1]-grandparent_node[1])**2)\n",
    "\n",
    "                        # Update the cost of the children of the node, and their children, and so on\n",
    "                        cost_reduction = previous_cost - cost[node]\n",
    "                        queue = []\n",
    "                        heapq.heappush(queue, node)\n",
    "                        while queue:\n",
    "                            x_, y_ = heapq.heappop(queue)\n",
    "                            # Check if the node has children\n",
    "                            if (x_, y_) in children:\n",
    "                                for child in children[(x_, y_)]:\n",
    "                                        cost[child] -= cost_reduction\n",
    "                                        heapq.heappush(queue, child)\n",
    "\n",
    "                        # Update the node, parent_node, and grandparent_node    \n",
    "                        node = grandparent_node\n",
    "                        parent_node = parent[node]\n",
    "                        grandparent_node = parent[parent_node]\n",
    "\n",
    "                    else:\n",
    "                        node = parent_node\n",
    "                        parent_node = parent[node]\n",
    "                        grandparent_node = parent[parent_node]\n",
    "\n",
    "                # Use the new optimized path as beacons to sample around\n",
    "                beacons = path[1:-1]\n",
    "                \n",
    "            # if not reached:\n",
    "                # Check if the new node is close to the goal cost of the node is less than the best cost\n",
    "                # if np.sqrt((x_new-x_goal)**2 + (y_new-y_goal)**2) < distance_threshold:\n",
    "            goal_distance = np.sqrt((x_new-x_goal)**2 + (y_new-y_goal)**2)\n",
    "            if goal_distance < 50 and cost[(x_new, y_new)] + goal_distance < c_best:\n",
    "                    end = time.time()\n",
    "                    print(\"Goal reached: \", end-start, \"seconds\")\n",
    "\n",
    "                    # Update the best cost\n",
    "                    c_best = cost[(x_new, y_new)] + goal_distance\n",
    "\n",
    "                    beacons = []\n",
    "\n",
    "                    # Add the final node as a child of the current node to complete the path\n",
    "                    children.setdefault((x_new, y_new), []).append((x_goal, y_goal))\n",
    "\n",
    "                    # Add the final node to the parent dictionary\n",
    "                    parent[(x_goal, y_goal)] = (x_new, y_new)\n",
    "\n",
    "                    # Save the final node\n",
    "                    x_final, y_final = x_goal, y_goal\n",
    "\n",
    "                    # Add the cost of the final node\n",
    "                    cost[(x_goal, y_goal)] = c_best\n",
    "\n",
    "                    # Save the iteration at which the goal is reached\n",
    "                    n = iterations\n",
    "\n",
    "                    print(\"Current Cost: \", cost[(x_final, y_final)])\n",
    "\n",
    "                    # Set the reached flag to True\n",
    "                    reached = True\n",
    "        \n",
    "print(\"Final cost: \", cost[(x_final, y_final)]) "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Informed-RRT*"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 56,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Goal reached:  5.82063102722168 seconds\n",
      "Current Cost:  1300.5089860626827\n",
      "New best cost:  1300.1531786457194\n",
      "New best cost:  1296.6105478835902\n",
      "New best cost:  1296.3381751837403\n",
      "New best cost:  1225.625020783743\n",
      "New best cost:  1225.4587913713794\n",
      "New best cost:  1225.3063702349677\n",
      "New best cost:  1225.2690782896723\n",
      "New best cost:  1224.5571792773715\n",
      "New best cost:  1224.4651862478204\n",
      "New best cost:  1222.1836536920896\n",
      "New best cost:  1221.9629979536646\n",
      "Final cost:  1221.9629979536646\n"
     ]
    }
   ],
   "source": [
    "canvas = map.copy()\n",
    "x_start, y_start = start_node\n",
    "x_goal, y_goal = goal_node\n",
    "\n",
    "step_size = 50\n",
    "distance_threshold = 1\n",
    "search_radius = 70\n",
    "# Make a lambda function to adjust the value of x to the visited space\n",
    "adjust = lambda x, threshold: int(int(round(x*2)/2)/threshold)\n",
    "\n",
    "# Dictionary to store visited nodes\n",
    "visited = {(adjust(x_start, distance_threshold),\n",
    "            adjust(y_start, distance_threshold)): 1}\n",
    "\n",
    "# Dictionary to store the parent of each node\n",
    "parent = {(x_start, y_start): (x_start, y_start)}\n",
    "\n",
    "# Dictionary to store the children of each node\n",
    "children = {(x_start, y_start): []}\n",
    "\n",
    "# Dictionary to store the cost of each node\n",
    "cost = {(x_start, y_start): 0}\n",
    "\n",
    "# Function to check if the node is valid\n",
    "def valid_node(x, y):\n",
    "    # Adjust the value for visited nodes\n",
    "    x_vis = adjust(x, distance_threshold)\n",
    "    y_vis = adjust(y, distance_threshold)\n",
    "    # Adjust the values for canvas\n",
    "    x_cvs = int(round(x*2)/2)\n",
    "    y_cvs = int(round(y*2)/2)\n",
    "\n",
    "    # Check if the node is within the boundaries\n",
    "    if 0 <= x < width and 0 <= y < height:\n",
    "        # Check if the node is not in the obstacle and not visited\n",
    "        if canvas[y_cvs, x_cvs, 0] != 0 and (x_vis, y_vis) not in visited:\n",
    "            return True\n",
    "    return False\n",
    "\n",
    "# Function to check if the child node and parent node are not intersecting with the obstacle\n",
    "def valid_edge(x_parent, y_parent, x_child, y_child):\n",
    "    # Sample 3 points on the line joining the parent and child nodes, \n",
    "    # not including the parent and child nodes\n",
    "    x_intermediate = np.linspace(x_parent, x_child, 5)[1:-1]\n",
    "    y_intermediate = np.linspace(y_parent, y_child, 5)[1:-1]\n",
    "\n",
    "    # Adjust the values for canvas\n",
    "    x_intermediate = [int(round(x*2)/2) for x in x_intermediate]\n",
    "    y_intermediate = [int(round(y*2)/2) for y in y_intermediate]\n",
    "\n",
    "    # Check if any of the intermediate points are in the obstacle\n",
    "    for x, y in zip(x_intermediate, y_intermediate):\n",
    "        if canvas[y, x, 0] == 0:\n",
    "            return False\n",
    "    return True\n",
    "\n",
    "def sample_unit_ball():\n",
    "    a = np.random.random()\n",
    "    b = np.random.random()\n",
    "\n",
    "    if b < a:\n",
    "        a, b = b, a\n",
    "\n",
    "    sample = (b * np.cos(2 * np.pi * a / b),\n",
    "              b * np.sin(2 * np.pi * a / b))\n",
    "    \n",
    "    return np.array([[sample[0]], [sample[1]], [0]])\n",
    "\n",
    "# Function to sample a node for Informed-RRT*\n",
    "def sample(x_start, y_start, x_goal, y_goal, c_max):\n",
    "    if c_max < float('inf'):\n",
    "\n",
    "        c_min = math.hypot(x_start - x_goal,\n",
    "                           y_start - y_goal)\n",
    "        x_center = np.array([[(x_start + x_goal) / 2.0],\n",
    "                             [(y_start + y_goal) / 2.0], [0]])\n",
    "        \n",
    "        a1 = np.array([[(x_start - x_goal) / c_min],\n",
    "                       [(y_start - y_goal) / c_min], [0]])\n",
    "        \n",
    "        id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)\n",
    "\n",
    "        m = a1 @ id1_t\n",
    "\n",
    "        u, s, vh = np.linalg.svd(m, True, True)\n",
    "        c = u @ np.diag(\n",
    "            [1.0, 1.0,\n",
    "             np.linalg.det(u) * np.linalg.det(np.transpose(vh))]) @ vh\n",
    "\n",
    "        r = [c_max / 2.0, math.sqrt(c_max ** 2 - c_min ** 2) / 2.0,\n",
    "                math.sqrt(c_max ** 2 - c_min ** 2) / 2.0]\n",
    "        \n",
    "        rl = np.diag(r)\n",
    "\n",
    "        x_ball = sample_unit_ball()\n",
    "\n",
    "        rnd = np.dot(np.dot(c, rl), x_ball) + x_center\n",
    "\n",
    "        x_rand = int(rnd[(0, 0)])\n",
    "        y_rand = int(rnd[(1, 0)])\n",
    "\n",
    "    else:\n",
    "        x_rand = np.random.randint(clearance, width-clearance)\n",
    "        y_rand = np.random.randint(clearance, height-clearance)  \n",
    "\n",
    "    return x_rand, y_rand\n",
    "\n",
    "reached = False\n",
    "iterations = 0 # Number of iterations to improve the solution\n",
    "c_best = float('inf') # Best cost so far\n",
    "start = time.time()\n",
    "\n",
    "while iterations < 100*5:\n",
    "    \n",
    "    # Get a new node\n",
    "    # x_node = np.random.randint(clearance, width-clearance)\n",
    "    # y_node = np.random.randint(clearance, height-clearance)\n",
    "\n",
    "    x_node, y_node = sample(x_start, y_start, x_goal, y_goal, c_best)    \n",
    "\n",
    "    # If the node is valid\n",
    "    if valid_node(x_node, y_node):\n",
    "\n",
    "        # Find the nearsest node in the tree to get the projected node\n",
    "        min_dist = float('inf')\n",
    "\n",
    "        for x, y in parent:\n",
    "            dist = np.sqrt((x-x_node)**2 + (y-y_node)**2)\n",
    "            if dist < min_dist:\n",
    "                min_dist = dist\n",
    "                x_parent, y_parent = x, y\n",
    "\n",
    "        # Get the angle of the new node\n",
    "        theta = np.arctan2(y_node-y_parent, x_node-x_parent)\n",
    "\n",
    "        # Calculate the new node\n",
    "        x_new = int(x_parent + step_size*np.cos(theta))\n",
    "        y_new = int(y_parent + step_size*np.sin(theta))\n",
    "        \n",
    "        if reached:\n",
    "            # Update c_best\n",
    "            if cost[(x_final, y_final)] < c_best:\n",
    "                c_best = cost[(x_final, y_final)]\n",
    "                print(\"New best cost: \", c_best)\n",
    "            iterations += 1\n",
    "\n",
    "        # Check if the new node is valid\n",
    "        if valid_node(x_new, y_new):\n",
    "\n",
    "            # Collect all the nodes in the search radius\n",
    "            neighbours = []\n",
    "            for x, y in parent:\n",
    "                if np.sqrt((x-x_new)**2 + (y-y_new)**2) < search_radius:\n",
    "                    neighbours.append((x, y))\n",
    "\n",
    "            # Find the node with the minimum cost to get new node\n",
    "            min_cost = cost[(x_parent, y_parent)] + step_size\n",
    "            for x, y in neighbours:\n",
    "                new_cost = cost[(x, y)] + np.sqrt((x-x_new)**2 + (y-y_new)**2)\n",
    "                if new_cost < min_cost:\n",
    "                    min_cost = new_cost\n",
    "                    x_parent, y_parent = x, y\n",
    "\n",
    "            # Check if the edge between the parent and child nodes is valid\n",
    "            if not valid_edge(x_parent, y_parent, x_new, y_new):\n",
    "                continue\n",
    "\n",
    "            # Add the cost of the new node\n",
    "            cost[(x_new, y_new)] = min_cost\n",
    "\n",
    "            # Check if rewiring to the newly added node will reduce the cost of the neighbours\n",
    "            for x, y in neighbours:\n",
    "                new_cost = cost[(x_new, y_new)] + np.sqrt((x-x_new)**2 + (y-y_new)**2)\n",
    "                if new_cost < cost[(x, y)]:\n",
    "\n",
    "                    # Check if the edge between the parent and child nodes is valid\n",
    "                    if not valid_edge(x, y, x_new, y_new):\n",
    "                        continue\n",
    "\n",
    "                    previous_cost = cost[(x, y)]\n",
    "                    cost[(x, y)] = new_cost\n",
    "                    \n",
    "                    # Remove this node from the children of its current parent\n",
    "                    x_parent_neighbour, y_parent_neighbour = parent[(x, y)]\n",
    "                    children[(x_parent_neighbour, y_parent_neighbour)].remove((x, y))\n",
    "\n",
    "                    # Add the new node to the parent dictionary\n",
    "                    parent[(x, y)] = (x_new, y_new)\n",
    "\n",
    "                    # Add the new node to the children dictionary\n",
    "                    children.setdefault((x_new, y_new), []).append((x, y))\n",
    "                    \n",
    "                    # If the cost of this node is reduced, we must update the cost of its children,\n",
    "                    # and their children, and so on\n",
    "                    cost_reduction = previous_cost - new_cost\n",
    "                    queue = []\n",
    "                    heapq.heappush(queue, ((x, y)))\n",
    "                    while queue:\n",
    "                        x_, y_ = heapq.heappop(queue)\n",
    "                        # Check if the node has children\n",
    "                        if (x_, y_) in children:\n",
    "                            for child in children[(x_, y_)]:\n",
    "                                    cost[child] -= cost_reduction\n",
    "                                    heapq.heappush(queue, child)\n",
    "                                    \n",
    "            x_achieved, y_achieved = x_new, y_new\n",
    "\n",
    "            x_adjusted = adjust(x_new, distance_threshold)\n",
    "            y_adjusted = adjust(y_new, distance_threshold)\n",
    "            \n",
    "            # Add the new node to the visited nodes\n",
    "            visited[(x_adjusted, y_adjusted)] = 1\n",
    "\n",
    "            # Add the new node to the parent dictionary\n",
    "            parent[(x_new, y_new)] = (x_parent, y_parent)\n",
    "\n",
    "            # Add the new node to the children dictionary\n",
    "            children.setdefault((x_parent, y_parent), []).append((x_new, y_new))\n",
    "\n",
    "            \n",
    "            # Check if the new node is close to the goal cost of the node is less than the best cost\n",
    "            goal_distance = np.sqrt((x_new-x_goal)**2 + (y_new-y_goal)**2)\n",
    "            if goal_distance < 50 and cost[(x_new, y_new)] + goal_distance < c_best:\n",
    "            # if np.sqrt((x_new-x_goal)**2 + (y_new-y_goal)**2) < 50:\n",
    "                end = time.time()\n",
    "                print(\"Goal reached: \", end-start, \"seconds\")\n",
    "\n",
    "                # Update the best cost\n",
    "                c_best = cost[(x_new, y_new)] + goal_distance\n",
    "\n",
    "                # Add the final node as a child of the current node to complete the path\n",
    "                children.setdefault((x_new, y_new), []).append((x_goal, y_goal))\n",
    "\n",
    "                # Add the final node to the parent dictionary\n",
    "                parent[(x_goal, y_goal)] = (x_new, y_new)\n",
    "\n",
    "                # Save the final node\n",
    "                x_final, y_final = x_goal, y_goal\n",
    "\n",
    "                # Add the cost of the final node\n",
    "                cost[(x_goal, y_goal)] = c_best\n",
    "\n",
    "                print(\"Current Cost: \", cost[(x_goal, y_goal)])\n",
    "\n",
    "                # Set the reached flag to True\n",
    "                reached = True\n",
    "\n",
    "print(\"Final cost: \", cost[(x_final, y_final)]) "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Generate optimal path"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 57,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Total distance:  1221.9629979536649\n",
      "Difference in cost:  -0.0\n"
     ]
    }
   ],
   "source": [
    "# Get the path from the parent dictionary\n",
    "path = []\n",
    "x, y = x_final, y_final\n",
    "while (x, y) != (x_start, y_start):\n",
    "    # print(x, y)\n",
    "    path.append((x, y))\n",
    "    x, y = parent[(x, y)]\n",
    "path.append((x, y))\n",
    "path.reverse()\n",
    "\n",
    "# Calculate the distance between every point in the path\n",
    "sum = 0\n",
    "for i in range(len(path)-1):\n",
    "    # print(path[i])\n",
    "    sum += np.sqrt((path[i][0]-path[i+1][0])**2 + (path[i][1]-path[i+1][1])**2)\n",
    "\n",
    "print(\"Total distance: \", sum)\n",
    "print(\"Difference in cost: \", np.round(cost[(x_final, y_final)] - sum, 2))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Plot the graph"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 58,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Plot the start and goal nodes\n",
    "# canvas = cv2.circle(canvas, (x_start, y_start), 5, (0, 0, 254), 10) \n",
    "# canvas = cv2.circle(canvas, (x_goal, y_goal), 5, (0, 0, 254), 10)\n",
    "\n",
    "canvas = map.copy()\n",
    "\n",
    "# Plot a line between the parent and child nodes\n",
    "for x, y in parent:\n",
    "    x_parent, y_parent = parent[(x, y)]\n",
    "    canvas = cv2.line(canvas, (x, y), (x_parent, y_parent), (0, 255, 0), 1)\n",
    "    # Draw a purple circle at the parent node\n",
    "    canvas = cv2.circle(canvas, (x_parent, y_parent), 5, (255, 0, 255), 2)\n",
    "\n",
    "# Plot a circle at all the child points \n",
    "for x, y in children:\n",
    "    for child in children[(x, y)]:\n",
    "        canvas = cv2.circle(canvas, child, 5, (255, 0, 255), 2)\n",
    "\n",
    "# Plot the path\n",
    "for i in range(len(path)-1):\n",
    "    x1, y1 = path[i]\n",
    "    x2, y2 = path[i+1]\n",
    "    canvas = cv2.line(canvas, (x1, y1), (x2, y2), (0, 0, 255), 2)\n",
    "\n",
    "# Resize the canvas by a factor of scale\n",
    "canvas_resized = cv2.resize(canvas, (width_resized, height_resized))\n",
    "\n",
    "cv2.imshow(\"Canvas\", canvas_resized)\n",
    "cv2.waitKey(0)\n",
    "cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Optionally, save the map"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 59,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "True"
      ]
     },
     "execution_count": 59,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Save the canvas, with captions for the different algorithms\n",
    "# cv2.imwrite(\"RRT_star5.png\", canvas_resized)\n",
    "\n",
    "# cv2.imwrite(\"RRT_star-SMART5.png\", canvas_resized)\n",
    "\n",
    "cv2.imwrite(\"Informed-RRT_star5.png\", canvas_resized)"
   ]
  }
 ],
 "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.11.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}