Module rrtTree
[hide private]
[frames] | no frames]

Source Code for Module rrtTree

  1  ################################################################################## 
  2  # Copyright (c) 2010, 2011, 2012, 2013, Daniel Urieli, Peter Stone 
  3  # University of Texas at Austin 
  4  # All right reserved 
  5  #  
  6  # Based On: 
  7  #  
  8  # Copyright (c) 2000-2003, Jelle Kok, University of Amsterdam 
  9  # All rights reserved. 
 10  #  
 11  # Redistribution and use in source and binary forms, with or without 
 12  # modification, are permitted provided that the following conditions are met: 
 13  #  
 14  # 1. Redistributions of source code must retain the above copyright notice, this 
 15  # list of conditions and the following disclaimer. 
 16  #  
 17  # 2. Redistributions in binary form must reproduce the above copyright notice, 
 18  # this list of conditions and the following disclaimer in the documentation 
 19  # and/or other materials provided with the distribution. 
 20  #  
 21  # 3. Neither the name of the University of Amsterdam nor the names of its 
 22  # contributors may be used to endorse or promote products derived from this 
 23  # software without specific prior written permission. 
 24  #  
 25  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 26  # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 27  # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
 28  # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 
 29  # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
 30  # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 
 31  # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
 32  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 
 33  # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
 34  # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 
 35  ################################################################################## 
 36   
 37   
 38   
 39   
 40  ################################################################### 
 41  # This file contains implementation of trees for the RRT algorithm. 
 42  # Trees are only responsible for storing nodes, finding a  
 43  # closest node, and finding a path from a root to a node. 
 44  ################################################################### 
 45   
 46  import random 
 47   
 48  from actions import * 
 49   
 50   
51 -class RRTNode:
52 """ 53 A node in the RRT tree. 54 Contains the state, a pointer to the parent, 55 and the edge from the parent. 56 The edge from the parent is the action that takes it 57 from the parent's state to the currrent state. 58 """
59 - def __init__(self, shipState, parent, edge=CompositeAction()):
60 self.state = shipState 61 self.parent = parent 62 self.edge = edge
63 64 65 66 67
68 -class RRTTree:
69 """ 70 Interface for an RRT. 71 Based on the paper: http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.36.7457 72 """ 73
74 - def addNode(self, node):
75 "Adds a new node to the tree." 76 abstract
77
78 - def getPathTo(self, currentState):
79 """ 80 Returns a path (a sequence of actions) from root to currentState. 81 82 Keyword arguments: 83 currentState -- ShipExternalState to which we want a path from the 84 initial state. 85 86 Returns: 87 A path of actions that would bring us to currentState 88 89 """ 90 abstract
91
92 - def findClosestNode(self, newState):
93 """ 94 Find closest node in the RRT w.r.t. x, y, orientation. 95 96 Keyword arguments: 97 newState -- ShipExternalState that is going to be part of the 98 new tree node. 99 100 Returns: 101 RRTNode which is the noded closest to 'newState' in the current RRT 102 103 """ 104 abstract
105 106 107
108 -class RRTTreeXYOrientationEuclidDist(RRTTree):
109 """ 110 Implements a simple RRT, which assumes a state is 111 (x,y,orientation), and the distance between two states 112 is euclidean, after normalizing their attributes to 113 a similar scale. 114 115 """
116 - def __init__(self, initialShipState, distanceUnit, angleUnit):
117 """Initialize the tree. 118 119 Keyword arguments: 120 initialShipState -- initial ShipExternalState that would be 121 the root of the tree 122 123 distanceUnit -- 124 angleUnit -- Both are used for normalization of x, y and angle 125 to the same scale, to determine similarity (or distance) 126 between two points (x1, y1, ang1) and (x2, y2, ang2). 127 These could be, for instance, the amount of distance / orientation 128 a ship can travel in a given time unit. 129 """ 130 self.root = RRTNode(initialShipState, parent=None) 131 self.nodes = [self.root] 132 133 self.distanceUnit = distanceUnit 134 self.angleUnit = angleUnit
135 136
137 - def addNode(self, node):
138 self.nodes.append(node)
139
140 - def getPathTo(self, currentState):
141 """Returns a path (a sequence of actions) from root to currentState.""" 142 actions = [] 143 144 # Scan for the node of currentState and return the path to it 145 for node in self.nodes: 146 if node.state == currentState: # by-reference comparison 147 # Found a node, construct a path of actions that lead to it 148 currentNode = node 149 while currentNode != None: # None is the parent of self.root 150 actions.append(currentNode.edge) 151 currentNode = currentNode.parent 152 actions.reverse() 153 154 # Append a stopping action 155 action = CompositeAction() 156 action.appendAction(PrimitiveAction("stop", [])) 157 actions.append(action) 158 159 return actions 160 161 # If got here didn't find a node 162 raise Exception("Didn't find the node in the tree")
163
164 - def getNodesPathTo(self, currentState):
165 """Returns a path OF NODES from root to currentState.""" 166 nodes = [] 167 168 # Scan for the node of currentState and return the path to it 169 for node in self.nodes: 170 if node.state == currentState: # by-reference comparison 171 # Found a node, construct a path of actions that lead to it 172 currentNode = node 173 while currentNode != None: # None is the parent of self.root 174 nodes.append(currentNode) 175 currentNode = currentNode.parent 176 nodes.reverse() 177 178 return nodes 179 180 # If got here didn't find a node 181 raise Exception("Didn't find the node in the tree")
182
183 - def findClosestNode(self, newState):
184 """ 185 Find closest node in the RRT w.r.t. x, y, orientation. 186 Distance is euclidean, where x, y and orientation are 187 normalized. 188 189 Keyword arguments: 190 newState -- ShipExternalState that is going to be part of the 191 new tree node. 192 193 Returns: 194 RRTNode which is the noded closest to 'newState' in the current RRT 195 196 """ 197 # Scanning for the minimal node in the pythonic way (no for loops) 198 states = [node.state for node in self.nodes] 199 xDiffs = [abs(newState.x - nodeState.x) / self.distanceUnit for nodeState in states] 200 yDiffs = [abs(newState.y - nodeState.y) / self.distanceUnit for nodeState in states] 201 angDiffs = [abs(newState.orientation - nodeState.orientation) / self.angleUnit for nodeState in states] 202 distances = map( lambda x,y,ang : x * x + y * y + ang * ang, xDiffs, yDiffs, angDiffs) 203 204 minDistance = min(distances) 205 epsilon = 0.00000001 206 minDistIndexes = [i for i, dist in enumerate(distances) if abs(dist - minDistance) < epsilon] 207 chosenIndex = random.choice(minDistIndexes) 208 return self.nodes[chosenIndex]
209