def onBuildWorldFromRandomObstacles(self):
distances = self.Sensor.raycastAll(self.frame)
firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, distances)
self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
self.Sensor.setLocator(self.LineSegmentLocator)
self.updateDrawIntersection(self.frame)
def drawFirstIntersections(self, frame, firstRaycast):
origin = np.array(frame.transform.GetPosition())
d = DebugData()
firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, firstRaycast)
for i in xrange(self.Sensor.numRays):
endpoint = firstRaycastLocations[i,:]
if firstRaycast[i] == 20.0:
d.addLine(origin, endpoint, color=[0,1,0])
else:
d.addLine(origin, endpoint, color=[1,0,0])
vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
self.Sensor.setLocator(self.LineSegmentLocator)
def runSingleSimulation(self, controllerType='default', simulationCutoff=None):
#self.setRandomCollisionFreeInitialState()
self.setInitialStateAtZero()
currentCarState = np.copy(self.Car.state)
nextCarState = np.copy(self.Car.state)
self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2])
firstRaycast = self.Sensor.raycastAll(self.frame)
firstRaycastLocations = self.Sensor.raycastAllLocations(self.frame)
self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
self.Sensor.setLocator(self.LineSegmentLocator)
nextRaycast = np.zeros(self.Sensor.numRays)
# record the reward data
runData = dict()
startIdx = self.counter
thisRunIndex = 0
NMaxSteps = 100
while (self.counter < self.numTimesteps - 1):
thisRunIndex += 1
idx = self.counter
currentTime = self.t[idx]
self.stateOverTime[idx,:] = currentCarState
x = self.stateOverTime[idx,0]
y = self.stateOverTime[idx,1]
theta = self.stateOverTime[idx,2]
self.setRobotFrameState(x,y,theta)
# self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2])
currentRaycast = self.Sensor.raycastAll(self.frame)
print "current Raycast ", currentRaycast
self.raycastData[idx,:] = currentRaycast
S_current = (currentCarState, currentRaycast)
if controllerType not in self.colorMap.keys():
print
raise ValueError("controller of type " + controllerType + " not supported")
if controllerType in ["default", "defaultRandom"]:
controlInput, controlInputIdx = self.Controller.computeControlInput(currentCarState,
currentTime, self.frame,
raycastDistance=currentRaycast,
randomize=False)
self.controlInputData[idx] = controlInput
nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt)
x = nextCarState[0]
y = nextCarState[1]
theta = nextCarState[2]
self.setRobotFrameState(x,y,theta)
nextRaycast = self.Sensor.raycastAll(self.frame)
# Compute the next control input
S_next = (nextCarState, nextRaycast)
if controllerType in ["default", "defaultRandom"]:
nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(nextCarState,
currentTime, self.frame,
raycastDistance=firstRaycast,
randomize=False)
#bookkeeping
currentCarState = nextCarState
currentRaycast = nextRaycast
self.counter+=1
# break if we are in collision
if self.checkInCollision(nextRaycast):
if self.verbose: print "Had a collision, terminating simulation"
break
if thisRunIndex > NMaxSteps:
print "was safe during N steps"
break
if self.counter >= simulationCutoff:
break
# fill in the last state by hand
self.stateOverTime[self.counter,:] = currentCarState
self.raycastData[self.counter,:] = currentRaycast
# this just makes sure we don't get stuck in an infinite loop.
if startIdx == self.counter:
self.counter += 1
return runData
