Informed-RRTstar.ipynb

{
 "cells": [
  {
   "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": [
    "## Step 1: Define the action set"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "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": 3,
   "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": 4,
   "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": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "def no_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",
    "    return canvas"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Step 3: Implement Informed-RRT* search algorithm to search the map and find the optimal path"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Goal reached:  5.439537048339844 seconds\n",
      "Current Cost:  7293.914953791265\n",
      "New best cost:  7293.914953791265\n",
      "New best cost:  7286.667352303965\n",
      "New best cost:  7286.653280460032\n",
      "New best cost:  7281.446483955566\n",
      "New best cost:  7249.971272275514\n",
      "New best cost:  7249.829530437143\n",
      "New best cost:  7248.6655804305365\n",
      "New best cost:  7247.0233903605495\n",
      "Final cost:  7247.0233903605495\n"
     ]
    }
   ],
   "source": [
    "canvas = obstacles()\n",
    "x_start, y_start, theta_start = clearance+1, clearance+1, 0\n",
    "x_goal, y_goal = width-clearance-1, clearance+1\n",
    "\n",
    "# canvas = no_obstacles()\n",
    "# # Start at the center of the canvas\n",
    "# x_start, y_start = width//2 -1500, height//2 \n",
    "# x_goal, y_goal = width//2 +1500, height//2 \n",
    "\n",
    "step_size = 100\n",
    "distance_threshold = 1\n",
    "# search_radius = step_size + distance_threshold\n",
    "search_radius = 200\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 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",
    "            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) < 100:\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": 24,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Goal reached:  2.132990837097168 seconds\n",
      "Current Cost:  7332.950125779261\n",
      "New best cost:  7328.3193235627405\n",
      "New best cost:  7327.244555584824\n",
      "New best cost:  7323.474767392411\n",
      "New best cost:  7320.946382886745\n",
      "New best cost:  7305.531117503414\n",
      "New best cost:  7302.833881310612\n",
      "New best cost:  7301.991288886892\n",
      "New best cost:  7298.221484951295\n",
      "New best cost:  7297.666901617145\n",
      "New best cost:  7287.370856258634\n",
      "New best cost:  7287.148154072103\n",
      "New best cost:  7281.89301609659\n",
      "New best cost:  7275.682860053709\n",
      "New best cost:  7271.343906660287\n",
      "Final cost:  7271.343906660287\n"
     ]
    }
   ],
   "source": [
    "canvas = obstacles()\n",
    "x_start, y_start, theta_start = clearance+1, clearance+1, 0\n",
    "x_goal, y_goal = width-clearance-1, clearance+1\n",
    "\n",
    "# canvas = no_obstacles()\n",
    "# # Start at the center of the canvas\n",
    "# x_start, y_start = width//2 -1500, height//2 \n",
    "# x_goal, y_goal = width//2 +1500, height//2 \n",
    "\n",
    "step_size = 100\n",
    "distance_threshold = 1\n",
    "# search_radius = step_size + distance_threshold\n",
    "search_radius = 200\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 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 < 100 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": [
    "## Step 4: Optimal Path"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Total distance:  7271.343906660287\n",
      "Difference in cost:  0.0\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": 26,
   "metadata": {},
   "outputs": [],
   "source": [
    "canvas = obstacles()\n",
    "# canvas = no_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",
    "# Mark the final node in red\n",
    "canvas = cv2.circle(canvas, (x_goal, y_goal), 5, (0, 0, 255), 10)\n",
    "# Mark the start node in green\n",
    "canvas = cv2.circle(canvas, (x_start, y_start), 5, (254, 0, 0), 10)\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": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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
}