tree implementation and node id stuff

PathPlanning/BatchInformedRRTStar/Untitled.ipynb

@@ -0,0 +1,132 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 51,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import numpy as np\n",
+    "def ConfigurationToGridCoord(config):\n",
+    "        \n",
+    "        # TODO:\n",
+    "        # This function maps a configuration in the full configuration space\n",
+    "        # to a grid coordinate in discrete space\n",
+    "        #\n",
+    "        coord = [0] * 2\n",
+    "        lower_lims = [0, 0]\n",
+    "        for i in range(0, len(coord)):\n",
+    "            start = lower_lims[i] # start of the configuration space\n",
+    "            coord[i] = np.around((config[i] - start)/ 1)\n",
+    "        return coord"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 52,
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "[2.0, 1.0]"
+      ]
+     },
+     "execution_count": 52,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "coord = ConfigurationToGridCoord([2,1])\n",
+    "coord"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 158,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "def GridCoordToNodeId(coord):\n",
+    "    upper_limits = [4, 3]\n",
+    "    lower_limits = [0, 0]\n",
+    "    num_cells = []\n",
+    "    for idx in range(2):\n",
+    "        num_cells.append(np.ceil((upper_limits[idx] - lower_limits[idx])/1))\n",
+    "        node_id = 0\n",
+    "    print(\"num_cells \", num_cells)\n",
+    "    for i in range(len(coord) - 1, -1, -1):\n",
+    "        # Get product of the grid space maximums\n",
+    "        prod = 1\n",
+    "        for j in range(0, i):\n",
+    "            print(\"j: \", j)\n",
+    "            prod  = prod * num_cells[j]\n",
+    "            print(\"prod: \", prod)\n",
+    "            # Add the product to the sum\n",
+    "        node_id = node_id + coord[i] * prod\n",
+    "        print(\"n: \", node_id)\n",
+    "    return int(node_id)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 159,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "num_cells  [4.0, 3.0]\n",
+      "j:  0\n",
+      "prod:  4.0\n",
+      "n:  8.0\n",
+      "n:  10.0\n"
+     ]
+    },
+    {
+     "data": {
+      "text/plain": [
+       "10"
+      ]
+     },
+     "execution_count": 159,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "GridCoordToNodeId([2, 2])"
+   ]
+  },
+  {
+   "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.6.3"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}

PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py

@@ -15,6 +15,43 @@ import matplotlib.pyplot as plt
 
 show_animation = True 
 
+class Tree(object):
+
+	def __init__(self, start, lowerLimit, upperLimit, resolution):
+		self.vertices = dict()
+		vertex_id = self.gridCoordinateToNodeId(start)
+		self.vertices[vid] = []
+		self.edges = []
+		self.start = start
+		self.lowerLimit = lowerLimit
+		self.upperLimit = upperLimit
+		for idx in range(len(lowerLimit)):
+			self.num_cells[idx] = np.ceil((upperLimit[idx] - lowerLimit[idx])/resolution)
+
+	def getRootId(self):
+		return 0
+
+	def addVertex(self, vertex):
+		vertex_id = self.gridCoordinateToNodeId(vertex)
+		self.vertices[vertex_id] = []
+		return vertex_id
+
+	def addEdge(self, v, x):
+		if (v, x) not in self.edges:
+			self.edges.append((v,x))
+		self.vertices[v].append(x)
+		self.vertices[x].append(v)
+
+	def gridCoordinateToNodeId(self, coord):
+		nodeId = 0
+		for i in range(len(coord) - 1, -1, -1):
+			product = 1
+			for j in range(0 , i):
+				product *= product * self.num_cells[j]
+			node_id = node_id + coord[i] * product
+		return node_id
+
+
 class Node():
 
     def __init__(self, x, y):
@@ -72,11 +109,16 @@ class BITStar():
 		C = np.dot(np.dot(U, np.diag(
 			[1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh)
 
+		foundGoal = False
+		# run until done
 		while (iterations < self.maxIter):
 			if len(self.vertex_queue) == 0 and len(self.edge_queue) == 0:
 				samples = self.informedSample(100, cBest, cMin, xCenter, C)
 				# prune the tree
 
+
+
+
 			if animation:
 				self.drawGraph(xCenter=xCenter, cBest=cBest,
 							   cMin=cMin, etheta=etheta, samples=samples)