RRT-Non-Holonomic-Robot.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# NonHolonomic Robot Path planning with Rapidly-exploring Random Trees"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### RRT Algorithm"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* The RRT Algorithm is a sampling-based path planning algorithm which randomly samples points in the environment and adds them to a tree based data structure. \n",
    "\n",
    "* They search high dimensional spaces by incrementally building random tree from random samples. The process of building a RRT and finding a path from starting to goal configuration can be broken down into simple steps:\n",
    "\n",
    "    1. Sample a random point within the environment\n",
    "    2. Find the node in the tree which is nearest to the random point and check for collisions with obstacles\n",
    "    3. Extend the tree towards the random point\n",
    "    4. Repeat steps 1 to 3. Stop iteration on reaching goal or within a threshold of the goal\n",
    "    \n",
    "**LINKS FOR NON HOLONOMIC RRT EVOLUTION VIDEO**\n",
    "https://www.youtube.com/watch?v=e1S03ds0wJY&feature=youtu.be"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Theory and Working of Non Holonomic Robot "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Non Holonomic Robot**\n",
    "\n",
    "* For nonholonomic robot, the controls are dependent on one another. When the controllable degree of freedom is less than the total degrees of freedom, then it is known as non-Holonomic drive. eg. car. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Working of RRT for non holonomic case"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* We used a differential drive robot for implementing RRT algorithm for the holonomic case. The robot with a length of 20 units and has two wheels aligned at an angle of 180 degrees from each other. \n",
    "\n",
    "**Differential Drive**\n",
    "* These robots have two wheels and the controllable parameters are velocity $v$ and angular velocity $\\omega$\n",
    "* $v_{x}$ and $v_{y}$ are not decoupled. They are coupled through the robot's instantaneous direction $\\theta$\n",
    "\n",
    "* **Differential Drive Equations**\n",
    "$$v_{cx}(t) = v*cos(\\omega t)$$\n",
    "$$v_{cy}(t) = v*sin(\\omega t)$$\n",
    "$$v_{cy}(t) = v_{cx}(t)*tan(\\omega t)$$\n",
    "\n",
    "\n",
    "* We used pygame interface to for custom selection of start and goal points. \n",
    "* For obstacle detection, we checked the intensity of all points in the RRT path and did not add the node to the RRT tree if black in color(obstacle).\n",
    "* Additionally, we also make sure the node is within the boundary\n",
    "* To find the RRT path, we used backtracking of all the parent nodes in the tree structure. \n",
    "* We also find the wheel trajectories for the two wheels of the robot by following the RRT path.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div>\n",
    "<img src=\"./images/non_holo_iter1.png\" width=\"300\"/>\n",
    "</div>\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "pygame 2.0.0.dev4 (SDL 2.0.10, python 3.7.3)\n",
      "Hello from the pygame community. https://www.pygame.org/contribute.html\n"
     ]
    }
   ],
   "source": [
    "# necessary imports\n",
    "\n",
    "import pygame\n",
    "from random import randint as ri\n",
    "pygame.init()\n",
    "import time\n",
    "import numpy as np\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Initialization of parameters of the environment\n",
    "\n",
    "screen = pygame.display.set_mode([500, 550])\n",
    "SIDE_x = 20\n",
    "SIDE_y = 40\n",
    "WINDOW_width = 440\n",
    "WINDOW_height = 400\n",
    "GAME_border = 3\n",
    "WHITE=(255,255,255)\n",
    "BLUE=(0,0,255)\n",
    "BLACK=(0,0,0)\n",
    "RED=(255,0,0)\n",
    "GREEN=(0,255,0)\n",
    "RAND=(120,120,120)\n",
    "C1=(39,38,53)\n",
    "C2=(19,51,62)\n",
    "C3=(31,3,24)\n",
    "theta_init=0\n",
    "theta_goal=np.pi/4\n",
    "step_size=30"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Initialization of non-holonomic constraints\n",
    "\n",
    "max_steering_angle=0.7\n",
    "max_linear_vel=40\n",
    "new_node_array_iter = 1 \n",
    "distance_new_node = 0 \n",
    "dt=0.1\n",
    "L=20\n",
    "L1=int(L/2)\n",
    "final_point=step_size-1\n",
    "dist_threshold=1\n",
    "theta_threshold=np.pi/9\n",
    "\n",
    "screen.fill(WHITE)\n",
    "INT_MAX = 100000000000000\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Construction of grid with obstacles"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Using pygame, we addded random obstacles at specific locations , one rectangle and two circles of different radii were added."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# The environment class contaning details about the the buttons and other attributes of the pygame window\n",
    "class Environment:\n",
    "    def __init__ (self, colour, x, y, width, height):\n",
    "        self.colour = colour\n",
    "        self.x = x\n",
    "        self.y = y\n",
    "        self.width = width\n",
    "        self.height = height\n",
    "    def create(self,screen):\n",
    "        pygame.draw.rect(screen, self.colour, [self.x, self.y,self.width ,self.height])\n",
    "\n",
    "\n",
    "\n",
    "    def point_inside_game(self,x,y):\n",
    "        if x>SIDE_x+GAME_border and x<SIDE_x + WINDOW_width - GAME_border:\n",
    "                    if y>SIDE_y+GAME_border and y < SIDE_y + WINDOW_height - GAME_border:\n",
    "                        return(True)\n",
    "        return(False)\n",
    "\n",
    "\n",
    "    # Random Point Generator inside Game\n",
    "    def random_point(self):\n",
    "        x_random = ri(SIDE_x+GAME_border , SIDE_x + WINDOW_width - GAME_border - 1)\n",
    "        y_random = ri(SIDE_y+GAME_border , SIDE_y + WINDOW_height - GAME_border - 1 )\n",
    "        theta_random= (2*np.pi - 0.0001)*ri(0,1)\n",
    "        return((x_random, y_random,theta_random))\n",
    "\n",
    "\n",
    "    # Point inside given Rectangle ?\n",
    "    def point_inside_rec(self,xr,yr,wr,hr,x,y):\n",
    "        if x> xr and x < xr + wr:\n",
    "                    if y > yr and y < yr + hr:\n",
    "                        return(True)\n",
    "        return(False)\n",
    "\n",
    "\n",
    "    #Point to Point Distance\n",
    "    def p2p_dist(self,p1,p2):\n",
    "        x1,y1,theta1=p1\n",
    "        x2,y2,theta2=p2\n",
    "        return ((x1-x2)**2 + (y1-y2)**2)**0.5\n",
    "        # return ( ( (x1-x2)**2 + (y1-y2)**2 + ((180/np.pi)**2)*min( [ (theta1 - theta2)**2, (theta1- theta2 - np.pi)**2, (theta1 - theta2 + np.pi)**2 ] ))**0.5 )\n",
    "        # return ( ( (x1-x2)**2 + (y1-y2)**2 + ((180/np.pi)**2)* (theta1 - theta2)**2  )**0.5 )\n",
    "\n",
    "\n",
    "    #Text on Environment\n",
    "    def ClickText(self):\n",
    "        font = pygame.font.Font('freesansbold.ttf', 12)\n",
    "        text = font.render('CLICK HERE', True, WHITE)\n",
    "        textRect = text.get_rect()\n",
    "        textRect.center = (75, 495)\n",
    "        screen.blit(text, textRect)\n",
    "\n",
    "\n",
    "    #Description Text\n",
    "    def DesText(self,s,x=315,y=485):\n",
    "        pygame.draw.rect(screen,WHITE,(125,470,500,30))\n",
    "        font = pygame.font.SysFont('segoeuisemilight', 15)\n",
    "        text = font.render('%s'%(s), True, BLACK)\n",
    "        textRect = text.get_rect()\n",
    "        #textRect.center = (255, 460)\n",
    "        textRect.center = (x, y)\n",
    "        screen.blit(text, textRect)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Configuration Space generation for obstacles"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Declration of configuration space with circular and rectangular obstacles\n",
    "\n",
    "def ConfSpace():\n",
    "    #CS for circle\n",
    "    pygame.draw.circle(screen,BLACK,(100,150),20+L1)\n",
    "    pygame.draw.circle(screen,BLACK,(360,300),30+L1)\n",
    "\n",
    "    #CS for rectangle\n",
    "    pygame.draw.rect(screen,BLACK,(200-L1,150-L1,100+L1,50+L1))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## RRT for Non-Holonomic Robot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "#RRT function which returns the nearest node in the graph for the random node\n",
    "\n",
    "def RRT(x,y,theta,parent):\n",
    "    x_m,y_m,theta_m= Start\n",
    "    if (x,y,theta) not in parent and screen.get_at((x,y)) != (0,0,0,255):\n",
    "        cur_min=INT_MAX\n",
    "        for v in parent:\n",
    "            if B1.p2p_dist(v,(x,y,theta))<cur_min:\n",
    "                x_m,y_m,theta_m=v\n",
    "                cur_min =  B1.p2p_dist(v,(x,y,theta))\n",
    "    \n",
    "    return(x_m,y_m,theta_m)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<rect(330, 270, 61, 61)>"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "\n",
    "running = True\n",
    "#Environment for Game\n",
    "pygame.draw.rect(screen,BLACK,(SIDE_x,SIDE_y,WINDOW_width,WINDOW_height),GAME_border)\n",
    "B1 = Environment(BLACK, 25, 470, 100, 50)\n",
    "B1.create(screen)\n",
    "\n",
    "# Random Obstacles\n",
    "pygame.draw.rect(screen,BLACK,(200,150,100,50))\n",
    "pygame.draw.circle(screen,BLACK,(100,150),20)\n",
    "pygame.draw.circle(screen,BLACK,(360,300),30)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "\n",
    "OBS=dict()\n",
    "\n",
    "#Number of forward Steps towards random sampled point\n",
    "Step = 30\n",
    "#Start stores a single point [Starting point- RED Point]\n",
    "Start=[]\n",
    "\n",
    "#End stores a set of destination point [Destination point- Green Point]\n",
    "#Multiple points allowed to make the point appear bigger, and fast discovery,\n",
    "#due to huge number of pixels in this game\n",
    "End=set()\n",
    "\n",
    "\n",
    "#parent stores the graph\n",
    "parent=dict()\n",
    "steer=dict()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "## Pygame Interface with user control\n",
    "level=0\n",
    "B1.ClickText()\n",
    "B1.DesText(\"Instruction :\",y=460)\n",
    "B1.DesText(\"Click the BLACK button below to view Configuration Space for Obstacles\")\n",
    "while running:\n",
    "    for event in pygame.event.get():\n",
    "        if event.type == pygame.QUIT:\n",
    "            running = False\n",
    "            break\n",
    "        if running==False:\n",
    "            break\n",
    "        m = pygame.mouse.get_pressed()\n",
    "        x,y = pygame.mouse.get_pos()\n",
    "\n",
    "        if m[0]==1:\n",
    "            if B1.point_inside_rec(B1.x,B1.y, B1.width, B1.height,x,y):\n",
    "                    if level==0:\n",
    "                        level+=1\n",
    "                        B1.colour=RED\n",
    "                        ConfSpace()\n",
    "                        B1.DesText(\"Click the RED button and select the STARTING POINT\")\n",
    "                    elif level==1 and Start==[]:\n",
    "                        level+=1\n",
    "                        B1.colour=GREEN\n",
    "                        B1.DesText(\"Click the GREEN button and select the DESTINATION POINT\")\n",
    "                    elif level==2 and Start:\n",
    "                        level+=1\n",
    "                        B1.colour=BLUE\n",
    "                        B1.DesText(\"Click the BLUE button to view the RRT path and wheel trajectories\")\n",
    "                    elif level==3 and End!=set():\n",
    "                        level+=1\n",
    "                        B1.colour=BLUE\n",
    "                        B1.DesText(\"Path is being explored using RRT Algorithm with wheel trajectories\")\n",
    "\n",
    "                    B1.create(screen)\n",
    "                    B1.ClickText()\n",
    "                    continue\n",
    "            elif level==1:\n",
    "                    OBS[(x,y)]=1\n",
    "\n",
    "            elif level == 2 and Start==[]:\n",
    "                if B1.point_inside_game(x,y):\n",
    "                    Start=(x,y,theta_init)\n",
    "                    pygame.draw.circle(screen, RED, (x, y), 10)\n",
    "            elif level == 3 :\n",
    "                if B1.point_inside_game(x,y):\n",
    "                    End=(x,y,theta_goal)\n",
    "                    pygame.draw.circle(screen, GREEN, (x, y), 10)\n",
    "        if level>=4:\n",
    "            running = False\n",
    "            break\n",
    "    pygame.display.update()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Start Point: (138, 324, 0)\n",
      "End point: (285, 227, 0.7853981633974483)\n"
     ]
    }
   ],
   "source": [
    "running = True\n",
    "parent[Start]=(-1,-1,0)\n",
    "steer[((-1,-1,0),Start)]=0\n",
    "\n",
    "Trace=[]\n",
    "Timer =  time.time()\n",
    "print(\"Start Point:\",Start)\n",
    "print(\"End point:\",End)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "def collision_check_and_inside_game(path):\n",
    "\n",
    "#No collision\n",
    "    collision=0\n",
    "    inside_game=1\n",
    "    for i in range(1,step_size):\n",
    "        initial=path[i-1]\n",
    "        final=path[i]\n",
    "        step=(final-initial)/20\n",
    "        for j in range(20):\n",
    "            new=initial+j*step\n",
    "            if( B1.point_inside_game(int(new[0]),int(new[1]))== False):\n",
    "                inside_game=0\n",
    "                break\n",
    "            else:\n",
    "                if screen.get_at((int(new[0]),int(new[1]))) == (0,0,0,255):\n",
    "                    collision =1\n",
    "                    break\n",
    "        if(inside_game == 0 or collision == 1):\n",
    "            break\n",
    "            return 0\n",
    "\n",
    "    if(inside_game ==1 and collision ==0):\n",
    "        return 1\n",
    "    else:\n",
    "        return 0"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Check for target location\n",
    "\n",
    "* While the RRT is exploring, a target_check() function is called to check whether the goal is within a particular threshold \n",
    "* It return true when the goal is reached else continues the RRT.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "def target_check(path):\n",
    "\n",
    "#No collision\n",
    "    reached=0\n",
    "    for i in range(1,step_size):\n",
    "        initial=path[i-1]\n",
    "        final=path[i]\n",
    "        step=(final-initial)/20\n",
    "        for j in range(20):\n",
    "            new=initial+j*step\n",
    "            distance=((new[0]-End[0])**2 + (new[1]-End[1])**2)**0.5\n",
    "            if(distance<10):\n",
    "                print(\"Reached the goal under the given threshold\")\n",
    "                reached =1\n",
    "                break\n",
    "            if screen.get_at((int(new[0]),int(new[1]))) == (0,255,0,255):\n",
    "                reached =1\n",
    "                break\n",
    "            \n",
    "\n",
    "    return reached\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Random exploration of RRT in action"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Reached the goal under the given threshold\n",
      "Reached the goal under the given threshold\n",
      "Reached the goal under the given threshold\n",
      "Reached the goal under the given threshold\n",
      "Reached the goal under the given threshold\n",
      "Reached the goal under the given threshold\n",
      "Target reached\n"
     ]
    }
   ],
   "source": [
    "index=0\n",
    "while(running):\n",
    "    index=index+1\n",
    "    for event in pygame.event.get():\n",
    "        if event.type == pygame.QUIT:\n",
    "            running = False\n",
    "            break\n",
    "\n",
    "    x,y,theta =B1.random_point()\n",
    "   \n",
    "    min_dist=INT_MAX\n",
    "    steering_angle = -1*max_steering_angle\n",
    "    linear_vel=max_linear_vel\n",
    "    \n",
    "    while(steering_angle <= max_steering_angle):\n",
    "        path=np.zeros((step_size,3))\n",
    "        x_near,y_near,theta_near=RRT(x,y,theta,parent)\n",
    "        path[0] = np.asarray([x_near, y_near, theta_near])\n",
    "        for i in range(1,step_size):\n",
    "            path[i][0]=path[i-1][0]+linear_vel*np.cos(path[i-1][2])*dt\n",
    "            path[i][1]=path[i-1][1]+linear_vel*np.sin(path[i-1][2])*dt\n",
    "            path[i][2]=(path[i-1][2]+(linear_vel/L)*np.tan(steering_angle)*dt)\n",
    "            \n",
    "\n",
    "        new_dist = B1.p2p_dist((x,y,theta),(path[final_point][0],path[final_point][1],path[final_point][2]))\n",
    "\n",
    "        if( collision_check_and_inside_game(path)):\n",
    "            for i in range(1,step_size):\n",
    "                # pygame.draw.line(screen, BLUE, (path[i-1][0],path[i-1][1]), (path[i][0],path[i][1]), 2)\n",
    "                pygame.display.update() \n",
    "\n",
    "            if new_dist < min_dist:\n",
    "                path_target=path\n",
    "                steer_t_new = steering_angle\n",
    "                min_dist =  new_dist\n",
    "\n",
    "        steering_angle = steering_angle + 0.05\n",
    "\n",
    "     \n",
    "\n",
    "    xn_int=int(path_target[final_point][0])\n",
    "    yn_int=int(path_target[final_point][1])\n",
    "    t_new=path_target[final_point][2]\n",
    "\n",
    "    if t_new<0:\n",
    "        t_new=2*np.pi-abs(t_new)\n",
    "    if t_new>2*np.pi:\n",
    "        while t_new>2*np.pi:\n",
    "            t_new=t_new-2*np.pi\n",
    "\n",
    "    if(collision_check_and_inside_game(path_target)):\n",
    "\n",
    "        for i in range(1,step_size):\n",
    "            pygame.draw.line(screen, BLUE, (path_target[i-1][0],path_target[i-1][1]), (path_target[i][0],path_target[i][1]), 2)\n",
    "            pygame.display.update()\n",
    "        if screen.get_at((xn_int,yn_int)) != (0,0,0,255) and (xn_int,yn_int,t_new) not in parent:\n",
    "            parent[(xn_int,yn_int,t_new)]=(path_target[0][0],path_target[0][1],path_target[0][2])\n",
    "            steer[((path_target[0][0],path_target[0][1],path_target[0][2]),(xn_int,yn_int,t_new))]=steer_t_new\n",
    "        if index==500:\n",
    "            running = False\n",
    "            break\n",
    "        if(target_check(path_target)):\n",
    "            Trace=(xn_int,yn_int,t_new)\n",
    "            print(\"Target reached\")\n",
    "            running=False\n",
    "            \n",
    "pygame.display.update()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Path via Backtracking with Wheel Trajectories "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Once the goal(green dot) is reached, we backtrack nodes from the goal to the starting position to obtain the final trajectory of the center of the robot. \n",
    "* Using this, we find the trajectories of the respective wheels as written below."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div>\n",
    "<img src=\"./images/non_holo_wheel.png\" width=\"300\"/>\n",
    "</div>\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Wheel trajectories for the two wheels "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* The wheel trajectories of the robot are obtained by geometric transformations from the center point of the robot along the RRT path to the two wheel positions with angles of 90 + $\\theta$ , $\\theta$ - 90\n",
    "\n",
    "* The equations are :\n",
    "$$ x_{wheel} = x_{center} + radius * \\cos{\\theta} $$\n",
    "$$ y_{wheel} = y_{center} + radius * \\sin{\\theta} $$\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "running = True\n",
    "#This loop gets the route back to Start point\n",
    "points=[]\n",
    "while(Trace and running):\n",
    "    for event in pygame.event.get():\n",
    "        if event.type == pygame.QUIT:\n",
    "            running = False\n",
    "            break\n",
    "    while(Trace!=Start):\n",
    "\n",
    "        points.append(Trace)\n",
    "        x,y,t = parent[Trace]\n",
    "        xc,yc,tc=tuple(Trace)\n",
    "        steer_angle=steer[((x,y,t),(xc,yc,tc))]\n",
    "        path_temp=np.zeros((step_size,3))\n",
    "        path_temp[0]=list(parent[Trace])\n",
    "\n",
    "        for i in range(1,step_size):\n",
    "\n",
    "            path_temp[i][0]=path_temp[i-1][0]+linear_vel*np.cos(path_temp[i-1][2])*dt\n",
    "            path_temp[i][1]=path_temp[i-1][1]+linear_vel*np.sin(path_temp[i-1][2])*dt\n",
    "            path_temp[i][2]=(path_temp[i-1][2]+(linear_vel/L)*np.tan(steer_angle)*dt)\n",
    "\n",
    "            x1=path_temp[i-1][0]\n",
    "            y1=path_temp[i-1][1]\n",
    "            x2=path_temp[i][0]\n",
    "            y2=path_temp[i][1]\n",
    "            theta_temp=np.arctan((y2-y1)/(x2-x1))\n",
    "\n",
    "\n",
    "\n",
    "        for i in range(1,step_size):\n",
    "            pygame.draw.line(screen, GREEN, (path_temp[i-1][0],path_temp[i-1][1]), (path_temp[i][0],path_temp[i][1]), 2)\n",
    "            pygame.display.update()\n",
    "\n",
    "        #############  Wheel trajectories  ################################\n",
    "        for i in range(1,step_size):\n",
    "            x1=path_temp[i-1][0]\n",
    "            y1=path_temp[i-1][1]\n",
    "            x2=path_temp[i][0]\n",
    "            y2=path_temp[i][1]\n",
    "            theta_temp=np.arctan((y2-y1)/(x2-x1))\n",
    "            xl1= x1 + L1*np.cos(theta_temp+np.pi/2)\n",
    "            yl1= y1 + L1*np.sin(theta_temp+np.pi/2)\n",
    "            xr1= x1 + L1*np.cos(theta_temp-np.pi/2)\n",
    "            yr1= y1 + L1*np.sin(theta_temp-np.pi/2)\n",
    "\n",
    "            xl2= x2 + L1*np.cos(theta_temp+np.pi/2)\n",
    "            yl2= y2 + L1*np.sin(theta_temp+np.pi/2)\n",
    "            xr2= x2 + L1*np.cos(theta_temp-np.pi/2)\n",
    "            yr2= y2 + L1*np.sin(theta_temp-np.pi/2)\n",
    "\n",
    "            pygame.draw.line(screen, C1, (xl1,yl1), (xl2,yl2), 2)\n",
    "            pygame.draw.line(screen, C2, (xr1,yr1), (xr2,yr2), 2)\n",
    "            # time.sleep(2)\n",
    "\n",
    "        Trace=(x,y,t)\n",
    "\n",
    "\n",
    "    B1.DesText(\"Green Colored Path is the Required Path\")\n",
    "    pygame.display.update()\n",
    "\n",
    "\n",
    "#Quit the Game\n",
    "pygame.quit()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Additional Features - Hand drawn obstacles using Pygame !!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* We used pygame to draw our own obstacles and then used it for non holonomic RRT exploration.\n",
    "* Run the file **<RRT_nonholonomic_drawn.py>** to view the results with pygame\n",
    "\n",
    "**LINK TO VIDEO**\n",
    "https://youtu.be/SwrZ-Usj5HA"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div>\n",
    "<img src=\"./images/non_holo_drawn.png\" width=\"300\"/>\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Contributions"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The assignment was done with equal contribution from both the members Sivani P. and Gowri L. <br>\n",
    "Gowri L. focussed on the holonomic part whereas Sivani focussed on the non-holonomic part. <br>\n",
    "The wheel trajectories were coded and debugged together. <br>"
   ]
  },
  {
   "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.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}