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 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