def Decompose(self, node): cell = 'free' r = node[0] rx = r.x ry = r.y rwidth = r.width rheight = r.height area = rwidth * rheight for o in self.domain.obstacles: if (o.CalculateOverlap(r) >= rwidth * rheight): cell = 'obstacle' break elif (o.CalculateOverlap(r) > 0.0): cell = 'mixed' break if (cell == 'mixed'): entropy = self.CalcEntropy(r) igH = 0.0 hSplitTop = None hSplitBottom = None vSplitLeft = None vSplitRight = None if (r.height / 2.0 > self.minimumSize): hSplitTop = Rectangle(rx, ry + rheight / 2.0, rwidth, rheight / 2.0) entHSplitTop = self.CalcEntropy(hSplitTop) hSplitBottom = Rectangle(rx, ry, rwidth, rheight / 2.0) entHSplitBottom = self.CalcEntropy(hSplitBottom) igH = entropy - ( r.width * r.height / 2.0 ) / area * entHSplitTop \ - ( r.width * r.height / 2.0 ) / area * entHSplitBottom igV = 0.0 if (r.width / 2.0 > self.minimumSize): vSplitLeft = Rectangle(rx, ry, rwidth / 2.0, rheight) entVSplitLeft = self.CalcEntropy(vSplitLeft) vSplitRight = Rectangle(rx + rwidth / 2.0, ry, rwidth / 2.0, rheight) entVSplitRight = self.CalcEntropy(vSplitRight) igV = entropy - ( r.width/2.0 * r.height ) / area * entVSplitLeft \ - ( r.width/2.0 * r.height ) / area * entVSplitRight children = [] if (igH > igV): if (igH > 0.0): if (hSplitTop is not None) and (hSplitBottom is not None): childTop = [hSplitTop, 'unknown', []] childBottom = [hSplitBottom, 'unknown', []] children = [childTop, childBottom] else: if (igV > 0.0): if (vSplitLeft is not None) and (vSplitRight is not None): childLeft = [vSplitLeft, 'unknown', []] childRight = [vSplitRight, 'unknown', []] children = [childLeft, childRight] for c in children: self.Decompose(c) node[2] = children node[1] = cell return node
def Decompose(self, node): cell = 'free' r = node[0] rx = r.x ry = r.y rwidth = r.width rheight = r.height for o in self.domain.obstacles: if (o.CalculateOverlap(r) >= rwidth * rheight): cell = 'obstacle' break elif (o.CalculateOverlap(r) > 0.0): cell = 'mixed' break if (cell == 'mixed'): if (rwidth / 2.0 > self.minimumSize) and (rheight / 2.0 > self.minimumSize): childt1 = [ Rectangle(rx, ry, rwidth / 2.0, rheight / 2.0), 'unknown', [], [], float('inf'), False ] qchild1 = self.Decompose(childt1) childt2 = [ Rectangle(rx + rwidth / 2.0, ry, rwidth / 2.0, rheight / 2.0), 'unknown', [], [], float('inf'), False ] qchild2 = self.Decompose(childt2) childt3 = [ Rectangle(rx, ry + rheight / 2.0, rwidth / 2.0, rheight / 2.0), 'unknown', [], [], float('inf'), False ] qchild3 = self.Decompose(childt3) childt4 = [ Rectangle(rx + rwidth / 2.0, ry + rheight / 2.0, rwidth / 2.0, rheight / 2.0), 'unknown', [], [], float('inf'), False ] qchild4 = self.Decompose(childt4) children = [qchild1, qchild2, qchild3, qchild4] node[2] = children else: cell = 'obstacle' elif cell == 'free': graph.append(node) node[1] = cell return node
def Decompose(self, node): cell = 'free' r = node[0] rx = r.x ry = r.y rwidth = r.width rheight = r.height initialCell = Rectangle(self.initialCell[0], self.initialCell[1], 0.1, 0.1) goalCell = Rectangle(self.goalsCell[0][0], self.goalsCell[0][1], 0.1, 0.1) for o in self.domain.obstacles: if o.CalculateOverlap(r) >= rwidth * rheight: cell = 'obstacle' break elif o.CalculateOverlap(r) > 0.0: cell = 'mixed' break if cell == 'mixed': if (((rwidth / 2.0 > self.minimumSize) and (rheight / 2.0 > self.minimumSize)) or goalCell.CalculateOverlap(r) > 0.0 or initialCell.CalculateOverlap(r) > 0.0): childt1 = [ Rectangle(rx, ry, rwidth / 2.0, rheight / 2.0), 'unknown', [] ] qchild1 = self.Decompose(childt1) childt2 = [ Rectangle(rx + rwidth / 2.0, ry, rwidth / 2.0, rheight / 2.0), 'unknown', [] ] qchild2 = self.Decompose(childt2) childt3 = [ Rectangle(rx, ry + rheight / 2.0, rwidth / 2.0, rheight / 2.0), 'unknown', [] ] qchild3 = self.Decompose(childt3) childt4 = [ Rectangle(rx + rwidth / 2.0, ry + rheight / 2.0, rwidth / 2.0, rheight / 2.0), 'unknown', [] ] qchild4 = self.Decompose(childt4) children = [qchild1, qchild2, qchild3, qchild4] node[2] = children else: cell = 'obstacle' node[1] = cell return node
def Decompose(self, parent): cell = 'free' r = parent[0] rx = r.x ry = r.y rwidth = r.width rheight = r.height for o in self.domain.obstacles: if (o.CalculateOverlap(r) >= rwidth * rheight): cell = 'obstacle' break elif (o.CalculateOverlap(r) > 0.0): cell = 'mixed' break # break down only when it s mixed if cell == 'mixed': if (rwidth / 2.0 > self.minimumSize) and (rheight / 2.0 > self.minimumSize): # node structure [rec, name, child, parent] soth_west = [ Rectangle(rx, ry, rwidth / 2.0, rheight / 2.0), 'unknown', [], parent, self.SW, 0 ] q_soth_west = self.Decompose(soth_west) south_east = [ Rectangle(rx + rwidth / 2.0, ry, rwidth / 2.0, rheight / 2.0), 'unknown', [], parent, self.SE, 0 ] q_south_east = self.Decompose(south_east) north_west = [ Rectangle(rx, ry + rheight / 2.0, rwidth / 2.0, rheight / 2.0), 'unknown', [], parent, self.NW, 0 ] q_north_west = self.Decompose(north_west) north_east = [ Rectangle(rx + rwidth / 2.0, ry + rheight / 2.0, rwidth / 2.0, rheight / 2.0), 'unknown', [], parent, self.NE, 0 ] q_north_east = self.Decompose(north_east) children = [ q_soth_west, q_south_east, q_north_west, q_north_east ] parent[2] = children else: cell = 'obstacle' parent[1] = cell return parent
def nbTopTest(self, rect): x = rect.x height = self.minimumSize y = rect.y + rect.height width = rect.width test = Rectangle(x, y, width, height) return test
def __init__(self, domain, minimumSize): self.domain = domain self.minimumSize = minimumSize self.root = [ Rectangle(0.0, 0.0, domain.width, domain.height), 'unknown', [], None, 0, 0 ]
def nbRightTest(self, rect): x = rect.x + rect.width width = self.minimumSize y = rect.y height = rect.height test = Rectangle(x, y, width, height) return test
def nbBotTest(self, rect): x = rect.x height = self.minimumSize y = rect.y - self.minimumSize width = rect.width test = Rectangle(x, y, width, height) return test
def __init__(self, domain, minimumSize): self.domain = domain self.minimumSize = minimumSize self.root = [ Rectangle(0.0, 0.0, domain.width, domain.height), 'unknown', [] ] self.free_cell = [] a = 1
def __init__(self, domain, minimumSize): self.domain = domain self.minimumSize = minimumSize self.root = [ Rectangle(0.0, 0.0, domain.width, domain.height), 'unknown', [], [], float('inf'), False ]
def Decompose(self, node, depth = 1): cell = 'free' r = node.rectangle rx = r.x ry = r.y rwidth = r.width rheight = r.height if depth > self.maxDepth: self.maxDepth = depth for o in self.domain.obstacles: if ( o.CalculateOverlap(r) >= rwidth * rheight ): cell = 'obstacle' break elif ( o.CalculateOverlap(r) > 0.0 ): cell = 'mixed' break if ( cell == 'mixed'): # Decomposition order # 1 2 # 3 4 if (rwidth / 2.0 > self.minimumSize) and (rheight / 2.0 > self.minimumSize): childt1 = Cell(Rectangle(rx, ry, rwidth/2.0, rheight/2.0), 'unknown', node) qchild1 = self.Decompose(childt1, depth + 1) childt2 = Cell(Rectangle(rx + rwidth/2.0, ry, rwidth/2.0, rheight/2.0), 'unknown', node) qchild2 = self.Decompose(childt2, depth + 1) childt3 = Cell(Rectangle(rx, ry + rheight/2.0, rwidth/2.0, rheight/2.0), 'unknown', node) qchild3 = self.Decompose(childt3, depth + 1) childt4 = Cell(Rectangle(rx + rwidth/2.0, ry + rheight/2.0, rwidth/2.0, rheight/2.0), 'unknown', node) qchild4 = self.Decompose(childt4, depth + 1) children = [ qchild1, qchild2, qchild3, qchild4 ] node.children = children else: cell = 'obstacle' node.occupancy = cell return node
def ExploreDomain( domain, initial, steps ): log = np.zeros((steps,2)) pos = np.array(initial) dd = 2 theta = 0.00/180.0 * math.pi for i in range(steps): newpos = pos + dd * np.array([dd * math.cos(theta), dd * math.sin(theta)]) r = Rectangle(newpos[0], newpos[1], 1, 1) if ( newpos[0] >= 0.0 ) and ( newpos[0] < domain.width ) and ( newpos[1] >= 0.0 ) and ( newpos[1] < domain.height ): if ( not domain.CheckOverlap( r ) ): pos = newpos theta = theta + random.uniform(-180.0/180.0 * math.pi, 180.0/180.0 * math.pi) while( theta >= math.pi ): theta = theta - 2 * math.pi while( theta < - math.pi ): theta = theta + 2 * math.pi log[i,:] = pos return log
def __init__(self, domain, minimumSize): self.domain = domain self.minimumSize = minimumSize self.root = Cell(Rectangle(0.0, 0.0, domain.width, domain.height), 'unknown', None) self.maxDepth = 1
def Decompose(self, node, depth = 1): cell = 'free' r = node.rectangle rx = r.x ry = r.y rwidth = r.width rheight = r.height area = rwidth * rheight if depth > self.maxDepth: self.maxDepth = depth for o in self.domain.obstacles: if ( o.CalculateOverlap(r) >= rwidth * rheight ): cell = 'obstacle' break elif ( o.CalculateOverlap(r) > 0.0 ): cell = 'mixed' break if ( cell == 'mixed'): entropy = self.CalcEntropy(r) hSplitTop = None hSplitBottom = None vSplitLeft = None vSplitRight = None igH = 0.0 if ( r.height / 2.0 > self.minimumSize): hSplitTop = Rectangle(rx, ry + rheight/2.0, rwidth, rheight/2.0) entHSplitTop = self.CalcEntropy(hSplitTop) hSplitBottom = Rectangle(rx, ry, rwidth, rheight/2.0) entHSplitBottom = self.CalcEntropy( hSplitBottom ) igH = entropy - ( r.width * r.height / 2.0 ) / area * entHSplitTop \ - ( r.width * r.height / 2.0 ) / area * entHSplitBottom igV = 0.0 if ( r.width / 2.0 > self.minimumSize ): vSplitLeft = Rectangle(rx, ry, rwidth/2.0, rheight ) entVSplitLeft = self.CalcEntropy( vSplitLeft ) vSplitRight = Rectangle( rx + rwidth/2.0, ry, rwidth/2.0, rheight) entVSplitRight = self.CalcEntropy( vSplitRight) igV = entropy - ( r.width/2.0 * r.height ) / area * entVSplitLeft \ - ( r.width/2.0 * r.height ) / area * entVSplitRight children = [] if ( igH > igV ): if ( igH > 0.0 ): if ( hSplitTop is not None ) and ( hSplitBottom is not None ): node.split = "horizontal" # Horizontal split child order: Top, Bottom childTop = CellFBSP(hSplitTop, 'unknown', node, "N") childBottom = CellFBSP(hSplitBottom, 'unknown', node, "S") children = [childTop, childBottom] else: if ( igV > 0.0 ): if ( vSplitLeft is not None ) and ( vSplitRight is not None ): node.split = "vertical" # Vertical split child order: Left, Right childLeft = CellFBSP(vSplitLeft, 'unknown', node, "W") childRight = CellFBSP(vSplitRight, 'unknown', node, "E") children = [childLeft, childRight] for c in children: self.Decompose(c, depth + 1) node.children = children node.occupancy = cell return node
def ExploreDomain(domain, initial, goal, maxSteps, stepDistance=0.1, biasToGoal=0, goalRadius=0.25): treeNodes = [] treeRoot = Node(initial[0], initial[1]) treeNodes.append(treeRoot) nodeNearGoal = None for i in range(maxSteps): # Get a target point in space to expand towards targetPoint = goal if random.uniform( 0, 1 ) < 1 - biasToGoal: # If we're going in a random direction this step targetPoint = [ random.uniform(0, domain.width), random.uniform(0, domain.height) ] # Expand node nearest to point towards point nearestToRandomPoint = treeRoot distNearestToRandomPoint = getDistanceBetweenPoints( nearestToRandomPoint.pos, targetPoint) for node in treeNodes: dist = getDistanceBetweenPoints(node.pos, targetPoint) if (dist < distNearestToRandomPoint): nearestToRandomPoint = node distNearestToRandomPoint = dist # Get a vector towards the target point dirVector = [ targetPoint[0] - nearestToRandomPoint.pos[0], targetPoint[1] - nearestToRandomPoint.pos[1] ] # Create direction vector dirVector[0] /= distNearestToRandomPoint # Normalize vector dirVector[1] /= distNearestToRandomPoint # Move the point nearest to target point towards target point, create new node newPoint = [ nearestToRandomPoint.pos[0] + dirVector[0] * stepDistance, nearestToRandomPoint.pos[1] + dirVector[1] * stepDistance ] if (newPoint[0] >= 0.0) and (newPoint[0] < domain.width) and ( newPoint[1] >= 0.0) and (newPoint[1] < domain.height): # within map newPointRect = Rectangle(newPoint[0], newPoint[1], 0.1, 0.1) if (not domain.CheckOverlap(newPointRect) ): # Not inside of an obstacle newNode = Node(newPoint[0], newPoint[1], nearestToRandomPoint) treeNodes.append( newNode) # Create new node parented to its nearest node # If node near goal then stop searching if getDistanceBetweenPoints(newNode.pos, goal) <= goalRadius: nodeNearGoal = newNode break return treeNodes, nodeNearGoal
def main(argv=None): if (argv == None): argv = sys.argv[1:] width = 100.0 height = 100.0 pp = PathPlanningProblem(width, height, 60, 40, 40) initial, goals = pp.CreateProblemInstance() fig = plt.figure() ax = fig.add_subplot(1, 2, 1, aspect='equal') ax.set_xlim(0.0, width) ax.set_ylim(0.0, height) for o in pp.obstacles: ax.add_patch(copy.copy(o.patch)) ip = plt.Rectangle((initial[0], initial[1]), 1.0, 1.0, facecolor='#ff0000') ax.add_patch(ip) for g in goals: g = plt.Rectangle((g[0], g[1]), 1.0, 1.0, facecolor='#00ff00') ax.add_patch(g) qtd = QuadTreeDecomposition(pp, 0.2, initial, goals) qtd.Draw(ax) n = qtd.CountCells() start = timeit.default_timer() astar = AStarSearch( qtd.domain, qtd.root, Rectangle(qtd.initialCell[0], qtd.initialCell[1], 0.1, 0.1), Rectangle(qtd.goalsCell[0][0], qtd.goalsCell[0][1], 0.1, 0.1)) stop = timeit.default_timer() print("A* Running Time: ", stop - start) print("A* Path Length : ", astar.path_length) plt.plot([x for (x, y) in astar.path], [y for (x, y) in astar.path], '-') ax.set_title('Quadtree Decomposition\n{0} cells'.format(n)) ax = fig.add_subplot(1, 2, 2, aspect='equal') ax.set_xlim(0.0, width) ax.set_ylim(0.0, height) for o in pp.obstacles: ax.add_patch(copy.copy(o.patch)) ip = plt.Rectangle((initial[0], initial[1]), 1, 1, facecolor='#ff0000') ax.add_patch(ip) goal = None for g in goals: goal = g g = plt.Rectangle((g[0], g[1]), 1, 1, facecolor='#00ff00') ax.add_patch(g) start = timeit.default_timer() spath = RRT.ExploreDomain(RRT(8), pp, initial, goal, 5000) path = spath[0] final = spath[1] if len(final) == 0: print("No path found") RRT.draw(RRT(0), plt, path, final) stop = timeit.default_timer() print("RRT Running Time: ", stop - start) if len(final) > 0: print("RRT Path Length : ", RRT.pathLen(RRT(0), final)) ax.set_title('RRT') plt.show()
def ExploreDomain(self, domain, initial, goal, steps): init = Node(initial[0], initial[1]) g = Node(goal[0], goal[1]) dd = self.get_step_len(domain) newpos = None nodeList = [init] while True: if steps == 0: break if random.randint(0, 100) > 5: rnd = [random.uniform(0, 100), random.uniform(0, 100)] else: rnd = [g.x, g.y] nind = self.getNearNeighbour(nodeList, rnd) nearestNode = nodeList[nind] theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) newNode = Node((nearestNode.x), (nearestNode.y)) newNode.parent = nearestNode newpos = Node((newNode.x + dd * math.cos(theta)), (newNode.y + dd * math.sin(theta))) newpos.parent = newNode xp = newNode.x yp = newNode.y over = False for i in range(dd): xp = xp + math.cos(theta) yp = yp + math.sin(theta) rec = Rectangle(xp, yp, 1, 1) if domain.CheckOverlap(rec): over = True break r = Rectangle(newpos.x, newpos.y, 1, 1) if (newpos.x >= 0.0) and (newpos.x < domain.width) and ( newpos.y >= 0.0) and (newpos.y < domain.height): if (not over and not domain.CheckOverlap(r)): steps = steps - 1 dx = newpos.x - g.x dy = newpos.y - g.y d = math.sqrt(dx * dx + dy * dy) if d <= dd: newpos = Node(g.x, g.y) newpos.parent = newNode nodeList.append(newpos) break nodeList.append(newpos) finalpath = [] lastIndex = len(nodeList) - 1 node = nodeList[lastIndex] if node.x == g.x and node.y == g.y: while node.parent is not None: finalpath.append(node) node = node.parent finalpath.append(init) result = [nodeList, finalpath] return result
def ExploreDomain(domain, initial, steps, goals, name): logging.info("Thread %s: starting", name) startTime = time.process_time() global solutionFound if name.__eq__("Thread 1"): pathColour = BLACK goalColour = BLUE thread = 1 global all_nodes else: pathColour = BROWN goalColour = PURPLE thread = 2 global all_nodes2 cost = None foundGoal = False numGoalPaths = 0 pos = np.array(initial) # Create the first node, this is where our implementation starts to deviate from sample initialNode = Node(initial[0], initial[1]) if thread is 1: all_nodes = [initialNode] else: all_nodes2 = [initialNode] print("Our initial Node on the graph is : ", initialNode) # Init random path randomPath = [[initialNode.x, initialNode.y]] # TEST CODE for i in range(steps): if solutionFound is not True: # Pick a spot on the graph, proceed rand = Node(random.uniform(0, MAX_FIELD_WIDTH), random.uniform(0, MAX_FIELD_HEIGHT)) # Init distance, closestNode = startingNode my_nodes = [] if thread is 1: closestNode = all_nodes[0] my_nodes = all_nodes else: closestNode = all_nodes2[0] my_nodes = all_nodes2 d = 0 # Iterate to max cycles (Can be changed to give our algorithm more time to find a solution for theNodes in my_nodes: # Find closest node to random point if distance(theNodes, rand) < distance(closestNode, rand): closestNode = theNodes d = math.hypot(rand.x - closestNode.x, rand.y - closestNode.y) newnode = steer(closestNode, rand) # Call nearest node function to find our neighbour neighbourNode = nearestNode(my_nodes, newnode) # Store the path inside each node, later we can fetch for goal if d <= PATH_DD: r = Rectangle(rand.x, rand.y, 1 * SCALE, 1 * SCALE) # If no violation, add the random point to the list of coordinates in our current node if not domain.CheckOverlap(r): newnode.xList.append(rand.x) newnode.yList.append(rand.y) # Our current node's parent is simply its neighbour newnode.parent = neighbourNode # Check for obstacles r = Rectangle(newnode.x, newnode.y, 1 * SCALE, 1 * SCALE) if not domain.CheckOverlap(r): my_nodes.append(newnode) # Draw the current connection made to the simulation pygame.draw.line(screen, pathColour, [closestNode.x, closestNode.y], [newnode.x, newnode.y]) pygame.display.update() # Check for goal and show goal path if found atGoal, cost, numGoalPaths = checkGoal(newnode, goals, my_nodes, cost, numGoalPaths, goalColour, thread) #Update if first time hitting goal if foundGoal is False and atGoal is True: foundGoal = True solutionFound = True print("Found goal") # Optional, if we wish to end program once any goal path is found (good for dual rrt) if atGoal: print(name, "\n") print( "Solution found, took the following length of time to complete : ", (time.process_time() - startTime), "s") return foundGoal # Check if user wishes to leave, IE ESCAPE endFlag = checkExitSimulation() if endFlag is True: print("Leaving simulation, sorry to see you go") exit(1) else: exit(1) # If we are out of the loop and still no goal, notify and return if foundGoal is False: print("Goal not found, try increasing max iterations") return foundGoal
def ExploreDomain(domain, initial, steps, goals): cost = None foundGoal = False numGoalPaths = 0 timeToGoal = 0 pos = np.array(initial) # Create the first node, this is where our implementation starts to deviate from sample initialNode = Node(initial[0], initial[1]) all_nodes = [initialNode] print("Our initial Node on the graph is : ", initialNode) # Init random path randomPath = [[initialNode.x, initialNode.y]] # TEST CODE for i in range(steps): # Pick a spot on the graph, proceed rand = Node(random.uniform(0, MAX_FIELD_WIDTH), random.uniform(0, MAX_FIELD_HEIGHT)) # Init distance, closestNode = startingNode closestNode = all_nodes[0] d = 0 # Iterate to max cycles (Can be changed to give our algorithm more time to find a solution for theNodes in all_nodes: # Find closest node to random point if distance(theNodes, rand) < distance(closestNode, rand): closestNode = theNodes d = math.hypot(rand.x - closestNode.x, rand.y - closestNode.y) newnode = steer(closestNode, rand) # Call nearest node function to find our neighbour neighbourNode = nearestNode(all_nodes, newnode) # Store the path inside each node, later we can fetch for goal if d <= PATH_DD: r = Rectangle(rand.x, rand.y, 1 * SCALE, 1 * SCALE) # If no violation, add the random point to the list of coordinates in our current node if not domain.CheckOverlap(r): newnode.xList.append(rand.x) newnode.yList.append(rand.y) # Our current node's parent is simply its neighbour newnode.parent = neighbourNode # Check for obstacles r = Rectangle(newnode.x, newnode.y, 1 * SCALE, 1 * SCALE) if not domain.CheckOverlap(r): all_nodes.append(newnode) # Draw the current connection made to the simulation pygame.draw.line(screen, BLACK, [closestNode.x, closestNode.y], [newnode.x, newnode.y]) pygame.display.update() # Check for goal and show goal path if found atGoal, cost, numGoalPaths = checkGoal(newnode, goals, all_nodes, cost, numGoalPaths) #Update if first time hitting goal if foundGoal is False and atGoal is True: foundGoal = True timeToGoal = time.process_time() print("Found goal") # Optional, if we wish to end program once any goal path is found #if atGoal: #return # Check if user wishes to leave, IE ESCAPE checkExitSimulation("Leaving simulation, sorry to see you go") # Print best cost path print("The current optimal cost of our goal path is: ", cost) # The number of unique goal paths found print("The number of goal paths explored: ", numGoalPaths) # If we are out of the loop and still no goal, notify and return if foundGoal is False: print("Goal not found, try increasing max iterations") return foundGoal, timeToGoal
def ExploreDomain( domain, initial, steps, goal ): paths = [] pos = np.array(initial) tree = [pos] dd = 0.1 total_steps = int( steps / dd ) diagLength = math.sqrt( 50*50 + 50*50 ) goal = np.array( goal ) atGoal = False iter = 0 while atGoal == False and iter < MAX_ITERS: newSpot = None log = np.zeros( ( total_steps, 2 ) ) rand = random.randint( 1, 4 ) if rand < 4: theta = random.uniform(-180.0/180.0 * math.pi, 180.0/180.0 * math.pi) while( theta >= math.pi ): theta = theta - 2 * math.pi while( theta < - math.pi ): theta = theta + 2 * math.pi # get a random spot. newSpot = [domain.width/2, domain.height/2]; newSpot = np.array( newSpot ) newDist = random.uniform( 0.0, diagLength ) newSpot = newSpot + newDist * np.array([newDist * math.cos( theta ), newDist * math.sin( theta )]) else: newSpot = goal # Get the cloeset node to the new point. pos, distance = getClosestNode( newSpot, tree ) if pos is not None: pos = np.array( pos ) log[0,:] = pos distanceRemaining = distance lineToMove = newSpot - pos lengthOfLine = lineLength( lineToMove ) lineToMove[0] /= lengthOfLine lineToMove[1] /= lengthOfLine i = 1 stop = False while i < total_steps and not stop: # newpos = pos + dd * np.array([dd * math.cos(theta), dd * math.sin(theta)]) newpos = np.array( pos + dd * np.array( lineToMove ) ) distanceRemaining -= dd r = Rectangle(newpos[0], newpos[1], 0.1, 0.1) if ( newpos[0] >= 0.0 ) and ( newpos[0] < domain.width ) and ( newpos[1] >= 0.0 ) and ( newpos[1] < domain.height ): if ( not domain.CheckOverlap( r ) ): pos = newpos log[i,:] = pos # Check if we reached the goal. atGoal = hitGoal( goal, pos ) if atGoal: stop = True else: stop = True else: stop = True i = i + 1 # we are at the new node. if distanceRemaining < 1e-9: stop = True # add the final position to the tree. tree.append( pos ) i = 1 while i < total_steps and log[i][0] != 0: i = i + 1 log = log[:i, :i] if len( log ) > 1: paths.append( log ) else: print( "no closest node found.... This is an error" ) iter = iter + 1 atGoal = hitGoal( goal, tree[len( tree ) - 1] ) if atGoal == True: print( "Found it!" ) return paths, atGoal
def Decompose(self, node): cell = 'free' r = node[0] rx = r.x ry = r.y rwidth = r.width rheight = r.height area = rwidth * rheight for o in self.domain.obstacles: # if fill the rect guarantee to be obstacle if (o.CalculateOverlap(r) >= rwidth * rheight): cell = 'obstacle' break # still can be obstacle when overlap elif (o.CalculateOverlap(r) > 0.0): cell = 'mixed' break cal = self.CalMultiOverLapArea(r, self.domain.obstacles, 1, 0) if cal >= rwidth * rheight: cell = 'obstacle' # elif cal>0: # cell='mixed' if (cell == 'mixed'): entropy = self.CalcEntropy(r) igH = 0.0 # horizontal split hSplitTop = None hSplitBottom = None # vertical split vSplitLeft = None vSplitRight = None # split horizontally into 2 pieces if (r.height / 2.0 > self.minimumSize): hSplitTop = Rectangle(rx, ry + rheight / 2.0, rwidth, rheight / 2.0) entHSplitTop = self.CalcEntropy(hSplitTop) hSplitBottom = Rectangle(rx, ry, rwidth, rheight / 2.0) entHSplitBottom = self.CalcEntropy(hSplitBottom) # information gain horizontal igH = entropy - ( r.width * r.height / 2.0 ) / area * entHSplitTop \ - ( r.width * r.height / 2.0 ) / area * entHSplitBottom igV = 0.0 # split vertically into 2 pieces if (r.width / 2.0 > self.minimumSize): vSplitLeft = Rectangle(rx, ry, rwidth / 2.0, rheight) entVSplitLeft = self.CalcEntropy(vSplitLeft) vSplitRight = Rectangle(rx + rwidth / 2.0, ry, rwidth / 2.0, rheight) entVSplitRight = self.CalcEntropy(vSplitRight) # information gain vertical igV = entropy - ( r.width/2.0 * r.height ) / area * entVSplitLeft \ - ( r.width/2.0 * r.height ) / area * entVSplitRight # decomposition children = [] if (igH > igV): if (igH > 0.0): if (hSplitTop is not None) and (hSplitBottom is not None): childTop = [hSplitTop, 'unknown', [], node, -1, 0] childBottom = [ hSplitBottom, 'unknown', [], node, -1, 0 ] children = [childTop, childBottom] else: if (igV > 0.0): if (vSplitLeft is not None) and (vSplitRight is not None): childLeft = [vSplitLeft, 'unknown', [], node, -1, 0] childRight = [vSplitRight, 'unknown', [], node, -1, 0] children = [childLeft, childRight] for c in children: self.Decompose(c) node[2] = children node[1] = cell return node