RRTstar.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cv2\n",
    "import heapq\n",
    "import time\n",
    "from math import dist\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Step 1: Define robot parameters"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "WHEEL_RADIUS = 33#/1000 # 33mm\n",
    "ROBOT_RADIUS = 220#/1000 # 220mm\n",
    "WHEEL_DISTANCE = 287#/1000 # 287mm\n",
    "clearance = 5\n",
    "clearance += ROBOT_RADIUS"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Step 2: Mathematical representation of Free Space"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "width = 6000\n",
    "height = 2000\n",
    "scale = 5\n",
    "\n",
    "clearance_color = (0, 255, 255)\n",
    "obstacle_color = (0, 0, 0)\n",
    "robot_radius_color = (254, 105, 180)\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), (255, 255, 255), -1)\n",
    "\n",
    "# OBSTACLE 1\n",
    "x1, x2 = 1500, 1750\n",
    "y1, y2 = 0, 1000\n",
    "# Draw the clearance\n",
    "for i in range(x1-clearance, x2+clearance):\n",
    "    for j in range(y1+clearance, y2+clearance):\n",
    "        canvas[j, i] = clearance_color\n",
    "# Draw the obstacle\n",
    "for i in range(x1, x2):\n",
    "    for j in range(y1, y2):\n",
    "        canvas[j, i] = obstacle_color\n",
    "\n",
    "# OBSTACLE 2\n",
    "x1, x2 = 2500, 2750\n",
    "y1, y2 = height-1000, height\n",
    "# Draw the clearance\n",
    "for i in range(x1-clearance, x2+clearance):\n",
    "    for j in range(y1-clearance, y2-clearance+1):\n",
    "        canvas[j, i] = clearance_color\n",
    "# Draw the obstacle\n",
    "for i in range(x1, x2):\n",
    "    for j in range(y1, y2):\n",
    "        canvas[j, i] = obstacle_color\n",
    "\n",
    "# OBSTACLE 3\n",
    "# Draw a circle at (4200, 800)\n",
    "center = (4200, 800)\n",
    "radius = 600\n",
    "# Draw the clearance\n",
    "canvas = cv2.circle(canvas, center, 600+clearance, clearance_color, -1)\n",
    "# Draw the obstacle\n",
    "canvas = cv2.circle(canvas, center, 600, obstacle_color, -1)\n",
    "        \n",
    "# x_start, y_start = clearance+1, clearance+1\n",
    "# x_goal, y_goal = width-clearance-1, clearance+1\n",
    "\n",
    "# Draw a circle at x_start, y_start\n",
    "# canvas = cv2.circle(canvas, (x_start, y_start), 5, (0, 255, 0), -1)\n",
    "# Draw a circle at x_goal, y_goal\n",
    "# canvas = cv2.circle(canvas, (x_goal, y_goal), 5, (0, 0, 255), -1)\n",
    "\n",
    "\n",
    "# Draw a red line at x = width/2\n",
    "# canvas = cv2.line(canvas, (int(width/2.5), 0), (int(width/2.5), height), (0, 0, 255), 10)\n",
    "# Resize the canvas by a factor of scale\n",
    "width_resized = int(width/scale)\n",
    "height_resized = int(height/scale)\n",
    "canvas_resized = cv2.resize(canvas, (width_resized, height_resized))\n",
    "# cv2.imshow(\"Canvas\", canvas_resized)\n",
    "# cv2.waitKey(0)\n",
    "# cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "def obstacles():\n",
    "    width = 6000\n",
    "    height = 2000\n",
    "    scale = 5\n",
    "\n",
    "    clearance_color = (0, 255, 255)\n",
    "    obstacle_color = (0, 0, 0)\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), (255, 255, 255), -1)\n",
    "\n",
    "    # OBSTACLE 1\n",
    "    x1, x2 = 1500, 1750\n",
    "    y1, y2 = 0, 1000\n",
    "    # Draw the clearance\n",
    "    for i in range(x1-clearance, x2+clearance):\n",
    "        for j in range(y1+clearance, y2+clearance):\n",
    "            canvas[j, i] = clearance_color\n",
    "    # Draw the obstacle\n",
    "    for i in range(x1, x2):\n",
    "        for j in range(y1, y2):\n",
    "            canvas[j, i] = obstacle_color\n",
    "\n",
    "    # OBSTACLE 2\n",
    "    x1, x2 = 2500, 2750\n",
    "    y1, y2 = height-1000, height\n",
    "    # Draw the clearance\n",
    "    for i in range(x1-clearance, x2+clearance):\n",
    "        for j in range(y1-clearance, y2-clearance+1):\n",
    "            canvas[j, i] = clearance_color\n",
    "    # Draw the obstacle\n",
    "    for i in range(x1, x2):\n",
    "        for j in range(y1, y2):\n",
    "            canvas[j, i] = obstacle_color\n",
    "\n",
    "    # OBSTACLE 3\n",
    "    # Draw a circle at (4200, 800)\n",
    "    center = (4200, 800)\n",
    "    radius = 600\n",
    "    # Draw the clearance\n",
    "    canvas = cv2.circle(canvas, center, 600+clearance, clearance_color, -1)\n",
    "    # Draw the obstacle\n",
    "    canvas = cv2.circle(canvas, center, 600, obstacle_color, -1)\n",
    "            \n",
    "    # x_center, y_center = 4200, 800\n",
    "    # radius = 600\n",
    "    # Draw the clearance\n",
    "    # for i in range(x_center-radius-clearance, x_center+radius+clearance):\n",
    "    #     for j in range(y_center-radius-clearance, y_center+radius+clearance):\n",
    "    #         if (i-x_center)**2 + (j-y_center)**2 <= (radius+clearance)**2 and canvas[j, i, 0] != 0:\n",
    "    #             canvas[j, i] = clearance_color\n",
    "    # # Draw the obstacle\n",
    "    # for i in range(x_center-radius, x_center+radius):\n",
    "    #     for j in range(y_center-radius, y_center+radius):\n",
    "    #         if (i-x_center)**2 + (j-y_center)**2 <= radius**2:\n",
    "    #             canvas[j, i] = obstacle_color\n",
    "\n",
    "    # x_start, y_start = clearance+1, clearance+1\n",
    "    # x_goal, y_goal = width-clearance-1, clearance+1\n",
    "\n",
    "    # Draw a circle at x_start, y_start\n",
    "    # canvas = cv2.circle(canvas, (x_start, y_start), 5, (0, 255, 0), -1)\n",
    "    # Draw a circle at x_goal, y_goal\n",
    "    # canvas = cv2.circle(canvas, (x_goal, y_goal), 5, (0, 0, 255), -1)\n",
    "\n",
    "\n",
    "    # Draw a red line at x = width/2\n",
    "    # canvas = cv2.line(canvas, (int(width/2.5), 0), (int(width/2.5), height), (0, 0, 255), 10)\n",
    "    # Resize the canvas by a factor of scale\n",
    "    width_resized = int(width/scale)\n",
    "    height_resized = int(height/scale)\n",
    "    canvas_resized = cv2.resize(canvas, (width_resized, height_resized))\n",
    "    # cv2.imshow(\"Canvas\", canvas_resized)\n",
    "    # cv2.waitKey(0)\n",
    "    # cv2.destroyAllWindows()\n",
    "    return canvas"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Step 3: Implement RRT* search algorithm to search the map and find the optimal path"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### RRT* with no rewired cost updates"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Goal reached:  0.46178388595581055 seconds\n",
      "Cost:  7881.914407364653\n"
     ]
    }
   ],
   "source": [
    "canvas = obstacles()\n",
    "\n",
    "x_start, y_start, theta_start = clearance+1, clearance+1, 0\n",
    "x_goal, y_goal = width-clearance-1, clearance+1\n",
    "\n",
    "step_size = 100\n",
    "distance_threshold = 100\n",
    "search_radius = 300\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 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",
    "reached = False\n",
    "iterations = 0\n",
    "start = time.time()\n",
    "\n",
    "while iterations < 100:\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 parent\n",
    "            min_cost = cost[(x_parent, y_parent)] + step_size\n",
    "\n",
    "            for x, y in neighbours:\n",
    "                if cost[(x, y)] + np.sqrt((x-x_new)**2 + (y-y_new)**2) < min_cost:\n",
    "                    min_cost = cost[(x, y)] + np.sqrt((x-x_new)**2 + (y-y_new)**2)\n",
    "                    x_parent, y_parent = x, y\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",
    "                if cost[(x_new, y_new)] + np.sqrt((x-x_new)**2 + (y-y_new)**2) < cost[(x, y)]:\n",
    "                    cost[(x, y)] = cost[(x_new, y_new)] + np.sqrt((x-x_new)**2 + (y-y_new)**2)\n",
    "                    parent[(x, y)] = (x_new, y_new)\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",
    "            # Check if the new node is close to the goal\n",
    "            if np.sqrt((x_new-x_goal)**2 + (y_new-y_goal)**2) < distance_threshold:\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",
    "                print(\"Cost: \", cost[(x_new, y_new)])\n",
    "                reached = True\n",
    "                x_final, y_final = x_new, y_new"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### RRT* with rewired cost updates"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Goal reached:  0.9292290210723877 seconds\n",
      "Current Cost:  7333.374693991585\n",
      "Final cost:  7272.552535564675\n"
     ]
    }
   ],
   "source": [
    "canvas = obstacles()\n",
    "\n",
    "x_start, y_start, theta_start = clearance+1, clearance+1, 0\n",
    "x_goal, y_goal = width-clearance-1, clearance+1\n",
    "\n",
    "step_size = 100\n",
    "distance_threshold = 100\n",
    "search_radius = 300\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 < 100:\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",
    "            if not reached:\n",
    "                # Check if the new node is close to the goal\n",
    "                if np.sqrt((x_new-x_goal)**2 + (y_new-y_goal)**2) < distance_threshold:\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",
    "                    # 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": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Goal reached:  0.7465260028839111 seconds\n",
      "Current Cost:  7407.142204977129\n",
      "Final cost:  7401.269489811563\n"
     ]
    }
   ],
   "source": [
    "canvas = obstacles()\n",
    "\n",
    "x_start, y_start, theta_start = clearance+1, clearance+1, 0\n",
    "x_goal, y_goal = width-clearance-1, clearance+1\n",
    "\n",
    "step_size = 100\n",
    "distance_threshold = 100\n",
    "search_radius = 300\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 < 100:\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) < distance_threshold 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": [
    "## Step 4: Optimal Path"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Total distance:  7221.680586238647\n",
      "Difference in cost:  -0.57\n"
     ]
    }
   ],
   "source": [
    "# Get the path from the parent dictionary\n",
    "path = []\n",
    "# x, y = x_goal, y_goal   \n",
    "# x, y = x_achieved, y_achieved\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": [
    "## Step 5: Plot the graph"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "canvas = obstacles()\n",
    "\n",
    "# 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",
    "# 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), 5)\n",
    "    # Draw a purple circle at the parent node\n",
    "    canvas = cv2.circle(canvas, (x_parent, y_parent), 5, (255, 0, 255), 3)\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), 3)\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), 5)\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()"
   ]
  }
 ],
 "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
}