def extend(self, stopCount=1000):
obstacleFreeVertices = False
count = 0
successFlag = True
while obstacleFreeVertices == False:
if count < stopCount:
newVertices = None
vRand = self.sample()
self.sampledPoints.append(vRand)
# if self.plotStore is not None:
# self.plotStore.sampledPoints.append(vRand)
# print 'vRand: ' + str(vRand.getState())
vNearest, vNearestIndex = self.getNN(vRand)
# print 'vNearest: ' + str(vNearest.getState())
if self.controlledSteering is False:
# print 'using uncontrolled steering'
newVertices = self.steerUncontrolled(
vNearest, vNearestIndex, vRand)
elif self.controlledSteering is True:
# print 'using controlled steering'
newVertices = self.steerControlled(vNearest, vNearestIndex,
vRand)
if newVertices is not None:
obstacleFreeStart = self.obstacleFree(
vNearest, Vertex(*newVertices[0]))
obstacleFreeVertices = self.obstacleFreeVertices(
newVertices)
if obstacleFreeStart and obstacleFreeVertices:
steeredVertex = Vertex(*newVertices[-1])
for i, v in enumerate(self.verticesSteered):
if self.getDistance(v, steeredVertex
) < self.computeSearchRadius():
if self.obstacleFree(v, steeredVertex):
if v.cost + self.getDistance(
v, steeredVertex
) < vNearest.cost + self.getDistance(
vNearest, steeredVertex):
vNearest = v
vNearestIndex = i
steeredVertex.parent = vNearestIndex
steeredVertex.cost = vNearest.cost + self.getDistance(
vNearest, steeredVertex)
if self.lastSteerOnly is False:
for i in range(newVertices.shape[0]):
self.vertices.append(Vertex(*newVertices[i]))
self.verticesSteered.append(
Vertex(*newVertices[i]))
else:
for i in range(newVertices.shape[0]):
self.vertices.append(Vertex(*newVertices[i]))
self.verticesSteered.append(steeredVertex)
# if self.plotStore is not None:
# self.plotStore.allRRTVertices.append(newVertices[-1])
# if self.plotStore is not None:
# if self.plottingInterval != 'end':
# self.plotAll()
for i, v in enumerate(self.verticesSteered):
if i != steeredVertex.parent:
if self.getDistance(
v, steeredVertex
) < self.computeSearchRadius():
if self.obstacleFree(v, steeredVertex):
if steeredVertex.cost + self.getDistance(
v, steeredVertex) < v.cost:
v.parent = len(
self.verticesSteered) - 1
v.cost = steeredVertex.cost + self.getDistance(
v, steeredVertex)
if self.plotStore is not None:
if self.plottingInterval != 'end':
if hasattr(self, 'controlSpline'):
if self.iterationCount % 10 == 0:
# print 'plotting!'
self.plotAll()
count += 1
print 'extend count is ' + str(count)
else:
print stopCount
successFlag = False
break
return successFlag
def extend(self,stopCount=1000):
obstacleFreeVertices = False
count = 0
successFlag = True
while obstacleFreeVertices == False:
if count<stopCount:
newVertices = None
vRand = self.sample()
if self.storeSampledPoints:
self.sampledPoints.append(vRand)
# if self.plotStore is not None:
# self.plotStore.sampledPoints.append(vRand)
# print 'vRand: ' + str(vRand.getState())
vNearest, vNearestIndex = self.getNN(vRand)
# print 'vNearest: ' + str(vNearest.getState())
if self.controlledSteering is False:
# print 'using uncontrolled steering'
newVertices = self.steerUncontrolled(vNearest, vNearestIndex, vRand)
elif self.controlledSteering is True:
# print 'using controlled steering'
newVertices = self.steerControlled(vNearest, vNearestIndex, vRand)
if newVertices is not None:
steeredVertex = Vertex(*newVertices[-1])
obstacleFreeStart = self.obstacleFree(vNearest,steeredVertex)
obstacleFreeVertices = self.obstacleFreeVertices(newVertices)
if obstacleFreeStart and obstacleFreeVertices:
for i,v in enumerate(self.verticesSteered):
# for i,v in enumerate(self.vertices):
if self.getDistance(v,steeredVertex) < self.computeSearchRadius():
if self.obstacleFree(v,steeredVertex):
if v.cost+self.getDistance(v,steeredVertex) < vNearest.cost+self.getDistance(vNearest,steeredVertex):
print 'rewiring happening'
print 'vNearest.state: ' + str(vNearest.getState())
vNearest = v
vNearestIndex = i
vNearest.controlInput = self.computeSteeringAngle(steeredVertex,vNearest)
print 'vNearest.state: ' + str(vNearest.getState())
print 'rewiring finished'
steeredVertex.parent = vNearestIndex
steeredVertex.cost = vNearest.cost+self.getDistance(vNearest,steeredVertex)
# So you did some rewiring. Now the steered vertex has a new parent. What does it take to get the parent
# to the child?
# On control values, the value of the control input for a given vertex represents the value used to generate the
# state difference in theta of the current vertex. The x and y state differences of the child vertex are generated
# via the amended theta state of the current vertex.
# Here, the steered vertex is the rewired child and the non steered vertex is the
# rewired parent
# The steered vertex does not have a child. Hence the control value is 0
steeredVertex.controlInput = 0
try:
deltaTheta = atan((steeredVertex.y-vNearest.y)/(steeredVertex.x-vNearest.x))-vNearest.theta
deltaTime = deltaTheta*self.r/steeredVertex.controlInput
# deltaXTime = acos((steeredVertex.x-vNearest.x)/self.velocity)/steeredVertex.controlInput
# deltaYTime = asin((steeredVertex.y-vNearest.y)/self.velocity)/steeredVertex.controlInput
except:
print 'steeredVertex.getState(): ' + str(steeredVertex.getState())
print 'vNearest.getState(): ' + str(vNearest.getState())
sys.exit()
v.time = deltaTime
#Debugging
# for v in self.verticesSteered:
# if self.obstacleFree(v,self.verticesSteered[v.parent]) is False:
# dill.dump(self.verticesSteered,open(self.saveDir+'RRT_verticesSteered.p','wb'))
# print 'Found collision path 1'
# sys.exit()
# Adding in state modification to time for rewired vertices
# vNearest.controlInput = vNearest.controlInput-(self.computeSteeringAngle(steeredVertex, vNearest) * self.dt)
# vNearest.controlInput = self.computeSteeringAngle(steeredVertex, vNearest)
# steeredVertex.time = vNearest.time+(self.getDistance(vNearest,steeredVertex)/self.velocity)
# print 'rewiring has finished by this point'
# sys.exit()
if self.lastSteerOnly is False:
for i in range(newVertices.shape[0]):
self.vertices.append(Vertex(*newVertices[i]))
self.verticesSteered.append(Vertex(*newVertices[i]))
else:
# for i in range(newVertices.shape[0]):
# self.vertices.append(Vertex(*newVertices[i]))
self.verticesSteered.append(steeredVertex)
# if self.plotStore is not None:
# self.plotStore.allRRTVertices.append(newVertices[-1])
# This plots every iteration of extend
# if self.plotStore is not None:
# if self.plottingInterval != 'end':
# self.plotAll()
for i,v in enumerate(self.verticesSteered):
# for i,v in enumerate(self.vertices):
# if i != steeredVertex.parent:
if self.getDistance(v,steeredVertex) < self.computeSearchRadius():
if self.obstacleFree(v,steeredVertex):
if steeredVertex.cost+self.getDistance(v,steeredVertex) < v.cost:
#Debugging
# print 'rewiring vertex ' + str(i)
# print 'v.parent: ' + str(v.parent)
# v.parent = len(self.verticesSteered)-1
flagFlag = abs(v.parent-len(self.vertices)+1) > 10
if flagFlag:
print 'rewiring happening'
print 'v.state: ' + str(v.getState())
# This is dangerous. I think it should be verticesSteered because it is verticesSteered in line 178
v.parent = len(self.verticesSteered)-1
v.cost = steeredVertex.cost+self.getDistance(v,steeredVertex)
# So you did some rewiring. Now the steered vertex has a new parent. What does it take to get the parent
# to the child?
v.controlInput = self.computeSteeringAngle(v,steeredVertex)
#Debugging
# print 'v.parent: ' + str(v.parent)
#Debugging
# if self.obstacleFree(v,self.verticesSteered[v.parent]) is False:
# print 'bad rewiring at iteration ' + str(i)
# dill.dump(self.verticesSteered,open(self.saveDir+'RRT_verticesSteered.p','wb'))
# sys.exit()
# Adding in state modification to time for rewired vertices
# I'm going to use the sine inverse function to compute time to travel
# from rewired parent to rewired child vertex using the control inputs from
# from the compute steering angle function.
# Take the max of the state difference in x and y
# Here, the steered vertex is the rewired parent and the non steered vertex is the
# rewired child
updatedTheta = (steeredVertex.controlInput*self.dt)+steeredVertex.theta
deltaXTime = (v.x-steeredVertex.x)/(self.velocity*cos(updatedTheta))
deltaYTime = (v.y-steeredVertex.y)/(self.velocity*sin(updatedTheta))
deltaTime = max(deltaXTime,deltaYTime)
# We do not want time to become negative
if deltaTime < 0:
print 'time became negative! F**k'
print 'v.getState(): ' + str(v.getState())
print 'steeredVertex.getState(): ' + str(steeredVertex.getState())
print 'updatedTheta: ' + str(updatedTheta)
sys.exit()
v.time = steeredVertex.time+deltaTime
#Debugging
# for i,v in enumerate(self.verticesSteered):
# if self.obstacleFree(v,self.verticesSteered[v.parent]) is False:
# dill.dump(self.verticesSteered,open(self.saveDir+'RRT_verticesSteered.p','wb'))
# print 'Found collision path 2 for vertex ' + str(i) + ' with parent ' + str(v.parent)
# sys.exit()
else:
print 'obstacleFreeStart: ' + str(obstacleFreeStart)
print 'obstacleFreeVertices: ' + str(obstacleFreeStart)
count += 1
print 'extend count is ' + str(count)
else:
print stopCount
successFlag = False
break
return successFlag
