1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46 import random
47
48 from actions import *
49
50
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 """
60 self.state = shipState
61 self.parent = parent
62 self.edge = edge
63
64
65
66
67
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
75 "Adds a new node to the tree."
76 abstract
77
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
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
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
138 self.nodes.append(node)
139
141 """Returns a path (a sequence of actions) from root to currentState."""
142 actions = []
143
144
145 for node in self.nodes:
146 if node.state == currentState:
147
148 currentNode = node
149 while currentNode != None:
150 actions.append(currentNode.edge)
151 currentNode = currentNode.parent
152 actions.reverse()
153
154
155 action = CompositeAction()
156 action.appendAction(PrimitiveAction("stop", []))
157 actions.append(action)
158
159 return actions
160
161
162 raise Exception("Didn't find the node in the tree")
163
165 """Returns a path OF NODES from root to currentState."""
166 nodes = []
167
168
169 for node in self.nodes:
170 if node.state == currentState:
171
172 currentNode = node
173 while currentNode != None:
174 nodes.append(currentNode)
175 currentNode = currentNode.parent
176 nodes.reverse()
177
178 return nodes
179
180
181 raise Exception("Didn't find the node in the tree")
182
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
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