class LCMForceDisplay(object):
'''
Displays foot force sensor signals in a status bar widget or label widget
'''
def onAtlasState(self,msg):
self.l_foot_force_z = msg.force_torque.l_foot_force_z
self.r_foot_force_z = msg.force_torque.r_foot_force_z
def __init__(self, channel, statusBar=None):
self.sub = lcmUtils.addSubscriber(channel, lcmdrc.atlas_state_t, self.onAtlasState)
self.label = QtGui.QLabel('')
statusBar.addPermanentWidget(self.label)
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.showRate
self.timer.start()
self.l_foot_force_z = 0
self.r_foot_force_z = 0
def __del__(self):
lcmUtils.removeSubscriber(self.sub)
def showRate(self):
global leftInContact, rightInContact
self.label.text = '%.2f | %.2f' % (self.l_foot_force_z,self.r_foot_force_z)
def showHandCloud(hand='left', view=None):
view = view or app.getCurrentRenderView()
if view is None:
return
assert hand in ('left', 'right')
maps = om.findObjectByName('Map Server')
assert maps is not None
viewId = 52 if hand == 'left' else 53
reader = maps.source.reader
def getCurrentViewId():
return reader.GetCurrentMapId(viewId)
p = vtk.vtkPolyData()
obj = showPolyData(p, '%s hand cloud' % hand, view=view, parent='sensors')
obj.currentViewId = -1
def updateCloud():
currentViewId = getCurrentViewId()
#print 'updateCloud: current view id:', currentViewId
if currentViewId != obj.currentViewId:
reader.GetDataForMapId(viewId, currentViewId, p)
#print 'updated poly data. %d points.' % p.GetNumberOfPoints()
obj._renderAllViews()
t = TimerCallback()
t.targetFps = 1
t.callback = updateCloud
t.start()
obj.updater = t
return obj
def main():
app = consoleapp.ConsoleApp()
meshCollection = lcmobjectcollection.LCMObjectCollection('MESH_COLLECTION_COMMAND')
affordanceCollection = lcmobjectcollection.LCMObjectCollection('AFFORDANCE_COLLECTION_COMMAND')
meshCollection.sendEchoRequest()
affordanceCollection.sendEchoRequest()
def printCollection():
print
print '----------------------------------------------------'
print datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S')
print '%d affordances' % len(affordanceCollection.collection)
for desc in affordanceCollection.collection.values():
print
print 'name:', desc['Name']
print 'type:', desc['classname']
timer = TimerCallback(targetFps=0.2)
timer.callback = printCollection
timer.start()
#app.showPythonConsole()
app.start()
def main():
app = consoleapp.ConsoleApp()
meshCollection = lcmobjectcollection.LCMObjectCollection(
'MESH_COLLECTION_COMMAND')
affordanceCollection = lcmobjectcollection.LCMObjectCollection(
'AFFORDANCE_COLLECTION_COMMAND')
meshCollection.sendEchoRequest()
affordanceCollection.sendEchoRequest()
def printCollection():
print
print '----------------------------------------------------'
print datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S')
print '%d affordances' % len(affordanceCollection.collection)
for desc in affordanceCollection.collection.values():
print
print 'name:', desc['Name']
print 'type:', desc['classname']
timer = TimerCallback(targetFps=0.2)
timer.callback = printCollection
timer.start()
#app.showPythonConsole()
app.start()
def start(self):
if self.reader is None:
self.reader = drc.vtkMultisenseSource()
self.reader.InitBotConfig(drcargs.args().config_file)
self.reader.SetDistanceRange(0.25, 4.0)
self.reader.Start()
TimerCallback.start(self)
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 BlackoutMonitor(object):
UPDATE_RATE = 5
AVERAGE_N = 5
def __init__(self, robotStateJointController, view, cameraview, mapServerSource):
self.robotStateJointController = robotStateJointController
self.view = view
self.cameraview = cameraview
self.mapServerSource = mapServerSource
self.lastCameraMessageTime = 0
self.lastScanBundleMessageTime = 0
self.lastBlackoutLengths = []
self.lastBlackoutLength = 0
self.inBlackout = False
self.averageBlackoutLength = 0.0
self.txt = vis.updateText("DATA AGE: 0 sec", "Data Age Text", view=self.view)
self.txt.addProperty('Show Avg Duration', False)
self.txt.setProperty('Visible', False)
self.updateTimer = TimerCallback(self.UPDATE_RATE)
self.updateTimer.callback = self.update
self.updateTimer.start()
def update(self):
self.lastCameraMessageTime = self.cameraview.imageManager.queue.getCurrentImageTime('CAMERA_LEFT')
self.lastScanBundleMessageTime = self.mapServerSource.reader.GetLastScanBundleUTime()
if self.robotStateJointController.lastRobotStateMessage:
elapsedCam = max((self.robotStateJointController.lastRobotStateMessage.utime - self.lastCameraMessageTime) / (1000*1000), 0.0)
elapsedScan = max((self.robotStateJointController.lastRobotStateMessage.utime - self.lastScanBundleMessageTime) / (1000*1000), 0.0)
# can't be deleted, only hidden, so this is ok
if (self.txt.getProperty('Visible')):
if (self.txt.getProperty('Show Avg Duration')):
textstr = "CAM AGE: %02d sec\nSCAN AGE: %02d sec AVG: %02d sec" % (math.floor(elapsedCam), math.floor(elapsedScan), math.floor(self.averageBlackoutLength))
else:
textstr = "CAM AGE: %02d sec\nSCAN AGE: %02d sec" % (math.floor(elapsedCam), math.floor(elapsedScan))
ssize = self.view.size
self.txt.setProperty('Text', textstr)
self.txt.setProperty('Position', [10, 10])
# count out blackouts
if elapsedCam > 1.0:
self.inBlackout = True
self.lastBlackoutLength = elapsedCam
else:
if (self.inBlackout):
# Don't count huge time jumps due to init
if (self.lastBlackoutLength < 100000):
self.lastBlackoutLengths.append(self.lastBlackoutLength)
if len(self.lastBlackoutLengths) > self.AVERAGE_N:
self.lastBlackoutLengths.pop(0)
if len(self.lastBlackoutLengths) > 0:
self.averageBlackoutLength = sum(self.lastBlackoutLengths) / float(len(self.lastBlackoutLengths))
self.inBlackout = False
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 CommittedRobotPlanListener(object):
def __init__(self):
self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN',
lcmdrc.robot_plan_t,
self.onRobotPlan)
lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t,
self.onPause)
self.animationTimer = None
def onPause(self, msg):
commandStream.stopStreaming()
if self.animationTimer:
self.animationTimer.stop()
def onRobotPlan(self, msg):
playback = planplayback.PlanPlayback()
playback.interpolationMethod = 'pchip'
poseTimes, poses = playback.getPlanPoses(msg)
f = playback.getPoseInterpolator(poseTimes, poses)
print 'received robot plan, %.2f seconds' % (poseTimes[-1] -
poseTimes[0])
commandStream.applyPlanDefaults()
commandStream.startStreaming()
timer = simpletimer.SimpleTimer()
def setPose(pose):
commandStream.setGoalPose(pose)
def updateAnimation():
tNow = timer.elapsed()
if tNow > poseTimes[-1]:
pose = poses[-1]
setPose(pose)
commandStream.applyDefaults()
print 'plan ended.'
return False
pose = f(tNow)
setPose(pose)
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 1000
self.animationTimer.callback = updateAnimation
self.animationTimer.start()
class TriggerFingerPublisher(): def __init__(self, lcmChannel): self.lcmChannel = lcmChannel self.reader = midi.MidiReader() self.timer = TimerCallback() self.timer.callback = self.tick self.msg = lcmdrc.trigger_finger_t() def startPublishing(self): print 'Publishing on ' + self.lcmChannel self.timer.start() def publish(self): messages = self.reader.getMessages() for message in messages: channel = message[2] val = message[3] / 127.0 if channel is 102: self.msg.slider1 = val elif channel is 103: self.msg.slider2 = val elif channel is 104: self.msg.slider3 = val elif channel is 105: self.msg.slider4 = val elif channel is 106: self.msg.knob1 = val elif channel is 107: self.msg.knob2 = val elif channel is 108: self.msg.knob3 = val elif channel is 109: self.msg.knob4 = val elif channel is 110: self.msg.knob5 = val elif channel is 111: self.msg.knob6 = val elif channel is 112: self.msg.knob7 = val elif channel is 113: self.msg.knob8 = val if len(messages) is not 0: self.msg.utime = getUtime() lcmUtils.publish(self.lcmChannel, self.msg) def tick(self): self.publish()
class CommittedRobotPlanListener(object):
def __init__(self):
self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN', lcmdrc.robot_plan_t, self.onRobotPlan)
lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t, self.onPause)
self.animationTimer = None
def onPause(self, msg):
commandStream.stopStreaming()
if self.animationTimer:
self.animationTimer.stop()
def onRobotPlan(self, msg):
playback = planplayback.PlanPlayback()
playback.interpolationMethod = 'pchip'
poseTimes, poses = playback.getPlanPoses(msg)
f = playback.getPoseInterpolator(poseTimes, poses)
print 'received robot plan, %.2f seconds' % (poseTimes[-1] - poseTimes[0])
commandStream.applyPlanDefaults()
commandStream.startStreaming()
timer = simpletimer.SimpleTimer()
def setPose(pose):
commandStream.setGoalPose(pose)
def updateAnimation():
tNow = timer.elapsed()
if tNow > poseTimes[-1]:
pose = poses[-1]
setPose(pose)
commandStream.applyDefaults()
print 'plan ended.'
return False
pose = f(tNow)
setPose(pose)
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 1000
self.animationTimer.callback = updateAnimation
self.animationTimer.start()
class TriggerFingerPublisher():
def __init__(self, lcmChannel):
self.lcmChannel = lcmChannel
self.reader = midi.MidiReader()
self.timer = TimerCallback()
self.timer.callback = self.tick
self.msg = lcmdrc.trigger_finger_t()
def startPublishing(self):
print 'Publishing on ' + self.lcmChannel
self.timer.start()
def publish(self):
messages = self.reader.getMessages()
for message in messages:
channel = message[2]
val = message[3] / 127.0
if channel is 102:
self.msg.slider1 = val
elif channel is 103:
self.msg.slider2 = val
elif channel is 104:
self.msg.slider3 = val
elif channel is 105:
self.msg.slider4 = val
elif channel is 106:
self.msg.knob1 = val
elif channel is 107:
self.msg.knob2 = val
elif channel is 108:
self.msg.knob3 = val
elif channel is 109:
self.msg.knob4 = val
elif channel is 110:
self.msg.knob5 = val
elif channel is 111:
self.msg.knob6 = val
elif channel is 112:
self.msg.knob7 = val
elif channel is 113:
self.msg.knob8 = val
if len(messages) is not 0:
self.msg.utime = getUtime()
lcmUtils.publish(self.lcmChannel, self.msg)
def tick(self):
self.publish()
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 ImageWidget(object):
def __init__(self, imageManager, imageName, view):
self.view = view
self.imageManager = imageManager
self.imageName = imageName
self.updateUtime = 0
self.initialized = False
self.imageWidget = vtk.vtkLogoWidget()
rep = self.imageWidget.GetRepresentation()
rep.GetImageProperty().SetOpacity(1.0)
self.imageWidget.SetInteractor(self.view.renderWindow().GetInteractor())
self.flip = vtk.vtkImageFlip()
self.flip.SetFilteredAxis(1)
self.flip.SetInput(imageManager.getImage(imageName))
rep.SetImage(self.flip.GetOutput())
self.timerCallback = TimerCallback()
self.timerCallback.targetFps = 60
self.timerCallback.callback = self.updateView
self.timerCallback.start()
def hide(self):
self.imageWidget.Off()
def show(self):
self.imageWidget.On()
def updateView(self):
if not self.view.isVisible():
return
currentUtime = self.imageManager.updateImage(self.imageName)
if currentUtime != self.updateUtime:
if not self.initialized:
self.show()
self.initialized = True
self.updateUtime = currentUtime
self.flip.Update()
self.view.render()
class ControllerRateLabel(object):
'''
Displays a controller frequency in the status bar
'''
def __init__(self, atlasDriver, statusBar):
self.atlasDriver = atlasDriver
self.label = QtGui.QLabel('')
statusBar.addPermanentWidget(self.label)
self.timer = TimerCallback(targetFps=1)
self.timer.callback = self.showRate
self.timer.start()
def showRate(self):
rate = self.atlasDriver.getControllerRate()
rate = 'unknown' if rate is None else '%d hz' % rate
self.label.text = 'Controller rate: %s' % rate
class ImageWidget(object):
def __init__(self, imageManager, imageName, view):
self.view = view
self.imageManager = imageManager
self.imageName = imageName
self.updateUtime = 0
self.initialized = False
self.imageWidget = vtk.vtkLogoWidget()
rep = self.imageWidget.GetRepresentation()
rep.GetImageProperty().SetOpacity(1.0)
self.imageWidget.SetInteractor(
self.view.renderWindow().GetInteractor())
self.flip = vtk.vtkImageFlip()
self.flip.SetFilteredAxis(1)
self.flip.SetInput(imageManager.getImage(imageName))
rep.SetImage(self.flip.GetOutput())
self.timerCallback = TimerCallback()
self.timerCallback.targetFps = 60
self.timerCallback.callback = self.updateView
self.timerCallback.start()
def hide(self):
self.imageWidget.Off()
def show(self):
self.imageWidget.On()
def updateView(self):
if not self.view.isVisible():
return
currentUtime = self.imageManager.updateImage(self.imageName)
if currentUtime != self.updateUtime:
if not self.initialized:
self.show()
self.initialized = True
self.updateUtime = currentUtime
self.flip.Update()
self.view.render()
class ControllerRateLabel(object):
'''
Displays a controller frequency in the status bar
'''
def __init__(self, atlasDriver, statusBar):
self.atlasDriver = atlasDriver
self.label = QtGui.QLabel('')
statusBar.addPermanentWidget(self.label)
self.timer = TimerCallback(targetFps=1)
self.timer.callback = self.showRate
self.timer.start()
def showRate(self):
rate = self.atlasDriver.getControllerRate()
rate = 'unknown' if rate is None else '%d hz' % rate
self.label.text = 'Controller rate: %s' % rate
class AffordanceTextureUpdater(object):
def __init__(self, affordanceManager):
self.affordanceManager = affordanceManager
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.updateTextures
self.timer.start()
def updateTexture(self, obj):
if obj.getProperty('Camera Texture Enabled'):
cameraview.applyCameraTexture(obj, cameraview.imageManager)
else:
cameraview.disableCameraTexture(obj)
obj._renderAllViews()
def updateTextures(self):
for aff in affordanceManager.getAffordances():
self.updateTexture(aff)
class FrameVisualizationPanel(object):
def __init__(self, view):
self.view = view
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddFrameVisualization.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.botFrameUpdater = BotFrameUpdater(self.ui.botFramesListWidget)
robotModel = om.findObjectByName('robot state model')
self.linkFrameUpdater = LinkFrameUpdater(robotModel,
self.ui.linkFramesListWidget)
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.ui.scrollArea.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)',
self.onEvent)
PythonQt.dd.ddGroupBoxHider(self.ui.botFramesGroup)
PythonQt.dd.ddGroupBoxHider(self.ui.linkFramesGroup)
self.updateTimer = TimerCallback(targetFps=60)
self.updateTimer.callback = self.updateFrames
self.updateTimer.start()
def onEvent(self, obj, event):
minSize = self.ui.scrollArea.widget().minimumSizeHint.width(
) + self.ui.scrollArea.verticalScrollBar().width
self.ui.scrollArea.setMinimumWidth(minSize)
def updateFrames(self):
self.botFrameUpdater.updateFrames()
def getNameFilter(self):
return str(self.ui.botFramesFilterEdit.text)
def onNameFilterChanged(self):
filter = self.getNameFilter()
class FrameVisualizationPanel(object):
def __init__(self, view):
self.view = view
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddFrameVisualization.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.botFrameUpdater = BotFrameUpdater(self.ui.botFramesListWidget)
robotModel = om.findObjectByName('robot state model')
self.linkFrameUpdater = LinkFrameUpdater(robotModel, self.ui.linkFramesListWidget)
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.ui.scrollArea.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.onEvent)
PythonQt.dd.ddGroupBoxHider(self.ui.botFramesGroup)
PythonQt.dd.ddGroupBoxHider(self.ui.linkFramesGroup)
self.updateTimer = TimerCallback(targetFps=60)
self.updateTimer.callback = self.updateFrames
self.updateTimer.start()
def onEvent(self, obj, event):
minSize = self.ui.scrollArea.widget().minimumSizeHint.width() + self.ui.scrollArea.verticalScrollBar().width
self.ui.scrollArea.setMinimumWidth(minSize)
def updateFrames(self):
self.botFrameUpdater.updateFrames()
def getNameFilter(self):
return str(self.ui.botFramesFilterEdit.text)
def onNameFilterChanged(self):
filter = self.getNameFilter()
class AffordanceTextureUpdater(object):
def __init__(self, affordanceManager):
self.affordanceManager = affordanceManager
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.updateTextures
self.timer.start()
def updateTexture(self, obj):
if obj.getProperty('Camera Texture Enabled'):
cameraview.applyCameraTexture(obj, cameraview.imageManager)
else:
cameraview.disableCameraTexture(obj)
obj._renderAllViews()
def updateTextures(self):
for aff in affordanceManager.getAffordances():
self.updateTexture(aff)
def startSwarmVisualization():
global timerCallback, nav_data, nav_cloud
nav_cloud = vtknp.getVtkPolyDataFromNumpyPoints(nav_data)
nav_cloud_obj = vis.showPolyData(shallowCopy(nav_cloud), 'nav data')
nav_cloud_obj.initialized = False
def updateSwarm():
global nav_cloud
if not nav_cloud_obj.initialized:
nav_cloud_obj.mapper.SetColorModeToMapScalars()
nav_cloud_obj.initialized = True
#print nav_data.shape[0], nav_cloud.GetNumberOfPoints()
nav_cloud = vtknp.getVtkPolyDataFromNumpyPoints(nav_data)
nav_cloud_obj.setPolyData(shallowCopy(nav_cloud))
#print nav_cloud_obj.polyData.GetNumberOfPoints()
timerCallback = TimerCallback(targetFps=30)
timerCallback.callback = updateSwarm
timerCallback.start()
def startSwarmVisualization():
global timerCallback, nav_data, nav_cloud
nav_cloud = vtknp.getVtkPolyDataFromNumpyPoints(nav_data)
nav_cloud_obj = vis.showPolyData(shallowCopy(nav_cloud), 'nav data')
nav_cloud_obj.initialized = False
def updateSwarm():
global nav_cloud
if not nav_cloud_obj.initialized:
nav_cloud_obj.mapper.SetColorModeToMapScalars()
nav_cloud_obj.initialized = True
#print nav_data.shape[0], nav_cloud.GetNumberOfPoints()
nav_cloud = vtknp.getVtkPolyDataFromNumpyPoints(nav_data)
nav_cloud_obj.setPolyData(shallowCopy(nav_cloud))
#print nav_cloud_obj.polyData.GetNumberOfPoints()
timerCallback = TimerCallback(targetFps=30)
timerCallback.callback = updateSwarm
timerCallback.start()
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 Gamepad(object):
def __init__(self, teleopPanel, teleopJointController, ikPlanner, view):
lcmUtils.addSubscriber('GAMEPAD_CHANNEL', lcmdrc.gamepad_cmd_t, self.onGamepadCommand)
self.speedMultiplier = 1.0
self.maxSpeedMultiplier = 3.0
self.minSpeedMultiplier = 0.2
self.travel = 1
self.angularTravel = 180
self.baseFrame = None
self.resetDeltas()
self.timer = TimerCallback()
self.timer.callback = self.tick
self.teleopPanel = teleopPanel
self.teleopJointController = teleopJointController
self.ikPlanner = ikPlanner
self.view = view
self.camera = view.camera()
self.cameraTarget = np.asarray(self.camera.GetFocalPoint())
self.endEffectorFrames = []
self.endEffectorIndex = 0
self.teleopOn = False
self.keyFramePoses = list()
self.cameraMode = False
self.timerStarted = False
def findEndEffectors(self):
planning = om.getOrCreateContainer('planning')
if planning is not None:
teleopFolder = planning.findChild('teleop plan')
if teleopFolder is not None:
framesFolder = teleopFolder.findChild('constraint frames')
if framesFolder is not None:
self.endEffectorFrames = framesFolder.children()
def cycleEndEffector(self):
if len(self.endEffectorFrames) is not 0:
self.clearKeyframePoses()
self.endEffectorFrames[self.endEffectorIndex].setProperty('Edit', False)
self.endEffectorIndex = (self.endEffectorIndex + 1) % len(self.endEffectorFrames)
self.endEffectorFrames[self.endEffectorIndex].setProperty('Edit', True)
om.setActiveObject(self.endEffectorFrames[self.endEffectorIndex])
self.baseFrame = self.endEffectorFrames[self.endEffectorIndex]
self.baseTransform = transformUtils.copyFrame(self.baseFrame.transform)
self.resetDeltas()
self.addKeyframePose()
def clearKeyframePoses(self):
self.keyFramePoses = list()
def addKeyframePose(self):
self.keyFramePoses.append(self.teleopJointController.q.copy())
def finalizeTrajectory(self):
self.ikPlanner.computeMultiPostureGoal(self.keyFramePoses)
def tick(self):
self.updateFrame()
def resetDeltas(self):
self.theta = 0.0
self.phi = 0.0
self.yaw = 0.0
self.X = 0.0
self.Y = 0.0
self.Z = 0.0
self.thetadot = 0.0
self.phidot = 0.0
self.yawdot = 0.0
self.Xdot = 0.0
self.Ydot = 0.0
self.Zdot = 0.0
def toggleTeleop(self):
self.teleopOn = not self.teleopOn
if self.teleopOn:
self.teleopPanel.endEffectorTeleop.activate()
self.teleopPanel.endEffectorTeleop.setLHandConstraint('ee fixed')
self.teleopPanel.endEffectorTeleop.setRHandConstraint('ee fixed')
else:
self.teleopPanel.endEffectorTeleop.deactivate()
def increaseTeleopSpeed(self):
self.speedMultiplier = self.clampTeleopSpeed(self.speedMultiplier + 0.2)
print 'teleop speed: ' + str(self.speedMultiplier)
def decreaseTeleopSpeed(self):
self.speedMultiplier = self.clampTeleopSpeed(self.speedMultiplier - 0.2)
print 'teleop speed: ' + str(self.speedMultiplier)
def clampTeleopSpeed(self, speed):
return np.clip(speed, self.minSpeedMultiplier, self.maxSpeedMultiplier)
def enableCameraMode(self):
print 'Gamepad camera mode on'
self.cameraMode = True
def disableCameraMode(self):
print 'Gamepad camera mode off'
self.cameraMode = False
def setCameraTarget(self):
if self.baseFrame is not None:
self.cameraTarget = np.asarray(self.baseFrame.transform.GetPosition())
def onGamepadCommand(self, msg):
if self.timerStarted is not True:
self.timer.start()
self.timerStarted = True
if msg.button == msg.GAMEPAD_BUTTON_A and msg.button_a == 1:
self.finalizeTrajectory()
if msg.button == msg.GAMEPAD_DPAD_X and msg.button_dpad_x < 0:
self.enableCameraMode()
self.setCameraTarget()
if msg.button == msg.GAMEPAD_DPAD_X and msg.button_dpad_x > 0:
self.disableCameraMode()
if msg.button == msg.GAMEPAD_BUTTON_Y and msg.button_y == 1:
self.findEndEffectors()
self.cycleEndEffector()
self.setCameraTarget()
if msg.button == msg.GAMEPAD_DPAD_Y and msg.button_dpad_y > 0:
self.increaseTeleopSpeed()
if msg.button == msg.GAMEPAD_DPAD_Y and msg.button_dpad_y < 0:
self.decreaseTeleopSpeed()
if msg.button == msg.GAMEPAD_BUTTON_START and msg.button_start == 1:
self.toggleTeleop()
if msg.button == msg.GAMEPAD_BUTTON_X and msg.button_x == 1:
self.addKeyframePose()
if msg.button == msg.GAMEPAD_BUTTON_LB:
self.yawdot = -self.angularTravel * msg.button_lb
if msg.button == msg.GAMEPAD_BUTTON_RB:
self.yawdot = self.angularTravel * msg.button_rb
if msg.button == msg.GAMEPAD_LEFT_X or msg.button == msg.GAMEPAD_LEFT_Y:
self.thetadot = msg.thumbpad_left_x * self.angularTravel
self.phidot = msg.thumbpad_left_y * self.angularTravel
if msg.button == msg.GAMEPAD_RIGHT_X or msg.button == msg.GAMEPAD_RIGHT_Y:
self.Xdot = self.travel * msg.thumbpad_right_x
self.Zdot = self.travel * msg.thumbpad_right_y
if msg.button == msg.GAMEPAD_TRIGGER_L:
self.Ydot = -self.travel * msg.trigger_left
if msg.button == msg.GAMEPAD_TRIGGER_R:
self.Ydot = self.travel * msg.trigger_right
def updateFrame(self):
norm = np.linalg.norm(np.array([self.thetadot, self.phidot, self.yawdot, self.Xdot, self.Ydot, self.Zdot]))
dt = 0.01 #self.timer.elapsed
if self.baseFrame is not None:
cameraFocus = np.asarray(self.camera.GetFocalPoint())
cameraInterp = cameraFocus + (self.cameraTarget - cameraFocus)*0.02
self.camera.SetFocalPoint(cameraInterp)
if self.cameraMode is not True:
if self.baseFrame is not None and norm > 0.1:
t = vtk.vtkTransform()
t.Concatenate(self.baseFrame.transform)
t.RotateZ(-self.thetadot * dt * self.speedMultiplier)
t.RotateX(self.phidot * dt * self.speedMultiplier)
t.RotateY(self.yawdot * dt * self.speedMultiplier)
t.Translate(self.Xdot * dt * self.speedMultiplier, self.Ydot * dt * self.speedMultiplier, self.Zdot * dt * self.speedMultiplier)
self.baseFrame.copyFrame(t)
else:
self.camera.Elevation(self.Zdot * self.speedMultiplier)
self.camera.Azimuth(self.Xdot * self.speedMultiplier)
self.camera.Zoom(1.0 + self.phidot/1000.0)
self.view.render()
class AtlasCommandPanel(object):
def __init__(self):
self.app = ConsoleApp()
self.view = self.app.createView()
self.robotSystem = robotsystem.create(self.view)
jointGroups = getJointGroups()
self.jointTeleopPanel = JointTeleopPanel(self.robotSystem, jointGroups)
self.jointCommandPanel = JointCommandPanel(self.robotSystem)
self.jointCommandPanel.ui.speedSpinBox.setEnabled(False)
self.jointCommandPanel.ui.mirrorArmsCheck.setChecked(
self.jointTeleopPanel.mirrorArms)
self.jointCommandPanel.ui.mirrorLegsCheck.setChecked(
self.jointTeleopPanel.mirrorLegs)
self.jointCommandPanel.ui.resetButton.connect(
'clicked()', self.resetJointTeleopSliders)
self.jointCommandPanel.ui.mirrorArmsCheck.connect(
'clicked()', self.mirrorJointsChanged)
self.jointCommandPanel.ui.mirrorLegsCheck.connect(
'clicked()', self.mirrorJointsChanged)
self.widget = QtGui.QWidget()
gl = QtGui.QGridLayout(self.widget)
gl.addWidget(self.app.showObjectModel(), 0, 0, 4,
1) # row, col, rowspan, colspan
gl.addWidget(self.view, 0, 1, 1, 1)
gl.addWidget(self.jointCommandPanel.widget, 1, 1, 1, 1)
gl.addWidget(self.jointTeleopPanel.widget, 0, 2, -1, 1)
gl.setRowStretch(0, 1)
gl.setColumnStretch(1, 1)
#self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN', lcmdrc.robot_plan_t, self.onRobotPlan)
lcmUtils.addSubscriber('STEERING_COMMAND_POSITION_GOAL',
lcmdrc.joint_position_goal_t,
self.onSingleJointPositionGoal)
lcmUtils.addSubscriber('THROTTLE_COMMAND_POSITION_GOAL',
lcmdrc.joint_position_goal_t,
self.onSingleJointPositionGoal)
def onSingleJointPositionGoal(self, msg):
jointPositionGoal = msg.joint_position
jointName = msg.joint_name
allowedJointNames = ['l_leg_aky', 'l_arm_lwy']
if not (jointName in allowedJointNames):
print 'Position goals are not allowed for this joint'
print 'ignoring this position goal'
print 'use the sliders instead'
return
if (jointName == 'l_arm_lwy') and (
not self.jointCommandPanel.steeringControlEnabled):
print 'Steering control not enabled'
print 'ignoring steering command'
return
if (jointName == 'l_leg_aky') and (
not self.jointCommandPanel.throttleControlEnabled):
print 'Throttle control not enabled'
print 'ignoring throttle command'
return
jointIdx = self.jointTeleopPanel.toJointIndex(joint_name)
self.jointTeleopPanel.endPose[jointIdx] = jointPositionGoal
self.jointTeleopPanel.updateSliders()
self.jointTeleopPanel.sliderChanged(jointName)
def onRobotPlan(self, msg):
playback = planplayback.PlanPlayback()
playback.interpolationMethod = 'pchip'
poseTimes, poses = playback.getPlanPoses(msg)
f = playback.getPoseInterpolator(poseTimes, poses)
jointController = self.robotSystem.teleopJointController
timer = simpletimer.SimpleTimer()
def setPose(pose):
jointController.setPose('plan_playback', pose)
self.jointTeleopPanel.endPose = pose
self.jointTeleopPanel.updateSliders()
commandStream.setGoalPose(pose)
def updateAnimation():
tNow = timer.elapsed()
if tNow > poseTimes[-1]:
pose = poses[-1]
setPose(pose)
return False
pose = f(tNow)
setPose(pose)
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 60
self.animationTimer.callback = updateAnimation
self.animationTimer.start()
def mirrorJointsChanged(self):
self.jointTeleopPanel.mirrorLegs = self.jointCommandPanel.ui.mirrorLegsCheck.checked
self.jointTeleopPanel.mirrorArms = self.jointCommandPanel.ui.mirrorArmsCheck.checked
def resetJointTeleopSliders(self):
self.jointTeleopPanel.resetPoseToRobotState()
class CameraImageView(object):
def __init__(self, imageManager, imageName, viewName=None, view=None):
imageManager.addImage(imageName)
self.imageManager = imageManager
self.viewName = viewName or imageName
self.imageName = imageName
self.imageInitialized = False
self.updateUtime = 0
self.initView(view)
self.initEventFilter()
def getImagePixel(self, displayPoint, restrictToImageDimensions=True):
worldPoint = [0.0, 0.0, 0.0, 0.0]
vtk.vtkInteractorObserver.ComputeDisplayToWorld(
self.view.renderer(), displayPoint[0], displayPoint[1], 0,
worldPoint)
imageDimensions = self.getImage().GetDimensions()
if 0.0 <= worldPoint[0] <= imageDimensions[0] and 0.0 <= worldPoint[
1] <= imageDimensions[1] or not restrictToImageDimensions:
return [worldPoint[0], worldPoint[1], 0.0]
else:
return None
def getWorldPositionAndRay(self, imagePixel, imageUtime=None):
'''
Given an XY image pixel, computes an equivalent ray in the world
coordinate system using the camera to local transform at the given
imageUtime. If imageUtime is None, then the utime of the most recent
image is used.
Returns the camera xyz position in world, and a ray unit vector.
'''
if imageUtime is None:
imageUtime = self.imageManager.getUtime(self.imageName)
# input is pixel u,v, output is unit x,y,z in camera coordinates
cameraPoint = self.imageManager.queue.unprojectPixel(
self.imageName, imagePixel[0], imagePixel[1])
cameraToLocal = vtk.vtkTransform()
self.imageManager.queue.getTransform(self.imageName, 'local',
imageUtime, cameraToLocal)
p = np.array(cameraToLocal.TransformPoint(cameraPoint))
cameraPosition = np.array(cameraToLocal.GetPosition())
ray = p - cameraPosition
ray /= np.linalg.norm(ray)
return cameraPosition, ray
def filterEvent(self, obj, event):
if self.eventFilterEnabled and event.type(
) == QtCore.QEvent.MouseButtonDblClick:
self.eventFilter.setEventHandlerResult(True)
elif event.type() == QtCore.QEvent.KeyPress:
if str(event.text()).lower() == 'p':
self.eventFilter.setEventHandlerResult(True)
elif str(event.text()).lower() == 'r':
self.eventFilter.setEventHandlerResult(True)
self.resetCamera()
def onRubberBandPick(self, obj, event):
displayPoints = self.interactorStyle.GetStartPosition(
), self.interactorStyle.GetEndPosition()
imagePoints = [
vis.pickImage(point, self.view)[1] for point in displayPoints
]
sendFOVRequest(self.imageName, imagePoints)
def getImage(self):
return self.imageManager.getImage(self.imageName)
def initView(self, view):
self.view = view or app.getViewManager().createView(
self.viewName, 'VTK View')
self.view.installImageInteractor()
self.interactorStyle = self.view.renderWindow().GetInteractor(
).GetInteractorStyle()
self.interactorStyle.AddObserver('SelectionChangedEvent',
self.onRubberBandPick)
self.imageActor = vtk.vtkImageActor()
self.imageActor.SetInput(self.getImage())
self.imageActor.SetVisibility(False)
self.view.renderer().AddActor(self.imageActor)
self.timerCallback = TimerCallback()
self.timerCallback.targetFps = 60
self.timerCallback.callback = self.updateView
self.timerCallback.start()
def initEventFilter(self):
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
qvtkwidget = self.view.vtkWidget()
qvtkwidget.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(
QtCore.QEvent.MouseButtonDblClick)
self.eventFilter.addFilteredEventType(QtCore.QEvent.KeyPress)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)',
self.filterEvent)
self.eventFilterEnabled = True
def resetCamera(self):
camera = self.view.camera()
camera.ParallelProjectionOn()
camera.SetFocalPoint(0, 0, 0)
camera.SetPosition(0, 0, -1)
camera.SetViewUp(0, -1, 0)
self.view.resetCamera()
self.fitImageToView()
self.view.render()
def fitImageToView(self):
camera = self.view.camera()
image = self.getImage()
imageWidth, imageHeight, _ = image.GetDimensions()
viewWidth, viewHeight = self.view.renderWindow().GetSize()
aspectRatio = float(viewWidth) / viewHeight
parallelScale = max(imageWidth / aspectRatio, imageHeight) / 2.0
camera.SetParallelScale(parallelScale)
def setImageName(self, imageName):
if imageName == self.imageName:
return
assert self.imageManager.hasImage(imageName)
self.imageName = imageName
self.imageInitialized = False
self.updateUtime = 0
self.imageActor.SetInput(self.imageManager.getImage(self.imageName))
self.imageActor.SetVisibility(False)
self.view.render()
def updateView(self):
if not self.view.isVisible():
return
currentUtime = self.imageManager.updateImage(self.imageName)
if currentUtime != self.updateUtime:
self.updateUtime = currentUtime
self.view.render()
if not self.imageInitialized and self.imageActor.GetInput(
).GetDimensions()[0]:
self.imageActor.SetVisibility(True)
self.resetCamera()
self.imageInitialized = True
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
class BlackoutMonitor(object):
UPDATE_RATE = 5
AVERAGE_N = 5
def __init__(self, robotStateJointController, view, cameraview,
mapServerSource):
self.robotStateJointController = robotStateJointController
self.view = view
self.cameraview = cameraview
self.mapServerSource = mapServerSource
self.lastCameraMessageTime = 0
self.lastScanBundleMessageTime = 0
self.lastBlackoutLengths = []
self.lastBlackoutLength = 0
self.inBlackout = False
self.averageBlackoutLength = 0.0
self.txt = vis.updateText("DATA AGE: 0 sec",
"Data Age Text",
view=self.view)
self.txt.addProperty('Show Avg Duration', False)
self.txt.setProperty('Visible', False)
self.updateTimer = TimerCallback(self.UPDATE_RATE)
self.updateTimer.callback = self.update
self.updateTimer.start()
def update(self):
self.lastCameraMessageTime = self.cameraview.imageManager.queue.getCurrentImageTime(
'CAMERA_LEFT')
self.lastScanBundleMessageTime = self.mapServerSource.reader.GetLastScanBundleUTime(
)
if self.robotStateJointController.lastRobotStateMessage:
elapsedCam = max(
(self.robotStateJointController.lastRobotStateMessage.utime -
self.lastCameraMessageTime) / (1000 * 1000), 0.0)
elapsedScan = max(
(self.robotStateJointController.lastRobotStateMessage.utime -
self.lastScanBundleMessageTime) / (1000 * 1000), 0.0)
# can't be deleted, only hidden, so this is ok
if (self.txt.getProperty('Visible')):
if (self.txt.getProperty('Show Avg Duration')):
textstr = "CAM AGE: %02d sec\nSCAN AGE: %02d sec AVG: %02d sec" % (
math.floor(elapsedCam), math.floor(elapsedScan),
math.floor(self.averageBlackoutLength))
else:
textstr = "CAM AGE: %02d sec\nSCAN AGE: %02d sec" % (
math.floor(elapsedCam), math.floor(elapsedScan))
ssize = self.view.size
self.txt.setProperty('Text', textstr)
self.txt.setProperty('Position', [10, 10])
# count out blackouts
if elapsedCam > 1.0:
self.inBlackout = True
self.lastBlackoutLength = elapsedCam
else:
if (self.inBlackout):
# Don't count huge time jumps due to init
if (self.lastBlackoutLength < 100000):
self.lastBlackoutLengths.append(
self.lastBlackoutLength)
if len(self.lastBlackoutLengths) > self.AVERAGE_N:
self.lastBlackoutLengths.pop(0)
if len(self.lastBlackoutLengths) > 0:
self.averageBlackoutLength = sum(
self.lastBlackoutLengths) / float(
len(self.lastBlackoutLengths))
self.inBlackout = False
class AtlasCommandPanel(object):
def __init__(self):
self.app = ConsoleApp()
self.view = self.app.createView()
self.robotSystem = robotsystem.create(self.view)
jointGroups = getJointGroups()
self.jointTeleopPanel = JointTeleopPanel(self.robotSystem, jointGroups)
self.jointCommandPanel = JointCommandPanel(self.robotSystem)
self.jointCommandPanel.ui.speedSpinBox.setEnabled(False)
self.jointCommandPanel.ui.mirrorArmsCheck.setChecked(self.jointTeleopPanel.mirrorArms)
self.jointCommandPanel.ui.mirrorLegsCheck.setChecked(self.jointTeleopPanel.mirrorLegs)
self.jointCommandPanel.ui.resetButton.connect('clicked()', self.resetJointTeleopSliders)
self.jointCommandPanel.ui.mirrorArmsCheck.connect('clicked()', self.mirrorJointsChanged)
self.jointCommandPanel.ui.mirrorLegsCheck.connect('clicked()', self.mirrorJointsChanged)
self.widget = QtGui.QWidget()
gl = QtGui.QGridLayout(self.widget)
gl.addWidget(self.app.showObjectModel(), 0, 0, 4, 1) # row, col, rowspan, colspan
gl.addWidget(self.view, 0, 1, 1, 1)
gl.addWidget(self.jointCommandPanel.widget, 1, 1, 1, 1)
gl.addWidget(self.jointTeleopPanel.widget, 0, 2, -1, 1)
gl.setRowStretch(0,1)
gl.setColumnStretch(1,1)
#self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN', lcmdrc.robot_plan_t, self.onRobotPlan)
lcmUtils.addSubscriber('STEERING_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal)
lcmUtils.addSubscriber('THROTTLE_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal)
def onSingleJointPositionGoal(self, msg):
jointPositionGoal = msg.joint_position
jointName = msg.joint_name
allowedJointNames = ['l_leg_aky','l_arm_lwy']
if not (jointName in allowedJointNames):
print 'Position goals are not allowed for this joint'
print 'ignoring this position goal'
print 'use the sliders instead'
return
if (jointName == 'l_arm_lwy') and (not self.jointCommandPanel.steeringControlEnabled):
print 'Steering control not enabled'
print 'ignoring steering command'
return
if (jointName == 'l_leg_aky') and (not self.jointCommandPanel.throttleControlEnabled):
print 'Throttle control not enabled'
print 'ignoring throttle command'
return
jointIdx = self.jointTeleopPanel.toJointIndex(joint_name)
self.jointTeleopPanel.endPose[jointIdx] = jointPositionGoal
self.jointTeleopPanel.updateSliders()
self.jointTeleopPanel.sliderChanged(jointName)
def onRobotPlan(self, msg):
playback = planplayback.PlanPlayback()
playback.interpolationMethod = 'pchip'
poseTimes, poses = playback.getPlanPoses(msg)
f = playback.getPoseInterpolator(poseTimes, poses)
jointController = self.robotSystem.teleopJointController
timer = simpletimer.SimpleTimer()
def setPose(pose):
jointController.setPose('plan_playback', pose)
self.jointTeleopPanel.endPose = pose
self.jointTeleopPanel.updateSliders()
commandStream.setGoalPose(pose)
def updateAnimation():
tNow = timer.elapsed()
if tNow > poseTimes[-1]:
pose = poses[-1]
setPose(pose)
return False
pose = f(tNow)
setPose(pose)
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 60
self.animationTimer.callback = updateAnimation
self.animationTimer.start()
def mirrorJointsChanged(self):
self.jointTeleopPanel.mirrorLegs = self.jointCommandPanel.ui.mirrorLegsCheck.checked
self.jointTeleopPanel.mirrorArms = self.jointCommandPanel.ui.mirrorArmsCheck.checked
def resetJointTeleopSliders(self):
self.jointTeleopPanel.resetPoseToRobotState()
class CameraView(object):
def __init__(self, imageManager, view=None):
self.imageManager = imageManager
self.updateUtimes = {}
self.sphereObjects = {}
self.sphereImages = [
'CAMERA_LEFT',
'CAMERACHEST_RIGHT',
'CAMERACHEST_LEFT']
for name in self.sphereImages:
imageManager.addImage(name)
self.updateUtimes[name] = 0
self.initView(view)
self.initEventFilter()
self.rayCallback = rayDebug
self.timerCallback = TimerCallback()
self.timerCallback.targetFps = 60
self.timerCallback.callback = self.updateView
self.timerCallback.start()
def onViewDoubleClicked(self, displayPoint):
obj, pickedPoint = vis.findPickedObject(displayPoint, self.view)
if pickedPoint is None or not obj:
return
imageName = obj.getProperty('Name')
imageUtime = self.imageManager.getUtime(imageName)
cameraToLocal = vtk.vtkTransform()
self.imageManager.queue.getTransform(imageName, 'local', imageUtime, cameraToLocal)
utorsoToLocal = vtk.vtkTransform()
self.imageManager.queue.getTransform('utorso', 'local', imageUtime, utorsoToLocal)
p = range(3)
utorsoToLocal.TransformPoint(pickedPoint, p)
ray = np.array(p) - np.array(cameraToLocal.GetPosition())
ray /= np.linalg.norm(ray)
if self.rayCallback:
self.rayCallback(np.array(cameraToLocal.GetPosition()), ray)
def filterEvent(self, obj, event):
if event.type() == QtCore.QEvent.MouseButtonDblClick:
self.eventFilter.setEventHandlerResult(True)
self.onViewDoubleClicked(vis.mapMousePosition(obj, event))
elif event.type() == QtCore.QEvent.KeyPress:
if str(event.text()).lower() == 'p':
self.eventFilter.setEventHandlerResult(True)
elif str(event.text()).lower() == 'r':
self.eventFilter.setEventHandlerResult(True)
self.resetCamera()
def initEventFilter(self):
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
qvtkwidget = self.view.vtkWidget()
qvtkwidget.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.MouseButtonDblClick)
self.eventFilter.addFilteredEventType(QtCore.QEvent.KeyPress)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.filterEvent)
def initView(self, view):
self.view = view or app.getViewManager().createView('Camera View', 'VTK View')
self.renderers = [self.view.renderer()]
renWin = self.view.renderWindow()
renWin.SetNumberOfLayers(3)
for i in [1, 2]:
ren = vtk.vtkRenderer()
ren.SetLayer(2)
ren.SetActiveCamera(self.view.camera())
renWin.AddRenderer(ren)
self.renderers.append(ren)
def applyCustomBounds():
self.view.addCustomBounds([-100, 100, -100, 100, -100, 100])
self.view.connect('computeBoundsRequest(ddQVTKWidgetView*)', applyCustomBounds)
app.setCameraTerrainModeEnabled(self.view, True)
self.resetCamera()
def resetCamera(self):
self.view.camera().SetViewAngle(90)
self.view.camera().SetPosition(-7.5, 0.0, 5.0)
self.view.camera().SetFocalPoint(0.0, 0.0, 0.0)
self.view.camera().SetViewUp(0.0, 0.0, 1.0)
self.view.render()
def getSphereGeometry(self, imageName):
sphereObj = self.sphereObjects.get(imageName)
if sphereObj:
return sphereObj
if not self.imageManager.getImage(imageName).GetDimensions()[0]:
return None
sphereResolution = 50
sphereRadii = {
'CAMERA_LEFT' : 20,
'CAMERACHEST_LEFT' : 20,
'CAMERACHEST_RIGHT' : 20
}
geometry = makeSphere(sphereRadii[imageName], sphereResolution)
self.imageManager.queue.computeTextureCoords(imageName, geometry)
tcoordsArrayName = 'tcoords_%s' % imageName
vtkNumpy.addNumpyToVtk(geometry, vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:,0].copy(), 'tcoords_U')
vtkNumpy.addNumpyToVtk(geometry, vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:,1].copy(), 'tcoords_V')
geometry = clipRange(geometry, 'tcoords_U', [0.0, 1.0])
geometry = clipRange(geometry, 'tcoords_V', [0.0, 1.0])
geometry.GetPointData().SetTCoords(geometry.GetPointData().GetArray(tcoordsArrayName))
sphereObj = vis.showPolyData(geometry, imageName, view=self.view, parent='cameras')
sphereObj.actor.SetTexture(self.imageManager.getTexture(imageName))
sphereObj.actor.GetProperty().LightingOff()
self.view.renderer().RemoveActor(sphereObj.actor)
rendererId = 2 - self.sphereImages.index(imageName)
self.renderers[rendererId].AddActor(sphereObj.actor)
self.sphereObjects[imageName] = sphereObj
return sphereObj
def updateSphereGeometry(self):
for imageName in self.sphereImages:
sphereObj = self.getSphereGeometry(imageName)
if not sphereObj:
continue
transform = vtk.vtkTransform()
self.imageManager.queue.getBodyToCameraTransform(imageName, transform)
sphereObj.actor.SetUserTransform(transform.GetLinearInverse())
def updateImages(self):
updated = False
for imageName, lastUtime in self.updateUtimes.iteritems():
currentUtime = self.imageManager.updateImage(imageName)
if currentUtime != lastUtime:
self.updateUtimes[imageName] = currentUtime
updated = True
return updated
def updateView(self):
if not self.view.isVisible():
return
if not self.updateImages():
return
self.updateSphereGeometry()
self.view.render()
class CameraView(object):
def __init__(self, imageManager, view=None):
self.imageManager = imageManager
self.updateUtimes = {}
self.sphereObjects = {}
self.sphereImages = [
'CAMERA_LEFT', 'CAMERACHEST_RIGHT', 'CAMERACHEST_LEFT'
]
for name in self.sphereImages:
imageManager.addImage(name)
self.updateUtimes[name] = 0
self.initView(view)
self.initEventFilter()
self.rayCallback = rayDebug
self.timerCallback = TimerCallback()
self.timerCallback.targetFps = 60
self.timerCallback.callback = self.updateView
self.timerCallback.start()
def onViewDoubleClicked(self, displayPoint):
obj, pickedPoint = vis.findPickedObject(displayPoint, self.view)
if pickedPoint is None or not obj:
return
imageName = obj.getProperty('Name')
imageUtime = self.imageManager.getUtime(imageName)
cameraToLocal = vtk.vtkTransform()
self.imageManager.queue.getTransform(imageName, 'local', imageUtime,
cameraToLocal)
utorsoToLocal = vtk.vtkTransform()
self.imageManager.queue.getTransform('utorso', 'local', imageUtime,
utorsoToLocal)
p = range(3)
utorsoToLocal.TransformPoint(pickedPoint, p)
ray = np.array(p) - np.array(cameraToLocal.GetPosition())
ray /= np.linalg.norm(ray)
if self.rayCallback:
self.rayCallback(np.array(cameraToLocal.GetPosition()), ray)
def filterEvent(self, obj, event):
if event.type() == QtCore.QEvent.MouseButtonDblClick:
self.eventFilter.setEventHandlerResult(True)
self.onViewDoubleClicked(vis.mapMousePosition(obj, event))
elif event.type() == QtCore.QEvent.KeyPress:
if str(event.text()).lower() == 'p':
self.eventFilter.setEventHandlerResult(True)
elif str(event.text()).lower() == 'r':
self.eventFilter.setEventHandlerResult(True)
self.resetCamera()
def initEventFilter(self):
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
qvtkwidget = self.view.vtkWidget()
qvtkwidget.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(
QtCore.QEvent.MouseButtonDblClick)
self.eventFilter.addFilteredEventType(QtCore.QEvent.KeyPress)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)',
self.filterEvent)
def initView(self, view):
self.view = view or app.getViewManager().createView(
'Camera View', 'VTK View')
self.renderers = [self.view.renderer()]
renWin = self.view.renderWindow()
renWin.SetNumberOfLayers(3)
for i in [1, 2]:
ren = vtk.vtkRenderer()
ren.SetLayer(2)
ren.SetActiveCamera(self.view.camera())
renWin.AddRenderer(ren)
self.renderers.append(ren)
def applyCustomBounds():
self.view.addCustomBounds([-100, 100, -100, 100, -100, 100])
self.view.connect('computeBoundsRequest(ddQVTKWidgetView*)',
applyCustomBounds)
app.setCameraTerrainModeEnabled(self.view, True)
self.resetCamera()
def resetCamera(self):
self.view.camera().SetViewAngle(90)
self.view.camera().SetPosition(-7.5, 0.0, 5.0)
self.view.camera().SetFocalPoint(0.0, 0.0, 0.0)
self.view.camera().SetViewUp(0.0, 0.0, 1.0)
self.view.render()
def getSphereGeometry(self, imageName):
sphereObj = self.sphereObjects.get(imageName)
if sphereObj:
return sphereObj
if not self.imageManager.getImage(imageName).GetDimensions()[0]:
return None
sphereResolution = 50
sphereRadii = {
'CAMERA_LEFT': 20,
'CAMERACHEST_LEFT': 20,
'CAMERACHEST_RIGHT': 20
}
geometry = makeSphere(sphereRadii[imageName], sphereResolution)
self.imageManager.queue.computeTextureCoords(imageName, geometry)
tcoordsArrayName = 'tcoords_%s' % imageName
vtkNumpy.addNumpyToVtk(
geometry,
vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 0].copy(),
'tcoords_U')
vtkNumpy.addNumpyToVtk(
geometry,
vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 1].copy(),
'tcoords_V')
geometry = clipRange(geometry, 'tcoords_U', [0.0, 1.0])
geometry = clipRange(geometry, 'tcoords_V', [0.0, 1.0])
geometry.GetPointData().SetTCoords(
geometry.GetPointData().GetArray(tcoordsArrayName))
sphereObj = vis.showPolyData(geometry,
imageName,
view=self.view,
parent='cameras')
sphereObj.actor.SetTexture(self.imageManager.getTexture(imageName))
sphereObj.actor.GetProperty().LightingOff()
self.view.renderer().RemoveActor(sphereObj.actor)
rendererId = 2 - self.sphereImages.index(imageName)
self.renderers[rendererId].AddActor(sphereObj.actor)
self.sphereObjects[imageName] = sphereObj
return sphereObj
def updateSphereGeometry(self):
for imageName in self.sphereImages:
sphereObj = self.getSphereGeometry(imageName)
if not sphereObj:
continue
transform = vtk.vtkTransform()
self.imageManager.queue.getBodyToCameraTransform(
imageName, transform)
sphereObj.actor.SetUserTransform(transform.GetLinearInverse())
def updateImages(self):
updated = False
for imageName, lastUtime in self.updateUtimes.iteritems():
currentUtime = self.imageManager.updateImage(imageName)
if currentUtime != lastUtime:
self.updateUtimes[imageName] = currentUtime
updated = True
return updated
def updateView(self):
if not self.view.isVisible():
return
if not self.updateImages():
return
self.updateSphereGeometry()
self.view.render()
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 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)
ql.setOrientation(QtCore.Qt.Vertical)
return ql
statusLabel = spawnBasicLabel()
l.addWidget(wrapInVTitledItem("Behavior", [statusLabel]))
def statusUpdate():
behavior = atlasDriver.getCurrentBehaviorName()
statusLabel.setText(behavior)
if (behavior != "user"):
statusLabel.setStyleSheet(LABEL_DEFAULT_STYLE_SHEET + "background-color:red; color:white")
else:
statusLabel.setStyleSheet(LABEL_DEFAULT_STYLE_SHEET + "background-color:white; color:black")
statusTimer = TimerCallback(targetFps=FPS)
statusTimer.callback = statusUpdate
statusTimer.start()
controllerLabel = spawnBasicLabel()
l.addWidget(wrapInVTitledItem("Controller", [controllerLabel]))
def controllerUpdate():
status = atlasDriver.getControllerStatus()
if not status:
status = "Unknown"
if len(status) > 6:
status = status[0:6]
rate = atlasDriver.getControllerRate()
if not rate:
rate = 0.0
controllerLabel.setText("%s\n%06.1fhz" % (status, rate))
if (rate < 500):
controllerLabel.setStyleSheet(LABEL_DEFAULT_STYLE_SHEET + "background-color:red; color:white")
class Gamepad(object):
def __init__(self, teleopPanel, teleopJointController, ikPlanner, view):
lcmUtils.addSubscriber('GAMEPAD_CHANNEL', lcmdrc.gamepad_cmd_t,
self.onGamepadCommand)
self.speedMultiplier = 1.0
self.maxSpeedMultiplier = 3.0
self.minSpeedMultiplier = 0.2
self.travel = 1
self.angularTravel = 180
self.baseFrame = None
self.resetDeltas()
self.timer = TimerCallback()
self.timer.callback = self.tick
self.teleopPanel = teleopPanel
self.teleopJointController = teleopJointController
self.ikPlanner = ikPlanner
self.view = view
self.camera = view.camera()
self.cameraTarget = np.asarray(self.camera.GetFocalPoint())
self.endEffectorFrames = []
self.endEffectorIndex = 0
self.teleopOn = False
self.keyFramePoses = list()
self.cameraMode = False
self.timerStarted = False
def findEndEffectors(self):
planning = om.getOrCreateContainer('planning')
if planning is not None:
teleopFolder = planning.findChild('teleop plan')
if teleopFolder is not None:
framesFolder = teleopFolder.findChild('constraint frames')
if framesFolder is not None:
self.endEffectorFrames = framesFolder.children()
def cycleEndEffector(self):
if len(self.endEffectorFrames) is not 0:
self.clearKeyframePoses()
self.endEffectorFrames[self.endEffectorIndex].setProperty(
'Edit', False)
self.endEffectorIndex = (self.endEffectorIndex + 1) % len(
self.endEffectorFrames)
self.endEffectorFrames[self.endEffectorIndex].setProperty(
'Edit', True)
om.setActiveObject(self.endEffectorFrames[self.endEffectorIndex])
self.baseFrame = self.endEffectorFrames[self.endEffectorIndex]
self.baseTransform = transformUtils.copyFrame(
self.baseFrame.transform)
self.resetDeltas()
self.addKeyframePose()
def clearKeyframePoses(self):
self.keyFramePoses = list()
def addKeyframePose(self):
self.keyFramePoses.append(self.teleopJointController.q.copy())
def finalizeTrajectory(self):
self.ikPlanner.computeMultiPostureGoal(self.keyFramePoses)
def tick(self):
self.updateFrame()
def resetDeltas(self):
self.theta = 0.0
self.phi = 0.0
self.yaw = 0.0
self.X = 0.0
self.Y = 0.0
self.Z = 0.0
self.thetadot = 0.0
self.phidot = 0.0
self.yawdot = 0.0
self.Xdot = 0.0
self.Ydot = 0.0
self.Zdot = 0.0
def toggleTeleop(self):
self.teleopOn = not self.teleopOn
if self.teleopOn:
self.teleopPanel.endEffectorTeleop.activate()
self.teleopPanel.endEffectorTeleop.setLHandConstraint('ee fixed')
self.teleopPanel.endEffectorTeleop.setRHandConstraint('ee fixed')
else:
self.teleopPanel.endEffectorTeleop.deactivate()
def increaseTeleopSpeed(self):
self.speedMultiplier = self.clampTeleopSpeed(self.speedMultiplier +
0.2)
print 'teleop speed: ' + str(self.speedMultiplier)
def decreaseTeleopSpeed(self):
self.speedMultiplier = self.clampTeleopSpeed(self.speedMultiplier -
0.2)
print 'teleop speed: ' + str(self.speedMultiplier)
def clampTeleopSpeed(self, speed):
return np.clip(speed, self.minSpeedMultiplier, self.maxSpeedMultiplier)
def enableCameraMode(self):
print 'Gamepad camera mode on'
self.cameraMode = True
def disableCameraMode(self):
print 'Gamepad camera mode off'
self.cameraMode = False
def setCameraTarget(self):
if self.baseFrame is not None:
self.cameraTarget = np.asarray(
self.baseFrame.transform.GetPosition())
def onGamepadCommand(self, msg):
if self.timerStarted is not True:
self.timer.start()
self.timerStarted = True
if msg.button == msg.GAMEPAD_BUTTON_A and msg.button_a == 1:
self.finalizeTrajectory()
if msg.button == msg.GAMEPAD_DPAD_X and msg.button_dpad_x < 0:
self.enableCameraMode()
self.setCameraTarget()
if msg.button == msg.GAMEPAD_DPAD_X and msg.button_dpad_x > 0:
self.disableCameraMode()
if msg.button == msg.GAMEPAD_BUTTON_Y and msg.button_y == 1:
self.findEndEffectors()
self.cycleEndEffector()
self.setCameraTarget()
if msg.button == msg.GAMEPAD_DPAD_Y and msg.button_dpad_y > 0:
self.increaseTeleopSpeed()
if msg.button == msg.GAMEPAD_DPAD_Y and msg.button_dpad_y < 0:
self.decreaseTeleopSpeed()
if msg.button == msg.GAMEPAD_BUTTON_START and msg.button_start == 1:
self.toggleTeleop()
if msg.button == msg.GAMEPAD_BUTTON_X and msg.button_x == 1:
self.addKeyframePose()
if msg.button == msg.GAMEPAD_BUTTON_LB:
self.yawdot = -self.angularTravel * msg.button_lb
if msg.button == msg.GAMEPAD_BUTTON_RB:
self.yawdot = self.angularTravel * msg.button_rb
if msg.button == msg.GAMEPAD_LEFT_X or msg.button == msg.GAMEPAD_LEFT_Y:
self.thetadot = msg.thumbpad_left_x * self.angularTravel
self.phidot = msg.thumbpad_left_y * self.angularTravel
if msg.button == msg.GAMEPAD_RIGHT_X or msg.button == msg.GAMEPAD_RIGHT_Y:
self.Xdot = self.travel * msg.thumbpad_right_x
self.Zdot = self.travel * msg.thumbpad_right_y
if msg.button == msg.GAMEPAD_TRIGGER_L:
self.Ydot = -self.travel * msg.trigger_left
if msg.button == msg.GAMEPAD_TRIGGER_R:
self.Ydot = self.travel * msg.trigger_right
def updateFrame(self):
norm = np.linalg.norm(
np.array([
self.thetadot, self.phidot, self.yawdot, self.Xdot, self.Ydot,
self.Zdot
]))
dt = 0.01 #self.timer.elapsed
if self.baseFrame is not None:
cameraFocus = np.asarray(self.camera.GetFocalPoint())
cameraInterp = cameraFocus + (self.cameraTarget -
cameraFocus) * 0.02
self.camera.SetFocalPoint(cameraInterp)
if self.cameraMode is not True:
if self.baseFrame is not None and norm > 0.1:
t = vtk.vtkTransform()
t.Concatenate(self.baseFrame.transform)
t.RotateZ(-self.thetadot * dt * self.speedMultiplier)
t.RotateX(self.phidot * dt * self.speedMultiplier)
t.RotateY(self.yawdot * dt * self.speedMultiplier)
t.Translate(self.Xdot * dt * self.speedMultiplier,
self.Ydot * dt * self.speedMultiplier,
self.Zdot * dt * self.speedMultiplier)
self.baseFrame.copyFrame(t)
else:
self.camera.Elevation(self.Zdot * self.speedMultiplier)
self.camera.Azimuth(self.Xdot * self.speedMultiplier)
self.camera.Zoom(1.0 + self.phidot / 1000.0)
self.view.render()
class CameraImageView(object):
def __init__(self, imageManager, imageName, viewName=None, view=None):
imageManager.addImage(imageName)
self.imageManager = imageManager
self.viewName = viewName or imageName
self.imageName = imageName
self.imageInitialized = False
self.updateUtime = 0
self.initView(view)
self.initEventFilter()
def getImagePixel(self, displayPoint, restrictToImageDimensions=True):
worldPoint = [0.0, 0.0, 0.0, 0.0]
vtk.vtkInteractorObserver.ComputeDisplayToWorld(self.view.renderer(), displayPoint[0], displayPoint[1], 0, worldPoint)
imageDimensions = self.getImage().GetDimensions()
if 0.0 <= worldPoint[0] <= imageDimensions[0] and 0.0 <= worldPoint[1] <= imageDimensions[1] or not restrictToImageDimensions:
return [worldPoint[0], worldPoint[1], 0.0]
else:
return None
def getWorldPositionAndRay(self, imagePixel, imageUtime=None):
'''
Given an XY image pixel, computes an equivalent ray in the world
coordinate system using the camera to local transform at the given
imageUtime. If imageUtime is None, then the utime of the most recent
image is used.
Returns the camera xyz position in world, and a ray unit vector.
'''
if imageUtime is None:
imageUtime = self.imageManager.getUtime(self.imageName)
# input is pixel u,v, output is unit x,y,z in camera coordinates
cameraPoint = self.imageManager.queue.unprojectPixel(self.imageName, imagePixel[0], imagePixel[1])
cameraToLocal = vtk.vtkTransform()
self.imageManager.queue.getTransform(self.imageName, 'local', imageUtime, cameraToLocal)
p = np.array(cameraToLocal.TransformPoint(cameraPoint))
cameraPosition = np.array(cameraToLocal.GetPosition())
ray = p - cameraPosition
ray /= np.linalg.norm(ray)
return cameraPosition, ray
def filterEvent(self, obj, event):
if self.eventFilterEnabled and event.type() == QtCore.QEvent.MouseButtonDblClick:
self.eventFilter.setEventHandlerResult(True)
elif event.type() == QtCore.QEvent.KeyPress:
if str(event.text()).lower() == 'p':
self.eventFilter.setEventHandlerResult(True)
elif str(event.text()).lower() == 'r':
self.eventFilter.setEventHandlerResult(True)
self.resetCamera()
def onRubberBandPick(self, obj, event):
displayPoints = self.interactorStyle.GetStartPosition(), self.interactorStyle.GetEndPosition()
imagePoints = [vis.pickImage(point, self.view)[1] for point in displayPoints]
sendFOVRequest(self.imageName, imagePoints)
def getImage(self):
return self.imageManager.getImage(self.imageName)
def initView(self, view):
self.view = view or app.getViewManager().createView(self.viewName, 'VTK View')
self.view.installImageInteractor()
self.interactorStyle = self.view.renderWindow().GetInteractor().GetInteractorStyle()
self.interactorStyle.AddObserver('SelectionChangedEvent', self.onRubberBandPick)
self.imageActor = vtk.vtkImageActor()
self.imageActor.SetInput(self.getImage())
self.imageActor.SetVisibility(False)
self.view.renderer().AddActor(self.imageActor)
self.timerCallback = TimerCallback()
self.timerCallback.targetFps = 60
self.timerCallback.callback = self.updateView
self.timerCallback.start()
def initEventFilter(self):
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
qvtkwidget = self.view.vtkWidget()
qvtkwidget.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.MouseButtonDblClick)
self.eventFilter.addFilteredEventType(QtCore.QEvent.KeyPress)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.filterEvent)
self.eventFilterEnabled = True
def resetCamera(self):
camera = self.view.camera()
camera.ParallelProjectionOn()
camera.SetFocalPoint(0,0,0)
camera.SetPosition(0,0,-1)
camera.SetViewUp(0,-1, 0)
self.view.resetCamera()
self.fitImageToView()
self.view.render()
def fitImageToView(self):
camera = self.view.camera()
image = self.getImage()
imageWidth, imageHeight, _ = image.GetDimensions()
viewWidth, viewHeight = self.view.renderWindow().GetSize()
aspectRatio = float(viewWidth)/viewHeight
parallelScale = max(imageWidth/aspectRatio, imageHeight) / 2.0
camera.SetParallelScale(parallelScale)
def setImageName(self, imageName):
if imageName == self.imageName:
return
assert self.imageManager.hasImage(imageName)
self.imageName = imageName
self.imageInitialized = False
self.updateUtime = 0
self.imageActor.SetInput(self.imageManager.getImage(self.imageName))
self.imageActor.SetVisibility(False)
self.view.render()
def updateView(self):
if not self.view.isVisible():
return
currentUtime = self.imageManager.updateImage(self.imageName)
if currentUtime != self.updateUtime:
self.updateUtime = currentUtime
self.view.render()
if not self.imageInitialized and self.imageActor.GetInput().GetDimensions()[0]:
self.imageActor.SetVisibility(True)
self.resetCamera()
self.imageInitialized = True
class PlanPlayback(object):
def __init__(self):
self.animationCallback = None
self.animationTimer = None
self.interpolationMethod = 'slinear'
self.playbackSpeed = 1.0
self.jointNameRegex = ''
@staticmethod
def getPlanPoses(msgOrList):
if isinstance(msgOrList, list):
messages = msgOrList
allPoseTimes, allPoses = PlanPlayback.getPlanPoses(messages[0])
for msg in messages[1:]:
poseTimes, poses = PlanPlayback.getPlanPoses(msg)
poseTimes += allPoseTimes[-1]
allPoseTimes = np.hstack((allPoseTimes, poseTimes[1:]))
allPoses += poses[1:]
return allPoseTimes, allPoses
else:
msg = robotstate.asRobotPlan(msgOrList)
poses = []
poseTimes = []
for plan in msg.plan:
pose = robotstate.convertStateMessageToDrakePose(plan)
poseTimes.append(plan.utime / 1e6)
poses.append(pose)
return np.array(poseTimes), poses
@staticmethod
def getPlanElapsedTime(msg):
msg = robotstate.asRobotPlan(msg)
startTime = msg.plan[0].utime
endTime = msg.plan[-1].utime
return (endTime - startTime) / 1e6
def stopAnimation(self):
if self.animationTimer:
self.animationTimer.stop()
def setInterpolationMethod(method):
self.interpolationMethod = method
def playPlan(self, msg, jointController):
self.playPlans(poseTimes, [msg], jointController)
def playPlans(self, messages, jointController):
assert len(messages)
poseTimes, poses = self.getPlanPoses(messages)
self.playPoses(poseTimes, poses, jointController)
def getPoseInterpolatorFromPlan(self, message):
poseTimes, poses = self.getPlanPoses(message)
return self.getPoseInterpolator(poseTimes, poses)
def getPoseInterpolator(self, poseTimes, poses, unwrap_rpy=True):
if unwrap_rpy:
poses = np.array(poses, copy=True)
poses[:,3:6] = np.unwrap(poses[:,3:6],axis=0)
if self.interpolationMethod in ['slinear', 'quadratic', 'cubic']:
f = scipy.interpolate.interp1d(poseTimes, poses, axis=0, kind=self.interpolationMethod)
elif self.interpolationMethod == 'pchip':
f = scipy.interpolate.pchip(poseTimes, poses, axis=0)
return f
def getPlanPoseMeshes(self, messages, jointController, robotModel, numberOfSamples):
poseTimes, poses = self.getPlanPoses(messages)
f = self.getPoseInterpolator(poseTimes, poses)
sampleTimes = np.linspace(poseTimes[0], poseTimes[-1], numberOfSamples)
meshes = []
for sampleTime in sampleTimes:
pose = f(sampleTime)
jointController.setPose('plan_playback', pose)
polyData = vtk.vtkPolyData()
robotModel.model.getModelMesh(polyData)
meshes.append(polyData)
return meshes
def showPoseAtTime(self, time, jointController, poseInterpolator):
pose = poseInterpolator(time)
jointController.setPose('plan_playback', pose)
def playPoses(self, poseTimes, poses, jointController):
f = self.getPoseInterpolator(poseTimes, poses)
timer = SimpleTimer()
def updateAnimation():
tNow = timer.elapsed() * self.playbackSpeed
if tNow > poseTimes[-1]:
pose = poses[-1]
jointController.setPose('plan_playback', pose)
if self.animationCallback:
self.animationCallback()
return False
pose = f(tNow)
jointController.setPose('plan_playback', pose)
if self.animationCallback:
self.animationCallback()
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 60
self.animationTimer.callback = updateAnimation
self.animationTimer.start()
updateAnimation()
def picklePlan(self, filename, msg):
poseTimes, poses = self.getPlanPoses(msg)
pickle.dump((poseTimes, poses), open(filename, 'w'))
def getMovingJointNames(self, msg):
poseTimes, poses = self.getPlanPoses(msg)
diffs = np.diff(poses, axis=0)
jointIds = np.unique(np.where(diffs != 0.0)[1])
jointNames = [robotstate.getDrakePoseJointNames()[jointId] for jointId in jointIds]
return jointNames
def plotPlan(self, msg):
poseTimes, poses = self.getPlanPoses(msg)
self.plotPoses(poseTimes, poses)
def plotPoses(self, poseTimes, poses):
import matplotlib.pyplot as plt
poses = np.array(poses)
if self.jointNameRegex:
jointIds = range(poses.shape[1])
else:
diffs = np.diff(poses, axis=0)
jointIds = np.unique(np.where(diffs != 0.0)[1])
jointNames = [robotstate.getDrakePoseJointNames()[jointId] for jointId in jointIds]
jointTrajectories = [poses[:,jointId] for jointId in jointIds]
seriesNames = []
sampleResolutionInSeconds = 0.01
numberOfSamples = (poseTimes[-1] - poseTimes[0]) / sampleResolutionInSeconds
xnew = np.linspace(poseTimes[0], poseTimes[-1], numberOfSamples)
fig = plt.figure()
ax = fig.add_subplot(111)
for jointId, jointName, jointTrajectory in zip(jointIds, jointNames, jointTrajectories):
if self.jointNameRegex and not re.match(self.jointNameRegex, jointName):
continue
x = poseTimes
y = jointTrajectory
y = np.rad2deg(y)
if self.interpolationMethod in ['slinear', 'quadratic', 'cubic']:
f = scipy.interpolate.interp1d(x, y, kind=self.interpolationMethod)
elif self.interpolationMethod == 'pchip':
f = scipy.interpolate.pchip(x, y)
ax.plot(x, y, 'ko')
seriesNames.append(jointName + ' points')
ax.plot(xnew, f(xnew), '-')
seriesNames.append(jointName + ' ' + self.interpolationMethod)
ax.legend(seriesNames, loc='upper right').draggable()
ax.set_xlabel('time (s)')
ax.set_ylabel('joint angle (deg)')
ax.set_title('joint trajectories')
plt.show()
class TaskUserPanel(object):
def __init__(self, windowTitle='Task Panel'):
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddTaskUserPanel.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.widget.setWindowTitle(windowTitle)
self.manualButtons = {}
self.imageViewLayout = QtGui.QHBoxLayout(self.ui.imageFrame)
self._setupParams()
self._setupPropertiesPanel()
self._initTaskPanel()
def addManualButton(self, text, callback):
b = QtGui.QPushButton(text)
b.connect('clicked()', callback)
self.manualButtons[text] = b
self.addManualWidget(b)
return b
def addManualSpacer(self):
line = QtGui.QFrame()
line.setFrameShape(QtGui.QFrame.HLine)
line.setFrameShadow(QtGui.QFrame.Sunken)
self.addManualWidget(line)
def addManualWidget(self, widget):
self.ui.manualButtonsLayout.insertWidget(self.ui.manualButtonsLayout.count()-1, widget)
def initImageView(self, imageView, activateAffordanceUpdater=True):
if activateAffordanceUpdater:
self.affordanceUpdater = affordanceupdater.AffordanceInCameraUpdater(segmentation.affordanceManager, imageView)
self.affordanceUpdater.timer.start()
self.imageViewLayout.addWidget(self.fitter.imageView.view)
def _setupParams(self):
self.params = om.ObjectModelItem('Task Params')
self.params.properties.connectPropertyChanged(self.onPropertyChanged)
def _setupPropertiesPanel(self):
l = QtGui.QVBoxLayout(self.ui.propertyFrame)
l.setMargin(0)
self.propertiesPanel = PythonQt.dd.ddPropertiesPanel()
self.propertiesPanel.setBrowserModeToWidget()
l.addWidget(self.propertiesPanel)
self.panelConnector = propertyset.PropertyPanelConnector(self.params.properties, self.propertiesPanel)
def onPropertyChanged(self, propertySet, propertyName):
pass
def getNextTasks(self):
return self.taskTree.getTasks(fromSelected=True)
def onContinue(self):
self._activatePrompts()
self.completedTasks = []
self.taskQueue.reset()
for obj in self.getNextTasks():
self.taskQueue.addTask(obj.task)
self.taskQueue.start()
def _activatePrompts(self):
rt.UserPromptTask.promptFunction = self.onTaskPrompt
rt.PrintTask.printFunction = self.appendMessage
def onStep(self):
assert not self.taskQueue.isRunning
self._activatePrompts()
tasks = self.getNextTasks()
if not tasks:
return
task = tasks[0].task
self.nextStepTask = tasks[1].task if len(tasks) > 1 else None
self.completedTasks = []
self.taskQueue.reset()
self.taskQueue.addTask(task)
self.taskQueue.start()
def updateTaskButtons(self):
self.ui.taskStepButton.setEnabled(not self.taskQueue.isRunning)
self.ui.taskContinueButton.setEnabled(not self.taskQueue.isRunning)
self.ui.taskPauseButton.setEnabled(self.taskQueue.isRunning)
def onPause(self):
if not self.taskQueue.isRunning:
return
self.nextStepTask = None
currentTask = self.taskQueue.currentTask
self.taskQueue.stop()
if currentTask:
currentTask.stop()
self.appendMessage('<font color="red">paused</font>')
def onQueueStarted(self, taskQueue):
self.updateTaskButtons()
def onQueueStopped(self, taskQueue):
self.clearPrompt()
self.updateTaskButtons()
def onTaskStarted(self, taskQueue, task):
msg = task.properties.getProperty('Name') + ' ... <font color="green">start</font>'
self.appendMessage(msg)
self.taskTree.selectTask(task)
item = self.taskTree.findTaskItem(task)
if len(self.completedTasks) and item.getProperty('Visible'):
self.appendMessage('<font color="red">paused</font>')
raise atq.AsyncTaskQueue.PauseException()
def onTaskEnded(self, taskQueue, task):
msg = task.properties.getProperty('Name') + ' ... <font color="green">end</font>'
self.appendMessage(msg)
self.completedTasks.append(task)
if self.taskQueue.tasks:
self.taskTree.selectTask(self.taskQueue.tasks[0])
elif self.nextStepTask:
self.taskTree.selectTask(self.nextStepTask)
#else:
# self.taskTree.selectTask(self.completedTasks[0])
def onTaskFailed(self, taskQueue, task):
msg = task.properties.getProperty('Name') + ' ... <font color="red">failed: %s</font>' % task.failReason
self.appendMessage(msg)
def onTaskPaused(self, taskQueue, task):
msg = task.properties.getProperty('Name') + ' ... <font color="red">paused</font>'
self.appendMessage(msg)
def onTaskException(self, taskQueue, task):
msg = task.properties.getProperty('Name') + ' ... <font color="red">exception:\n\n%s</font>' % traceback.format_exc()
self.appendMessage(msg)
def appendMessage(self, msg):
if msg == self.lastStatusMessage:
return
self.lastStatusMessage = msg
self.ui.outputConsole.append(msg.replace('\n', '<br/>'))
def updateTaskStatus(self):
currentTask = self.taskQueue.currentTask
if not currentTask or not currentTask.statusMessage:
return
name = currentTask.properties.getProperty('Name')
status = currentTask.statusMessage
msg = name + ': ' + status
self.appendMessage(msg)
def clearPrompt(self):
self.promptTask = None
self.ui.promptLabel.text = ''
self.ui.promptAcceptButton.enabled = False
self.ui.promptRejectButton.enabled = False
def onAcceptPrompt(self):
self.promptTask.accept()
self.clearPrompt()
def onRejectPrompt(self):
self.promptTask.reject()
self.clearPrompt()
def onTaskPrompt(self, task, message):
self.promptTask = task
self.ui.promptLabel.text = message
self.ui.promptAcceptButton.enabled = True
self.ui.promptRejectButton.enabled = True
def _initTaskPanel(self):
self.lastStatusMessage = ''
self.nextStepTask = None
self.completedTasks = []
self.taskQueue = atq.AsyncTaskQueue()
self.taskQueue.connectQueueStarted(self.onQueueStarted)
self.taskQueue.connectQueueStopped(self.onQueueStopped)
self.taskQueue.connectTaskStarted(self.onTaskStarted)
self.taskQueue.connectTaskEnded(self.onTaskEnded)
self.taskQueue.connectTaskPaused(self.onTaskPaused)
self.taskQueue.connectTaskFailed(self.onTaskFailed)
self.taskQueue.connectTaskException(self.onTaskException)
self.timer = TimerCallback(targetFps=2)
self.timer.callback = self.updateTaskStatus
self.timer.start()
self.taskTree = tmw.TaskTree()
self.ui.taskFrame.layout().insertWidget(0, self.taskTree.treeWidget)
l = QtGui.QVBoxLayout(self.ui.taskPropertiesGroupBox)
l.addWidget(self.taskTree.propertiesPanel)
PythonQt.dd.ddGroupBoxHider(self.ui.taskPropertiesGroupBox)
self.ui.taskStepButton.connect('clicked()', self.onStep)
self.ui.taskContinueButton.connect('clicked()', self.onContinue)
self.ui.taskPauseButton.connect('clicked()', self.onPause)
self.ui.promptAcceptButton.connect('clicked()', self.onAcceptPrompt)
self.ui.promptRejectButton.connect('clicked()', self.onRejectPrompt)
self.clearPrompt()
self.updateTaskButtons()
class TaskUserPanel(object):
def __init__(self, windowTitle='Task Panel'):
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddTaskUserPanel.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.widget.setWindowTitle(windowTitle)
self.manualButtons = {}
self.imageViewLayout = QtGui.QHBoxLayout(self.ui.imageFrame)
self._setupParams()
self._setupPropertiesPanel()
self._initTaskPanel()
def addManualButton(self, text, callback):
b = QtGui.QPushButton(text)
b.connect('clicked()', callback)
self.manualButtons[text] = b
self.addManualWidget(b)
return b
def addManualSpacer(self):
line = QtGui.QFrame()
line.setFrameShape(QtGui.QFrame.HLine)
line.setFrameShadow(QtGui.QFrame.Sunken)
self.addManualWidget(line)
def addManualWidget(self, widget):
self.ui.manualButtonsLayout.insertWidget(
self.ui.manualButtonsLayout.count() - 1, widget)
def initImageView(self, imageView):
self.affordanceUpdater = affordanceupdater.AffordanceInCameraUpdater(
segmentation.affordanceManager, imageView)
self.affordanceUpdater.timer.start()
self.imageViewLayout.addWidget(self.fitter.imageView.view)
def _setupParams(self):
self.params = om.ObjectModelItem('Task Params')
self.params.properties.connectPropertyChanged(self.onPropertyChanged)
def _setupPropertiesPanel(self):
l = QtGui.QVBoxLayout(self.ui.propertyFrame)
l.setMargin(0)
self.propertiesPanel = PythonQt.dd.ddPropertiesPanel()
self.propertiesPanel.setBrowserModeToWidget()
l.addWidget(self.propertiesPanel)
self.panelConnector = propertyset.PropertyPanelConnector(
self.params.properties, self.propertiesPanel)
def onPropertyChanged(self, propertySet, propertyName):
pass
def getNextTasks(self):
return self.taskTree.getTasks(fromSelected=True)
def onContinue(self):
self._activatePrompts()
self.completedTasks = []
self.taskQueue.reset()
for obj in self.getNextTasks():
self.taskQueue.addTask(obj.task)
self.taskQueue.start()
def _activatePrompts(self):
rt.UserPromptTask.promptFunction = self.onTaskPrompt
rt.PrintTask.printFunction = self.appendMessage
def onStep(self):
assert not self.taskQueue.isRunning
self._activatePrompts()
tasks = self.getNextTasks()
if not tasks:
return
task = tasks[0].task
self.nextStepTask = tasks[1].task if len(tasks) > 1 else None
self.completedTasks = []
self.taskQueue.reset()
self.taskQueue.addTask(task)
self.taskQueue.start()
def updateTaskButtons(self):
self.ui.taskStepButton.setEnabled(not self.taskQueue.isRunning)
self.ui.taskContinueButton.setEnabled(not self.taskQueue.isRunning)
self.ui.taskPauseButton.setEnabled(self.taskQueue.isRunning)
def onPause(self):
if not self.taskQueue.isRunning:
return
self.nextStepTask = None
currentTask = self.taskQueue.currentTask
self.taskQueue.stop()
if currentTask:
currentTask.stop()
self.appendMessage('<font color="red">paused</font>')
def onQueueStarted(self, taskQueue):
self.updateTaskButtons()
def onQueueStopped(self, taskQueue):
self.clearPrompt()
self.updateTaskButtons()
def onTaskStarted(self, taskQueue, task):
msg = task.properties.getProperty(
'Name') + ' ... <font color="green">start</font>'
self.appendMessage(msg)
self.taskTree.selectTask(task)
item = self.taskTree.findTaskItem(task)
if len(self.completedTasks) and item.getProperty('Visible'):
self.appendMessage('<font color="red">paused</font>')
raise atq.AsyncTaskQueue.PauseException()
def onTaskEnded(self, taskQueue, task):
msg = task.properties.getProperty(
'Name') + ' ... <font color="green">end</font>'
self.appendMessage(msg)
self.completedTasks.append(task)
if self.taskQueue.tasks:
self.taskTree.selectTask(self.taskQueue.tasks[0])
elif self.nextStepTask:
self.taskTree.selectTask(self.nextStepTask)
#else:
# self.taskTree.selectTask(self.completedTasks[0])
def onTaskFailed(self, taskQueue, task):
msg = task.properties.getProperty(
'Name'
) + ' ... <font color="red">failed: %s</font>' % task.failReason
self.appendMessage(msg)
def onTaskPaused(self, taskQueue, task):
msg = task.properties.getProperty(
'Name') + ' ... <font color="red">paused</font>'
self.appendMessage(msg)
def onTaskException(self, taskQueue, task):
msg = task.properties.getProperty(
'Name'
) + ' ... <font color="red">exception:\n\n%s</font>' % traceback.format_exc(
)
self.appendMessage(msg)
def appendMessage(self, msg):
if msg == self.lastStatusMessage:
return
self.lastStatusMessage = msg
self.ui.outputConsole.append(msg.replace('\n', '<br/>'))
def updateTaskStatus(self):
currentTask = self.taskQueue.currentTask
if not currentTask or not currentTask.statusMessage:
return
name = currentTask.properties.getProperty('Name')
status = currentTask.statusMessage
msg = name + ': ' + status
self.appendMessage(msg)
def clearPrompt(self):
self.promptTask = None
self.ui.promptLabel.text = ''
self.ui.promptAcceptButton.enabled = False
self.ui.promptRejectButton.enabled = False
def onAcceptPrompt(self):
self.promptTask.accept()
self.clearPrompt()
def onRejectPrompt(self):
self.promptTask.reject()
self.clearPrompt()
def onTaskPrompt(self, task, message):
self.promptTask = task
self.ui.promptLabel.text = message
self.ui.promptAcceptButton.enabled = True
self.ui.promptRejectButton.enabled = True
def _initTaskPanel(self):
self.lastStatusMessage = ''
self.nextStepTask = None
self.completedTasks = []
self.taskQueue = atq.AsyncTaskQueue()
self.taskQueue.connectQueueStarted(self.onQueueStarted)
self.taskQueue.connectQueueStopped(self.onQueueStopped)
self.taskQueue.connectTaskStarted(self.onTaskStarted)
self.taskQueue.connectTaskEnded(self.onTaskEnded)
self.taskQueue.connectTaskPaused(self.onTaskPaused)
self.taskQueue.connectTaskFailed(self.onTaskFailed)
self.taskQueue.connectTaskException(self.onTaskException)
self.timer = TimerCallback(targetFps=2)
self.timer.callback = self.updateTaskStatus
self.timer.start()
self.taskTree = tmw.TaskTree()
self.ui.taskFrame.layout().insertWidget(0, self.taskTree.treeWidget)
l = QtGui.QVBoxLayout(self.ui.taskPropertiesGroupBox)
l.addWidget(self.taskTree.propertiesPanel)
PythonQt.dd.ddGroupBoxHider(self.ui.taskPropertiesGroupBox)
self.ui.taskStepButton.connect('clicked()', self.onStep)
self.ui.taskContinueButton.connect('clicked()', self.onContinue)
self.ui.taskPauseButton.connect('clicked()', self.onPause)
self.ui.promptAcceptButton.connect('clicked()', self.onAcceptPrompt)
self.ui.promptRejectButton.connect('clicked()', self.onRejectPrompt)
self.clearPrompt()
self.updateTaskButtons()
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 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 HandControlPanel(object):
def __init__(self, lDriver, rDriver, robotStateModel,
robotStateJointController, view):
self.robotStateModel = robotStateModel
self.robotStateJointController = robotStateJointController
self.drivers = {}
self.drivers['left'] = lDriver
self.drivers['right'] = rDriver
self.storedCommand = {'left': None, 'right': None}
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddHandControl.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self._updateBlocked = True
self.widget.advanced.sendButton.setEnabled(True)
# Store calibration for wrist f/t sensors
self.wristftvis = WristForceTorqueVisualizer(
robotStateModel, robotStateJointController, view)
# connect the callbacks
self.widget.basic.openButton.clicked.connect(self.openClicked)
self.widget.basic.closeButton.clicked.connect(self.closeClicked)
self.widget.basic.waitOpenButton.clicked.connect(self.waitOpenClicked)
self.widget.basic.waitCloseButton.clicked.connect(
self.waitCloseClicked)
self.widget.advanced.sendButton.clicked.connect(self.sendClicked)
self.widget.advanced.calibrateButton.clicked.connect(
self.calibrateClicked)
self.widget.advanced.setModeButton.clicked.connect(self.setModeClicked)
self.widget.advanced.regraspButton.clicked.connect(self.regraspClicked)
self.widget.advanced.dropButton.clicked.connect(self.dropClicked)
self.widget.advanced.repeatRateSpinner.valueChanged.connect(
self.rateSpinnerChanged)
self.ui.fingerControlButton.clicked.connect(self.fingerControlButton)
self.widget.sensors.rightTareButton.clicked.connect(
self.wristftvis.tareRightFT)
self.widget.sensors.leftTareButton.clicked.connect(
self.wristftvis.tareLeftFT)
self.widget.sensors.rightCalibButton.clicked.connect(
self.wristftvis.calibRightFT)
self.widget.sensors.leftCalibButton.clicked.connect(
self.wristftvis.calibLeftFT)
self.widget.sensors.rightCalibClearButton.clicked.connect(
self.wristftvis.calibRightClearFT)
self.widget.sensors.leftCalibClearButton.clicked.connect(
self.wristftvis.calibLeftClearFT)
self.widget.sensors.rightVisCheck.clicked.connect(
self.updateWristFTVis)
self.widget.sensors.leftVisCheck.clicked.connect(self.updateWristFTVis)
self.widget.sensors.torqueVisCheck.clicked.connect(
self.updateWristFTVis)
PythonQt.dd.ddGroupBoxHider(self.ui.sensors)
PythonQt.dd.ddGroupBoxHider(self.ui.fingerControl)
# Bug fix... for some reason one slider is set as disabled
self.ui.fingerASlider.setEnabled(True)
# create a timer to repeat commands
self.updateTimer = TimerCallback()
self.updateTimer.callback = self.updatePanel
self.updateTimer.targetFps = 3
self.updateTimer.start()
def getModeInt(self, inputStr):
if inputStr == 'Basic':
return 0
if inputStr == 'Pinch':
return 1
if inputStr == 'Wide':
return 2
if inputStr == 'Scissor':
return 3
return 0
def openClicked(self):
if self.widget.handSelect.leftButton.checked:
side = 'left'
else:
side = 'right'
self.widget.advanced.closePercentSpinner.setValue(0.0)
position = 0.0
force = float(self.widget.advanced.forcePercentSpinner.value)
velocity = float(self.widget.advanced.velocityPercentSpinner.value)
mode = self.getModeInt(self.widget.advanced.modeBox.currentText)
self.drivers[side].sendCustom(position, force, velocity, mode)
self.storedCommand[side] = (position, force, velocity, mode)
def closeClicked(self):
if self.widget.handSelect.leftButton.checked:
side = 'left'
else:
side = 'right'
self.widget.advanced.closePercentSpinner.setValue(100.0)
position = 100.0
force = float(self.widget.advanced.forcePercentSpinner.value)
velocity = float(self.widget.advanced.velocityPercentSpinner.value)
mode = self.getModeInt(self.widget.advanced.modeBox.currentText)
self.drivers[side].sendCustom(position, force, velocity, mode)
self.storedCommand[side] = (position, force, velocity, mode)
def waitOpenClicked(self):
sleep(10)
self.openClicked()
def waitCloseClicked(self):
sleep(10)
self.closeClicked()
def sendClicked(self):
if self.widget.handSelect.leftButton.checked:
side = 'left'
else:
side = 'right'
position = float(self.widget.advanced.closePercentSpinner.value)
force = float(self.widget.advanced.forcePercentSpinner.value)
velocity = float(self.widget.advanced.velocityPercentSpinner.value)
mode = self.getModeInt(self.widget.advanced.modeBox.currentText)
self.drivers[side].sendCustom(position, force, velocity, mode)
self.storedCommand[side] = (position, force, velocity, mode)
def setModeClicked(self):
if self.widget.handSelect.leftButton.checked:
side = 'left'
else:
side = 'right'
mode = self.getModeInt(self.widget.advanced.modeBox.currentText)
self.drivers[side].setMode(mode)
self.storedCommand[side] = None
def calibrateClicked(self):
if self.widget.handSelect.leftButton.checked:
side = 'left'
else:
side = 'right'
self.drivers[side].sendCalibrate()
self.storedCommand[side] = None
def dropClicked(self):
if self.widget.handSelect.leftButton.checked:
side = 'left'
else:
side = 'right'
self.drivers[side].sendDrop()
self.storedCommand[side] = None
def regraspClicked(self):
if self.widget.handSelect.leftButton.checked:
side = 'left'
else:
side = 'right'
position = float(self.widget.advanced.closePercentSpinner.value)
force = float(self.widget.advanced.forcePercentSpinner.value)
velocity = float(self.widget.advanced.velocityPercentSpinner.value)
mode = self.getModeInt(self.widget.advanced.modeBox.currentText)
self.drivers[side].sendRegrasp(position, force, velocity, mode)
self.storedCommand[side] = (position, force, velocity, mode)
def rateSpinnerChanged(self):
self.updateTimer.targetFps = self.ui.repeatRateSpinner.value
def fingerControlButton(self):
if self.widget.handSelect.leftButton.checked:
side = 'left'
else:
side = 'right'
if not self.ui.scissorControl.isChecked():
self.drivers[side].sendFingerControl(
int(self.ui.fingerAValue.text), int(self.ui.fingerBValue.text),
int(self.ui.fingerCValue.text),
float(self.widget.advanced.forcePercentSpinner.value),
float(self.widget.advanced.velocityPercentSpinner.value), None,
self.getModeInt(self.widget.advanced.modeBox.currentText))
else:
self.drivers[side].sendFingerControl(
int(self.ui.fingerAValue.text), int(self.ui.fingerBValue.text),
int(self.ui.fingerCValue.text),
float(self.widget.advanced.forcePercentSpinner.value),
float(self.widget.advanced.velocityPercentSpinner.value),
int(self.ui.scissorValue.text),
0) # can ignore mode because scissor will override
self.storedCommand[side] = None
def updateWristFTVis(self):
self.wristftvis.updateDrawSettings(self.ui.leftVisCheck.checked,
self.ui.rightVisCheck.checked,
self.ui.torqueVisCheck.checked)
def updatePanel(self):
if self.ui.repeaterCheckBox.checked and self.storedCommand['left']:
position, force, velocity, mode = self.storedCommand['left']
self.drivers['left'].sendCustom(position, force, velocity, mode)
if self.ui.repeaterCheckBox.checked and self.storedCommand['right']:
position, force, velocity, mode = self.storedCommand['right']
self.drivers['right'].sendCustom(position, force, velocity, mode)
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 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 LCMLoggerWidget(object):
def __init__(self, statusBar=None):
self.manager = lcmUtils.LCMLoggerManager()
self.statusBar = statusBar
self.lastActiveLogFile = None
self.numProcesses = 0
self.numLogFiles = 0
self.userTag = ''
self.button = QtGui.QPushButton('')
self.button.setContextMenuPolicy(QtCore.Qt.CustomContextMenu)
self.button.connect('customContextMenuRequested(const QPoint&)',
self.showContextMenu)
self.button.connect('clicked()', self.onClick)
self.timer = TimerCallback(targetFps=0.25)
self.timer.callback = self.updateState
self.timer.start()
def updateState(self):
t = SimpleTimer()
self.manager.updateExistingLoggerProcesses()
activeLogFiles = self.manager.getActiveLogFilenames()
self.numProcesses = len(self.manager.getActiveLoggerPids())
self.numLogFiles = len(activeLogFiles)
if self.numLogFiles == 1:
self.lastActiveLogFile = activeLogFiles[0]
if self.numProcesses == 0:
self.button.text = 'start logger'
elif self.numProcesses == 1:
self.button.text = 'stop logger'
elif self.numProcesses > 1:
self.button.text = 'stop all loggers'
statusDescription = 'active' if self.numProcesses else 'last'
logFileDescription = self.lastActiveLogFile or '<unknown>'
self.button.setToolTip('%s log file: %s' %
(statusDescription, logFileDescription))
def onClick(self):
if self.numProcesses == 0:
self.manager.startNewLogger(tag=self.userTag)
self.updateState()
self.showStatusMessage('start logging: ' + self.lastActiveLogFile)
else:
self.manager.killAllLoggingProcesses()
self.showStatusMessage('stopped logging')
self.updateState()
def showStatusMessage(self, msg, timeout=2000):
if self.statusBar:
self.statusBar.showMessage(msg, timeout)
def showContextMenu(self, clickPosition):
globalPos = self.button.mapToGlobal(clickPosition)
menu = QtGui.QMenu()
action = menu.addAction('Stop logger')
action.enabled = (self.numProcesses > 0)
action = menu.addAction('Stop and delete log file')
action.enabled = (self.numProcesses > 0 and self.lastActiveLogFile)
action = menu.addAction('Set logger tag')
action.enabled = (self.numProcesses == 0)
action = menu.addAction('Copy log filename')
action.enabled = (self.lastActiveLogFile is not None)
action = menu.addAction('Review log')
action.enabled = (self.lastActiveLogFile is not None)
selectedAction = menu.exec_(globalPos)
if selectedAction is None:
return
if selectedAction.text == 'Copy log filename':
clipboard = QtGui.QApplication.instance().clipboard()
clipboard.setText(self.lastActiveLogFile)
self.showStatusMessage('copy to clipboard: ' +
self.lastActiveLogFile)
elif selectedAction.text == 'Stop logger':
self.manager.killAllLoggingProcesses()
self.showStatusMessage('stopped logger')
self.updateState()
elif selectedAction.text == 'Stop and delete log file':
logFileToRemove = self.lastActiveLogFile
self.manager.killAllLoggingProcesses()
self.updateState()
os.remove(logFileToRemove)
self.showStatusMessage('deleted: ' + logFileToRemove)
elif selectedAction.text == 'Set logger tag':
inputDialog = QtGui.QInputDialog()
inputDialog.setInputMode(inputDialog.TextInput)
inputDialog.setLabelText('Log file tag:')
inputDialog.setWindowTitle('Enter tag')
inputDialog.setTextValue(self.userTag)
result = inputDialog.exec_()
if result:
tag = inputDialog.textValue()
self.userTag = tag
self.showStatusMessage('Set lcm logger tag: ' + self.userTag)
elif selectedAction.text == 'Review log':
newEnv = dict(os.environ)
newEnv['LCM_DEFAULT_URL'] = newEnv['LCM_REVIEW_DEFAULT_URL']
devnull = open(os.devnull, 'w')
subprocess.Popen('drake-designer',
stdout=devnull,
stderr=devnull,
env=newEnv)
subprocess.Popen(['lcm-logplayer-gui', self.lastActiveLogFile],
stdout=devnull,
stderr=devnull,
env=newEnv)
subprocess.Popen(['bot-procman-sheriff', '-o'],
stdout=devnull,
stderr=devnull,
env=newEnv)
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 AtlasDriverPanel(object):
def __init__(self, driver):
self.driver = driver
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddAtlasDriverPanel.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.widget.setWindowTitle('Atlas Driver Panel')
self.ui = WidgetDict(self.widget.children())
# Main Panel
self.ui.calibrateEncodersButton.connect('clicked()', self.onCalibrateEncoders)
self.ui.prepButton.connect('clicked()', self.onPrep)
self.ui.combinedStandButton.connect('clicked()', self.onCombinedStand)
self.ui.stopButton.connect('clicked()', self.onStop)
self.ui.freezeButton.connect('clicked()', self.onFreeze)
self.ui.calibrateNullBiasButton.connect('clicked()', self.onCalibrateNullBias)
self.ui.calibrateElectricArmsButton.connect('clicked()', self.onCalibrateElectricArms)
self.ui.initNavButton.connect('clicked()', self.onInitNav)
self.ui.standButton.connect('clicked()', self.onStand)
self.ui.mitStandButton.connect('clicked()', self.onMITStand)
self.ui.userButton.connect('clicked()', self.onUser)
self.ui.manipButton.connect('clicked()', self.onManip)
self.ui.recoveryOnButton.connect('clicked()', self.driver.sendRecoveryEnable)
self.ui.recoveryOffButton.connect('clicked()', self.driver.sendRecoveryDisable)
self.ui.bracingOnButton.connect('clicked()', self.driver.sendBracingEnable)
self.ui.bracingOffButton.connect('clicked()', self.driver.sendBracingDisable)
for psi in [1000, 1500, 2000, 2400, 2650]:
self.ui.__dict__['setPressure' + str(psi) + 'Button'].connect('clicked()', partial(self.driver.sendDesiredPumpPsi, psi))
self.ui.sendCustomPressureButton.connect('clicked()', self.sendCustomPressure)
self.setupElectricArmCheckBoxes()
PythonQt.dd.ddGroupBoxHider(self.ui.calibrationGroupBox)
PythonQt.dd.ddGroupBoxHider(self.ui.pumpStatusGroupBox)
PythonQt.dd.ddGroupBoxHider(self.ui.electricArmStatusGroupBox)
self.updateTimer = TimerCallback(targetFps=5)
self.updateTimer.callback = self.updatePanel
self.updateTimer.start()
self.updatePanel()
def updatePanel(self):
self.updateBehaviorLabel()
self.updateControllerStatusLabel()
self.updateRecoveryEnabledLabel()
self.updateBracingEnabledLabel()
self.updateBatteryStatusLabel()
self.updateStatus()
self.updateButtons()
self.updateElectricArmStatus()
self.driver.updateCombinedStandLogic()
def updateBehaviorLabel(self):
self.ui.behaviorLabel.text = self.driver.getCurrentBehaviorName() or '<unknown>'
def updateControllerStatusLabel(self):
self.ui.controllerStatusLabel.text = self.driver.getControllerStatus() or '<unknown>'
def updateBatteryStatusLabel(self):
charge = self.driver.getBatteryChargeRemaining()
self.ui.batteryStatusLabel.text = '<unknown>' if charge is None else '%d%%' % charge
def updateRecoveryEnabledLabel(self):
self.ui.recoveryEnabledLabel.text = self.driver.getRecoveryEnabledStatus() or '<unknown>'
def updateBracingEnabledLabel(self):
self.ui.bracingEnabledLabel.text = self.driver.getBracingEnabledStatus() or '<unknown>'
def updateStatus(self):
self.ui.inletPressure.value = self.driver.getCurrentInletPressure()
self.ui.supplyPressure.value = self.driver.getCurrentSupplyPressure()
self.ui.returnPressure.value = self.driver.getCurrentReturnPressure()
self.ui.airSumpPressure.value = self.driver.getCurrentAirSumpPressure()
self.ui.pumpRpm.value = self.driver.getCurrentPumpRpm()
self.ui.maxActuatorPressure.value = self.driver.getMaxActuatorPressure()
def getElectricArmCheckBoxes(self):
return [self.ui.armCheck1,
self.ui.armCheck2,
self.ui.armCheck3,
self.ui.armCheck4,
self.ui.armCheck5,
self.ui.armCheck6]
def setupElectricArmCheckBoxes(self):
for check in self.getElectricArmCheckBoxes():
check.connect('clicked()', self.onEnableElectricArmChecked)
def updateElectricArmStatus(self):
temps = [self.ui.armTemp1,
self.ui.armTemp2,
self.ui.armTemp3,
self.ui.armTemp4,
self.ui.armTemp5,
self.ui.armTemp6]
for i, check in enumerate(self.getElectricArmCheckBoxes()):
enabled = self.driver.getElectricArmEnabledStatus(i)
check.setText('yes' if enabled else 'no')
for i, temp in enumerate(temps):
temp.setValue(self.driver.getElectricArmTemperature(i))
def updateButtons(self):
behavior = self.driver.getCurrentBehaviorName()
behaviorIsFreeze = behavior == 'freeze'
self.ui.calibrateNullBiasButton.setEnabled(behaviorIsFreeze)
self.ui.calibrateElectricArmsButton.setEnabled(behaviorIsFreeze)
self.ui.calibrateEncodersButton.setEnabled(behaviorIsFreeze)
self.ui.prepButton.setEnabled(behaviorIsFreeze)
self.ui.standButton.setEnabled(behavior in ('prep', 'stand', 'user', 'manip', 'step', 'walk'))
self.ui.mitStandButton.setEnabled(behavior=='user')
self.ui.manipButton.setEnabled(behavior in ('stand', 'manip'))
self.ui.userButton.setEnabled(behavior is not None)
def onEnableElectricArmChecked(self):
enabledState = [bool(check.checked) for check in self.getElectricArmCheckBoxes()]
self.driver.sendElectricArmEnabledState(enabledState)
def onFreeze(self):
self.driver.sendFreezeCommand()
def onStop(self):
self.driver.sendStopCommand()
def onCalibrateEncoders(self):
self.driver.sendCalibrateEncodersCommand()
def onCalibrateNullBias(self):
self.driver.sendCalibrateNullBiasCommand()
def onCalibrateElectricArms(self):
self.driver.sendCalibrateElectricArmsCommand()
def onInitNav(self):
self.driver.sendInitAtZero()
def onPrep(self):
self.driver.sendPrepCommand()
def onStand(self):
self.driver.sendStandCommand()
def onCombinedStand(self):
self.driver.sendCombinedStandCommand()
def onMITStand(self):
self.driver.sendMITStandCommand()
def onManip(self):
self.driver.sendManipCommand()
def onUser(self):
self.driver.sendUserCommand()
def sendCustomPressure(self):
self.driver.sendDesiredPumpPsi(self.ui.customPumpPressure.value)
class MidiBehaviorControl(object):
def __init__(self):
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.tick
self.stop = self.timer.stop
self.reader = None
self.initReader()
self.inputs = {
'slider' : [0, 8, True],
'dial' : [16, 8, True],
'r_button' : [64, 8, False],
'm_button' : [48, 8, False],
's_button' : [32, 8, False],
'track_left' : [58, 1, False],
'track_right' : [59, 1, False],
'cycle' : [46, 1, False],
'marker_set' : [60, 1, False],
'marker_left' : [61, 1, False],
'marker_right' : [62, 1, False],
'rewind' : [43, 1, False],
'fastforward' : [44, 1, False],
'stop' : [42, 1, False],
'play' : [41, 1, False],
'record' : [45, 1, False],
}
signalNames = []
for inputName, inputDescription in self.inputs.iteritems():
channelStart, numChannels, isContinuous = inputDescription
for i in xrange(numChannels):
channelId = '' if numChannels == 1 else '_%d' % i
if isContinuous:
signalNames.append('%s%s_value_changed' % (inputName, channelId))
else:
signalNames.append('%s%s_pressed' % (inputName, channelId))
signalNames.append('%s%s_released' % (inputName, channelId))
self.callbacks = callbacks.CallbackRegistry(signalNames)
def start(self):
self.initReader()
if self.reader is not None:
self.timer.start()
def initReader(self):
if self.reader:
return
try:
self.reader = midi.MidiReader(midi.findKorgNanoKontrol2())
except:
print 'midi controller not found.'
self.reader = None
def onMidiCommand(self, channel, value):
#print channel, '%.2f' % value
inputs = self.inputs
for inputName, inputDescription in inputs.iteritems():
channelStart, numChannels, isContinuous = inputDescription
if channelStart <= channel < (channelStart + numChannels):
if numChannels > 1:
inputName = '%s_%d' % (inputName, channel - channelStart)
if isContinuous:
self.onContinuousInput(inputName, value)
elif value == 1:
self.onButtonDown(inputName)
elif value == 0:
self.onButtonUp(inputName)
def onContinuousInput(self, name, value):
#print name, '%.2f' % value
self.callbacks.process(name + '_value_changed', value)
def onButtonDown(self, name):
#print name, 'down'
self.callbacks.process(name + '_pressed')
def onButtonUp(self, name):
#print name, 'up'
self.callbacks.process(name + '_released')
def tick(self):
try:
messages = self.reader.getMessages()
except:
messages = []
if not messages:
return
targets = {}
for message in messages:
channel = message[2]
value = message[3]
targets[channel] = value
for channel, value in targets.iteritems():
position = value/127.0
self.onMidiCommand(channel, position)
class AffordancePanel(object):
def __init__(self, view, affordanceManager, ikServer, jointController, raycastDriver):
self.view = view
self.affordanceManager = affordanceManager
self.ikServer = ikServer
self.jointController = jointController
self.raycastDriver = raycastDriver
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddAffordancePanel.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.ui.affordanceListWidget.hide()
self.ui.spawnBoxButton.connect('clicked()', self.onSpawnBox)
self.ui.spawnSphereButton.connect('clicked()', self.onSpawnSphere)
self.ui.spawnCylinderButton.connect('clicked()', self.onSpawnCylinder)
self.ui.spawnCapsuleButton.connect('clicked()', self.onSpawnCapsule)
self.ui.spawnRingButton.connect('clicked()', self.onSpawnRing)
self.ui.spawnMeshButton.connect('clicked()', self.onSpawnMesh)
self.ui.getRaycastTerrainButton.connect('clicked()', self.onGetRaycastTerrain)
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.ui.scrollArea.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.onEvent)
self.updateTimer = TimerCallback(targetFps=30)
self.updateTimer.callback = self.updatePanel
self.updateTimer.start()
def onEvent(self, obj, event):
minSize = self.ui.scrollArea.widget().minimumSizeHint.width() + self.ui.scrollArea.verticalScrollBar().width
self.ui.scrollArea.setMinimumWidth(minSize)
def updatePanel(self):
if not self.widget.isVisible():
return
def getSpawnFrame(self):
pos = self.jointController.q[:3]
rpy = np.degrees(self.jointController.q[3:6])
frame = transformUtils.frameFromPositionAndRPY(pos, rpy)
frame.PreMultiply()
frame.Translate(0.5, 0.0, 0.3)
return frame
def onGetRaycastTerrain(self):
affs = self.affordanceManager.getCollisionAffordances()
xy = self.jointController.q[:2]
self.raycastDriver.requestRaycast(affs, xy-2, xy+2)
def onSpawnBox(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='BoxAffordanceItem', Name='box', uuid=newUUID(), pose=pose)
return self.affordanceManager.newAffordanceFromDescription(desc)
def onSpawnSphere(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='SphereAffordanceItem', Name='sphere', uuid=newUUID(), pose=pose)
return self.affordanceManager.newAffordanceFromDescription(desc)
def onSpawnCylinder(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='CylinderAffordanceItem', Name='cylinder', uuid=newUUID(), pose=pose)
return self.affordanceManager.newAffordanceFromDescription(desc)
def onSpawnCapsule(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='CapsuleAffordanceItem', Name='capsule', uuid=newUUID(), pose=pose)
return self.affordanceManager.newAffordanceFromDescription(desc)
def onSpawnRing(self):
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='CapsuleRingAffordanceItem', Name='ring', uuid=newUUID(), pose=pose)
return self.affordanceManager.newAffordanceFromDescription(desc)
def onSpawnMesh(self):
d = DebugData()
d.addArrow((0,0,0), (0,0,0.3))
pd = d.getPolyData()
meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='MeshAffordanceItem', Name='mesh', Filename=meshId, uuid=newUUID(), pose=pose)
return self.affordanceManager.newAffordanceFromDescription(desc)
