class KinectSource(TimerCallback):
def __init__(self, view, _KinectQueue):
self.view = view
self.KinectQueue = _KinectQueue
self.visible = True
self.p = vtk.vtkPolyData()
utime = KinectQueue.getPointCloudFromKinect(self.p)
self.polyDataObj = vis.PolyDataItem('kinect source',
shallowCopy(self.p), view)
self.polyDataObj.actor.SetPickable(1)
self.polyDataObj.initialized = False
om.addToObjectModel(self.polyDataObj)
self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread())
self.queue.init(lcmUtils.getGlobalLCMThread(),
drcargs.args().config_file)
self.targetFps = 30
self.timerCallback = TimerCallback(targetFps=self.targetFps)
self.timerCallback.callback = self._updateSource
#self.timerCallback.start()
def start(self):
self.timerCallback.start()
def stop(self):
self.timerCallback.stop()
def setFPS(self, framerate):
self.targetFps = framerate
self.timerCallback.stop()
self.timerCallback.targetFps = framerate
self.timerCallback.start()
def setVisible(self, visible):
self.polyDataObj.setProperty('Visible', visible)
def _updateSource(self):
p = vtk.vtkPolyData()
utime = self.KinectQueue.getPointCloudFromKinect(p)
if not p.GetNumberOfPoints():
return
cameraToLocalFused = vtk.vtkTransform()
self.queue.getTransform('KINECT_RGB', 'local', utime,
cameraToLocalFused)
p = filterUtils.transformPolyData(p, cameraToLocalFused)
self.polyDataObj.setPolyData(p)
if not self.polyDataObj.initialized:
self.polyDataObj.setProperty('Color By', 'rgb_colors')
self.polyDataObj.initialized = True
class KinectSource(TimerCallback):
def __init__(self, view, _KinectQueue):
self.view = view
self.KinectQueue = _KinectQueue
self.visible = True
self.p = vtk.vtkPolyData()
utime = KinectQueue.getPointCloudFromKinect(self.p)
self.polyDataObj = vis.PolyDataItem('kinect source', shallowCopy(self.p), view)
self.polyDataObj.actor.SetPickable(1)
self.polyDataObj.initialized = False
om.addToObjectModel(self.polyDataObj)
self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread())
self.queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
self.targetFps = 30
self.timerCallback = TimerCallback(targetFps=self.targetFps)
self.timerCallback.callback = self._updateSource
#self.timerCallback.start()
def start(self):
self.timerCallback.start()
def stop(self):
self.timerCallback.stop()
def setFPS(self, framerate):
self.targetFps = framerate
self.timerCallback.stop()
self.timerCallback.targetFps = framerate
self.timerCallback.start()
def setVisible(self, visible):
self.polyDataObj.setProperty('Visible', visible)
def _updateSource(self):
p = vtk.vtkPolyData()
utime = self.KinectQueue.getPointCloudFromKinect(p)
if not p.GetNumberOfPoints():
return
cameraToLocalFused = vtk.vtkTransform()
self.queue.getTransform('KINECT_RGB', 'local', utime, cameraToLocalFused)
p = filterUtils.transformPolyData(p,cameraToLocalFused)
self.polyDataObj.setPolyData(p)
if not self.polyDataObj.initialized:
self.polyDataObj.setProperty('Color By', 'rgb_colors')
self.polyDataObj.initialized = True
class SpindleSpinChecker(object):
def __init__(self, spindleMonitor):
self.spindleMonitor = spindleMonitor
self.timer = TimerCallback(targetFps=3)
self.timer.callback = self.update
self.warningButton = None
self.action = None
def update(self):
if abs(self.spindleMonitor.getAverageSpindleVelocity()) < 0.2:
self.notifyUserStatusBar()
else:
self.clearStatusBarWarning()
def start(self):
self.action.checked = True
self.timer.start()
def stop(self):
self.action.checked = False
self.timer.stop()
def setupMenuAction(self):
self.action = app.addMenuAction('Tools', 'Spindle Stuck Warning')
self.action.setCheckable(True)
self.action.checked = self.timer.isActive()
self.action.connect('triggered()', self.onActionChanged)
def onActionChanged(self):
if self.action.checked:
self.start()
else:
self.stop()
def clearStatusBarWarning(self):
if self.warningButton:
self.warningButton.deleteLater()
self.warningButton = None
def notifyUserStatusBar(self):
if self.warningButton:
return
self.warningButton = QtGui.QPushButton('Spindle Stuck Warning')
self.warningButton.setStyleSheet("background-color:red")
app.getMainWindow().statusBar().insertPermanentWidget(
0, self.warningButton)
class SpindleSpinChecker(object):
def __init__(self, spindleMonitor):
self.spindleMonitor = spindleMonitor
self.timer = TimerCallback(targetFps=3)
self.timer.callback = self.update
self.warningButton = None
self.action = None
def update(self):
if abs(self.spindleMonitor.getAverageSpindleVelocity()) < 0.2:
self.notifyUserStatusBar()
else:
self.clearStatusBarWarning()
def start(self):
self.action.checked = True
self.timer.start()
def stop(self):
self.action.checked = False
self.timer.stop()
def setupMenuAction(self):
self.action = app.addMenuAction('Tools', 'Spindle Stuck Warning')
self.action.setCheckable(True)
self.action.checked = self.timer.isActive()
self.action.connect('triggered()', self.onActionChanged)
def onActionChanged(self):
if self.action.checked:
self.start()
else:
self.stop()
def clearStatusBarWarning(self):
if self.warningButton:
self.warningButton.deleteLater()
self.warningButton = None
def notifyUserStatusBar(self):
if self.warningButton:
return
self.warningButton = QtGui.QPushButton('Spindle Stuck Warning')
self.warningButton.setStyleSheet("background-color:red")
app.getMainWindow().statusBar().insertPermanentWidget(0, self.warningButton)
class PointerTracker(object):
'''
See segmentation.estimatePointerTip() documentation.
'''
def __init__(self, robotModel, stereoPointCloudItem):
self.robotModel = robotModel
self.stereoPointCloudItem = stereoPointCloudItem
self.timer = TimerCallback(targetFps=5)
self.timer.callback = self.updateFit
def start(self):
self.timer.start()
def stop(self):
self.timer.stop()
def cleanup(self):
om.removeFromObjectModel(om.findObjectByName('segmentation'))
def updateFit(self, polyData=None):
#if not self.stereoPointCloudItem.getProperty('Visible'):
# return
if not polyData:
self.stereoPointCloudItem.update()
polyData = self.stereoPointCloudItem.polyData
if not polyData or not polyData.GetNumberOfPoints():
self.cleanup()
return
self.tipPosition = segmentation.estimatePointerTip(self.robotModel, polyData)
if self.tipPosition is None:
self.cleanup()
def getPointerTip(self):
return self.tipPosition
class SpindleAxisDebug(vis.PolyDataItem):
def __init__(self, frameProvider):
vis.PolyDataItem.__init__(self, 'spindle axis', vtk.vtkPolyData(), view=None)
self.frameProvider = frameProvider
self.timer = TimerCallback()
self.timer.callback = self.update
self.setProperty('Color', QtGui.QColor(0, 255, 0))
self.setProperty('Visible', False)
def _onPropertyChanged(self, propertySet, propertyName):
vis.PolyDataItem._onPropertyChanged(self, propertySet, propertyName)
if propertyName == 'Visible':
if self.getProperty(propertyName):
self.timer.start()
else:
self.timer.stop()
def onRemoveFromObjectModel(self):
vis.PolyDataItem.onRemoveFromObjectModel(self)
self.timer.stop()
def update(self):
t = self.frameProvider.getFrame('SCAN')
p1 = [0.0, 0.0, 0.0]
p2 = [2.0, 0.0, 0.0]
p1 = t.TransformPoint(p1)
p2 = t.TransformPoint(p2)
d = DebugData()
d.addSphere(p1, radius=0.01, color=[0,1,0])
d.addLine(p1, p2, color=[0,1,0])
self.setPolyData(d.getPolyData())
class AtlasCommandStream(object):
def __init__(self):
self.timer = TimerCallback(targetFps=10)
#self.timer.disableScheduledTimer()
self.app = ConsoleApp()
self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model')
self.fpsCounter = simpletimer.FPSCounter()
self.drakePoseJointNames = robotstate.getDrakePoseJointNames()
self.fpsCounter.printToConsole = True
self.timer.callback = self._tick
self._initialized = False
self.publishChannel = 'JOINT_POSITION_GOAL'
# self.lastCommandMessage = newAtlasCommandMessageAtZero()
self._numPositions = len(robotstate.getDrakePoseJointNames())
self._previousElapsedTime = 100
self._baseFlag = 0;
self.jointLimitsMin = np.array([self.robotModel.model.getJointLimits(jointName)[0] for jointName in robotstate.getDrakePoseJointNames()])
self.jointLimitsMax = np.array([self.robotModel.model.getJointLimits(jointName)[1] for jointName in robotstate.getDrakePoseJointNames()])
self.useControllerFlag = False
self.drivingGainsFlag = False
self.applyDefaults()
def initialize(self, currentRobotPose):
assert not self._initialized
self._currentCommandedPose = np.array(currentRobotPose)
self._previousCommandedPose = np.array(currentRobotPose)
self._goalPose = np.array(currentRobotPose)
self._initialized = True
def useController(self):
self.useControllerFlag = True
self.publishChannel = 'QP_CONTROLLER_INPUT'
def setKp(self, Kp, jointName=None):
if jointName is None:
self._Kp = Kp*np.ones(self._numPositions)
else:
idx = robotstate.getDrakePoseJointNames().index(jointName)
self._maxSpeed[idx] = Kp
self.updateKd()
def setMaxSpeed(self, speed, jointName=None):
if jointName is None:
self._maxSpeed = np.deg2rad(speed)*np.ones(self._numPositions)
else:
idx = robotstate.getDrakePoseJointNames().index(jointName)
self._maxSpeed[idx] = np.deg2rad(speed)
def updateKd(self):
self._dampingRatio = 1;
self._Kd = 2*self._dampingRatio*np.sqrt(self._Kp)
def applyDefaults(self):
self.setKp(10)
self.setMaxSpeed(10)
if self.drivingGainsFlag:
self.setMaxSpeed(70,'l_arm_lwy')
self.setKp(50,'l_arm_lwy')
self.setMaxSpeed(100,'l_leg_aky')
self.setKp(100,'l_leg_aky')
def applyDrivingDefaults(self):
self.drivingGainsFlag = True
self.applyDefaults()
def applyPlanDefaults(self):
self.setKp(10)
self.setMaxSpeed(60)
def startStreaming(self):
assert self._initialized
if not self.timer.isActive():
self.timer.start()
def stopStreaming(self):
self.timer.stop()
def setGoalPose(self, pose):
self._goalPose = self.clipPoseToJointLimits(pose)
def setIndividualJointGoalPose(self, pose, jointName):
jointIdx = self.drakePoseJointNames.index(jointName)
self._goalPose[jointIdx] = pose
def clipPoseToJointLimits(self,pose):
pose = np.array(pose)
pose = np.clip(pose, self.jointLimitsMin, self.jointLimitsMax)
return pose
def waitForRobotState(self):
msg = lcmUtils.captureMessage('EST_ROBOT_STATE', lcmdrc.robot_state_t)
pose = robotstate.convertStateMessageToDrakePose(msg)
pose[:6] = np.zeros(6)
self.initialize(pose)
def _updateAndSendCommandMessage(self):
self._currentCommandedPose = self.clipPoseToJointLimits(self._currentCommandedPose)
if self._baseFlag:
msg = robotstate.drakePoseToRobotState(self._currentCommandedPose)
else:
msg = drakePoseToAtlasCommand(self._currentCommandedPose)
if self.useControllerFlag:
msg = drakePoseToQPInput(self._currentCommandedPose)
lcmUtils.publish(self.publishChannel, msg)
def _tick(self):
self.fpsCounter.tick()
elapsed = self.timer.elapsed # time since last tick
#nominalElapsed = (1.0 / self.timer.targetFps)
#if elapsed > 2*nominalElapsed:
# elapsed = nominalElapsed
# move current pose toward goal pose
previousPose = self._previousCommandedPose.copy()
currentPose = self._currentCommandedPose.copy()
goalPose = self.clipPoseToJointLimits(self._goalPose.copy())
nextPose = self._computeNextPose(previousPose,currentPose, goalPose, elapsed,
self._previousElapsedTime, self._maxSpeed)
self._currentCommandedPose = nextPose
# have the base station directly send the command through
if self._baseFlag:
self._currentCommandedPose = goalPose
# publish
self._updateAndSendCommandMessage()
# bookkeeping
self._previousElapsedTime = elapsed
self._previousCommandedPose = currentPose
def _computeNextPose(self, previousPose, currentPose, goalPose, elapsed, elapsedPrevious, maxSpeed):
v = 1.0/elapsedPrevious * (currentPose - previousPose)
u = -self._Kp*(currentPose - goalPose) - self._Kd*v # u = -K*x
v_next = v + elapsed*u
v_next = np.clip(v_next,-maxSpeed,maxSpeed) # velocity clamp
nextPose = currentPose + v_next*elapsed
nextPose = self.clipPoseToJointLimits(nextPose)
return nextPose
class MultisensePanel(object):
def __init__(self, multisenseDriver, neckDriver):
self.multisenseDriver = multisenseDriver
self.neckDriver = neckDriver
self.neckPitchChanged = False
self.multisenseChanged = False
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddMultisense.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.updateTimer = TimerCallback(targetFps=2)
self.updateTimer.callback = self.updatePanel
self.updateTimer.start()
self.widget.headCamGainSpinner.setEnabled(False)
self.widget.headCamExposureSpinner.setEnabled(False)
#connect the callbacks
self.widget.neckPitchSpinner.valueChanged.connect(self.neckPitchChange)
self.widget.spinRateSpinner.valueChanged.connect(self.spinRateChange)
self.widget.scanDurationSpinner.valueChanged.connect(self.scanDurationChange)
self.widget.headCamFpsSpinner.valueChanged.connect(self.headCamFpsChange)
self.widget.headCamGainSpinner.valueChanged.connect(self.headCamGainChange)
self.widget.headCamExposureSpinner.valueChanged.connect(self.headCamExposureChange)
self.widget.headAutoGainCheck.clicked.connect(self.headCamAutoGainChange)
self.widget.ledOnCheck.clicked.connect(self.ledOnCheckChange)
self.widget.ledBrightnessSpinner.valueChanged.connect(self.ledBrightnessChange)
self.widget.sendButton.clicked.connect(self.sendButtonClicked)
self.updatePanel()
def getCameraFps(self):
return self.widget.headCamFpsSpinner.value
def getCameraGain(self):
return self.widget.headCamGainSpinner.value
def getCameraExposure(self):
return self.widget.headCamExposureSpinner.value
def getCameraLedOn(self):
return self.widget.ledOnCheck.isChecked()
def getCameraLedBrightness(self):
return self.widget.ledBrightnessSpinner.value
def getCameraAutoGain(self):
return self.widget.headAutoGainCheck.isChecked()
def getSpinRate(self):
return self.widget.spinRateSpinner.value
def getScanDuration(self):
return self.widget.scanDurationSpinner.value
def getNeckPitch(self):
return self.widget.neckPitchSpinner.value
def ledBrightnessChange(self, event):
self.multisenseChanged = True
def ledOnCheckChange(self, event):
self.multisenseChanged = True
def headCamExposureChange(self, event):
self.multisenseChanged = True
def headCamAutoGainChange(self, event):
self.multisenseChanged = True
self.widget.headCamGainSpinner.setEnabled(not self.getCameraAutoGain())
self.widget.headCamExposureSpinner.setEnabled(not self.getCameraAutoGain())
def neckPitchChange(self, event):
self.neckPitchChanged = True
self.updateTimer.stop()
def headCamFpsChange(self, event):
self.multisenseChanged = True
def headCamGainChange(self, event):
self.multisenseChanged = True
def spinRateChange(self, event):
self.multisenseChanged = True
spinRate = self.getSpinRate()
if spinRate == 0.0:
scanDuration = 240.0
else:
scanDuration = abs(60.0 / (spinRate * 2))
if scanDuration > 240.0:
scanDuration = 240.0
self.widget.scanDurationSpinner.blockSignals(True)
self.widget.scanDurationSpinner.value = scanDuration
self.widget.scanDurationSpinner.blockSignals(False)
def scanDurationChange(self, event):
self.multisenseChanged = True
scanDuration = self.getScanDuration()
spinRate = abs(60.0 / (scanDuration * 2))
self.widget.spinRateSpinner.blockSignals(True)
self.widget.spinRateSpinner.value = spinRate
self.widget.spinRateSpinner.blockSignals(False)
def sendButtonClicked(self, event):
self.publishCommand()
def updatePanel(self):
if not self.widget.isVisible():
return
if not self.neckPitchChanged:
self.widget.neckPitchSpinner.blockSignals(True)
self.widget.neckPitchSpinner.setValue(self.neckDriver.getNeckPitchDegrees())
self.widget.neckPitchSpinner.blockSignals(False)
def publishCommand(self):
fps = self.getCameraFps()
camGain = self.getCameraGain()
exposure = 1000*self.getCameraExposure()
ledFlash = self.getCameraLedOn()
ledDuty = self.getCameraLedBrightness()
spinRate = self.getSpinRate()
autoGain = 1 if self.getCameraAutoGain() else 0
self.multisenseDriver.sendMultisenseCommand(fps, camGain, exposure, autoGain, spinRate, ledFlash, ledDuty)
self.neckDriver.setNeckPitch(self.getNeckPitch())
self.multisenseChanged = False
self.neckPitchChanged = False
self.updateTimer.start()
class AtlasCommandStream(object):
def __init__(self):
self.timer = TimerCallback(targetFps=10)
#self.timer.disableScheduledTimer()
self.app = ConsoleApp()
self.robotModel, self.jointController = roboturdf.loadRobotModel(
'robot model')
self.fpsCounter = simpletimer.FPSCounter()
self.drakePoseJointNames = robotstate.getDrakePoseJointNames()
self.fpsCounter.printToConsole = True
self.timer.callback = self._tick
self._initialized = False
self.publishChannel = 'JOINT_POSITION_GOAL'
# self.lastCommandMessage = newAtlasCommandMessageAtZero()
self._numPositions = len(robotstate.getDrakePoseJointNames())
self._previousElapsedTime = 100
self._baseFlag = 0
self.jointLimitsMin = np.array([
self.robotModel.model.getJointLimits(jointName)[0]
for jointName in robotstate.getDrakePoseJointNames()
])
self.jointLimitsMax = np.array([
self.robotModel.model.getJointLimits(jointName)[1]
for jointName in robotstate.getDrakePoseJointNames()
])
self.useControllerFlag = False
self.drivingGainsFlag = False
self.applyDefaults()
def initialize(self, currentRobotPose):
assert not self._initialized
self._currentCommandedPose = np.array(currentRobotPose)
self._previousCommandedPose = np.array(currentRobotPose)
self._goalPose = np.array(currentRobotPose)
self._initialized = True
def useController(self):
self.useControllerFlag = True
self.publishChannel = 'QP_CONTROLLER_INPUT'
def setKp(self, Kp, jointName=None):
if jointName is None:
self._Kp = Kp * np.ones(self._numPositions)
else:
idx = robotstate.getDrakePoseJointNames().index(jointName)
self._maxSpeed[idx] = Kp
self.updateKd()
def setMaxSpeed(self, speed, jointName=None):
if jointName is None:
self._maxSpeed = np.deg2rad(speed) * np.ones(self._numPositions)
else:
idx = robotstate.getDrakePoseJointNames().index(jointName)
self._maxSpeed[idx] = np.deg2rad(speed)
def updateKd(self):
self._dampingRatio = 1
self._Kd = 2 * self._dampingRatio * np.sqrt(self._Kp)
def applyDefaults(self):
self.setKp(10)
self.setMaxSpeed(10)
if self.drivingGainsFlag:
self.setMaxSpeed(70, 'l_arm_lwy')
self.setKp(50, 'l_arm_lwy')
self.setMaxSpeed(100, 'l_leg_aky')
self.setKp(100, 'l_leg_aky')
def applyDrivingDefaults(self):
self.drivingGainsFlag = True
self.applyDefaults()
def applyPlanDefaults(self):
self.setKp(10)
self.setMaxSpeed(60)
def startStreaming(self):
assert self._initialized
if not self.timer.isActive():
self.timer.start()
def stopStreaming(self):
self.timer.stop()
def setGoalPose(self, pose):
self._goalPose = self.clipPoseToJointLimits(pose)
def setIndividualJointGoalPose(self, pose, jointName):
jointIdx = self.drakePoseJointNames.index(jointName)
self._goalPose[jointIdx] = pose
def clipPoseToJointLimits(self, pose):
pose = np.array(pose)
pose = np.clip(pose, self.jointLimitsMin, self.jointLimitsMax)
return pose
def waitForRobotState(self):
msg = lcmUtils.captureMessage('EST_ROBOT_STATE', lcmdrc.robot_state_t)
pose = robotstate.convertStateMessageToDrakePose(msg)
pose[:6] = np.zeros(6)
self.initialize(pose)
def _updateAndSendCommandMessage(self):
self._currentCommandedPose = self.clipPoseToJointLimits(
self._currentCommandedPose)
if self._baseFlag:
msg = robotstate.drakePoseToRobotState(self._currentCommandedPose)
else:
msg = drakePoseToAtlasCommand(self._currentCommandedPose)
if self.useControllerFlag:
msg = drakePoseToQPInput(self._currentCommandedPose)
lcmUtils.publish(self.publishChannel, msg)
def _tick(self):
self.fpsCounter.tick()
elapsed = self.timer.elapsed # time since last tick
#nominalElapsed = (1.0 / self.timer.targetFps)
#if elapsed > 2*nominalElapsed:
# elapsed = nominalElapsed
# move current pose toward goal pose
previousPose = self._previousCommandedPose.copy()
currentPose = self._currentCommandedPose.copy()
goalPose = self.clipPoseToJointLimits(self._goalPose.copy())
nextPose = self._computeNextPose(previousPose, currentPose, goalPose,
elapsed, self._previousElapsedTime,
self._maxSpeed)
self._currentCommandedPose = nextPose
# have the base station directly send the command through
if self._baseFlag:
self._currentCommandedPose = goalPose
# publish
self._updateAndSendCommandMessage()
# bookkeeping
self._previousElapsedTime = elapsed
self._previousCommandedPose = currentPose
def _computeNextPose(self, previousPose, currentPose, goalPose, elapsed,
elapsedPrevious, maxSpeed):
v = 1.0 / elapsedPrevious * (currentPose - previousPose)
u = -self._Kp * (currentPose - goalPose) - self._Kd * v # u = -K*x
v_next = v + elapsed * u
v_next = np.clip(v_next, -maxSpeed, maxSpeed) # velocity clamp
nextPose = currentPose + v_next * elapsed
nextPose = self.clipPoseToJointLimits(nextPose)
return nextPose
class PlaybackPanel(object):
def __init__(self, planPlayback, playbackRobotModel,
playbackJointController, robotStateModel,
robotStateJointController, manipPlanner):
self.planPlayback = planPlayback
self.playbackRobotModel = playbackRobotModel
self.playbackJointController = playbackJointController
self.robotStateModel = robotStateModel
self.robotStateJointController = robotStateJointController
self.manipPlanner = manipPlanner
manipPlanner.connectPlanCommitted(self.onPlanCommitted)
manipPlanner.connectUseSupports(self.updateButtonColor)
self.autoPlay = True
self.useOperationColors()
self.planFramesObj = None
self.plan = None
self.poseInterpolator = None
self.startTime = 0.0
self.endTime = 1.0
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 60
self.animationTimer.callback = self.updateAnimation
self.animationClock = SimpleTimer()
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddPlaybackPanel.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
uifile.close()
self.ui = WidgetDict(self.widget.children())
self.ui.viewModeCombo.connect('currentIndexChanged(const QString&)',
self.viewModeChanged)
self.ui.playbackSpeedCombo.connect(
'currentIndexChanged(const QString&)', self.playbackSpeedChanged)
self.ui.interpolationCombo.connect(
'currentIndexChanged(const QString&)', self.interpolationChanged)
self.ui.samplesSpinBox.connect('valueChanged(int)',
self.numberOfSamplesChanged)
self.ui.playbackSlider.connect('valueChanged(int)',
self.playbackSliderValueChanged)
self.ui.animateButton.connect('clicked()', self.animateClicked)
self.ui.hideButton.connect('clicked()', self.hideClicked)
self.ui.executeButton.connect('clicked()', self.executeClicked)
self.ui.executeButton.setShortcut(QtGui.QKeySequence('Ctrl+Return'))
self.ui.stopButton.connect('clicked()', self.stopClicked)
self.ui.executeButton.setContextMenuPolicy(QtCore.Qt.CustomContextMenu)
self.ui.executeButton.connect(
'customContextMenuRequested(const QPoint&)',
self.showExecuteContextMenu)
self.setPlan(None)
self.hideClicked()
def useDevelopmentColors(self):
self.robotStateModelDisplayAlpha = 0.1
self.playbackRobotModelUseTextures = True
self.playbackRobotModelDisplayAlpha = 1
def useOperationColors(self):
self.robotStateModelDisplayAlpha = 1
self.playbackRobotModelUseTextures = False
self.playbackRobotModelDisplayAlpha = 0.5
def showExecuteContextMenu(self, clickPosition):
globalPos = self.ui.executeButton.mapToGlobal(clickPosition)
menu = QtGui.QMenu()
menu.addAction('Visualization Only')
if not self.isPlanFeasible():
menu.addSeparator()
if self.isPlanAPlanWithSupports():
menu.addAction('Execute infeasible plan with supports')
else:
menu.addAction('Execute infeasible plan')
elif self.isPlanAPlanWithSupports():
menu.addSeparator()
menu.addAction('Execute plan with supports')
selectedAction = menu.exec_(globalPos)
if not selectedAction:
return
if selectedAction.text == 'Visualization Only':
self.executePlan(visOnly=True)
elif selectedAction.text == 'Execute infeasible plan':
self.executePlan(overrideInfeasibleCheck=True)
elif selectedAction.text == 'Execute plan with supports':
self.executePlan(overrideSupportsCheck=True)
elif selectedAction.text == 'Execute infeasible plan with supports':
self.executePlan(overrideInfeasibleCheck=True,
overrideSupportsCheck=True)
def getViewMode(self):
return str(self.ui.viewModeCombo.currentText)
def setViewMode(self, mode):
'''
Set the mode of the view widget. input arg: 'continous', 'frames', 'hidden'
e.g. can hide all plan playback with 'hidden'
'''
self.ui.viewModeCombo.setCurrentIndex(
self.ui.viewModeCombo.findText(mode))
def getPlaybackSpeed(self):
s = str(self.ui.playbackSpeedCombo.currentText).replace('x', '')
if '/' in s:
n, d = s.split('/')
return float(n) / float(d)
return float(s)
def getInterpolationMethod(self):
return str(self.ui.interpolationCombo.currentText)
def getNumberOfSamples(self):
return self.ui.samplesSpinBox.value
def viewModeChanged(self):
viewMode = self.getViewMode()
if viewMode == 'continuous':
playbackVisible = True
samplesVisible = False
else:
playbackVisible = False
samplesVisible = True
self.ui.samplesLabel.setEnabled(samplesVisible)
self.ui.samplesSpinBox.setEnabled(samplesVisible)
self.ui.playbackSpeedLabel.setVisible(playbackVisible)
self.ui.playbackSpeedCombo.setVisible(playbackVisible)
self.ui.playbackSlider.setEnabled(playbackVisible)
self.ui.animateButton.setVisible(playbackVisible)
self.ui.timeLabel.setVisible(playbackVisible)
if self.plan:
if self.getViewMode() == 'continuous' and self.autoPlay:
self.startAnimation()
else:
self.ui.playbackSlider.value = 0
self.stopAnimation()
self.updatePlanFrames()
def playbackSpeedChanged(self):
self.planPlayback.playbackSpeed = self.getPlaybackSpeed()
def getPlaybackTime(self):
sliderValue = self.ui.playbackSlider.value
return (sliderValue / 1000.0) * self.endTime
def updateTimeLabel(self):
playbackTime = self.getPlaybackTime()
self.ui.timeLabel.text = 'Time: %.2f s' % playbackTime
def playbackSliderValueChanged(self):
self.updateTimeLabel()
self.showPoseAtTime(self.getPlaybackTime())
def interpolationChanged(self):
methods = {
'linear': 'slinear',
'cubic spline': 'cubic',
'pchip': 'pchip'
}
self.planPlayback.interpolationMethod = methods[
self.getInterpolationMethod()]
self.poseInterpolator = self.planPlayback.getPoseInterpolatorFromPlan(
self.plan)
self.updatePlanFrames()
def numberOfSamplesChanged(self):
self.updatePlanFrames()
def animateClicked(self):
self.startAnimation()
def hideClicked(self):
if self.ui.hideButton.text == 'hide':
self.ui.playbackFrame.setEnabled(False)
self.hidePlan()
self.ui.hideButton.text = 'show'
self.ui.executeButton.setEnabled(False)
if not self.plan:
self.ui.hideButton.setEnabled(False)
else:
self.ui.playbackFrame.setEnabled(True)
self.ui.hideButton.text = 'hide'
self.ui.hideButton.setEnabled(True)
self.ui.executeButton.setEnabled(True)
self.viewModeChanged()
self.updateButtonColor()
def executeClicked(self):
self.executePlan()
def executePlan(self,
visOnly=False,
overrideInfeasibleCheck=False,
overrideSupportsCheck=False):
if visOnly:
_, poses = self.planPlayback.getPlanPoses(self.plan)
self.onPlanCommitted(self.plan)
self.robotStateJointController.setPose('EST_ROBOT_STATE',
poses[-1])
else:
if (self.isPlanFeasible() or overrideInfeasibleCheck) and (
not self.isPlanAPlanWithSupports()
or overrideSupportsCheck):
self.manipPlanner.commitManipPlan(self.plan)
def onPlanCommitted(self, plan):
self.setPlan(None)
self.hideClicked()
def stopClicked(self):
self.stopAnimation()
self.manipPlanner.sendPlanPause()
def isPlanFeasible(self):
plan = robotstate.asRobotPlan(self.plan)
return plan is not None and max(plan.plan_info) < 10
def getPlanInfo(self, plan):
plan = robotstate.asRobotPlan(self.plan)
return max(plan.plan_info)
def isPlanAPlanWithSupports(self):
return hasattr(
self.plan,
'support_sequence') or self.manipPlanner.publishPlansWithSupports
def updatePlanFrames(self):
if self.getViewMode() != 'frames':
return
numberOfSamples = self.getNumberOfSamples()
meshes = self.planPlayback.getPlanPoseMeshes(
self.plan, self.playbackJointController, self.playbackRobotModel,
numberOfSamples)
d = DebugData()
startColor = [0.8, 0.8, 0.8]
endColor = [85 / 255.0, 255 / 255.0, 255 / 255.0]
colorFunc = scipy.interpolate.interp1d([0, numberOfSamples - 1],
[startColor, endColor],
axis=0,
kind='slinear')
for i, mesh in reversed(list(enumerate(meshes))):
d.addPolyData(mesh, color=colorFunc(i))
pd = d.getPolyData()
clean = vtk.vtkCleanPolyData()
clean.SetInput(pd)
clean.Update()
pd = clean.GetOutput()
self.planFramesObj = vis.updatePolyData(d.getPolyData(),
'robot plan',
alpha=1.0,
visible=False,
colorByName='RGB255',
parent='planning')
self.showPlanFrames()
def showPlanFrames(self):
self.planFramesObj.setProperty('Visible', True)
self.robotStateModel.setProperty('Visible', False)
self.playbackRobotModel.setProperty('Visible', False)
def startAnimation(self):
self.showPlaybackModel()
self.stopAnimation()
self.ui.playbackSlider.value = 0
self.animationClock.reset()
self.animationTimer.start()
self.updateAnimation()
def stopAnimation(self):
self.animationTimer.stop()
def showPlaybackModel(self):
self.robotStateModel.setProperty('Visible', True)
self.playbackRobotModel.setProperty('Visible', True)
self.playbackRobotModel.setProperty('Textures',
self.playbackRobotModelUseTextures)
self.robotStateModel.setProperty('Alpha',
self.robotStateModelDisplayAlpha)
self.playbackRobotModel.setProperty(
'Alpha', self.playbackRobotModelDisplayAlpha)
if self.planFramesObj:
self.planFramesObj.setProperty('Visible', False)
def hidePlan(self):
self.stopAnimation()
if self.planFramesObj:
self.planFramesObj.setProperty('Visible', False)
if self.playbackRobotModel:
self.playbackRobotModel.setProperty('Visible', False)
self.robotStateModel.setProperty('Visible', True)
self.robotStateModel.setProperty('Alpha', 1.0)
def showPoseAtTime(self, time):
pose = self.poseInterpolator(time)
self.playbackJointController.setPose('plan_playback', pose)
def updateAnimation(self):
tNow = self.animationClock.elapsed() * self.planPlayback.playbackSpeed
if tNow > self.endTime:
tNow = self.endTime
sliderValue = int(1000.0 * tNow / self.endTime)
self.ui.playbackSlider.blockSignals(True)
self.ui.playbackSlider.value = sliderValue
self.ui.playbackSlider.blockSignals(False)
self.updateTimeLabel()
self.showPoseAtTime(tNow)
return tNow < self.endTime
def updateButtonColor(self):
if self.ui.executeButton.enabled and self.plan and not self.isPlanFeasible(
):
styleSheet = 'background-color:red'
elif self.ui.executeButton.enabled and self.plan and self.isPlanAPlanWithSupports(
):
styleSheet = 'background-color:orange'
else:
styleSheet = ''
self.ui.executeButton.setStyleSheet(styleSheet)
def setPlan(self, plan):
self.ui.playbackSlider.value = 0
self.ui.timeLabel.text = 'Time: 0.00 s'
self.ui.planNameLabel.text = ''
self.plan = plan
self.endTime = 1.0
self.updateButtonColor()
if not self.plan:
return
planText = 'Plan: %d. %.2f seconds' % (
plan.utime, self.planPlayback.getPlanElapsedTime(plan))
self.ui.planNameLabel.text = planText
self.startTime = 0.0
self.endTime = self.planPlayback.getPlanElapsedTime(plan)
self.interpolationChanged()
info = self.getPlanInfo(plan)
app.displaySnoptInfo(info)
if self.ui.hideButton.text == 'show':
self.hideClicked()
else:
self.viewModeChanged()
self.updateButtonColor()
if self.autoPlay and self.widget.parent() is not None:
self.widget.parent().show()
class FootLockEstimator(object):
def __init__(self):
self.footBias = 0.5
self.timer = TimerCallback(targetFps=1.0)
self.timer.callback = self.publishCorrection
self.clear()
def capture(self):
self.pelvisFrame = robotStateModel.getLinkFrame('pelvis')
self.lfootFrame = robotStateModel.getLinkFrame('l_foot')
self.rfootFrame = robotStateModel.getLinkFrame('r_foot')
def clear(self):
self.lfootFrame = None
self.rfootFrame = None
self.pelvisFrame = None
def printCaptured(self):
print '--------------------'
print 'l_foot yaw:', self.lfootFrame.GetOrientation()[2]
print 'r_foot yaw:', self.rfootFrame.GetOrientation()[2]
print 'pelvis yaw:', self.pelvisFrame.GetOrientation()[2]
def getPelvisEstimateFromFoot(self, pelvisFrame, footFrame, previousFootFrame):
pelvisToWorld = pelvisFrame
footToWorld = footFrame
oldFootToWorld = previousFootFrame
worldToFoot = footToWorld.GetLinearInverse()
pelvisToFoot = concat(pelvisToWorld, worldToFoot)
return concat(pelvisToFoot, oldFootToWorld)
def getPelvisEstimate(self):
p = robotStateModel.getLinkFrame('pelvis')
lf = robotStateModel.getLinkFrame('l_foot')
rf = robotStateModel.getLinkFrame('r_foot')
pelvisLeft = self.getPelvisEstimateFromFoot(p, lf, self.lfootFrame)
pelvisRight = self.getPelvisEstimateFromFoot(p, rf, self.rfootFrame)
return transformUtils.frameInterpolate(pelvisLeft, pelvisRight, self.footBias)
def onRobotStateModelChanged(self, model=None):
if self.lfootFrame is None:
return
newPelvisToWorld = self.getPelvisEstimate()
q = robotStateJointController.q.copy()
q[0:3] = newPelvisToWorld.GetPosition()
q[3:6] = transformUtils.rollPitchYawFromTransform(newPelvisToWorld)
playbackJointController.setPose('lock_pose', q)
def initVisualization(self):
robotStateModel.connectModelChanged(self.onRobotStateModelChanged)
playbackRobotModel.setProperty('Visible', True)
def publishCorrection(self, channel='POSE_YAW_LOCK'):
pelvisToWorld = self.getPelvisEstimate()
position, quat = transformUtils.poseFromTransform(pelvisToWorld)
msg = lcmbot.pose_t()
msg.utime = robotStateJointController.lastRobotStateMessage.utime
msg.pos = [0.0, 0.0, 0.0]
msg.orientation = quat.tolist()
lcmUtils.publish(channel, msg)
def enable(self):
self.capture()
self.timer.start()
def disable(self):
self.timer.stop()
self.clear()
class AsyncTaskQueue(object):
QUEUE_STARTED_SIGNAL = 'QUEUE_STARTED_SIGNAL'
QUEUE_STOPPED_SIGNAL = 'QUEUE_STOPPED_SIGNAL'
TASK_STARTED_SIGNAL = 'TASK_STARTED_SIGNAL'
TASK_ENDED_SIGNAL = 'TASK_ENDED_SIGNAL'
TASK_PAUSED_SIGNAL = 'TASK_PAUSED_SIGNAL'
TASK_FAILED_SIGNAL = 'TASK_FAILED_SIGNAL'
TASK_EXCEPTION_SIGNAL = 'TASK_EXCEPTION_SIGNAL'
class PauseException(Exception):
pass
class FailException(Exception):
pass
def __init__(self):
self.tasks = []
self.generators = []
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.callbackLoop
self.callbacks = callbacks.CallbackRegistry([self.QUEUE_STARTED_SIGNAL,
self.QUEUE_STOPPED_SIGNAL,
self.TASK_STARTED_SIGNAL,
self.TASK_ENDED_SIGNAL,
self.TASK_PAUSED_SIGNAL,
self.TASK_FAILED_SIGNAL,
self.TASK_EXCEPTION_SIGNAL])
self.currentTask = None
self.isRunning = False
def reset(self):
assert not self.isRunning
assert not self.generators
self.tasks = []
def start(self):
self.isRunning = True
self.callbacks.process(self.QUEUE_STARTED_SIGNAL, self)
self.timer.start()
def stop(self):
self.isRunning = False
self.currentTask = None
self.generators = []
self.timer.stop()
self.callbacks.process(self.QUEUE_STOPPED_SIGNAL, self)
def wrapGenerator(self, generator):
def generatorWrapper():
return generator
return generatorWrapper
def addTask(self, task):
if isinstance(task, types.GeneratorType):
task = self.wrapGenerator(task)
assert hasattr(task, '__call__')
self.tasks.append(task)
def callbackLoop(self):
try:
for i in xrange(10):
self.doWork()
if not self.tasks:
self.stop()
except AsyncTaskQueue.PauseException:
assert self.currentTask
self.callbacks.process(self.TASK_PAUSED_SIGNAL, self, self.currentTask)
self.stop()
except AsyncTaskQueue.FailException:
assert self.currentTask
self.callbacks.process(self.TASK_FAILED_SIGNAL, self, self.currentTask)
self.stop()
except:
assert self.currentTask
self.callbacks.process(self.TASK_EXCEPTION_SIGNAL, self, self.currentTask)
self.stop()
raise
return self.isRunning
def popTask(self):
assert not self.isRunning
assert not self.currentTask
if self.tasks:
self.tasks.pop(0)
def completePreviousTask(self):
assert self.currentTask
self.tasks.remove(self.currentTask)
self.callbacks.process(self.TASK_ENDED_SIGNAL, self, self.currentTask)
self.currentTask = None
def startNextTask(self):
self.currentTask = self.tasks[0]
self.callbacks.process(self.TASK_STARTED_SIGNAL, self, self.currentTask)
result = self.currentTask()
if isinstance(result, types.GeneratorType):
self.generators.insert(0, result)
def doWork(self):
if self.generators:
self.handleGenerator(self.generators[0])
else:
if self.currentTask:
self.completePreviousTask()
if self.tasks:
self.startNextTask()
def handleGenerator(self, generator):
try:
result = generator.next()
except StopIteration:
self.generators.remove(generator)
else:
if isinstance(result, types.GeneratorType):
self.generators.insert(0, result)
def connectQueueStarted(self, func):
return self.callbacks.connect(self.QUEUE_STARTED_SIGNAL, func)
def disconnectQueueStarted(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectQueueStopped(self, func):
return self.callbacks.connect(self.QUEUE_STOPPED_SIGNAL, func)
def disconnectQueueStopped(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskStarted(self, func):
return self.callbacks.connect(self.TASK_STARTED_SIGNAL, func)
def disconnectTaskStarted(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskEnded(self, func):
return self.callbacks.connect(self.TASK_ENDED_SIGNAL, func)
def disconnectTaskEnded(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskPaused(self, func):
return self.callbacks.connect(self.TASK_PAUSED_SIGNAL, func)
def disconnectTaskPaused(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskFailed(self, func):
return self.callbacks.connect(self.TASK_FAILED_SIGNAL, func)
def disconnectTaskFailed(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskException(self, func):
return self.callbacks.connect(self.TASK_EXCEPTION_SIGNAL, func)
def disconnectTaskException(self, callbackId):
self.callbacks.disconnect(callbackId)
class AffordanceInCameraUpdater(object):
def __init__(self, affordanceManager, imageView):
self.affordanceManager = affordanceManager
self.prependImageName = False
self.projectFootsteps = False
self.projectAffordances = True
self.extraObjects = []
self.imageView = imageView
self.imageQueue = imageView.imageManager.queue
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.update
def getOverlayRenderer(self, imageView):
if not hasattr(imageView, 'overlayRenderer'):
renWin = imageView.view.renderWindow()
renWin.SetNumberOfLayers(2)
ren = vtk.vtkRenderer()
ren.SetLayer(1)
ren.SetActiveCamera(imageView.view.camera())
renWin.AddRenderer(ren)
imageView.overlayRenderer = ren
return imageView.overlayRenderer
def addActorToImageOverlay(self, obj, imageView):
obj.addToView(imageView.view)
imageView.view.renderer().RemoveActor(obj.actor)
renderers = obj.extraViewRenderers.setdefault(imageView.view, [])
overlayRenderer = self.getOverlayRenderer(imageView)
if overlayRenderer not in renderers:
overlayRenderer.AddActor(obj.actor)
renderers.append(overlayRenderer)
def getFolderName(self):
if self.prependImageName:
return self.imageView.imageName + ' camera overlay'
else:
return 'camera overlay'
def setupObjectInCamera(self, obj):
imageView = self.imageView
obj = vis.updatePolyData(vtk.vtkPolyData(), self.getTransformedName(obj), view=imageView.view, color=obj.getProperty('Color'), parent=self.getFolderName(), visible=obj.getProperty('Visible'))
self.addActorToImageOverlay(obj, imageView)
return obj
def getTransformedName(self, obj):
if self.prependImageName:
return 'overlay ' + self.imageView.imageName + ' ' + obj.getProperty('Name')
else:
return 'overlay ' + obj.getProperty('Name')
def getFootsteps(self):
plan = om.findObjectByName('footstep plan')
if plan:
return [child for child in plan.children() if child.getProperty('Name').startswith('step ')]
else:
return []
def getObjectsToUpdate(self):
objs = []
if self.projectAffordances:
objs += self.affordanceManager.getAffordances()
if self.projectFootsteps:
objs += self.getFootsteps()
objs += self.extraObjects
return objs
def getObjectInCamera(self, obj):
overlayObj = om.findObjectByName(self.getTransformedName(obj))
return overlayObj or self.setupObjectInCamera(obj)
def cleanUp(self):
self.timer.stop()
om.removeFromObjectModel(om.findObjectByName(self.getFolderName()))
def update(self):
imageView = self.imageView
if not imageView.imageInitialized:
return
if not imageView.view.isVisible():
return
updated = set()
for obj in self.getObjectsToUpdate():
cameraObj = self.getObjectInCamera(obj)
self.updateObjectInCamera(obj, cameraObj)
updated.add(cameraObj)
folder = om.findObjectByName(self.getFolderName())
if folder:
for child in folder.children():
if child not in updated:
om.removeFromObjectModel(child)
def updateObjectInCamera(self, obj, cameraObj):
imageView = self.imageView
objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform() or vtk.vtkTransform())
localToCameraT = vtk.vtkTransform()
self.imageQueue.getTransform('local', imageView.imageName, localToCameraT)
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(objToLocalT)
t.Concatenate(localToCameraT)
pd = filterUtils.transformPolyData(obj.polyData, t)
'''
normals = pd.GetPointData().GetNormals()
cameraToImageT = vtk.vtkTransform()
imageQueue.getCameraProjectionTransform(imageView.imageName, cameraToImageT)
pd = filterUtils.transformPolyData(pd, cameraToImageT)
pts = vnp.getNumpyFromVtk(pd, 'Points')
pts[:,0] /= pts[:,2]
pts[:,1] /= pts[:,2]
pd.GetPointData().SetNormals(normals)
'''
self.imageQueue.projectPoints(imageView.imageName, pd)
cameraObj.setPolyData(pd)
self.addActorToImageOverlay(cameraObj, imageView)
class AffordanceInCameraUpdater(object):
def __init__(self, affordanceManager, imageView):
self.affordanceManager = affordanceManager
self.prependImageName = False
self.projectFootsteps = False
self.projectAffordances = True
self.extraObjects = []
self.imageView = imageView
self.imageQueue = imageView.imageManager.queue
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.update
def getOverlayRenderer(self, imageView):
if not hasattr(imageView, 'overlayRenderer'):
renWin = imageView.view.renderWindow()
renWin.SetNumberOfLayers(2)
ren = vtk.vtkRenderer()
ren.SetLayer(1)
ren.SetActiveCamera(imageView.view.camera())
renWin.AddRenderer(ren)
imageView.overlayRenderer = ren
return imageView.overlayRenderer
def addActorToImageOverlay(self, obj, imageView):
obj.addToView(imageView.view)
imageView.view.renderer().RemoveActor(obj.actor)
renderers = obj.extraViewRenderers.setdefault(imageView.view, [])
overlayRenderer = self.getOverlayRenderer(imageView)
if overlayRenderer not in renderers:
overlayRenderer.AddActor(obj.actor)
renderers.append(overlayRenderer)
def getFolderName(self):
if self.prependImageName:
return self.imageView.imageName + ' camera overlay'
else:
return 'camera overlay'
def setupObjectInCamera(self, obj):
imageView = self.imageView
obj = vis.updatePolyData(vtk.vtkPolyData(),
self.getTransformedName(obj),
view=imageView.view,
color=obj.getProperty('Color'),
parent=self.getFolderName(),
visible=obj.getProperty('Visible'))
self.addActorToImageOverlay(obj, imageView)
return obj
def getTransformedName(self, obj):
if self.prependImageName:
return 'overlay ' + self.imageView.imageName + ' ' + obj.getProperty(
'Name')
else:
return 'overlay ' + obj.getProperty('Name')
def getFootsteps(self):
plan = om.findObjectByName('footstep plan')
if plan:
return [
child for child in plan.children()
if child.getProperty('Name').startswith('step ')
]
else:
return []
def getObjectsToUpdate(self):
objs = []
if self.projectAffordances:
objs += self.affordanceManager.getAffordances()
if self.projectFootsteps:
objs += self.getFootsteps()
objs += self.extraObjects
return objs
def getObjectInCamera(self, obj):
overlayObj = om.findObjectByName(self.getTransformedName(obj))
return overlayObj or self.setupObjectInCamera(obj)
def cleanUp(self):
self.timer.stop()
om.removeFromObjectModel(om.findObjectByName(self.getFolderName()))
def update(self):
imageView = self.imageView
if not imageView.imageInitialized:
return
if not imageView.view.isVisible():
return
updated = set()
for obj in self.getObjectsToUpdate():
cameraObj = self.getObjectInCamera(obj)
self.updateObjectInCamera(obj, cameraObj)
updated.add(cameraObj)
folder = om.findObjectByName(self.getFolderName())
if folder:
for child in folder.children():
if child not in updated:
om.removeFromObjectModel(child)
def updateObjectInCamera(self, obj, cameraObj):
imageView = self.imageView
objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform()
or vtk.vtkTransform())
localToCameraT = vtk.vtkTransform()
self.imageQueue.getTransform('local', imageView.imageName,
localToCameraT)
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(objToLocalT)
t.Concatenate(localToCameraT)
pd = filterUtils.transformPolyData(obj.polyData, t)
'''
normals = pd.GetPointData().GetNormals()
cameraToImageT = vtk.vtkTransform()
imageQueue.getCameraProjectionTransform(imageView.imageName, cameraToImageT)
pd = filterUtils.transformPolyData(pd, cameraToImageT)
pts = vnp.getNumpyFromVtk(pd, 'Points')
pts[:,0] /= pts[:,2]
pts[:,1] /= pts[:,2]
pd.GetPointData().SetNormals(normals)
'''
self.imageQueue.projectPoints(imageView.imageName, pd)
cameraObj.setPolyData(pd)
self.addActorToImageOverlay(cameraObj, imageView)
class FootLockEstimator(object):
def __init__(self):
self.footBias = 0.5
self.timer = TimerCallback(targetFps=1.0)
self.timer.callback = self.publishCorrection
self.clear()
def capture(self):
self.pelvisFrame = robotStateModel.getLinkFrame('pelvis')
self.lfootFrame = robotStateModel.getLinkFrame('l_foot')
self.rfootFrame = robotStateModel.getLinkFrame('r_foot')
def clear(self):
self.lfootFrame = None
self.rfootFrame = None
self.pelvisFrame = None
def printCaptured(self):
print '--------------------'
print 'l_foot yaw:', self.lfootFrame.GetOrientation()[2]
print 'r_foot yaw:', self.rfootFrame.GetOrientation()[2]
print 'pelvis yaw:', self.pelvisFrame.GetOrientation()[2]
def getPelvisEstimateFromFoot(self, pelvisFrame, footFrame,
previousFootFrame):
pelvisToWorld = pelvisFrame
footToWorld = footFrame
oldFootToWorld = previousFootFrame
worldToFoot = footToWorld.GetLinearInverse()
pelvisToFoot = concat(pelvisToWorld, worldToFoot)
return concat(pelvisToFoot, oldFootToWorld)
def getPelvisEstimate(self):
p = robotStateModel.getLinkFrame('pelvis')
lf = robotStateModel.getLinkFrame('l_foot')
rf = robotStateModel.getLinkFrame('r_foot')
pelvisLeft = self.getPelvisEstimateFromFoot(p, lf, self.lfootFrame)
pelvisRight = self.getPelvisEstimateFromFoot(p, rf, self.rfootFrame)
return transformUtils.frameInterpolate(pelvisLeft, pelvisRight,
self.footBias)
def onRobotStateModelChanged(self, model=None):
if self.lfootFrame is None:
return
newPelvisToWorld = self.getPelvisEstimate()
q = robotStateJointController.q.copy()
q[0:3] = newPelvisToWorld.GetPosition()
q[3:6] = transformUtils.rollPitchYawFromTransform(newPelvisToWorld)
playbackJointController.setPose('lock_pose', q)
def initVisualization(self):
robotStateModel.connectModelChanged(self.onRobotStateModelChanged)
playbackRobotModel.setProperty('Visible', True)
def publishCorrection(self, channel='POSE_YAW_LOCK'):
pelvisToWorld = self.getPelvisEstimate()
position, quat = transformUtils.poseFromTransform(pelvisToWorld)
msg = lcmbot.pose_t()
msg.utime = robotStateJointController.lastRobotStateMessage.utime
msg.pos = [0.0, 0.0, 0.0]
msg.orientation = quat.tolist()
lcmUtils.publish(channel, msg)
def enable(self):
self.capture()
self.timer.start()
def disable(self):
self.timer.stop()
self.clear()
class MultisensePanel(object):
def __init__(self, multisenseDriver, neckDriver):
self.multisenseDriver = multisenseDriver
self.neckDriver = neckDriver
self.neckPitchChanged = False
self.multisenseChanged = False
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddMultisense.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.updateTimer = TimerCallback(targetFps=2)
self.updateTimer.callback = self.updatePanel
self.updateTimer.start()
self.widget.headCamGainSpinner.setEnabled(False)
self.widget.headCamExposureSpinner.setEnabled(False)
#connect the callbacks
self.widget.neckPitchSpinner.valueChanged.connect(self.neckPitchChange)
self.widget.spinRateSpinner.valueChanged.connect(self.spinRateChange)
self.widget.scanDurationSpinner.valueChanged.connect(
self.scanDurationChange)
self.widget.headCamFpsSpinner.valueChanged.connect(
self.headCamFpsChange)
self.widget.headCamGainSpinner.valueChanged.connect(
self.headCamGainChange)
self.widget.headCamExposureSpinner.valueChanged.connect(
self.headCamExposureChange)
self.widget.headAutoGainCheck.clicked.connect(
self.headCamAutoGainChange)
self.widget.ledOnCheck.clicked.connect(self.ledOnCheckChange)
self.widget.ledBrightnessSpinner.valueChanged.connect(
self.ledBrightnessChange)
self.widget.sendButton.clicked.connect(self.sendButtonClicked)
self.updatePanel()
def getCameraFps(self):
return self.widget.headCamFpsSpinner.value
def getCameraGain(self):
return self.widget.headCamGainSpinner.value
def getCameraExposure(self):
return self.widget.headCamExposureSpinner.value
def getCameraLedOn(self):
return self.widget.ledOnCheck.isChecked()
def getCameraLedBrightness(self):
return self.widget.ledBrightnessSpinner.value
def getCameraAutoGain(self):
return self.widget.headAutoGainCheck.isChecked()
def getSpinRate(self):
return self.widget.spinRateSpinner.value
def getScanDuration(self):
return self.widget.scanDurationSpinner.value
def getNeckPitch(self):
return self.widget.neckPitchSpinner.value
def ledBrightnessChange(self, event):
self.multisenseChanged = True
def ledOnCheckChange(self, event):
self.multisenseChanged = True
def headCamExposureChange(self, event):
self.multisenseChanged = True
def headCamAutoGainChange(self, event):
self.multisenseChanged = True
self.widget.headCamGainSpinner.setEnabled(not self.getCameraAutoGain())
self.widget.headCamExposureSpinner.setEnabled(
not self.getCameraAutoGain())
def neckPitchChange(self, event):
self.neckPitchChanged = True
self.updateTimer.stop()
def headCamFpsChange(self, event):
self.multisenseChanged = True
def headCamGainChange(self, event):
self.multisenseChanged = True
def spinRateChange(self, event):
self.multisenseChanged = True
spinRate = self.getSpinRate()
if spinRate == 0.0:
scanDuration = 240.0
else:
scanDuration = abs(60.0 / (spinRate * 2))
if scanDuration > 240.0:
scanDuration = 240.0
self.widget.scanDurationSpinner.blockSignals(True)
self.widget.scanDurationSpinner.value = scanDuration
self.widget.scanDurationSpinner.blockSignals(False)
def scanDurationChange(self, event):
self.multisenseChanged = True
scanDuration = self.getScanDuration()
spinRate = abs(60.0 / (scanDuration * 2))
self.widget.spinRateSpinner.blockSignals(True)
self.widget.spinRateSpinner.value = spinRate
self.widget.spinRateSpinner.blockSignals(False)
def sendButtonClicked(self, event):
self.publishCommand()
def updatePanel(self):
if not self.widget.isVisible():
return
if not self.neckPitchChanged:
self.widget.neckPitchSpinner.blockSignals(True)
self.widget.neckPitchSpinner.setValue(
self.neckDriver.getNeckPitchDegrees())
self.widget.neckPitchSpinner.blockSignals(False)
def publishCommand(self):
fps = self.getCameraFps()
camGain = self.getCameraGain()
exposure = 1000 * self.getCameraExposure()
ledFlash = self.getCameraLedOn()
ledDuty = self.getCameraLedBrightness()
spinRate = self.getSpinRate()
autoGain = 1 if self.getCameraAutoGain() else 0
self.multisenseDriver.sendMultisenseCommand(fps, camGain, exposure,
autoGain, spinRate,
ledFlash, ledDuty)
self.neckDriver.setNeckPitch(self.getNeckPitch())
self.multisenseChanged = False
self.neckPitchChanged = False
self.updateTimer.start()
class Simulator(object):
def __init__(self,
percentObsDensity=20,
endTime=40,
nonRandomWorld=False,
circleRadius=0.7,
worldScale=1.0,
autoInitialize=True,
verbose=True):
self.verbose = verbose
self.startSimTime = time.time()
self.collisionThreshold = 1.3
self.randomSeed = 5
self.Sensor_rayLength = 8
self.percentObsDensity = percentObsDensity
self.defaultControllerTime = 1000
self.nonRandomWorld = nonRandomWorld
self.circleRadius = circleRadius
self.worldScale = worldScale
# create the visualizer object
self.app = ConsoleApp()
self.view = self.app.createView(useGrid=False)
self.initializeOptions()
self.initializeColorMap()
if autoInitialize:
self.initialize()
def initializeOptions(self):
self.options = dict()
self.options['World'] = dict()
self.options['World']['obstaclesInnerFraction'] = 0.85
self.options['World']['randomSeed'] = 40
self.options['World']['percentObsDensity'] = 7.5
self.options['World']['nonRandomWorld'] = True
self.options['World']['circleRadius'] = 1.0
self.options['World']['scale'] = 1.0
self.options['Sensor'] = dict()
self.options['Sensor']['rayLength'] = 10
self.options['Sensor']['numRays'] = 20
self.options['Quad'] = dict()
self.options['Quad']['velocity'] = 18
self.options['dt'] = 0.05
self.options['runTime'] = dict()
self.options['runTime']['defaultControllerTime'] = 10
def setDefaultOptions(self):
defaultOptions = dict()
defaultOptions['World'] = dict()
defaultOptions['World']['obstaclesInnerFraction'] = 0.85
defaultOptions['World']['randomSeed'] = 40
defaultOptions['World']['percentObsDensity'] = 7.5
defaultOptions['World']['nonRandomWorld'] = True
defaultOptions['World']['circleRadius'] = 1.75
defaultOptions['World']['scale'] = 1.0
defaultOptions['Sensor'] = dict()
defaultOptions['Sensor']['rayLength'] = 10
defaultOptions['Sensor']['numRays'] = 20
defaultOptions['Quad'] = dict()
defaultOptions['Quad']['velocity'] = 18
defaultOptions['dt'] = 0.05
defaultOptions['runTime'] = dict()
defaultOptions['runTime']['defaultControllerTime'] = 10
for k in defaultOptions:
self.options.setdefault(k, defaultOptions[k])
for k in defaultOptions:
if not isinstance(defaultOptions[k], dict):
continue
for j in defaultOptions[k]:
self.options[k].setdefault(j, defaultOptions[k][j])
def initializeColorMap(self):
self.colorMap = dict()
self.colorMap['default'] = [0, 1, 0]
def initialize(self):
self.dt = self.options['dt']
self.controllerTypeOrder = ['default']
self.setDefaultOptions()
self.Sensor = SensorObj(rayLength=self.options['Sensor']['rayLength'],
numRays=self.options['Sensor']['numRays'])
self.Controller = ControllerObj(self.Sensor)
self.Quad = QuadPlant(controller=self.Controller,
velocity=self.options['Quad']['velocity'])
# create the things needed for simulation
om.removeFromObjectModel(om.findObjectByName('world'))
self.world = World.buildWarehouseWorld(
percentObsDensity=self.options['World']['percentObsDensity'],
circleRadius=self.options['World']['circleRadius'],
nonRandom=self.options['World']['nonRandomWorld'],
scale=self.options['World']['scale'],
randomSeed=self.options['World']['randomSeed'],
obstaclesInnerFraction=self.options['World']
['obstaclesInnerFraction'])
om.removeFromObjectModel(om.findObjectByName('robot'))
self.robot, self.frame = World.buildRobot()
self.locator = World.buildCellLocator(self.world.visObj.polyData)
self.Sensor.setLocator(self.locator)
self.frame = self.robot.getChildFrame()
self.frame.setProperty('Scale', 3)
self.frame.setProperty('Edit', True)
self.frame.widget.HandleRotationEnabledOff()
rep = self.frame.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
self.defaultControllerTime = self.options['runTime'][
'defaultControllerTime']
self.Quad.setFrame(self.frame)
print "Finished initialization"
def runSingleSimulation(self,
controllerType='default',
simulationCutoff=None):
self.setCollisionFreeInitialState()
currentQuadState = np.copy(self.Quad.state)
nextQuadState = np.copy(self.Quad.state)
self.setRobotFrameState(currentQuadState[0], currentQuadState[1],
currentQuadState[2])
currentRaycast = self.Sensor.raycastAll(self.frame)
nextRaycast = np.zeros(self.Sensor.numRays)
# record the reward data
runData = dict()
startIdx = self.counter
while (self.counter < self.numTimesteps - 1):
idx = self.counter
currentTime = self.t[idx]
self.stateOverTime[idx, :] = currentQuadState
x = self.stateOverTime[idx, 0]
y = self.stateOverTime[idx, 1]
theta = self.stateOverTime[idx, 2]
self.setRobotFrameState(x, y, theta)
# self.setRobotState(currentQuadState[0], currentQuadState[1], currentQuadState[2])
currentRaycast = self.Sensor.raycastAll(self.frame)
self.raycastData[idx, :] = currentRaycast
S_current = (currentQuadState, 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(
currentQuadState,
currentTime,
self.frame,
raycastDistance=currentRaycast,
randomize=False)
self.controlInputData[idx] = controlInput
nextQuadState = self.Quad.simulateOneStep(
controlInput=controlInput, dt=self.dt)
# want to compute nextRaycast so we can do the SARSA algorithm
x = nextQuadState[0]
y = nextQuadState[1]
theta = nextQuadState[2]
self.setRobotFrameState(x, y, theta)
nextRaycast = self.Sensor.raycastAll(self.frame)
# Compute the next control input
S_next = (nextQuadState, nextRaycast)
if controllerType in ["default", "defaultRandom"]:
nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(
nextQuadState,
currentTime,
self.frame,
raycastDistance=nextRaycast,
randomize=False)
#bookkeeping
currentQuadState = nextQuadState
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 self.counter >= simulationCutoff:
break
# fill in the last state by hand
self.stateOverTime[self.counter, :] = currentQuadState
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
def setNumpyRandomSeed(self, seed=1):
np.random.seed(seed)
def runBatchSimulation(self, endTime=None, dt=0.05):
# for use in playback
self.dt = self.options['dt']
self.endTime = self.defaultControllerTime # used to be the sum of the other times as well
self.t = np.arange(0.0, self.endTime, dt)
maxNumTimesteps = np.size(self.t)
self.stateOverTime = np.zeros((maxNumTimesteps + 1, 3))
self.raycastData = np.zeros((maxNumTimesteps + 1, self.Sensor.numRays))
self.controlInputData = np.zeros(maxNumTimesteps + 1)
self.numTimesteps = maxNumTimesteps
self.controllerTypeOrder = ['default']
self.counter = 0
self.simulationData = []
self.initializeStatusBar()
self.idxDict = dict()
numRunsCounter = 0
self.idxDict['default'] = self.counter
loopStartIdx = self.counter
simCutoff = min(loopStartIdx + self.defaultControllerTime / dt,
self.numTimesteps)
while ((self.counter - loopStartIdx < self.defaultControllerTime / dt)
and self.counter < self.numTimesteps - 1):
self.printStatusBar()
startIdx = self.counter
runData = self.runSingleSimulation(controllerType='default',
simulationCutoff=simCutoff)
runData['startIdx'] = startIdx
runData['controllerType'] = "default"
runData['duration'] = self.counter - runData['startIdx']
runData['endIdx'] = self.counter
runData['runNumber'] = numRunsCounter
numRunsCounter += 1
self.simulationData.append(runData)
# BOOKKEEPING
# truncate stateOverTime, raycastData, controlInputs to be the correct size
self.numTimesteps = self.counter + 1
self.stateOverTime = self.stateOverTime[0:self.counter + 1, :]
self.raycastData = self.raycastData[0:self.counter + 1, :]
self.controlInputData = self.controlInputData[0:self.counter + 1]
self.endTime = 1.0 * self.counter / self.numTimesteps * self.endTime
def initializeStatusBar(self):
self.numTicks = 10
self.nextTickComplete = 1.0 / float(self.numTicks)
self.nextTickIdx = 1
print "Simulation percentage complete: (", self.numTicks, " # is complete)"
def printStatusBar(self):
fractionDone = float(self.counter) / float(self.numTimesteps)
if fractionDone > self.nextTickComplete:
self.nextTickIdx += 1
self.nextTickComplete += 1.0 / self.numTicks
timeSoFar = time.time() - self.startSimTime
estimatedTimeLeft_sec = (1 -
fractionDone) * timeSoFar / fractionDone
estimatedTimeLeft_minutes = estimatedTimeLeft_sec / 60.0
print "#" * self.nextTickIdx, "-" * (
self.numTicks - self.nextTickIdx
), "estimated", estimatedTimeLeft_minutes, "minutes left"
def setCollisionFreeInitialState(self):
tol = 5
while True:
x = 0.0
y = -5.0
theta = 0 #+ np.random.uniform(0,2*np.pi,1)[0] * 0.01
self.Quad.setQuadState(x, y, theta)
self.setRobotFrameState(x, y, theta)
print "In loop"
if not self.checkInCollision():
break
return x, y, theta
def setRandomCollisionFreeInitialState(self):
tol = 5
while True:
x = np.random.uniform(self.world.Xmin + tol, self.world.Xmax - tol,
1)[0]
y = np.random.uniform(self.world.Ymin + tol, self.world.Ymax - tol,
1)[0]
theta = np.random.uniform(0, 2 * np.pi, 1)[0]
self.Quad.setQuadState(x, y, theta)
self.setRobotFrameState(x, y, theta)
if not self.checkInCollision():
break
return x, y, theta
def setupPlayback(self):
self.timer = TimerCallback(targetFps=30)
self.timer.callback = self.tick
playButtonFps = 1.0 / self.dt
print "playButtonFPS", playButtonFps
self.playTimer = TimerCallback(targetFps=playButtonFps)
self.playTimer.callback = self.playTimerCallback
self.sliderMovedByPlayTimer = False
panel = QtGui.QWidget()
l = QtGui.QHBoxLayout(panel)
playButton = QtGui.QPushButton('Play/Pause')
playButton.connect('clicked()', self.onPlayButton)
slider = QtGui.QSlider(QtCore.Qt.Horizontal)
slider.connect('valueChanged(int)', self.onSliderChanged)
self.sliderMax = self.numTimesteps
slider.setMaximum(self.sliderMax)
self.slider = slider
l.addWidget(playButton)
l.addWidget(slider)
w = QtGui.QWidget()
l = QtGui.QVBoxLayout(w)
l.addWidget(self.view)
l.addWidget(panel)
w.showMaximized()
self.frame.connectFrameModified(self.updateDrawIntersection)
self.updateDrawIntersection(self.frame)
applogic.resetCamera(viewDirection=[0.2, 0, -1])
self.view.showMaximized()
self.view.raise_()
panel = screengrabberpanel.ScreenGrabberPanel(self.view)
panel.widget.show()
elapsed = time.time() - self.startSimTime
simRate = self.counter / elapsed
print "Total run time", elapsed
print "Ticks (Hz)", simRate
print "Number of steps taken", self.counter
self.app.start()
def run(self, launchApp=True):
self.counter = 1
self.runBatchSimulation()
# self.Sarsa.plotWeights()
if launchApp:
self.setupPlayback()
def updateDrawIntersection(self, frame):
origin = np.array(frame.transform.GetPosition())
#print "origin is now at", origin
d = DebugData()
sliderIdx = self.slider.value
controllerType = self.getControllerTypeFromCounter(sliderIdx)
colorMaxRange = self.colorMap[controllerType]
for j in xrange(self.Sensor.numLayers):
for i in xrange(self.Sensor.numRays):
ray = self.Sensor.rays[:, i, j]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
#print "rayTransformed is", rayTransformed
intersection = self.Sensor.raycast(
self.locator, origin,
origin + rayTransformed * self.Sensor.rayLength)
if intersection is not None:
d.addLine(origin, intersection, color=[1, 0, 0])
else:
d.addLine(origin,
origin + rayTransformed * self.Sensor.rayLength,
color=colorMaxRange)
vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
#camera = self.view.camera()
#camera.SetFocalPoint(frame.transform.GetPosition())
#camera.SetPosition(frame.transform.TransformPoint((-30,0,10)))
def getControllerTypeFromCounter(self, counter):
name = self.controllerTypeOrder[0]
for controllerType in self.controllerTypeOrder[1:]:
if counter >= self.idxDict[controllerType]:
name = controllerType
return name
def setRobotFrameState(self, x, y, theta):
t = vtk.vtkTransform()
t.Translate(x, y, 0.0)
t.RotateZ(np.degrees(theta))
self.robot.getChildFrame().copyFrame(t)
# returns true if we are in collision
def checkInCollision(self, raycastDistance=None):
if raycastDistance is None:
self.setRobotFrameState(self.Quad.state[0], self.Quad.state[1],
self.Quad.state[2])
raycastDistance = self.Sensor.raycastAll(self.frame)
if np.min(raycastDistance) < self.collisionThreshold:
return True
else:
return False
def tick(self):
#print timer.elapsed
#simulate(t.elapsed)
x = np.sin(time.time())
y = np.cos(time.time())
self.setRobotFrameState(x, y, 0.0)
if (time.time() - self.playTime) > self.endTime:
self.playTimer.stop()
def tick2(self):
newtime = time.time() - self.playTime
print time.time() - self.playTime
x = np.sin(newtime)
y = np.cos(newtime)
self.setRobotFrameState(x, y, 0.0)
# just increment the slider, stop the timer if we get to the end
def playTimerCallback(self):
self.sliderMovedByPlayTimer = True
currentIdx = self.slider.value
nextIdx = currentIdx + 1
self.slider.setSliderPosition(nextIdx)
if currentIdx >= self.sliderMax:
print "reached end of tape, stopping playTime"
self.playTimer.stop()
def onSliderChanged(self, value):
if not self.sliderMovedByPlayTimer:
self.playTimer.stop()
numSteps = len(self.stateOverTime)
idx = int(np.floor(numSteps * (1.0 * value / self.sliderMax)))
idx = min(idx, numSteps - 1)
x, y, theta = self.stateOverTime[idx]
self.setRobotFrameState(x, y, theta)
self.sliderMovedByPlayTimer = False
def onPlayButton(self):
if self.playTimer.isActive():
self.onPauseButton()
return
print 'play'
self.playTimer.start()
self.playTime = time.time()
def onPauseButton(self):
print 'pause'
self.playTimer.stop()
def saveToFile(self, filename):
# should also save the run data if it is available, i.e. stateOverTime, rewardOverTime
filename = 'data/' + filename + ".out"
my_shelf = shelve.open(filename, 'n')
my_shelf['options'] = self.options
my_shelf['simulationData'] = self.simulationData
my_shelf['stateOverTime'] = self.stateOverTime
my_shelf['raycastData'] = self.raycastData
my_shelf['controlInputData'] = self.controlInputData
my_shelf['numTimesteps'] = self.numTimesteps
my_shelf['idxDict'] = self.idxDict
my_shelf['counter'] = self.counter
my_shelf.close()
@staticmethod
def loadFromFile(filename):
filename = 'data/' + filename + ".out"
sim = Simulator(autoInitialize=False, verbose=False)
my_shelf = shelve.open(filename)
sim.options = my_shelf['options']
sim.initialize()
sim.simulationData = my_shelf['simulationData']
sim.stateOverTime = np.array(my_shelf['stateOverTime'])
sim.raycastData = np.array(my_shelf['raycastData'])
sim.controlInputData = np.array(my_shelf['controlInputData'])
sim.numTimesteps = my_shelf['numTimesteps']
sim.idxDict = my_shelf['idxDict']
sim.counter = my_shelf['counter']
my_shelf.close()
return sim
class PlaybackPanel(object):
def __init__(self, planPlayback, playbackRobotModel, playbackJointController, robotStateModel, robotStateJointController, manipPlanner):
self.planPlayback = planPlayback
self.playbackRobotModel = playbackRobotModel
self.playbackJointController = playbackJointController
self.robotStateModel = robotStateModel
self.robotStateJointController = robotStateJointController
self.manipPlanner = manipPlanner
manipPlanner.connectPlanCommitted(self.onPlanCommitted)
manipPlanner.connectUseSupports(self.updateButtonColor)
self.autoPlay = True
self.useOperationColors()
self.planFramesObj = None
self.plan = None
self.poseInterpolator = None
self.startTime = 0.0
self.endTime = 1.0
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 60
self.animationTimer.callback = self.updateAnimation
self.animationClock = SimpleTimer()
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddPlaybackPanel.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
uifile.close()
self.ui = WidgetDict(self.widget.children())
self.ui.viewModeCombo.connect('currentIndexChanged(const QString&)', self.viewModeChanged)
self.ui.playbackSpeedCombo.connect('currentIndexChanged(const QString&)', self.playbackSpeedChanged)
self.ui.interpolationCombo.connect('currentIndexChanged(const QString&)', self.interpolationChanged)
self.ui.samplesSpinBox.connect('valueChanged(int)', self.numberOfSamplesChanged)
self.ui.playbackSlider.connect('valueChanged(int)', self.playbackSliderValueChanged)
self.ui.animateButton.connect('clicked()', self.animateClicked)
self.ui.hideButton.connect('clicked()', self.hideClicked)
self.ui.executeButton.connect('clicked()', self.executeClicked)
self.ui.executeButton.setShortcut(QtGui.QKeySequence('Ctrl+Return'))
self.ui.stopButton.connect('clicked()', self.stopClicked)
self.ui.executeButton.setContextMenuPolicy(QtCore.Qt.CustomContextMenu)
self.ui.executeButton.connect('customContextMenuRequested(const QPoint&)', self.showExecuteContextMenu)
self.setPlan(None)
self.hideClicked()
def useDevelopmentColors(self):
self.robotStateModelDisplayAlpha = 0.1
self.playbackRobotModelUseTextures = True
self.playbackRobotModelDisplayAlpha = 1
def useOperationColors(self):
self.robotStateModelDisplayAlpha = 1
self.playbackRobotModelUseTextures = False
self.playbackRobotModelDisplayAlpha = 0.5
def showExecuteContextMenu(self, clickPosition):
globalPos = self.ui.executeButton.mapToGlobal(clickPosition)
menu = QtGui.QMenu()
menu.addAction('Visualization Only')
if not self.isPlanFeasible():
menu.addSeparator()
if self.isPlanAPlanWithSupports():
menu.addAction('Execute infeasible plan with supports')
else:
menu.addAction('Execute infeasible plan')
elif self.isPlanAPlanWithSupports():
menu.addSeparator()
menu.addAction('Execute plan with supports')
selectedAction = menu.exec_(globalPos)
if not selectedAction:
return
if selectedAction.text == 'Visualization Only':
self.executePlan(visOnly=True)
elif selectedAction.text == 'Execute infeasible plan':
self.executePlan(overrideInfeasibleCheck=True)
elif selectedAction.text == 'Execute plan with supports':
self.executePlan(overrideSupportsCheck=True)
elif selectedAction.text == 'Execute infeasible plan with supports':
self.executePlan(overrideInfeasibleCheck=True, overrideSupportsCheck=True)
def getViewMode(self):
return str(self.ui.viewModeCombo.currentText)
def setViewMode(self, mode):
'''
Set the mode of the view widget. input arg: 'continous', 'frames', 'hidden'
e.g. can hide all plan playback with 'hidden'
'''
self.ui.viewModeCombo.setCurrentIndex(self.ui.viewModeCombo.findText(mode))
def getPlaybackSpeed(self):
s = str(self.ui.playbackSpeedCombo.currentText).replace('x', '')
if '/' in s:
n, d = s.split('/')
return float(n)/float(d)
return float(s)
def getInterpolationMethod(self):
return str(self.ui.interpolationCombo.currentText)
def getNumberOfSamples(self):
return self.ui.samplesSpinBox.value
def viewModeChanged(self):
viewMode = self.getViewMode()
if viewMode == 'continuous':
playbackVisible = True
samplesVisible = False
else:
playbackVisible = False
samplesVisible = True
self.ui.samplesLabel.setEnabled(samplesVisible)
self.ui.samplesSpinBox.setEnabled(samplesVisible)
self.ui.playbackSpeedLabel.setVisible(playbackVisible)
self.ui.playbackSpeedCombo.setVisible(playbackVisible)
self.ui.playbackSlider.setEnabled(playbackVisible)
self.ui.animateButton.setVisible(playbackVisible)
self.ui.timeLabel.setVisible(playbackVisible)
if self.plan:
if self.getViewMode() == 'continuous' and self.autoPlay:
self.startAnimation()
else:
self.ui.playbackSlider.value = 0
self.stopAnimation()
self.updatePlanFrames()
def playbackSpeedChanged(self):
self.planPlayback.playbackSpeed = self.getPlaybackSpeed()
def getPlaybackTime(self):
sliderValue = self.ui.playbackSlider.value
return (sliderValue / 1000.0) * self.endTime
def updateTimeLabel(self):
playbackTime = self.getPlaybackTime()
self.ui.timeLabel.text = 'Time: %.2f s' % playbackTime
def playbackSliderValueChanged(self):
self.updateTimeLabel()
self.showPoseAtTime(self.getPlaybackTime())
def interpolationChanged(self):
methods = {'linear' : 'slinear',
'cubic spline' : 'cubic',
'pchip' : 'pchip' }
self.planPlayback.interpolationMethod = methods[self.getInterpolationMethod()]
self.poseInterpolator = self.planPlayback.getPoseInterpolatorFromPlan(self.plan)
self.updatePlanFrames()
def numberOfSamplesChanged(self):
self.updatePlanFrames()
def animateClicked(self):
self.startAnimation()
def hideClicked(self):
if self.ui.hideButton.text == 'hide':
self.ui.playbackFrame.setEnabled(False)
self.hidePlan()
self.ui.hideButton.text = 'show'
self.ui.executeButton.setEnabled(False)
if not self.plan:
self.ui.hideButton.setEnabled(False)
else:
self.ui.playbackFrame.setEnabled(True)
self.ui.hideButton.text = 'hide'
self.ui.hideButton.setEnabled(True)
self.ui.executeButton.setEnabled(True)
self.viewModeChanged()
self.updateButtonColor()
def executeClicked(self):
self.executePlan()
def executePlan(self, visOnly=False, overrideInfeasibleCheck=False, overrideSupportsCheck=False):
if visOnly:
_, poses = self.planPlayback.getPlanPoses(self.plan)
self.onPlanCommitted(self.plan)
self.robotStateJointController.setPose('EST_ROBOT_STATE', poses[-1])
else:
if (self.isPlanFeasible() or overrideInfeasibleCheck) and (not self.isPlanAPlanWithSupports() or overrideSupportsCheck):
self.manipPlanner.commitManipPlan(self.plan)
def onPlanCommitted(self, plan):
self.setPlan(None)
self.hideClicked()
def stopClicked(self):
self.stopAnimation()
self.manipPlanner.sendPlanPause()
def isPlanFeasible(self):
plan = robotstate.asRobotPlan(self.plan)
return plan is not None and max(plan.plan_info) < 10
def getPlanInfo(self, plan):
plan = robotstate.asRobotPlan(self.plan)
return max(plan.plan_info)
def isPlanAPlanWithSupports(self):
return hasattr(self.plan, 'support_sequence') or self.manipPlanner.publishPlansWithSupports
def updatePlanFrames(self):
if self.getViewMode() != 'frames':
return
numberOfSamples = self.getNumberOfSamples()
meshes = self.planPlayback.getPlanPoseMeshes(self.plan, self.playbackJointController, self.playbackRobotModel, numberOfSamples)
d = DebugData()
startColor = [0.8, 0.8, 0.8]
endColor = [85/255.0, 255/255.0, 255/255.0]
colorFunc = scipy.interpolate.interp1d([0, numberOfSamples-1], [startColor, endColor], axis=0, kind='slinear')
for i, mesh in reversed(list(enumerate(meshes))):
d.addPolyData(mesh, color=colorFunc(i))
pd = d.getPolyData()
clean = vtk.vtkCleanPolyData()
clean.SetInput(pd)
clean.Update()
pd = clean.GetOutput()
self.planFramesObj = vis.updatePolyData(d.getPolyData(), 'robot plan', alpha=1.0, visible=False, colorByName='RGB255', parent='planning')
self.showPlanFrames()
def showPlanFrames(self):
self.planFramesObj.setProperty('Visible', True)
self.robotStateModel.setProperty('Visible', False)
self.playbackRobotModel.setProperty('Visible', False)
def startAnimation(self):
self.showPlaybackModel()
self.stopAnimation()
self.ui.playbackSlider.value = 0
self.animationClock.reset()
self.animationTimer.start()
self.updateAnimation()
def stopAnimation(self):
self.animationTimer.stop()
def showPlaybackModel(self):
self.robotStateModel.setProperty('Visible', True)
self.playbackRobotModel.setProperty('Visible', True)
self.playbackRobotModel.setProperty('Textures', self.playbackRobotModelUseTextures)
self.robotStateModel.setProperty('Alpha', self.robotStateModelDisplayAlpha)
self.playbackRobotModel.setProperty('Alpha', self.playbackRobotModelDisplayAlpha)
if self.planFramesObj:
self.planFramesObj.setProperty('Visible', False)
def hidePlan(self):
self.stopAnimation()
if self.planFramesObj:
self.planFramesObj.setProperty('Visible', False)
if self.playbackRobotModel:
self.playbackRobotModel.setProperty('Visible', False)
self.robotStateModel.setProperty('Visible', True)
self.robotStateModel.setProperty('Alpha', 1.0)
def showPoseAtTime(self, time):
pose = self.poseInterpolator(time)
self.playbackJointController.setPose('plan_playback', pose)
def updateAnimation(self):
tNow = self.animationClock.elapsed() * self.planPlayback.playbackSpeed
if tNow > self.endTime:
tNow = self.endTime
sliderValue = int(1000.0 * tNow / self.endTime)
self.ui.playbackSlider.blockSignals(True)
self.ui.playbackSlider.value = sliderValue
self.ui.playbackSlider.blockSignals(False)
self.updateTimeLabel()
self.showPoseAtTime(tNow)
return tNow < self.endTime
def updateButtonColor(self):
if self.ui.executeButton.enabled and self.plan and not self.isPlanFeasible():
styleSheet = 'background-color:red'
elif self.ui.executeButton.enabled and self.plan and self.isPlanAPlanWithSupports():
styleSheet = 'background-color:orange'
else:
styleSheet = ''
self.ui.executeButton.setStyleSheet(styleSheet)
def setPlan(self, plan):
self.ui.playbackSlider.value = 0
self.ui.timeLabel.text = 'Time: 0.00 s'
self.ui.planNameLabel.text = ''
self.plan = plan
self.endTime = 1.0
self.updateButtonColor()
if not self.plan:
return
planText = 'Plan: %d. %.2f seconds' % (plan.utime, self.planPlayback.getPlanElapsedTime(plan))
self.ui.planNameLabel.text = planText
self.startTime = 0.0
self.endTime = self.planPlayback.getPlanElapsedTime(plan)
self.interpolationChanged()
info = self.getPlanInfo(plan)
app.displaySnoptInfo(info)
if self.ui.hideButton.text == 'show':
self.hideClicked()
else:
self.viewModeChanged()
self.updateButtonColor()
if self.autoPlay and self.widget.parent() is not None:
self.widget.parent().show()
class Simulator(object):
def __init__(self,
percentObsDensity=20,
endTime=40,
randomizeControl=False,
nonRandomWorld=False,
circleRadius=0.7,
worldScale=1.0,
supervisedTrainingTime=500,
autoInitialize=True,
verbose=True,
sarsaType="discrete"):
self.verbose = verbose
self.randomizeControl = randomizeControl
self.startSimTime = time.time()
self.collisionThreshold = 1.3
self.randomSeed = 5
self.Sarsa_numInnerBins = 4
self.Sarsa_numOuterBins = 4
self.Sensor_rayLength = 8
self.sarsaType = sarsaType
self.percentObsDensity = percentObsDensity
self.supervisedTrainingTime = 10
self.learningRandomTime = 10
self.learningEvalTime = 10
self.defaultControllerTime = 10
self.nonRandomWorld = nonRandomWorld
self.circleRadius = circleRadius
self.worldScale = worldScale
# create the visualizer object
self.app = ConsoleApp()
# view = app.createView(useGrid=False)
self.view = self.app.createView(useGrid=False)
self.initializeOptions()
self.initializeColorMap()
if autoInitialize:
self.initialize()
def initializeOptions(self):
self.options = dict()
self.options['Reward'] = dict()
self.options['Reward']['actionCost'] = 0.1
self.options['Reward']['collisionPenalty'] = 100.0
self.options['Reward']['raycastCost'] = 20.0
self.options['SARSA'] = dict()
self.options['SARSA']['type'] = "discrete"
self.options['SARSA']['lam'] = 0.7
self.options['SARSA']['alphaStepSize'] = 0.2
self.options['SARSA']['useQLearningUpdate'] = False
self.options['SARSA']['epsilonGreedy'] = 0.2
self.options['SARSA']['burnInTime'] = 500
self.options['SARSA']['epsilonGreedyExponent'] = 0.3
self.options['SARSA'][
'exponentialDiscountFactor'] = 0.05 #so gamma = e^(-rho*dt)
self.options['SARSA']['numInnerBins'] = 5
self.options['SARSA']['numOuterBins'] = 4
self.options['SARSA']['binCutoff'] = 0.5
self.options['World'] = dict()
self.options['World']['obstaclesInnerFraction'] = 0.85
self.options['World']['randomSeed'] = 40
self.options['World']['percentObsDensity'] = 7.5
self.options['World']['nonRandomWorld'] = True
self.options['World']['circleRadius'] = 1.75
self.options['World']['scale'] = 1.0
self.options['Sensor'] = dict()
self.options['Sensor']['rayLength'] = 10
self.options['Sensor']['numRays'] = 20
self.options['Car'] = dict()
self.options['Car']['velocity'] = 12
self.options['dt'] = 0.05
self.options['runTime'] = dict()
self.options['runTime']['supervisedTrainingTime'] = 10
self.options['runTime']['learningRandomTime'] = 10
self.options['runTime']['learningEvalTime'] = 10
self.options['runTime']['defaultControllerTime'] = 10
def setDefaultOptions(self):
defaultOptions = dict()
defaultOptions['Reward'] = dict()
defaultOptions['Reward']['actionCost'] = 0.1
defaultOptions['Reward']['collisionPenalty'] = 100.0
defaultOptions['Reward']['raycastCost'] = 20.0
defaultOptions['SARSA'] = dict()
defaultOptions['SARSA']['type'] = "discrete"
defaultOptions['SARSA']['lam'] = 0.7
defaultOptions['SARSA']['alphaStepSize'] = 0.2
defaultOptions['SARSA']['useQLearningUpdate'] = False
defaultOptions['SARSA']['useSupervisedTraining'] = True
defaultOptions['SARSA']['epsilonGreedy'] = 0.2
defaultOptions['SARSA']['burnInTime'] = 500
defaultOptions['SARSA']['epsilonGreedyExponent'] = 0.3
defaultOptions['SARSA'][
'exponentialDiscountFactor'] = 0.05 #so gamma = e^(-rho*dt)
defaultOptions['SARSA']['numInnerBins'] = 5
defaultOptions['SARSA']['numOuterBins'] = 4
defaultOptions['SARSA']['binCutoff'] = 0.5
defaultOptions['SARSA']['forceDriveStraight'] = True
defaultOptions['World'] = dict()
defaultOptions['World']['obstaclesInnerFraction'] = 0.85
defaultOptions['World']['randomSeed'] = 40
defaultOptions['World']['percentObsDensity'] = 7.5
defaultOptions['World']['nonRandomWorld'] = True
defaultOptions['World']['circleRadius'] = 1.75
defaultOptions['World']['scale'] = 1.0
defaultOptions['Sensor'] = dict()
defaultOptions['Sensor']['rayLength'] = 10
defaultOptions['Sensor']['numRays'] = 20
defaultOptions['Car'] = dict()
defaultOptions['Car']['velocity'] = 12
defaultOptions['dt'] = 0.05
defaultOptions['runTime'] = dict()
defaultOptions['runTime']['supervisedTrainingTime'] = 10
defaultOptions['runTime']['learningRandomTime'] = 10
defaultOptions['runTime']['learningEvalTime'] = 10
defaultOptions['runTime']['defaultControllerTime'] = 10
for k in defaultOptions:
self.options.setdefault(k, defaultOptions[k])
for k in defaultOptions:
if not isinstance(defaultOptions[k], dict):
continue
for j in defaultOptions[k]:
self.options[k].setdefault(j, defaultOptions[k][j])
def initializeColorMap(self):
self.colorMap = dict()
self.colorMap['defaultRandom'] = [0, 0, 1]
self.colorMap['learnedRandom'] = [1.0, 0.54901961,
0.] # this is orange
self.colorMap['learned'] = [0.58039216, 0.0,
0.82745098] # this is yellow
self.colorMap['default'] = [0, 1, 0]
def initialize(self):
self.dt = self.options['dt']
self.controllerTypeOrder = [
'defaultRandom', 'learnedRandom', 'learned', 'default'
]
self.setDefaultOptions()
self.Sensor = SensorObj(rayLength=self.options['Sensor']['rayLength'],
numRays=self.options['Sensor']['numRays'])
self.Controller = ControllerObj(self.Sensor)
self.Car = CarPlant(controller=self.Controller,
velocity=self.options['Car']['velocity'])
self.Reward = Reward(
self.Sensor,
collisionThreshold=self.collisionThreshold,
actionCost=self.options['Reward']['actionCost'],
collisionPenalty=self.options['Reward']['collisionPenalty'],
raycastCost=self.options['Reward']['raycastCost'])
self.setSARSA()
# create the things needed for simulation
om.removeFromObjectModel(om.findObjectByName('world'))
self.world = World.buildCircleWorld(
percentObsDensity=self.options['World']['percentObsDensity'],
circleRadius=self.options['World']['circleRadius'],
nonRandom=self.options['World']['nonRandomWorld'],
scale=self.options['World']['scale'],
randomSeed=self.options['World']['randomSeed'],
obstaclesInnerFraction=self.options['World']
['obstaclesInnerFraction'])
om.removeFromObjectModel(om.findObjectByName('robot'))
self.robot, self.frame = World.buildRobot()
self.locator = World.buildCellLocator(self.world.visObj.polyData)
self.Sensor.setLocator(self.locator)
self.frame = self.robot.getChildFrame()
self.frame.setProperty('Scale', 3)
self.frame.setProperty('Edit', True)
self.frame.widget.HandleRotationEnabledOff()
rep = self.frame.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
self.supervisedTrainingTime = self.options['runTime'][
'supervisedTrainingTime']
self.learningRandomTime = self.options['runTime']['learningRandomTime']
self.learningEvalTime = self.options['runTime']['learningEvalTime']
self.defaultControllerTime = self.options['runTime'][
'defaultControllerTime']
self.Car.setFrame(self.frame)
print "Finished initialization"
def setSARSA(self, type=None):
if type is None:
type = self.options['SARSA']['type']
if type == "discrete":
self.Sarsa = SARSADiscrete(
sensorObj=self.Sensor,
actionSet=self.Controller.actionSet,
collisionThreshold=self.collisionThreshold,
alphaStepSize=self.options['SARSA']['alphaStepSize'],
useQLearningUpdate=self.options['SARSA']['useQLearningUpdate'],
lam=self.options['SARSA']['lam'],
numInnerBins=self.options['SARSA']['numInnerBins'],
numOuterBins=self.options['SARSA']['numOuterBins'],
binCutoff=self.options['SARSA']['binCutoff'],
burnInTime=self.options['SARSA']['burnInTime'],
epsilonGreedy=self.options['SARSA']['epsilonGreedy'],
epsilonGreedyExponent=self.options['SARSA']
['epsilonGreedyExponent'],
forceDriveStraight=self.options['SARSA']['forceDriveStraight'])
elif type == "continuous":
self.Sarsa = SARSAContinuous(
sensorObj=self.Sensor,
actionSet=self.Controller.actionSet,
lam=self.options['SARSA']['lam'],
alphaStepSize=self.options['SARSA']['alphaStepSize'],
collisionThreshold=self.collisionThreshold,
burnInTime=self.options['SARSA']['burnInTime'],
epsilonGreedy=self.options['SARSA']['epsilonGreedy'],
epsilonGreedyExponent=self.options['SARSA']
['epsilonGreedyExponent'])
else:
raise ValueError(
"sarsa type must be either discrete or continuous")
def runSingleSimulation(self,
updateQValues=True,
controllerType='default',
simulationCutoff=None):
if self.verbose:
print "using QValue based controller = ", useQValueController
self.setCollisionFreeInitialState()
currentCarState = np.copy(self.Car.state)
nextCarState = np.copy(self.Car.state)
self.setRobotFrameState(currentCarState[0], currentCarState[1],
currentCarState[2])
currentRaycast = self.Sensor.raycastAll(self.frame)
nextRaycast = np.zeros(self.Sensor.numRays)
self.Sarsa.resetElibilityTraces()
# record the reward data
runData = dict()
reward = 0
discountedReward = 0
avgReward = 0
startIdx = self.counter
while (self.counter < self.numTimesteps - 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)
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"]:
if controllerType == "defaultRandom":
controlInput, controlInputIdx = self.Controller.computeControlInput(
currentCarState,
currentTime,
self.frame,
raycastDistance=currentRaycast,
randomize=True)
else:
controlInput, controlInputIdx = self.Controller.computeControlInput(
currentCarState,
currentTime,
self.frame,
raycastDistance=currentRaycast,
randomize=False)
if controllerType in ["learned", "learnedRandom"]:
if controllerType == "learned":
randomizeControl = False
else:
randomizeControl = True
counterForGreedyDecay = self.counter - self.idxDict[
"learnedRandom"]
controlInput, controlInputIdx, emptyQValue = self.Sarsa.computeGreedyControlPolicy(
S_current,
counter=counterForGreedyDecay,
randomize=randomizeControl)
self.emptyQValue[idx] = emptyQValue
if emptyQValue and self.options['SARSA'][
'useSupervisedTraining']:
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)
# want to compute nextRaycast so we can do the SARSA algorithm
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"]:
if controllerType == "defaultRandom":
nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(
nextCarState,
currentTime,
self.frame,
raycastDistance=nextRaycast,
randomize=True)
else:
nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(
nextCarState,
currentTime,
self.frame,
raycastDistance=nextRaycast,
randomize=False)
if controllerType in ["learned", "learnedRandom"]:
if controllerType == "learned":
randomizeControl = False
else:
randomizeControl = True
counterForGreedyDecay = self.counter - self.idxDict[
"learnedRandom"]
nextControlInput, nextControlInputIdx, emptyQValue = self.Sarsa.computeGreedyControlPolicy(
S_next,
counter=counterForGreedyDecay,
randomize=randomizeControl)
if emptyQValue and self.options['SARSA'][
'useSupervisedTraining']:
nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(
nextCarState,
currentTime,
self.frame,
raycastDistance=nextRaycast,
randomize=False)
# if useQValueController:
# nextControlInput, nextControlInputIdx, emptyQValue = self.Sarsa.computeGreedyControlPolicy(S_next)
#
# if not useQValueController or emptyQValue:
# nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(nextCarState, currentTime, self.frame,
# raycastDistance=nextRaycast)
# compute the reward
reward = self.Reward.computeReward(S_next, controlInput)
self.rewardData[idx] = reward
discountedReward += self.Sarsa.gamma**(self.counter -
startIdx) * reward
avgReward += reward
###### SARSA update
if updateQValues:
self.Sarsa.sarsaUpdate(S_current, controlInputIdx, reward,
S_next, nextControlInputIdx)
#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 self.counter >= simulationCutoff:
break
# fill in the last state by hand
self.stateOverTime[self.counter, :] = currentCarState
self.raycastData[self.counter, :] = currentRaycast
# return the total reward
avgRewardNoCollisionPenalty = avgReward - reward
avgReward = avgReward * 1.0 / max(1, self.counter - startIdx)
avgRewardNoCollisionPenalty = avgRewardNoCollisionPenalty * 1.0 / max(
1, self.counter - startIdx)
# this extra multiplication is so that it is in the same "units" as avgReward
runData['discountedReward'] = discountedReward * (1 - self.Sarsa.gamma)
runData['avgReward'] = avgReward
runData['avgRewardNoCollisionPenalty'] = avgRewardNoCollisionPenalty
# this just makes sure we don't get stuck in an infinite loop.
if startIdx == self.counter:
self.counter += 1
return runData
def setNumpyRandomSeed(self, seed=1):
np.random.seed(seed)
def runBatchSimulation(self, endTime=None, dt=0.05):
# for use in playback
self.dt = self.options['dt']
self.Sarsa.setDiscountFactor(dt)
self.endTime = self.supervisedTrainingTime + self.learningRandomTime + self.learningEvalTime + self.defaultControllerTime
self.t = np.arange(0.0, self.endTime, dt)
maxNumTimesteps = np.size(self.t)
self.stateOverTime = np.zeros((maxNumTimesteps + 1, 3))
self.raycastData = np.zeros((maxNumTimesteps + 1, self.Sensor.numRays))
self.controlInputData = np.zeros(maxNumTimesteps + 1)
self.rewardData = np.zeros(maxNumTimesteps + 1)
self.emptyQValue = np.zeros(maxNumTimesteps + 1, dtype='bool')
self.numTimesteps = maxNumTimesteps
self.controllerTypeOrder = [
'defaultRandom', 'learnedRandom', 'learned', 'default'
]
self.counter = 0
self.simulationData = []
self.initializeStatusBar()
self.idxDict = dict()
numRunsCounter = 0
# three while loops for different phases of simulation, supervisedTraining, learning, default
self.idxDict['defaultRandom'] = self.counter
loopStartIdx = self.counter
simCutoff = min(loopStartIdx + self.supervisedTrainingTime / dt,
self.numTimesteps)
while ((self.counter - loopStartIdx < self.supervisedTrainingTime / dt)
and self.counter < self.numTimesteps):
self.printStatusBar()
useQValueController = False
startIdx = self.counter
runData = self.runSingleSimulation(updateQValues=True,
controllerType='defaultRandom',
simulationCutoff=simCutoff)
runData['startIdx'] = startIdx
runData['controllerType'] = "defaultRandom"
runData['duration'] = self.counter - runData['startIdx']
runData['endIdx'] = self.counter
runData['runNumber'] = numRunsCounter
numRunsCounter += 1
self.simulationData.append(runData)
self.idxDict['learnedRandom'] = self.counter
loopStartIdx = self.counter
simCutoff = min(loopStartIdx + self.learningRandomTime / dt,
self.numTimesteps)
while ((self.counter - loopStartIdx < self.learningRandomTime / dt)
and self.counter < self.numTimesteps):
self.printStatusBar()
startIdx = self.counter
runData = self.runSingleSimulation(updateQValues=True,
controllerType='learnedRandom',
simulationCutoff=simCutoff)
runData['startIdx'] = startIdx
runData['controllerType'] = "learnedRandom"
runData['duration'] = self.counter - runData['startIdx']
runData['endIdx'] = self.counter
runData['runNumber'] = numRunsCounter
numRunsCounter += 1
self.simulationData.append(runData)
self.idxDict['learned'] = self.counter
loopStartIdx = self.counter
simCutoff = min(loopStartIdx + self.learningEvalTime / dt,
self.numTimesteps)
while ((self.counter - loopStartIdx < self.learningEvalTime / dt)
and self.counter < self.numTimesteps):
self.printStatusBar()
startIdx = self.counter
runData = self.runSingleSimulation(updateQValues=False,
controllerType='learned',
simulationCutoff=simCutoff)
runData['startIdx'] = startIdx
runData['controllerType'] = "learned"
runData['duration'] = self.counter - runData['startIdx']
runData['endIdx'] = self.counter
runData['runNumber'] = numRunsCounter
numRunsCounter += 1
self.simulationData.append(runData)
self.idxDict['default'] = self.counter
loopStartIdx = self.counter
simCutoff = min(loopStartIdx + self.defaultControllerTime / dt,
self.numTimesteps)
while ((self.counter - loopStartIdx < self.defaultControllerTime / dt)
and self.counter < self.numTimesteps - 1):
self.printStatusBar()
startIdx = self.counter
runData = self.runSingleSimulation(updateQValues=False,
controllerType='default',
simulationCutoff=simCutoff)
runData['startIdx'] = startIdx
runData['controllerType'] = "default"
runData['duration'] = self.counter - runData['startIdx']
runData['endIdx'] = self.counter
runData['runNumber'] = numRunsCounter
numRunsCounter += 1
self.simulationData.append(runData)
# BOOKKEEPING
# truncate stateOverTime, raycastData, controlInputs to be the correct size
self.numTimesteps = self.counter + 1
self.stateOverTime = self.stateOverTime[0:self.counter + 1, :]
self.raycastData = self.raycastData[0:self.counter + 1, :]
self.controlInputData = self.controlInputData[0:self.counter + 1]
self.rewardData = self.rewardData[0:self.counter + 1]
self.endTime = 1.0 * self.counter / self.numTimesteps * self.endTime
def initializeStatusBar(self):
self.numTicks = 10
self.nextTickComplete = 1.0 / float(self.numTicks)
self.nextTickIdx = 1
print "Simulation percentage complete: (", self.numTicks, " # is complete)"
def printStatusBar(self):
fractionDone = float(self.counter) / float(self.numTimesteps)
if fractionDone > self.nextTickComplete:
self.nextTickIdx += 1
self.nextTickComplete += 1.0 / self.numTicks
timeSoFar = time.time() - self.startSimTime
estimatedTimeLeft_sec = (1 -
fractionDone) * timeSoFar / fractionDone
estimatedTimeLeft_minutes = estimatedTimeLeft_sec / 60.0
print "#" * self.nextTickIdx, "-" * (
self.numTicks - self.nextTickIdx
), "estimated", estimatedTimeLeft_minutes, "minutes left"
def setCollisionFreeInitialState(self):
tol = 5
while True:
x = np.random.uniform(self.world.Xmin + tol, self.world.Xmax - tol,
1)[0]
y = np.random.uniform(self.world.Ymin + tol, self.world.Ymax - tol,
1)[0]
theta = np.random.uniform(0, 2 * np.pi, 1)[0]
self.Car.setCarState(x, y, theta)
self.setRobotFrameState(x, y, theta)
if not self.checkInCollision():
break
# if self.checkInCollision():
# print "IN COLLISION"
# else:
# print "COLLISION FREE"
return x, y, theta
def setupPlayback(self):
self.timer = TimerCallback(targetFps=30)
self.timer.callback = self.tick
playButtonFps = 1.0 / self.dt
print "playButtonFPS", playButtonFps
self.playTimer = TimerCallback(targetFps=playButtonFps)
self.playTimer.callback = self.playTimerCallback
self.sliderMovedByPlayTimer = False
panel = QtGui.QWidget()
l = QtGui.QHBoxLayout(panel)
playButton = QtGui.QPushButton('Play/Pause')
playButton.connect('clicked()', self.onPlayButton)
slider = QtGui.QSlider(QtCore.Qt.Horizontal)
slider.connect('valueChanged(int)', self.onSliderChanged)
self.sliderMax = self.numTimesteps
slider.setMaximum(self.sliderMax)
self.slider = slider
l.addWidget(playButton)
l.addWidget(slider)
w = QtGui.QWidget()
l = QtGui.QVBoxLayout(w)
l.addWidget(self.view)
l.addWidget(panel)
w.showMaximized()
self.frame.connectFrameModified(self.updateDrawIntersection)
self.updateDrawIntersection(self.frame)
applogic.resetCamera(viewDirection=[0.2, 0, -1])
self.view.showMaximized()
self.view.raise_()
panel = screengrabberpanel.ScreenGrabberPanel(self.view)
panel.widget.show()
elapsed = time.time() - self.startSimTime
simRate = self.counter / elapsed
print "Total run time", elapsed
print "Ticks (Hz)", simRate
print "Number of steps taken", self.counter
self.app.start()
def run(self, launchApp=True):
self.counter = 1
self.runBatchSimulation()
# self.Sarsa.plotWeights()
if launchApp:
self.setupPlayback()
def updateDrawIntersection(self, frame):
origin = np.array(frame.transform.GetPosition())
#print "origin is now at", origin
d = DebugData()
sliderIdx = self.slider.value
controllerType = self.getControllerTypeFromCounter(sliderIdx)
colorMaxRange = self.colorMap[controllerType]
# if the QValue was empty then color it green
if self.emptyQValue[sliderIdx]:
colorMaxRange = [1, 1, 0] # this is yellow
for i in xrange(self.Sensor.numRays):
ray = self.Sensor.rays[:, i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
#print "rayTransformed is", rayTransformed
intersection = self.Sensor.raycast(
self.locator, origin,
origin + rayTransformed * self.Sensor.rayLength)
if intersection is not None:
d.addLine(origin, intersection, color=[1, 0, 0])
else:
d.addLine(origin,
origin + rayTransformed * self.Sensor.rayLength,
color=colorMaxRange)
vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
#camera = self.view.camera()
#camera.SetFocalPoint(frame.transform.GetPosition())
#camera.SetPosition(frame.transform.TransformPoint((-30,0,10)))
def getControllerTypeFromCounter(self, counter):
name = self.controllerTypeOrder[0]
for controllerType in self.controllerTypeOrder[1:]:
if counter >= self.idxDict[controllerType]:
name = controllerType
return name
def setRobotFrameState(self, x, y, theta):
t = vtk.vtkTransform()
t.Translate(x, y, 0.0)
t.RotateZ(np.degrees(theta))
self.robot.getChildFrame().copyFrame(t)
# returns true if we are in collision
def checkInCollision(self, raycastDistance=None):
if raycastDistance is None:
self.setRobotFrameState(self.Car.state[0], self.Car.state[1],
self.Car.state[2])
raycastDistance = self.Sensor.raycastAll(self.frame)
if np.min(raycastDistance) < self.collisionThreshold:
return True
else:
return False
def tick(self):
#print timer.elapsed
#simulate(t.elapsed)
x = np.sin(time.time())
y = np.cos(time.time())
self.setRobotFrameState(x, y, 0.0)
if (time.time() - self.playTime) > self.endTime:
self.playTimer.stop()
def tick2(self):
newtime = time.time() - self.playTime
print time.time() - self.playTime
x = np.sin(newtime)
y = np.cos(newtime)
self.setRobotFrameState(x, y, 0.0)
# just increment the slider, stop the timer if we get to the end
def playTimerCallback(self):
self.sliderMovedByPlayTimer = True
currentIdx = self.slider.value
nextIdx = currentIdx + 1
self.slider.setSliderPosition(nextIdx)
if currentIdx >= self.sliderMax:
print "reached end of tape, stopping playTime"
self.playTimer.stop()
def onSliderChanged(self, value):
if not self.sliderMovedByPlayTimer:
self.playTimer.stop()
numSteps = len(self.stateOverTime)
idx = int(np.floor(numSteps * (1.0 * value / self.sliderMax)))
idx = min(idx, numSteps - 1)
x, y, theta = self.stateOverTime[idx]
self.setRobotFrameState(x, y, theta)
self.sliderMovedByPlayTimer = False
def onPlayButton(self):
if self.playTimer.isActive():
self.onPauseButton()
return
print 'play'
self.playTimer.start()
self.playTime = time.time()
def onPauseButton(self):
print 'pause'
self.playTimer.stop()
def computeRunStatistics(self):
numRuns = len(self.simulationData)
runStart = np.zeros(numRuns)
runDuration = np.zeros(numRuns)
grid = np.arange(1, numRuns + 1)
discountedReward = np.zeros(numRuns)
avgReward = np.zeros(numRuns)
avgRewardNoCollisionPenalty = np.zeros(numRuns)
idxMap = dict()
for controllerType, color in self.colorMap.iteritems():
idxMap[controllerType] = np.zeros(numRuns, dtype=bool)
for idx, val in enumerate(self.simulationData):
runStart[idx] = val['startIdx']
runDuration[idx] = val['duration']
discountedReward[idx] = val['discountedReward']
avgReward[idx] = val['avgReward']
avgRewardNoCollisionPenalty[idx] = val[
'avgRewardNoCollisionPenalty']
controllerType = val['controllerType']
idxMap[controllerType][idx] = True
def plotRunData(self,
controllerTypeToPlot=None,
showPlot=True,
onlyLearned=False):
if controllerTypeToPlot == None:
controllerTypeToPlot = self.colorMap.keys()
if onlyLearned:
controllerTypeToPlot = ['learnedRandom', 'learned']
numRuns = len(self.simulationData)
runStart = np.zeros(numRuns)
runDuration = np.zeros(numRuns)
grid = np.arange(1, numRuns + 1)
discountedReward = np.zeros(numRuns)
avgReward = np.zeros(numRuns)
avgRewardNoCollisionPenalty = np.zeros(numRuns)
idxMap = dict()
for controllerType, color in self.colorMap.iteritems():
idxMap[controllerType] = np.zeros(numRuns, dtype=bool)
for idx, val in enumerate(self.simulationData):
runStart[idx] = val['startIdx']
runDuration[idx] = val['duration']
discountedReward[idx] = val['discountedReward']
avgReward[idx] = val['avgReward']
avgRewardNoCollisionPenalty[idx] = val[
'avgRewardNoCollisionPenalty']
controllerType = val['controllerType']
idxMap[controllerType][idx] = True
# usedQValueController[idx] = (val['controllerType'] == "QValue")
# usedDefaultController[idx] = (val['controllerType'] == "default")
# usedDefaultRandomController[idx] = (val['controllerType'] == "defaultRandom")
# usedQValueRandomController[idx] = (val['controllerType'] == "QValueRandom")
self.runStatistics = dict()
dataMap = {
'duration': runDuration,
'discountedReward': discountedReward,
'avgReward': avgReward,
'avgRewardNoCollisionPenalty': avgRewardNoCollisionPenalty
}
def computeRunStatistics(dataMap):
for controllerType, idx in idxMap.iteritems():
d = dict()
for dataName, dataSet in dataMap.iteritems():
# average the appropriate values in dataset
d[dataName] = np.sum(
dataSet[idx]) / (1.0 * np.size(dataSet[idx]))
self.runStatistics[controllerType] = d
computeRunStatistics(dataMap)
if not showPlot:
return
plt.figure()
#
# idxDefaultRandom = np.where(usedDefaultRandomController==True)[0]
# idxQValueController = np.where(usedQValueController==True)[0]
# idxQValueControllerRandom = np.where(usedQValueControllerRandom==True)[0]
# idxDefault = np.where(usedDefaultController==True)[0]
#
# plotData = dict()
# plotData['defaultRandom'] = {'idx': idxDefaultRandom, 'color': 'b'}
# plotData['QValue'] = {'idx': idxQValueController, 'color': 'y'}
# plotData['default'] = {'idx': idxDefault, 'color': 'g'}
def scatterPlot(dataToPlot):
for controllerType in controllerTypeToPlot:
idx = idxMap[controllerType]
plt.scatter(grid[idx],
dataToPlot[idx],
color=self.colorMap[controllerType])
def barPlot(dataName):
plt.title(dataName)
barWidth = 0.5
barCounter = 0
index = np.arange(len(controllerTypeToPlot))
for controllerType in controllerTypeToPlot:
val = self.runStatistics[controllerType]
plt.bar(barCounter,
val[dataName],
barWidth,
color=self.colorMap[controllerType],
label=controllerType)
barCounter += 1
plt.xticks(index + barWidth / 2.0, controllerTypeToPlot)
plt.subplot(4, 1, 1)
plt.title('run duration')
scatterPlot(runDuration)
# for controllerType, idx in idxMap.iteritems():
# plt.scatter(grid[idx], runDuration[idx], color=self.colorMap[controllerType])
# plt.scatter(runStart[idxDefaultRandom], runDuration[idxDefaultRandom], color='b')
# plt.scatter(runStart[idxQValueController], runDuration[idxQValueController], color='y')
# plt.scatter(runStart[idxDefault], runDuration[idxDefault], color='g')
plt.xlabel('run #')
plt.ylabel('episode duration')
plt.subplot(2, 1, 2)
plt.title('discounted reward')
scatterPlot(discountedReward)
# for key, val in plotData.iteritems():
# plt.scatter(grid[idx], discountedReward[idx], color=self.colorMap[controllerType])
# plt.subplot(3,1,3)
# plt.title("average reward")
# scatterPlot(avgReward)
# for key, val in plotData.iteritems():
# plt.scatter(grid[val['idx']],avgReward[val['idx']], color=val['color'])
# plt.subplot(4,1,4)
# plt.title("average reward no collision penalty")
# scatterPlot(avgRewardNoCollisionPenalty)
# # for key, val in plotData.iteritems():
# # plt.scatter(grid[val['idx']],avgRewardNoCollisionPenalty[val['idx']], color=val['color'])
## plot summary statistics
plt.figure()
plt.subplot(2, 1, 1)
barPlot("duration")
plt.subplot(2, 1, 2)
barPlot("discountedReward")
# plt.subplot(3,1,3)
# barPlot("avgReward")
#
# plt.subplot(4,1,4)
# barPlot("avgRewardNoCollisionPenalty")
plt.show()
def plotMultipleRunData(self,
simList,
toPlot=['duration', 'discountedReward'],
controllerType='learned'):
plt.figure()
numPlots = len(toPlot)
grid = np.arange(len(simList))
def plot(fieldToPlot, plotNum):
plt.subplot(numPlots, 1, plotNum)
plt.title(fieldToPlot)
val = 0 * grid
barWidth = 0.5
barCounter = 0
for idx, sim in enumerate(simList):
value = sim.runStatistics[controllerType][fieldToPlot]
plt.bar(idx, value, barWidth)
counter = 1
for fieldToPlot in toPlot:
plot(fieldToPlot, counter)
counter += 1
plt.show()
def saveToFile(self, filename):
# should also save the run data if it is available, i.e. stateOverTime, rewardOverTime
filename = 'data/' + filename + ".out"
my_shelf = shelve.open(filename, 'n')
my_shelf['options'] = self.options
if self.options['SARSA']['type'] == "discrete":
my_shelf['SARSA_QValues'] = self.Sarsa.QValues
my_shelf['simulationData'] = self.simulationData
my_shelf['stateOverTime'] = self.stateOverTime
my_shelf['raycastData'] = self.raycastData
my_shelf['controlInputData'] = self.controlInputData
my_shelf['emptyQValue'] = self.emptyQValue
my_shelf['numTimesteps'] = self.numTimesteps
my_shelf['idxDict'] = self.idxDict
my_shelf['counter'] = self.counter
my_shelf.close()
@staticmethod
def loadFromFile(filename):
filename = 'data/' + filename + ".out"
sim = Simulator(autoInitialize=False, verbose=False)
my_shelf = shelve.open(filename)
sim.options = my_shelf['options']
sim.initialize()
if sim.options['SARSA']['type'] == "discrete":
sim.Sarsa.QValues = np.array(my_shelf['SARSA_QValues'])
sim.simulationData = my_shelf['simulationData']
# sim.runStatistics = my_shelf['runStatistics']
sim.stateOverTime = np.array(my_shelf['stateOverTime'])
sim.raycastData = np.array(my_shelf['raycastData'])
sim.controlInputData = np.array(my_shelf['controlInputData'])
sim.emptyQValue = np.array(my_shelf['emptyQValue'])
sim.numTimesteps = my_shelf['numTimesteps']
sim.idxDict = my_shelf['idxDict']
sim.counter = my_shelf['counter']
my_shelf.close()
return sim
class Simulator(object):
def __init__(
self,
percentObsDensity=20,
endTime=40,
randomizeControl=False,
nonRandomWorld=False,
circleRadius=0.7,
worldScale=1.0,
supervisedTrainingTime=500,
autoInitialize=True,
verbose=True,
sarsaType="discrete",
):
self.verbose = verbose
self.randomizeControl = randomizeControl
self.startSimTime = time.time()
self.collisionThreshold = 1.3
self.randomSeed = 5
self.Sarsa_numInnerBins = 4
self.Sarsa_numOuterBins = 4
self.Sensor_rayLength = 8
self.sarsaType = sarsaType
self.percentObsDensity = percentObsDensity
self.supervisedTrainingTime = 10
self.learningRandomTime = 10
self.learningEvalTime = 10
self.defaultControllerTime = 10
self.nonRandomWorld = nonRandomWorld
self.circleRadius = circleRadius
self.worldScale = worldScale
# create the visualizer object
self.app = ConsoleApp()
# view = app.createView(useGrid=False)
self.view = self.app.createView(useGrid=False)
self.initializeOptions()
self.initializeColorMap()
if autoInitialize:
self.initialize()
def initializeOptions(self):
self.options = dict()
self.options["Reward"] = dict()
self.options["Reward"]["actionCost"] = 0.1
self.options["Reward"]["collisionPenalty"] = 100.0
self.options["Reward"]["raycastCost"] = 20.0
self.options["SARSA"] = dict()
self.options["SARSA"]["type"] = "discrete"
self.options["SARSA"]["lam"] = 0.7
self.options["SARSA"]["alphaStepSize"] = 0.2
self.options["SARSA"]["useQLearningUpdate"] = False
self.options["SARSA"]["epsilonGreedy"] = 0.2
self.options["SARSA"]["burnInTime"] = 500
self.options["SARSA"]["epsilonGreedyExponent"] = 0.3
self.options["SARSA"]["exponentialDiscountFactor"] = 0.05 # so gamma = e^(-rho*dt)
self.options["SARSA"]["numInnerBins"] = 5
self.options["SARSA"]["numOuterBins"] = 4
self.options["SARSA"]["binCutoff"] = 0.5
self.options["World"] = dict()
self.options["World"]["obstaclesInnerFraction"] = 0.85
self.options["World"]["randomSeed"] = 40
self.options["World"]["percentObsDensity"] = 7.5
self.options["World"]["nonRandomWorld"] = True
self.options["World"]["circleRadius"] = 1.75
self.options["World"]["scale"] = 1.0
self.options["Sensor"] = dict()
self.options["Sensor"]["rayLength"] = 10
self.options["Sensor"]["numRays"] = 20
self.options["Car"] = dict()
self.options["Car"]["velocity"] = 12
self.options["dt"] = 0.05
self.options["runTime"] = dict()
self.options["runTime"]["supervisedTrainingTime"] = 10
self.options["runTime"]["learningRandomTime"] = 10
self.options["runTime"]["learningEvalTime"] = 10
self.options["runTime"]["defaultControllerTime"] = 10
def setDefaultOptions(self):
defaultOptions = dict()
defaultOptions["Reward"] = dict()
defaultOptions["Reward"]["actionCost"] = 0.1
defaultOptions["Reward"]["collisionPenalty"] = 100.0
defaultOptions["Reward"]["raycastCost"] = 20.0
defaultOptions["SARSA"] = dict()
defaultOptions["SARSA"]["type"] = "discrete"
defaultOptions["SARSA"]["lam"] = 0.7
defaultOptions["SARSA"]["alphaStepSize"] = 0.2
defaultOptions["SARSA"]["useQLearningUpdate"] = False
defaultOptions["SARSA"]["useSupervisedTraining"] = True
defaultOptions["SARSA"]["epsilonGreedy"] = 0.2
defaultOptions["SARSA"]["burnInTime"] = 500
defaultOptions["SARSA"]["epsilonGreedyExponent"] = 0.3
defaultOptions["SARSA"]["exponentialDiscountFactor"] = 0.05 # so gamma = e^(-rho*dt)
defaultOptions["SARSA"]["numInnerBins"] = 5
defaultOptions["SARSA"]["numOuterBins"] = 4
defaultOptions["SARSA"]["binCutoff"] = 0.5
defaultOptions["SARSA"]["forceDriveStraight"] = True
defaultOptions["World"] = dict()
defaultOptions["World"]["obstaclesInnerFraction"] = 0.85
defaultOptions["World"]["randomSeed"] = 40
defaultOptions["World"]["percentObsDensity"] = 7.5
defaultOptions["World"]["nonRandomWorld"] = True
defaultOptions["World"]["circleRadius"] = 1.75
defaultOptions["World"]["scale"] = 1.0
defaultOptions["Sensor"] = dict()
defaultOptions["Sensor"]["rayLength"] = 10
defaultOptions["Sensor"]["numRays"] = 20
defaultOptions["Car"] = dict()
defaultOptions["Car"]["velocity"] = 12
defaultOptions["dt"] = 0.05
defaultOptions["runTime"] = dict()
defaultOptions["runTime"]["supervisedTrainingTime"] = 10
defaultOptions["runTime"]["learningRandomTime"] = 10
defaultOptions["runTime"]["learningEvalTime"] = 10
defaultOptions["runTime"]["defaultControllerTime"] = 10
for k in defaultOptions:
self.options.setdefault(k, defaultOptions[k])
for k in defaultOptions:
if not isinstance(defaultOptions[k], dict):
continue
for j in defaultOptions[k]:
self.options[k].setdefault(j, defaultOptions[k][j])
def initializeColorMap(self):
self.colorMap = dict()
self.colorMap["defaultRandom"] = [0, 0, 1]
self.colorMap["learnedRandom"] = [1.0, 0.54901961, 0.0] # this is orange
self.colorMap["learned"] = [0.58039216, 0.0, 0.82745098] # this is yellow
self.colorMap["default"] = [0, 1, 0]
def initialize(self):
self.dt = self.options["dt"]
self.controllerTypeOrder = ["defaultRandom", "learnedRandom", "learned", "default"]
self.setDefaultOptions()
self.Sensor = SensorObj(
rayLength=self.options["Sensor"]["rayLength"], numRays=self.options["Sensor"]["numRays"]
)
self.Controller = ControllerObj(self.Sensor)
self.Car = CarPlant(controller=self.Controller, velocity=self.options["Car"]["velocity"])
self.Reward = Reward(
self.Sensor,
collisionThreshold=self.collisionThreshold,
actionCost=self.options["Reward"]["actionCost"],
collisionPenalty=self.options["Reward"]["collisionPenalty"],
raycastCost=self.options["Reward"]["raycastCost"],
)
self.setSARSA()
# create the things needed for simulation
om.removeFromObjectModel(om.findObjectByName("world"))
self.world = World.buildCircleWorld(
percentObsDensity=self.options["World"]["percentObsDensity"],
circleRadius=self.options["World"]["circleRadius"],
nonRandom=self.options["World"]["nonRandomWorld"],
scale=self.options["World"]["scale"],
randomSeed=self.options["World"]["randomSeed"],
obstaclesInnerFraction=self.options["World"]["obstaclesInnerFraction"],
)
om.removeFromObjectModel(om.findObjectByName("robot"))
self.robot, self.frame = World.buildRobot()
self.locator = World.buildCellLocator(self.world.visObj.polyData)
self.Sensor.setLocator(self.locator)
self.frame = self.robot.getChildFrame()
self.frame.setProperty("Scale", 3)
self.frame.setProperty("Edit", True)
self.frame.widget.HandleRotationEnabledOff()
rep = self.frame.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
self.supervisedTrainingTime = self.options["runTime"]["supervisedTrainingTime"]
self.learningRandomTime = self.options["runTime"]["learningRandomTime"]
self.learningEvalTime = self.options["runTime"]["learningEvalTime"]
self.defaultControllerTime = self.options["runTime"]["defaultControllerTime"]
self.Car.setFrame(self.frame)
print "Finished initialization"
def setSARSA(self, type=None):
if type is None:
type = self.options["SARSA"]["type"]
if type == "discrete":
self.Sarsa = SARSADiscrete(
sensorObj=self.Sensor,
actionSet=self.Controller.actionSet,
collisionThreshold=self.collisionThreshold,
alphaStepSize=self.options["SARSA"]["alphaStepSize"],
useQLearningUpdate=self.options["SARSA"]["useQLearningUpdate"],
lam=self.options["SARSA"]["lam"],
numInnerBins=self.options["SARSA"]["numInnerBins"],
numOuterBins=self.options["SARSA"]["numOuterBins"],
binCutoff=self.options["SARSA"]["binCutoff"],
burnInTime=self.options["SARSA"]["burnInTime"],
epsilonGreedy=self.options["SARSA"]["epsilonGreedy"],
epsilonGreedyExponent=self.options["SARSA"]["epsilonGreedyExponent"],
forceDriveStraight=self.options["SARSA"]["forceDriveStraight"],
)
elif type == "continuous":
self.Sarsa = SARSAContinuous(
sensorObj=self.Sensor,
actionSet=self.Controller.actionSet,
lam=self.options["SARSA"]["lam"],
alphaStepSize=self.options["SARSA"]["alphaStepSize"],
collisionThreshold=self.collisionThreshold,
burnInTime=self.options["SARSA"]["burnInTime"],
epsilonGreedy=self.options["SARSA"]["epsilonGreedy"],
epsilonGreedyExponent=self.options["SARSA"]["epsilonGreedyExponent"],
)
else:
raise ValueError("sarsa type must be either discrete or continuous")
def runSingleSimulation(self, updateQValues=True, controllerType="default", simulationCutoff=None):
if self.verbose:
print "using QValue based controller = ", useQValueController
self.setCollisionFreeInitialState()
currentCarState = np.copy(self.Car.state)
nextCarState = np.copy(self.Car.state)
self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2])
currentRaycast = self.Sensor.raycastAll(self.frame)
nextRaycast = np.zeros(self.Sensor.numRays)
self.Sarsa.resetElibilityTraces()
# record the reward data
runData = dict()
reward = 0
discountedReward = 0
avgReward = 0
startIdx = self.counter
while self.counter < self.numTimesteps - 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)
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"]:
if controllerType == "defaultRandom":
controlInput, controlInputIdx = self.Controller.computeControlInput(
currentCarState, currentTime, self.frame, raycastDistance=currentRaycast, randomize=True
)
else:
controlInput, controlInputIdx = self.Controller.computeControlInput(
currentCarState, currentTime, self.frame, raycastDistance=currentRaycast, randomize=False
)
if controllerType in ["learned", "learnedRandom"]:
if controllerType == "learned":
randomizeControl = False
else:
randomizeControl = True
counterForGreedyDecay = self.counter - self.idxDict["learnedRandom"]
controlInput, controlInputIdx, emptyQValue = self.Sarsa.computeGreedyControlPolicy(
S_current, counter=counterForGreedyDecay, randomize=randomizeControl
)
self.emptyQValue[idx] = emptyQValue
if emptyQValue and self.options["SARSA"]["useSupervisedTraining"]:
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)
# want to compute nextRaycast so we can do the SARSA algorithm
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"]:
if controllerType == "defaultRandom":
nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(
nextCarState, currentTime, self.frame, raycastDistance=nextRaycast, randomize=True
)
else:
nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(
nextCarState, currentTime, self.frame, raycastDistance=nextRaycast, randomize=False
)
if controllerType in ["learned", "learnedRandom"]:
if controllerType == "learned":
randomizeControl = False
else:
randomizeControl = True
counterForGreedyDecay = self.counter - self.idxDict["learnedRandom"]
nextControlInput, nextControlInputIdx, emptyQValue = self.Sarsa.computeGreedyControlPolicy(
S_next, counter=counterForGreedyDecay, randomize=randomizeControl
)
if emptyQValue and self.options["SARSA"]["useSupervisedTraining"]:
nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(
nextCarState, currentTime, self.frame, raycastDistance=nextRaycast, randomize=False
)
# if useQValueController:
# nextControlInput, nextControlInputIdx, emptyQValue = self.Sarsa.computeGreedyControlPolicy(S_next)
#
# if not useQValueController or emptyQValue:
# nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(nextCarState, currentTime, self.frame,
# raycastDistance=nextRaycast)
# compute the reward
reward = self.Reward.computeReward(S_next, controlInput)
self.rewardData[idx] = reward
discountedReward += self.Sarsa.gamma ** (self.counter - startIdx) * reward
avgReward += reward
###### SARSA update
if updateQValues:
self.Sarsa.sarsaUpdate(S_current, controlInputIdx, reward, S_next, nextControlInputIdx)
# 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 self.counter >= simulationCutoff:
break
# fill in the last state by hand
self.stateOverTime[self.counter, :] = currentCarState
self.raycastData[self.counter, :] = currentRaycast
# return the total reward
avgRewardNoCollisionPenalty = avgReward - reward
avgReward = avgReward * 1.0 / max(1, self.counter - startIdx)
avgRewardNoCollisionPenalty = avgRewardNoCollisionPenalty * 1.0 / max(1, self.counter - startIdx)
# this extra multiplication is so that it is in the same "units" as avgReward
runData["discountedReward"] = discountedReward * (1 - self.Sarsa.gamma)
runData["avgReward"] = avgReward
runData["avgRewardNoCollisionPenalty"] = avgRewardNoCollisionPenalty
# this just makes sure we don't get stuck in an infinite loop.
if startIdx == self.counter:
self.counter += 1
return runData
def setNumpyRandomSeed(self, seed=1):
np.random.seed(seed)
def runBatchSimulation(self, endTime=None, dt=0.05):
# for use in playback
self.dt = self.options["dt"]
self.Sarsa.setDiscountFactor(dt)
self.endTime = (
self.supervisedTrainingTime + self.learningRandomTime + self.learningEvalTime + self.defaultControllerTime
)
self.t = np.arange(0.0, self.endTime, dt)
maxNumTimesteps = np.size(self.t)
self.stateOverTime = np.zeros((maxNumTimesteps + 1, 3))
self.raycastData = np.zeros((maxNumTimesteps + 1, self.Sensor.numRays))
self.controlInputData = np.zeros(maxNumTimesteps + 1)
self.rewardData = np.zeros(maxNumTimesteps + 1)
self.emptyQValue = np.zeros(maxNumTimesteps + 1, dtype="bool")
self.numTimesteps = maxNumTimesteps
self.controllerTypeOrder = ["defaultRandom", "learnedRandom", "learned", "default"]
self.counter = 0
self.simulationData = []
self.initializeStatusBar()
self.idxDict = dict()
numRunsCounter = 0
# three while loops for different phases of simulation, supervisedTraining, learning, default
self.idxDict["defaultRandom"] = self.counter
loopStartIdx = self.counter
simCutoff = min(loopStartIdx + self.supervisedTrainingTime / dt, self.numTimesteps)
while (self.counter - loopStartIdx < self.supervisedTrainingTime / dt) and self.counter < self.numTimesteps:
self.printStatusBar()
useQValueController = False
startIdx = self.counter
runData = self.runSingleSimulation(
updateQValues=True, controllerType="defaultRandom", simulationCutoff=simCutoff
)
runData["startIdx"] = startIdx
runData["controllerType"] = "defaultRandom"
runData["duration"] = self.counter - runData["startIdx"]
runData["endIdx"] = self.counter
runData["runNumber"] = numRunsCounter
numRunsCounter += 1
self.simulationData.append(runData)
self.idxDict["learnedRandom"] = self.counter
loopStartIdx = self.counter
simCutoff = min(loopStartIdx + self.learningRandomTime / dt, self.numTimesteps)
while (self.counter - loopStartIdx < self.learningRandomTime / dt) and self.counter < self.numTimesteps:
self.printStatusBar()
startIdx = self.counter
runData = self.runSingleSimulation(
updateQValues=True, controllerType="learnedRandom", simulationCutoff=simCutoff
)
runData["startIdx"] = startIdx
runData["controllerType"] = "learnedRandom"
runData["duration"] = self.counter - runData["startIdx"]
runData["endIdx"] = self.counter
runData["runNumber"] = numRunsCounter
numRunsCounter += 1
self.simulationData.append(runData)
self.idxDict["learned"] = self.counter
loopStartIdx = self.counter
simCutoff = min(loopStartIdx + self.learningEvalTime / dt, self.numTimesteps)
while (self.counter - loopStartIdx < self.learningEvalTime / dt) and self.counter < self.numTimesteps:
self.printStatusBar()
startIdx = self.counter
runData = self.runSingleSimulation(
updateQValues=False, controllerType="learned", simulationCutoff=simCutoff
)
runData["startIdx"] = startIdx
runData["controllerType"] = "learned"
runData["duration"] = self.counter - runData["startIdx"]
runData["endIdx"] = self.counter
runData["runNumber"] = numRunsCounter
numRunsCounter += 1
self.simulationData.append(runData)
self.idxDict["default"] = self.counter
loopStartIdx = self.counter
simCutoff = min(loopStartIdx + self.defaultControllerTime / dt, self.numTimesteps)
while (self.counter - loopStartIdx < self.defaultControllerTime / dt) and self.counter < self.numTimesteps - 1:
self.printStatusBar()
startIdx = self.counter
runData = self.runSingleSimulation(
updateQValues=False, controllerType="default", simulationCutoff=simCutoff
)
runData["startIdx"] = startIdx
runData["controllerType"] = "default"
runData["duration"] = self.counter - runData["startIdx"]
runData["endIdx"] = self.counter
runData["runNumber"] = numRunsCounter
numRunsCounter += 1
self.simulationData.append(runData)
# BOOKKEEPING
# truncate stateOverTime, raycastData, controlInputs to be the correct size
self.numTimesteps = self.counter + 1
self.stateOverTime = self.stateOverTime[0 : self.counter + 1, :]
self.raycastData = self.raycastData[0 : self.counter + 1, :]
self.controlInputData = self.controlInputData[0 : self.counter + 1]
self.rewardData = self.rewardData[0 : self.counter + 1]
self.endTime = 1.0 * self.counter / self.numTimesteps * self.endTime
def initializeStatusBar(self):
self.numTicks = 10
self.nextTickComplete = 1.0 / float(self.numTicks)
self.nextTickIdx = 1
print "Simulation percentage complete: (", self.numTicks, " # is complete)"
def printStatusBar(self):
fractionDone = float(self.counter) / float(self.numTimesteps)
if fractionDone > self.nextTickComplete:
self.nextTickIdx += 1
self.nextTickComplete += 1.0 / self.numTicks
timeSoFar = time.time() - self.startSimTime
estimatedTimeLeft_sec = (1 - fractionDone) * timeSoFar / fractionDone
estimatedTimeLeft_minutes = estimatedTimeLeft_sec / 60.0
print "#" * self.nextTickIdx, "-" * (
self.numTicks - self.nextTickIdx
), "estimated", estimatedTimeLeft_minutes, "minutes left"
def setCollisionFreeInitialState(self):
tol = 5
while True:
x = np.random.uniform(self.world.Xmin + tol, self.world.Xmax - tol, 1)[0]
y = np.random.uniform(self.world.Ymin + tol, self.world.Ymax - tol, 1)[0]
theta = np.random.uniform(0, 2 * np.pi, 1)[0]
self.Car.setCarState(x, y, theta)
self.setRobotFrameState(x, y, theta)
if not self.checkInCollision():
break
# if self.checkInCollision():
# print "IN COLLISION"
# else:
# print "COLLISION FREE"
return x, y, theta
def setupPlayback(self):
self.timer = TimerCallback(targetFps=30)
self.timer.callback = self.tick
playButtonFps = 1.0 / self.dt
print "playButtonFPS", playButtonFps
self.playTimer = TimerCallback(targetFps=playButtonFps)
self.playTimer.callback = self.playTimerCallback
self.sliderMovedByPlayTimer = False
panel = QtGui.QWidget()
l = QtGui.QHBoxLayout(panel)
playButton = QtGui.QPushButton("Play/Pause")
playButton.connect("clicked()", self.onPlayButton)
slider = QtGui.QSlider(QtCore.Qt.Horizontal)
slider.connect("valueChanged(int)", self.onSliderChanged)
self.sliderMax = self.numTimesteps
slider.setMaximum(self.sliderMax)
self.slider = slider
l.addWidget(playButton)
l.addWidget(slider)
w = QtGui.QWidget()
l = QtGui.QVBoxLayout(w)
l.addWidget(self.view)
l.addWidget(panel)
w.showMaximized()
self.frame.connectFrameModified(self.updateDrawIntersection)
self.updateDrawIntersection(self.frame)
applogic.resetCamera(viewDirection=[0.2, 0, -1])
self.view.showMaximized()
self.view.raise_()
panel = screengrabberpanel.ScreenGrabberPanel(self.view)
panel.widget.show()
elapsed = time.time() - self.startSimTime
simRate = self.counter / elapsed
print "Total run time", elapsed
print "Ticks (Hz)", simRate
print "Number of steps taken", self.counter
self.app.start()
def run(self, launchApp=True):
self.counter = 1
self.runBatchSimulation()
# self.Sarsa.plotWeights()
if launchApp:
self.setupPlayback()
def updateDrawIntersection(self, frame):
origin = np.array(frame.transform.GetPosition())
# print "origin is now at", origin
d = DebugData()
sliderIdx = self.slider.value
controllerType = self.getControllerTypeFromCounter(sliderIdx)
colorMaxRange = self.colorMap[controllerType]
# if the QValue was empty then color it green
if self.emptyQValue[sliderIdx]:
colorMaxRange = [1, 1, 0] # this is yellow
for i in xrange(self.Sensor.numRays):
ray = self.Sensor.rays[:, i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
# print "rayTransformed is", rayTransformed
intersection = self.Sensor.raycast(self.locator, origin, origin + rayTransformed * self.Sensor.rayLength)
if intersection is not None:
d.addLine(origin, intersection, color=[1, 0, 0])
else:
d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange)
vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255")
# camera = self.view.camera()
# camera.SetFocalPoint(frame.transform.GetPosition())
# camera.SetPosition(frame.transform.TransformPoint((-30,0,10)))
def getControllerTypeFromCounter(self, counter):
name = self.controllerTypeOrder[0]
for controllerType in self.controllerTypeOrder[1:]:
if counter >= self.idxDict[controllerType]:
name = controllerType
return name
def setRobotFrameState(self, x, y, theta):
t = vtk.vtkTransform()
t.Translate(x, y, 0.0)
t.RotateZ(np.degrees(theta))
self.robot.getChildFrame().copyFrame(t)
# returns true if we are in collision
def checkInCollision(self, raycastDistance=None):
if raycastDistance is None:
self.setRobotFrameState(self.Car.state[0], self.Car.state[1], self.Car.state[2])
raycastDistance = self.Sensor.raycastAll(self.frame)
if np.min(raycastDistance) < self.collisionThreshold:
return True
else:
return False
def tick(self):
# print timer.elapsed
# simulate(t.elapsed)
x = np.sin(time.time())
y = np.cos(time.time())
self.setRobotFrameState(x, y, 0.0)
if (time.time() - self.playTime) > self.endTime:
self.playTimer.stop()
def tick2(self):
newtime = time.time() - self.playTime
print time.time() - self.playTime
x = np.sin(newtime)
y = np.cos(newtime)
self.setRobotFrameState(x, y, 0.0)
# just increment the slider, stop the timer if we get to the end
def playTimerCallback(self):
self.sliderMovedByPlayTimer = True
currentIdx = self.slider.value
nextIdx = currentIdx + 1
self.slider.setSliderPosition(nextIdx)
if currentIdx >= self.sliderMax:
print "reached end of tape, stopping playTime"
self.playTimer.stop()
def onSliderChanged(self, value):
if not self.sliderMovedByPlayTimer:
self.playTimer.stop()
numSteps = len(self.stateOverTime)
idx = int(np.floor(numSteps * (1.0 * value / self.sliderMax)))
idx = min(idx, numSteps - 1)
x, y, theta = self.stateOverTime[idx]
self.setRobotFrameState(x, y, theta)
self.sliderMovedByPlayTimer = False
def onPlayButton(self):
if self.playTimer.isActive():
self.onPauseButton()
return
print "play"
self.playTimer.start()
self.playTime = time.time()
def onPauseButton(self):
print "pause"
self.playTimer.stop()
def computeRunStatistics(self):
numRuns = len(self.simulationData)
runStart = np.zeros(numRuns)
runDuration = np.zeros(numRuns)
grid = np.arange(1, numRuns + 1)
discountedReward = np.zeros(numRuns)
avgReward = np.zeros(numRuns)
avgRewardNoCollisionPenalty = np.zeros(numRuns)
idxMap = dict()
for controllerType, color in self.colorMap.iteritems():
idxMap[controllerType] = np.zeros(numRuns, dtype=bool)
for idx, val in enumerate(self.simulationData):
runStart[idx] = val["startIdx"]
runDuration[idx] = val["duration"]
discountedReward[idx] = val["discountedReward"]
avgReward[idx] = val["avgReward"]
avgRewardNoCollisionPenalty[idx] = val["avgRewardNoCollisionPenalty"]
controllerType = val["controllerType"]
idxMap[controllerType][idx] = True
def plotRunData(self, controllerTypeToPlot=None, showPlot=True, onlyLearned=False):
if controllerTypeToPlot == None:
controllerTypeToPlot = self.colorMap.keys()
if onlyLearned:
controllerTypeToPlot = ["learnedRandom", "learned"]
numRuns = len(self.simulationData)
runStart = np.zeros(numRuns)
runDuration = np.zeros(numRuns)
grid = np.arange(1, numRuns + 1)
discountedReward = np.zeros(numRuns)
avgReward = np.zeros(numRuns)
avgRewardNoCollisionPenalty = np.zeros(numRuns)
idxMap = dict()
for controllerType, color in self.colorMap.iteritems():
idxMap[controllerType] = np.zeros(numRuns, dtype=bool)
for idx, val in enumerate(self.simulationData):
runStart[idx] = val["startIdx"]
runDuration[idx] = val["duration"]
discountedReward[idx] = val["discountedReward"]
avgReward[idx] = val["avgReward"]
avgRewardNoCollisionPenalty[idx] = val["avgRewardNoCollisionPenalty"]
controllerType = val["controllerType"]
idxMap[controllerType][idx] = True
# usedQValueController[idx] = (val['controllerType'] == "QValue")
# usedDefaultController[idx] = (val['controllerType'] == "default")
# usedDefaultRandomController[idx] = (val['controllerType'] == "defaultRandom")
# usedQValueRandomController[idx] = (val['controllerType'] == "QValueRandom")
self.runStatistics = dict()
dataMap = {
"duration": runDuration,
"discountedReward": discountedReward,
"avgReward": avgReward,
"avgRewardNoCollisionPenalty": avgRewardNoCollisionPenalty,
}
def computeRunStatistics(dataMap):
for controllerType, idx in idxMap.iteritems():
d = dict()
for dataName, dataSet in dataMap.iteritems():
# average the appropriate values in dataset
d[dataName] = np.sum(dataSet[idx]) / (1.0 * np.size(dataSet[idx]))
self.runStatistics[controllerType] = d
computeRunStatistics(dataMap)
if not showPlot:
return
plt.figure()
#
# idxDefaultRandom = np.where(usedDefaultRandomController==True)[0]
# idxQValueController = np.where(usedQValueController==True)[0]
# idxQValueControllerRandom = np.where(usedQValueControllerRandom==True)[0]
# idxDefault = np.where(usedDefaultController==True)[0]
#
# plotData = dict()
# plotData['defaultRandom'] = {'idx': idxDefaultRandom, 'color': 'b'}
# plotData['QValue'] = {'idx': idxQValueController, 'color': 'y'}
# plotData['default'] = {'idx': idxDefault, 'color': 'g'}
def scatterPlot(dataToPlot):
for controllerType in controllerTypeToPlot:
idx = idxMap[controllerType]
plt.scatter(grid[idx], dataToPlot[idx], color=self.colorMap[controllerType])
def barPlot(dataName):
plt.title(dataName)
barWidth = 0.5
barCounter = 0
index = np.arange(len(controllerTypeToPlot))
for controllerType in controllerTypeToPlot:
val = self.runStatistics[controllerType]
plt.bar(barCounter, val[dataName], barWidth, color=self.colorMap[controllerType], label=controllerType)
barCounter += 1
plt.xticks(index + barWidth / 2.0, controllerTypeToPlot)
plt.subplot(4, 1, 1)
plt.title("run duration")
scatterPlot(runDuration)
# for controllerType, idx in idxMap.iteritems():
# plt.scatter(grid[idx], runDuration[idx], color=self.colorMap[controllerType])
# plt.scatter(runStart[idxDefaultRandom], runDuration[idxDefaultRandom], color='b')
# plt.scatter(runStart[idxQValueController], runDuration[idxQValueController], color='y')
# plt.scatter(runStart[idxDefault], runDuration[idxDefault], color='g')
plt.xlabel("run #")
plt.ylabel("episode duration")
plt.subplot(2, 1, 2)
plt.title("discounted reward")
scatterPlot(discountedReward)
# for key, val in plotData.iteritems():
# plt.scatter(grid[idx], discountedReward[idx], color=self.colorMap[controllerType])
# plt.subplot(3,1,3)
# plt.title("average reward")
# scatterPlot(avgReward)
# for key, val in plotData.iteritems():
# plt.scatter(grid[val['idx']],avgReward[val['idx']], color=val['color'])
# plt.subplot(4,1,4)
# plt.title("average reward no collision penalty")
# scatterPlot(avgRewardNoCollisionPenalty)
# # for key, val in plotData.iteritems():
# # plt.scatter(grid[val['idx']],avgRewardNoCollisionPenalty[val['idx']], color=val['color'])
## plot summary statistics
plt.figure()
plt.subplot(2, 1, 1)
barPlot("duration")
plt.subplot(2, 1, 2)
barPlot("discountedReward")
# plt.subplot(3,1,3)
# barPlot("avgReward")
#
# plt.subplot(4,1,4)
# barPlot("avgRewardNoCollisionPenalty")
plt.show()
def plotMultipleRunData(self, simList, toPlot=["duration", "discountedReward"], controllerType="learned"):
plt.figure()
numPlots = len(toPlot)
grid = np.arange(len(simList))
def plot(fieldToPlot, plotNum):
plt.subplot(numPlots, 1, plotNum)
plt.title(fieldToPlot)
val = 0 * grid
barWidth = 0.5
barCounter = 0
for idx, sim in enumerate(simList):
value = sim.runStatistics[controllerType][fieldToPlot]
plt.bar(idx, value, barWidth)
counter = 1
for fieldToPlot in toPlot:
plot(fieldToPlot, counter)
counter += 1
plt.show()
def saveToFile(self, filename):
# should also save the run data if it is available, i.e. stateOverTime, rewardOverTime
filename = "data/" + filename + ".out"
my_shelf = shelve.open(filename, "n")
my_shelf["options"] = self.options
if self.options["SARSA"]["type"] == "discrete":
my_shelf["SARSA_QValues"] = self.Sarsa.QValues
my_shelf["simulationData"] = self.simulationData
my_shelf["stateOverTime"] = self.stateOverTime
my_shelf["raycastData"] = self.raycastData
my_shelf["controlInputData"] = self.controlInputData
my_shelf["emptyQValue"] = self.emptyQValue
my_shelf["numTimesteps"] = self.numTimesteps
my_shelf["idxDict"] = self.idxDict
my_shelf["counter"] = self.counter
my_shelf.close()
@staticmethod
def loadFromFile(filename):
filename = "data/" + filename + ".out"
sim = Simulator(autoInitialize=False, verbose=False)
my_shelf = shelve.open(filename)
sim.options = my_shelf["options"]
sim.initialize()
if sim.options["SARSA"]["type"] == "discrete":
sim.Sarsa.QValues = np.array(my_shelf["SARSA_QValues"])
sim.simulationData = my_shelf["simulationData"]
# sim.runStatistics = my_shelf['runStatistics']
sim.stateOverTime = np.array(my_shelf["stateOverTime"])
sim.raycastData = np.array(my_shelf["raycastData"])
sim.controlInputData = np.array(my_shelf["controlInputData"])
sim.emptyQValue = np.array(my_shelf["emptyQValue"])
sim.numTimesteps = my_shelf["numTimesteps"]
sim.idxDict = my_shelf["idxDict"]
sim.counter = my_shelf["counter"]
my_shelf.close()
return sim
