class AnimateRobot(object):
def __init__(self, obj, view):
self.obj = obj
self.view = view
self.timer = TimerCallback(targetFps=60)
self.timer.callback = self.tick
def start(self):
self.startTime = time.time()
self.timer.start()
def stop(self):
self.timer.stop()
def tick(self):
tNow = time.time()
elapsed = tNow - self.startTime
x, y = np.sin(elapsed), np.cos(elapsed)
angle = -elapsed
t = vtk.vtkTransform()
t.PostMultiply()
t.RotateZ(np.degrees(angle))
t.Translate(x, y, 0)
self.obj.getChildFrame().copyFrame(t)
self.view.render()
class AnimateRobot(object):
def __init__(self, obj, view):
self.obj = obj
self.view = view
self.timer = TimerCallback(targetFps=60)
self.timer.callback = self.tick
def start(self):
self.startTime = time.time()
self.timer.start()
def stop(self):
self.timer.stop()
def tick(self):
tNow = time.time()
elapsed = tNow - self.startTime
x, y = np.sin(elapsed), np.cos(elapsed)
angle = -elapsed
t = vtk.vtkTransform()
t.PostMultiply()
t.RotateZ(np.degrees(angle))
t.Translate(x, y, 0)
self.obj.getChildFrame().copyFrame(t)
self.view.render()
class KinectSource(TimerCallback):
def __init__(self, view, _KinectQueue):
self.view = view
self.KinectQueue = _KinectQueue
self.visible = True
self.p = vtk.vtkPolyData()
utime = KinectQueue.getPointCloudFromKinect(self.p)
self.polyDataObj = vis.PolyDataItem('kinect source',
shallowCopy(self.p), view)
self.polyDataObj.actor.SetPickable(1)
self.polyDataObj.initialized = False
om.addToObjectModel(self.polyDataObj)
self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread())
self.queue.init(lcmUtils.getGlobalLCMThread(),
drcargs.args().config_file)
self.targetFps = 30
self.timerCallback = TimerCallback(targetFps=self.targetFps)
self.timerCallback.callback = self._updateSource
#self.timerCallback.start()
def start(self):
self.timerCallback.start()
def stop(self):
self.timerCallback.stop()
def setFPS(self, framerate):
self.targetFps = framerate
self.timerCallback.stop()
self.timerCallback.targetFps = framerate
self.timerCallback.start()
def setVisible(self, visible):
self.polyDataObj.setProperty('Visible', visible)
def _updateSource(self):
p = vtk.vtkPolyData()
utime = self.KinectQueue.getPointCloudFromKinect(p)
if not p.GetNumberOfPoints():
return
cameraToLocalFused = vtk.vtkTransform()
self.queue.getTransform('KINECT_RGB', 'local', utime,
cameraToLocalFused)
p = filterUtils.transformPolyData(p, cameraToLocalFused)
self.polyDataObj.setPolyData(p)
if not self.polyDataObj.initialized:
self.polyDataObj.setProperty('Color By', 'rgb_colors')
self.polyDataObj.initialized = True
class KinectSource(TimerCallback):
def __init__(self, view, _KinectQueue):
self.view = view
self.KinectQueue = _KinectQueue
self.visible = True
self.p = vtk.vtkPolyData()
utime = KinectQueue.getPointCloudFromKinect(self.p)
self.polyDataObj = vis.PolyDataItem('kinect source', shallowCopy(self.p), view)
self.polyDataObj.actor.SetPickable(1)
self.polyDataObj.initialized = False
om.addToObjectModel(self.polyDataObj)
self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread())
self.queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
self.targetFps = 30
self.timerCallback = TimerCallback(targetFps=self.targetFps)
self.timerCallback.callback = self._updateSource
#self.timerCallback.start()
def start(self):
self.timerCallback.start()
def stop(self):
self.timerCallback.stop()
def setFPS(self, framerate):
self.targetFps = framerate
self.timerCallback.stop()
self.timerCallback.targetFps = framerate
self.timerCallback.start()
def setVisible(self, visible):
self.polyDataObj.setProperty('Visible', visible)
def _updateSource(self):
p = vtk.vtkPolyData()
utime = self.KinectQueue.getPointCloudFromKinect(p)
if not p.GetNumberOfPoints():
return
cameraToLocalFused = vtk.vtkTransform()
self.queue.getTransform('KINECT_RGB', 'local', utime, cameraToLocalFused)
p = filterUtils.transformPolyData(p,cameraToLocalFused)
self.polyDataObj.setPolyData(p)
if not self.polyDataObj.initialized:
self.polyDataObj.setProperty('Color By', 'rgb_colors')
self.polyDataObj.initialized = True
class TaskRunner(object):
def __init__(self):
self.interval = 1 / 60.0
sys.setcheckinterval(1000)
#sys.setswitchinterval(self.interval) # sys.setswitchinterval is only Python 3
self.taskQueue = asynctaskqueue.AsyncTaskQueue()
self.pendingTasks = []
self.threads = []
self.timer = TimerCallback(callback=self._onTimer,
targetFps=1 / self.interval)
def _onTimer(self):
# Give up control to another python thread in self.threads
# that might be running
time.sleep(self.interval)
# add all tasks in self.pendingTasks to the AsyncTaskQueue
if self.pendingTasks:
while True:
try:
self.taskQueue.addTask(self.pendingTasks.pop(0))
except IndexError:
break
# start the AsyncTaskQueue if it's not already running
if self.taskQueue.tasks and not self.taskQueue.isRunning:
self.taskQueue.start()
# check which threads are live
liveThreads = []
for t in self.threads:
if t.is_alive():
liveThreads.append(t)
# only retain the live threads
self.threads = liveThreads
# if no liveThreads then stop the timer
if len(self.threads) == 0:
self.timer.stop()
def callOnMain(self, func, *args, **kwargs):
self.pendingTasks.append(lambda: func(*args, **kwargs))
self.timer.start()
def callOnThread(self, func, *args, **kwargs):
t = Thread(target=lambda: func(*args, **kwargs))
self.threads.append(t)
t.start()
self.timer.start()
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 EstRobotStatePublisher(object):
def __init__(self, robotSystem):
self.robotSystem = robotSystem
self.timer = TimerCallback(targetFps=25)
self.timer.callback = self.publishEstRobotState
def publishEstRobotState(self):
q = self.robotSystem.robotStateJointController.q
stateMsg = robotstate.drakePoseToRobotState(q)
stateMsg.utime = utimeUtil.getUtime()
lcmUtils.publish("EST_ROBOT_STATE", stateMsg)
def start(self):
self.timer.start()
def stop(self):
self.timer.stop()
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("MULTISENSE_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 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 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 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 AffordanceInCameraUpdater(object):
def __init__(self, affordanceManager, imageView):
self.affordanceManager = affordanceManager
self.prependImageName = False
self.projectFootsteps = False
self.projectAffordances = True
self.extraObjects = []
self.imageView = imageView
self.imageQueue = imageView.imageManager.queue
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.update
def getOverlayRenderer(self, imageView):
if not hasattr(imageView, 'overlayRenderer'):
renWin = imageView.view.renderWindow()
renWin.SetNumberOfLayers(2)
ren = vtk.vtkRenderer()
ren.SetLayer(1)
ren.SetActiveCamera(imageView.view.camera())
renWin.AddRenderer(ren)
imageView.overlayRenderer = ren
return imageView.overlayRenderer
def addActorToImageOverlay(self, obj, imageView):
obj.addToView(imageView.view)
imageView.view.renderer().RemoveActor(obj.actor)
renderers = obj.extraViewRenderers.setdefault(imageView.view, [])
overlayRenderer = self.getOverlayRenderer(imageView)
if overlayRenderer not in renderers:
overlayRenderer.AddActor(obj.actor)
renderers.append(overlayRenderer)
def getFolderName(self):
if self.prependImageName:
return self.imageView.imageName + ' camera overlay'
else:
return 'camera overlay'
def setupObjectInCamera(self, obj):
imageView = self.imageView
obj = vis.updatePolyData(vtk.vtkPolyData(), self.getTransformedName(obj), view=imageView.view, color=obj.getProperty('Color'), parent=self.getFolderName(), visible=obj.getProperty('Visible'))
self.addActorToImageOverlay(obj, imageView)
return obj
def getTransformedName(self, obj):
if self.prependImageName:
return 'overlay ' + self.imageView.imageName + ' ' + obj.getProperty('Name')
else:
return 'overlay ' + obj.getProperty('Name')
def getFootsteps(self):
plan = om.findObjectByName('footstep plan')
if plan:
return [child for child in plan.children() if child.getProperty('Name').startswith('step ')]
else:
return []
def getObjectsToUpdate(self):
objs = []
if self.projectAffordances:
objs += self.affordanceManager.getAffordances()
if self.projectFootsteps:
objs += self.getFootsteps()
objs += self.extraObjects
return objs
def getObjectInCamera(self, obj):
overlayObj = om.findObjectByName(self.getTransformedName(obj))
return overlayObj or self.setupObjectInCamera(obj)
def cleanUp(self):
self.timer.stop()
om.removeFromObjectModel(om.findObjectByName(self.getFolderName()))
def update(self):
imageView = self.imageView
if not imageView.imageInitialized:
return
if not imageView.view.isVisible():
return
updated = set()
for obj in self.getObjectsToUpdate():
cameraObj = self.getObjectInCamera(obj)
self.updateObjectInCamera(obj, cameraObj)
updated.add(cameraObj)
folder = om.findObjectByName(self.getFolderName())
if folder:
for child in folder.children():
if child not in updated:
om.removeFromObjectModel(child)
def updateObjectInCamera(self, obj, cameraObj):
imageView = self.imageView
objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform() or vtk.vtkTransform())
localToCameraT = vtk.vtkTransform()
self.imageQueue.getTransform('local', imageView.imageName, localToCameraT)
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(objToLocalT)
t.Concatenate(localToCameraT)
pd = filterUtils.transformPolyData(obj.polyData, t)
'''
normals = pd.GetPointData().GetNormals()
cameraToImageT = vtk.vtkTransform()
imageQueue.getCameraProjectionTransform(imageView.imageName, cameraToImageT)
pd = filterUtils.transformPolyData(pd, cameraToImageT)
pts = vnp.getNumpyFromVtk(pd, 'Points')
pts[:,0] /= pts[:,2]
pts[:,1] /= pts[:,2]
pd.GetPointData().SetNormals(normals)
'''
self.imageQueue.projectPoints(imageView.imageName, pd)
cameraObj.setPolyData(pd)
self.addActorToImageOverlay(cameraObj, imageView)
class ValueSlider(object):
events = Flags('VALUE_CHANGED')
def __init__(self, minValue=0.0, maxValue=1.0, resolution=1000):
self._value = 0.0
self.spinbox = QtGui.QDoubleSpinBox()
self.spinbox.setSuffix('s')
self.slider = QtGui.QSlider(QtCore.Qt.Horizontal)
self.playButton = QtGui.QPushButton('Play')
self.setValueRange(minValue, maxValue)
self.setResolution(resolution)
self.slider.connect('valueChanged(int)', self._onSliderValueChanged)
self.spinbox.connect('valueChanged(double)',
self._onSpinboxValueChanged)
self.playButton.connect('clicked()', self._onPlayClicked)
self.widget = QtGui.QWidget()
layout = QtGui.QHBoxLayout(self.widget)
layout.addWidget(self.playButton)
layout.addWidget(self.spinbox)
layout.addWidget(self.slider)
self.animationPrevTime = 0.0
self.animationRate = 1.0
self.animationRateTarget = 1.0
self.animationRateAlpha = 1.0
self.animationTimer = TimerCallback(callback=self._tick, targetFps=60)
self.useRealTime = True
self.callbacks = callbacks.CallbackRegistry(self.events._fields)
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.eventFilter.connect('handleEvent(QObject*, QEvent*)',
self._filterEvent)
self.eventFilter.addFilteredEventType(QtCore.QEvent.MouseButtonPress)
self.eventFilter.addFilteredEventType(QtCore.QEvent.MouseMove)
self.slider.installEventFilter(self.eventFilter)
def _tick(self):
if self.useRealTime:
tnow = time.time()
dt = tnow - self.animationPrevTime
self.animationPrevTime = tnow
else:
dt = (1.0 / self.animationTimer.targetFps)
self.animationRate = (
1.0 - self.animationRateAlpha
) * self.animationRate + self.animationRateAlpha * self.animationRateTarget
valueChange = dt * self.animationRate
value = self.getValue() + valueChange
if value > self.maxValue:
self.setValue(self.maxValue)
self.playButton.setText('Play')
return False
self.setValue(value)
def setAnimationRate(self, animationRate, rateAlpha=1.0):
self.animationRateTarget = animationRate
self.animationRateAlpha = rateAlpha
def play(self):
self.playButton.setText('Pause')
self.animationPrevTime = time.time()
self.animationTimer.start()
def pause(self):
self.playButton.setText('Play')
self.animationTimer.stop()
def _onPlayClicked(self):
if self.animationTimer.isActive():
self.pause()
else:
self.play()
def _filterEvent(self, obj, event):
if event.type() == QtCore.QEvent.MouseButtonPress and event.button(
) == QtCore.Qt.LeftButton:
val = QtGui.QStyle.sliderValueFromPosition(obj.minimum,
obj.maximum, event.x(),
obj.width)
self.eventFilter.setEventHandlerResult(True)
obj.setValue(val)
elif event.type() == QtCore.QEvent.MouseMove:
val = QtGui.QStyle.sliderValueFromPosition(obj.minimum,
obj.maximum, event.x(),
obj.width)
self.eventFilter.setEventHandlerResult(True)
obj.setValue(val)
def setResolution(self, resolution):
with qtutils.BlockSignals(self.slider):
self.slider.maximum = resolution
self._syncSlider()
def setValueRange(self, minValue, maxValue):
newValue = np.clip(self._value, minValue, maxValue)
changed = newValue != self._value
self.minValue = minValue
self.maxValue = maxValue
self._value = newValue
with qtutils.BlockSignals(self.spinbox):
self.spinbox.minimum = minValue
self.spinbox.maximum = maxValue
self._syncSpinBox()
self._syncSlider()
if changed:
self._notifyValueChanged()
def getValue(self):
return self._value
def setValue(self, value):
self._value = np.clip(value, self.minValue, self.maxValue)
self._syncSlider()
self._syncSpinBox()
self._notifyValueChanged()
def connectValueChanged(self, callback):
return self.callbacks.connect(self.events.VALUE_CHANGED, callback)
def _notifyValueChanged(self):
self.callbacks.process(self.events.VALUE_CHANGED, self._value)
def _syncSlider(self):
with qtutils.BlockSignals(self.slider):
self.slider.value = self.slider.maximum * (
self._value - self.minValue) / float(self.maxValue -
self.minValue)
def _syncSpinBox(self):
with qtutils.BlockSignals(self.spinbox):
self.spinbox.value = self._value
def _onSpinboxValueChanged(self, spinboxValue):
self._value = spinboxValue
self._syncSlider()
self._notifyValueChanged()
def _onSliderValueChanged(self, sliderValue):
self._value = (self.minValue + (self.maxValue - self.minValue) *
(sliderValue / float(self.slider.maximum)))
self._syncSpinBox()
self._notifyValueChanged()
class AsyncTaskQueue(object):
QUEUE_STARTED_SIGNAL = "QUEUE_STARTED_SIGNAL"
QUEUE_STOPPED_SIGNAL = "QUEUE_STOPPED_SIGNAL"
TASK_STARTED_SIGNAL = "TASK_STARTED_SIGNAL"
TASK_ENDED_SIGNAL = "TASK_ENDED_SIGNAL"
TASK_PAUSED_SIGNAL = "TASK_PAUSED_SIGNAL"
TASK_FAILED_SIGNAL = "TASK_FAILED_SIGNAL"
TASK_EXCEPTION_SIGNAL = "TASK_EXCEPTION_SIGNAL"
class PauseException(Exception):
pass
class FailException(Exception):
pass
def __init__(self):
self.tasks = []
self.generators = []
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.callbackLoop
self.callbacks = callbacks.CallbackRegistry([
self.QUEUE_STARTED_SIGNAL,
self.QUEUE_STOPPED_SIGNAL,
self.TASK_STARTED_SIGNAL,
self.TASK_ENDED_SIGNAL,
self.TASK_PAUSED_SIGNAL,
self.TASK_FAILED_SIGNAL,
self.TASK_EXCEPTION_SIGNAL,
])
self.currentTask = None
self.isRunning = False
def reset(self):
assert not self.isRunning
assert not self.generators
self.tasks = []
def start(self):
self.isRunning = True
self.callbacks.process(self.QUEUE_STARTED_SIGNAL, self)
self.timer.start()
def stop(self):
self.isRunning = False
self.currentTask = None
self.generators = []
self.timer.stop()
self.callbacks.process(self.QUEUE_STOPPED_SIGNAL, self)
def wrapGenerator(self, generator):
def generatorWrapper():
return generator
return generatorWrapper
def addTask(self, task):
if isinstance(task, types.GeneratorType):
task = self.wrapGenerator(task)
assert hasattr(task, "__call__")
self.tasks.append(task)
def callbackLoop(self):
try:
for i in xrange(10):
self.doWork()
if not self.tasks:
self.stop()
except AsyncTaskQueue.PauseException:
assert self.currentTask
self.callbacks.process(self.TASK_PAUSED_SIGNAL, self,
self.currentTask)
self.stop()
except AsyncTaskQueue.FailException:
assert self.currentTask
self.callbacks.process(self.TASK_FAILED_SIGNAL, self,
self.currentTask)
self.stop()
except:
assert self.currentTask
self.callbacks.process(self.TASK_EXCEPTION_SIGNAL, self,
self.currentTask)
self.stop()
raise
return self.isRunning
def popTask(self):
assert not self.isRunning
assert not self.currentTask
if self.tasks:
self.tasks.pop(0)
def completePreviousTask(self):
assert self.currentTask
self.tasks.remove(self.currentTask)
self.callbacks.process(self.TASK_ENDED_SIGNAL, self, self.currentTask)
self.currentTask = None
def startNextTask(self):
self.currentTask = self.tasks[0]
self.callbacks.process(self.TASK_STARTED_SIGNAL, self,
self.currentTask)
result = self.currentTask()
if isinstance(result, types.GeneratorType):
self.generators.insert(0, result)
def doWork(self):
if self.generators:
self.handleGenerator(self.generators[0])
else:
if self.currentTask:
self.completePreviousTask()
if self.tasks:
self.startNextTask()
def handleGenerator(self, generator):
try:
result = generator.next()
except StopIteration:
self.generators.remove(generator)
else:
if isinstance(result, types.GeneratorType):
self.generators.insert(0, result)
def connectQueueStarted(self, func):
return self.callbacks.connect(self.QUEUE_STARTED_SIGNAL, func)
def disconnectQueueStarted(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectQueueStopped(self, func):
return self.callbacks.connect(self.QUEUE_STOPPED_SIGNAL, func)
def disconnectQueueStopped(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskStarted(self, func):
return self.callbacks.connect(self.TASK_STARTED_SIGNAL, func)
def disconnectTaskStarted(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskEnded(self, func):
return self.callbacks.connect(self.TASK_ENDED_SIGNAL, func)
def disconnectTaskEnded(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskPaused(self, func):
return self.callbacks.connect(self.TASK_PAUSED_SIGNAL, func)
def disconnectTaskPaused(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskFailed(self, func):
return self.callbacks.connect(self.TASK_FAILED_SIGNAL, func)
def disconnectTaskFailed(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskException(self, func):
return self.callbacks.connect(self.TASK_EXCEPTION_SIGNAL, func)
def disconnectTaskException(self, callbackId):
self.callbacks.disconnect(callbackId)
class CameraTrackerManager(object):
def __init__(self):
self.target = None
self.targetFrame = None
self.trackerClass = None
self.camera = None
self.view = None
self.timer = TimerCallback()
self.timer.callback = self.updateTimer
self.addTrackers()
self.initTracker()
def updateTimer(self):
tNow = time.time()
dt = tNow - self.tLast
if dt < self.timer.elapsed / 2.0:
return
self.update()
def setView(self, view):
self.view = view
self.camera = view.camera()
def setTarget(self, target):
'''
target should be an instance of TargetFrameConverter or
any object that provides a method getTargetFrame().
'''
if target == self.target:
return
self.disableActiveTracker()
if not target:
return
self.target = target
self.targetFrame = target.getTargetFrame()
self.callbackId = self.targetFrame.connectFrameModified(
self.onTargetFrameModified)
self.initTracker()
def disableActiveTracker(self):
if self.targetFrame:
self.targetFrame.disconnectFrameModified(self.callbackId)
self.target = None
self.targetFrame = None
self.initTracker()
def update(self):
tNow = time.time()
dt = tNow - self.tLast
self.tLast = tNow
if self.activeTracker:
self.activeTracker.dt = dt
self.activeTracker.update()
def reset(self):
self.tLast = time.time()
if self.activeTracker:
self.activeTracker.reset()
def getModeActions(self):
if self.activeTracker:
return self.activeTracker.actions
return []
def onModeAction(self, actionName):
if self.activeTracker:
self.activeTracker.onAction(actionName)
def getModeProperties(self):
if self.activeTracker:
return self.activeTracker.properties
return None
def onTargetFrameModified(self, frame):
self.update()
def initTracker(self):
self.timer.stop()
self.activeTracker = self.trackerClass(
self.view, self.targetFrame) if (self.trackerClass
and self.targetFrame) else None
self.reset()
self.update()
if self.activeTracker:
minimumUpdateRate = self.activeTracker.getMinimumUpdateRate()
if minimumUpdateRate > 0:
self.timer.targetFps = minimumUpdateRate
self.timer.start()
def addTrackers(self):
self.trackers = OrderedDict([
['Off', None],
['Position', PositionTracker],
['Position & Orientation', PositionOrientationTracker],
['Smooth Follow', SmoothFollowTracker],
['Look At', LookAtTracker],
['Orbit', OrbitTracker],
])
def setTrackerMode(self, modeName):
assert modeName in self.trackers
self.trackerClass = self.trackers[modeName]
self.initTracker()
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 AffordanceInCameraUpdater(object):
def __init__(self, affordanceManager, imageView):
self.affordanceManager = affordanceManager
self.prependImageName = False
self.projectFootsteps = False
self.projectAffordances = True
self.extraObjects = []
self.imageView = imageView
self.imageQueue = imageView.imageManager.queue
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.update
def getOverlayRenderer(self, imageView):
if not hasattr(imageView, "overlayRenderer"):
renWin = imageView.view.renderWindow()
renWin.SetNumberOfLayers(2)
ren = vtk.vtkRenderer()
ren.SetLayer(1)
ren.SetActiveCamera(imageView.view.camera())
renWin.AddRenderer(ren)
imageView.overlayRenderer = ren
return imageView.overlayRenderer
def addActorToImageOverlay(self, obj, imageView):
obj.addToView(imageView.view)
imageView.view.renderer().RemoveActor(obj.actor)
renderers = obj.extraViewRenderers.setdefault(imageView.view, [])
overlayRenderer = self.getOverlayRenderer(imageView)
if overlayRenderer not in renderers:
overlayRenderer.AddActor(obj.actor)
renderers.append(overlayRenderer)
def getFolderName(self):
if self.prependImageName:
return self.imageView.imageName + " camera overlay"
else:
return "camera overlay"
def setupObjectInCamera(self, obj):
imageView = self.imageView
obj = vis.updatePolyData(
vtk.vtkPolyData(),
self.getTransformedName(obj),
view=imageView.view,
color=obj.getProperty("Color"),
parent=self.getFolderName(),
visible=obj.getProperty("Visible"),
)
self.addActorToImageOverlay(obj, imageView)
return obj
def getTransformedName(self, obj):
if self.prependImageName:
return "overlay " + self.imageView.imageName + " " + obj.getProperty(
"Name")
else:
return "overlay " + obj.getProperty("Name")
def getFootsteps(self):
plan = om.findObjectByName("footstep plan")
if plan:
return [
child for child in plan.children()
if child.getProperty("Name").startswith("step ")
]
else:
return []
def getObjectsToUpdate(self):
objs = []
if self.projectAffordances:
objs += self.affordanceManager.getAffordances()
if self.projectFootsteps:
objs += self.getFootsteps()
objs += self.extraObjects
return objs
def getObjectInCamera(self, obj):
overlayObj = om.findObjectByName(self.getTransformedName(obj))
return overlayObj or self.setupObjectInCamera(obj)
def cleanUp(self):
self.timer.stop()
om.removeFromObjectModel(om.findObjectByName(self.getFolderName()))
def update(self):
imageView = self.imageView
if not imageView.imageInitialized:
return
if not imageView.view.isVisible():
return
updated = set()
for obj in self.getObjectsToUpdate():
cameraObj = self.getObjectInCamera(obj)
self.updateObjectInCamera(obj, cameraObj)
updated.add(cameraObj)
folder = om.findObjectByName(self.getFolderName())
if folder:
for child in folder.children():
if child not in updated:
om.removeFromObjectModel(child)
def updateObjectInCamera(self, obj, cameraObj):
imageView = self.imageView
objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform()
or vtk.vtkTransform())
localToCameraT = vtk.vtkTransform()
self.imageQueue.getTransform("local", imageView.imageName,
localToCameraT)
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(objToLocalT)
t.Concatenate(localToCameraT)
pd = filterUtils.transformPolyData(obj.polyData, t)
"""
normals = pd.GetPointData().GetNormals()
cameraToImageT = vtk.vtkTransform()
imageQueue.getCameraProjectionTransform(imageView.imageName, cameraToImageT)
pd = filterUtils.transformPolyData(pd, cameraToImageT)
pts = vnp.getNumpyFromVtk(pd, 'Points')
pts[:,0] /= pts[:,2]
pts[:,1] /= pts[:,2]
pd.GetPointData().SetNormals(normals)
"""
self.imageQueue.projectPoints(imageView.imageName, pd)
cameraObj.setPolyData(pd)
self.addActorToImageOverlay(cameraObj, imageView)
class 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 = 0.4
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.98
self.options['World']['randomSeed'] = 40
self.options['World']['percentObsDensity'] = 10
self.options['World']['nonRandomWorld'] = True
self.options['World']['circleRadius'] = 1.0
self.options['World']['scale'] = 1
self.options['Sensor'] = dict()
self.options['Sensor']['rayLength'] = 20
self.options['Sensor']['numRays'] = 21
self.options['Car'] = dict()
self.options['Car']['velocity'] = 4.0
self.options['dt'] = 0.05
self.options['runTime'] = dict()
self.options['runTime']['defaultControllerTime'] = 100
def setDefaultOptions(self):
defaultOptions = dict()
defaultOptions['World'] = dict()
defaultOptions['World']['obstaclesInnerFraction'] = 0.98
defaultOptions['World']['randomSeed'] = 40
defaultOptions['World']['percentObsDensity'] = 30
defaultOptions['World']['nonRandomWorld'] = True
defaultOptions['World']['circleRadius'] = 1.75
defaultOptions['World']['scale'] = 2.5
defaultOptions['Sensor'] = dict()
defaultOptions['Sensor']['rayLength'] = 20
defaultOptions['Sensor']['numRays'] = 41
defaultOptions['Car'] = dict()
defaultOptions['Car']['velocity'] = 20
defaultOptions['dt'] = 0.05
defaultOptions['runTime'] = dict()
defaultOptions['runTime']['defaultControllerTime'] = 100
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.SensorApproximator = SensorApproximatorObj(numRays=self.options['Sensor']['numRays'], circleRadius=self.options['World']['circleRadius'], )
self.Controller = ControllerObj(self.Sensor, self.SensorApproximator)
self.Car = CarPlant(controller=self.Controller,
velocity=self.options['Car']['velocity'])
self.Controller.initializeVelocity(self.Car.v)
# create the things needed for simulation
om.removeFromObjectModel(om.findObjectByName('world'))
self.world = World.buildLineSegmentTestWorld(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('Visible', False)
#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.Car.setFrame(self.frame)
print "Finished initialization"
def runSingleSimulation(self, controllerType='default', simulationCutoff=None):
#self.setRandomCollisionFreeInitialState()
self.setInitialStateAtZero()
currentCarState = np.copy(self.Car.state)
nextCarState = np.copy(self.Car.state)
self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2])
firstRaycast = self.Sensor.raycastAll(self.frame)
firstRaycastLocations = self.Sensor.raycastAllLocations(self.frame)
self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
self.Sensor.setLocator(self.LineSegmentLocator)
nextRaycast = np.zeros(self.Sensor.numRays)
# record the reward data
runData = dict()
startIdx = self.counter
thisRunIndex = 0
NMaxSteps = 100
while (self.counter < self.numTimesteps - 1):
thisRunIndex += 1
idx = self.counter
currentTime = self.t[idx]
self.stateOverTime[idx,:] = currentCarState
x = self.stateOverTime[idx,0]
y = self.stateOverTime[idx,1]
theta = self.stateOverTime[idx,2]
self.setRobotFrameState(x,y,theta)
# self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2])
currentRaycast = self.Sensor.raycastAll(self.frame)
print "current Raycast ", currentRaycast
self.raycastData[idx,:] = currentRaycast
S_current = (currentCarState, currentRaycast)
if controllerType not in self.colorMap.keys():
print
raise ValueError("controller of type " + controllerType + " not supported")
if controllerType in ["default", "defaultRandom"]:
controlInput, controlInputIdx = self.Controller.computeControlInput(currentCarState,
currentTime, self.frame,
raycastDistance=currentRaycast,
randomize=False)
self.controlInputData[idx] = controlInput
nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt)
x = nextCarState[0]
y = nextCarState[1]
theta = nextCarState[2]
self.setRobotFrameState(x,y,theta)
nextRaycast = self.Sensor.raycastAll(self.frame)
# Compute the next control input
S_next = (nextCarState, nextRaycast)
if controllerType in ["default", "defaultRandom"]:
nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(nextCarState,
currentTime, self.frame,
raycastDistance=firstRaycast,
randomize=False)
#bookkeeping
currentCarState = nextCarState
currentRaycast = nextRaycast
self.counter+=1
# break if we are in collision
if self.checkInCollision(nextRaycast):
if self.verbose: print "Had a collision, terminating simulation"
break
if thisRunIndex > NMaxSteps:
print "was safe during N steps"
break
if self.counter >= simulationCutoff:
break
# fill in the last state by hand
self.stateOverTime[self.counter,:] = currentCarState
self.raycastData[self.counter,:] = currentRaycast
# this just makes sure we don't get stuck in an infinite loop.
if startIdx == self.counter:
self.counter += 1
return runData
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.Car.setCarState(x,y,theta)
self.setRobotFrameState(x,y,theta)
print "In loop"
if not self.checkInCollision():
break
return x,y,theta
def setInitialStateAtZero(self):
x = 0.0
y = 0.0
theta = 0.0
self.Car.setCarState(x,y,theta)
self.setRobotFrameState(x,y,theta)
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.Car.setCarState(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)
self.view.orientationMarkerWidget().Off()
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()
cameracontrolpanel.CameraControlPanel(self.view).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()
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 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.LineSegmentLocator, 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
# if raycastDistance[(len(raycastDistance)+1)/2] < 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',
lcmbotcore.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 DirectorROSVisualizer(object):
def __init__(self, tf_buffer=None):
self.taskRunner = TaskRunner()
if tf_buffer is None:
self.setup_TF()
else:
self._tf_buffer = tf_buffer
self.clear_visualization()
self._subscribers = dict()
self._expressed_in_frame = "base"
def setup_TF(self):
"""
Sets up TF
:return:
:rtype:
"""
if self._tf_buffer is None:
self._tf_buffer = tf2_ros.Buffer()
self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
def clear_visualization(self):
"""
Clears the visualization
:return:
:rtype:
"""
container_name = "ROS"
self._vis_container = om.getOrCreateContainer(container_name)
om.removeFromObjectModel(self._vis_container)
self._vis_container = om.getOrCreateContainer(container_name)
def add_subscriber(self,
topic,
name=None,
call_in_thread=True,
visualize=False,
msg_type=None):
"""
Adds a subscriber
:param topic:
:type topic:
:param call_in_thread:
:type call_in_thread:
:return:
:rtype:
"""
if call_in_thread:
self.add_subscriber(topic,
name=name,
call_in_thread=False,
visualize=visualize,
msg_type=msg_type)
return
if msg_type is None:
msg_type = sensor_msgs.msg.PointCloud2
if name is None:
name = topic
subscriber = ros_utils.SimpleSubscriber(topic, msg_type)
subscriber.start()
d = dict()
d['subscriber'] = subscriber
d['topic'] = topic
d['visualize'] = visualize
d['name'] = name
self._subscribers[topic] = d
def update(self, snapshot=False):
"""
Visualizes the pointclouds set to true. This should be called from the main thread
:return:
:rtype:
"""
for topic, data in self._subscribers.iteritems():
if not data['visualize']:
continue
msg = data['subscriber'].lastMsg
if msg is None:
continue
# get frame
# TransformStamped
# this might need to be called in thread
try:
T_W_pointcloud_stamped = self._tf_buffer.lookup_transform(
self._expressed_in_frame, msg.header.frame_id,
msg.header.stamp)
except:
continue
T_W_pointcloud = ros_numpy.numpify(
T_W_pointcloud_stamped.transform)
T_W_pointcloud_vtk = transformUtils.getTransformFromNumpy(
T_W_pointcloud)
pointcloud_numpy = DirectorROSVisualizer.numpy_from_pointcloud2_msg(
msg)
pointcloud_vtk = vnp.getVtkPolyDataFromNumpyPoints(
pointcloud_numpy)
pointcloud_vtk = filterUtils.transformPolyData(
pointcloud_vtk, T_W_pointcloud_vtk)
data['pointcloud'] = pointcloud_vtk
if snapshot:
name = data["name"] + " snapshot"
vis.showPolyData(pointcloud_vtk,
name,
parent=self._vis_container)
else:
vis.updatePolyData(pointcloud_vtk,
data['name'],
parent=self._vis_container)
def start(self, rate_hz=30):
"""
Launches a timercallback that just calls update()
:param rate_hz:
:type rate_hz:
:return:
:rtype:
"""
self._timercallback = TimerCallback(targetFps=rate_hz,
callback=self.update)
self._timercallback.start()
def stop(self):
"""
Stops the visualization callback
:return:
:rtype:
"""
self._timercallback.stop()
def snapshot(self):
self.update(snapshot=True)
def save_snapshot(self):
save_dir = os.path.join(spartan_utils.get_sandbox_dir(), "pointclouds")
if not os.path.exists(save_dir):
os.makedirs(save_dir)
for topic, data in self._subscribers.iteritems():
filename = os.path.join(save_dir, "%s.ply" % (data['name']))
filename.replace(" ", "_")
ioUtils.writePolyData(data['pointcloud'], filename)
@staticmethod
def numpy_from_pointcloud2_msg(msg):
"""
:param msg: sensor_msgs/PointCloud2
:type msg:
:return:
:rtype:
"""
pc = ros_numpy.numpify(msg)
num_points = msg.width * msg.height
points = np.zeros((num_points, 3))
points[:, 0] = pc['x'].flatten()
points[:, 1] = pc['y'].flatten()
points[:, 2] = pc['z'].flatten()
return points
@staticmethod
def pointcloud2_msg_from_numpy(pc_numpy):
"""
:param pc_numpy: N x 3
:type pc_numpy:
:return:
:rtype:
"""
N = pc_numpy.shape[0]
pc = np.zeros(N,
dtype=[('x', np.float64), ('y', np.float64),
('z', np.float64)])
pc['x'] = pc_numpy[:, 0]
pc['y'] = pc_numpy[:, 1]
pc['z'] = pc_numpy[:, 2]
msg = ros_numpy.msgify(sensor_msgs.msg.PointCloud2, pc)
return msg
class DataCollectionPlanRunner(object):
def __init__(self,
dataCollection,
robotSystem,
targetFrames,
configFilename=None):
self.robotSystem = robotSystem
self.dataCollection = dataCollection
self.timer = TimerCallback(targetFps=5)
self.timer.callback = self.callback
self.targetFrames = targetFrames
self.counter = 0
self.configFilename = configFilename
self.initialized = False
self.loadConfig(self.configFilename)
def loadConfig(self, configFilename):
if configFilename is None:
configFilename = 'data_collection.yaml'
fullFilename = os.path.join(CorlUtils.getCorlBaseDir(), 'config',
configFilename)
self.config = CorlUtils.getDictFromYamlFilename(fullFilename)
def start(self):
print "starting data collection plan runner"
self.timer.start()
os.system(
"cd /home/robot-lab/newdata && sleep 4 && auto_start_data_collect &"
)
def stop(self):
print "stopping data collection plan runner"
self.timer.stop()
def callback(self):
if self.initialized:
utime = self.getUtime()
if utime < self.planData['endUTime']:
return
self.initialized = True
if self.counter >= len(self.targetFrames):
print "finished reaching all target frames"
self.stop()
return
planData = self.makeNextPlan()
if planData['info'] == 1:
self.commitNextPlan()
else:
self.stop()
raise ValueError(' plan info was not 1, stopping execution')
def makeNextPlan(self):
targetFrame = self.targetFrames[self.counter].transform
graspToHandLinkFrame = CorlUtils.getCameraToKukaEndEffectorFrame()
maxDegreesPerSecond = self.config['planning']['maxDegreesPerSecond']
self.planData = self.dataCollection.runIK(
targetFrame,
graspToHandLinkFrame=graspToHandLinkFrame,
maxDegreesPerSecond=maxDegreesPerSecond)
return self.planData
def getUtime(self):
return self.robotSystem.robotStateJointController.lastRobotStateMessage.utime
def commitNextPlan(self):
print "committed a new plan"
self.robotSystem.manipPlanner.commitManipPlan(self.planData['plan'])
planDuration = self.planData['plan'].plan[-1].utime
self.planData['endUTime'] = self.getUtime() + 1.1 * planDuration
self.counter += 1
class LcmLogPlayer(object):
def __init__(self, lcmHandle=None):
if not lcmHandle:
lcmHandle = lcmUtils.getGlobalLCM()
self.lcmHandle = lcmHandle
self.log = None
self.filePositions = []
self.playbackFactor = 1.0
self.timer = TimerCallback()
self.timestamps = np.array([])
self.timestampOffset = 0.0
def findEventIndex(self, timestampRequest):
requestIndex = self.timestamps.searchsorted(timestampRequest)
if requestIndex >= len(self.timestamps):
requestIndex = len(self.timestamps) - 1
return requestIndex
def resetPlayPosition(self, playTime):
self.nextEventIndex = self.findEventIndex(playTime * 1e6)
filepos = self.filePositions[self.nextEventIndex]
self.log.seek(filepos)
def advanceTime(self, playLength, onFrame=None):
numEvents = len(self.timestamps)
if self.nextEventIndex >= numEvents:
return
startTimestamp = self.timestamps[self.nextEventIndex]
endTimestamp = startTimestamp + playLength * 1e6
good = True
while good:
event = self.log.read_next_event()
self.nextEventIndex += 1
self.lcmHandle.publish(event.channel, event.data)
good = (self.nextEventIndex < numEvents
and self.timestamps[self.nextEventIndex] <= endTimestamp)
if onFrame and good:
onFrame(self.timestamps[self.nextEventIndex] / 1.e6)
def skipToTime(self, timeRequest, playLength=0.0):
self.resetPlayPosition(timeRequest)
self.advanceTime(playLength)
def getEndTime(self):
assert len(self.timestamps)
return self.timestamps[-1] * 1e-6
def stop(self):
self.timer.stop()
def playback(self, startTime, playLength, onFrame=None, onStop=None):
self.resetPlayPosition(startTime)
startTimestamp = self.timestamps[self.nextEventIndex]
endTimestamp = startTimestamp + playLength * 1e6
def onTick():
elapsed = self.timer.elapsed * self.playbackFactor
self.advanceTime(elapsed, onFrame)
good = (self.nextEventIndex < len(self.timestamps)
and self.timestamps[self.nextEventIndex] <= endTimestamp)
if onFrame and good:
onFrame(self.timestamps[self.nextEventIndex] / 1.e6)
if onStop and not good:
onStop()
return bool(good) #convert numpy.bool to bool
self.timer.callback = onTick
self.timer.start()
def readLog(self, filename, eventTimeFunction=None, progressFunction=None):
log = lcm.EventLog(filename, 'r')
self.log = log
timestamps = []
filePositions = []
offsetIsDefined = False
timestampOffset = 0
lastEventTimestamp = 0
nextProgressTime = 0.0
while True:
filepos = log.tell()
event = log.read_next_event()
if not event:
break
if eventTimeFunction:
eventTimestamp = eventTimeFunction(event)
else:
eventTimestamp = event.timestamp
if eventTimestamp is None:
eventTimestamp = lastEventTimestamp
elif not offsetIsDefined:
timestampOffset = eventTimestamp
offsetIsDefined = True
lastEventTimestamp = eventTimestamp
timestamp = eventTimestamp - timestampOffset
if progressFunction:
progressTime = timestamp * 1e-6
if progressTime >= nextProgressTime:
nextProgressTime += 1.0
if not progressFunction(timestamp * 1e-6):
break
filePositions.append(filepos)
timestamps.append(timestamp)
self.filePositions = filePositions
self.timestamps = np.array(timestamps)
self.timestampOffset = timestampOffset
class ExternalForce(object):
def __init__(self,
robotSystem,
configFilename="contact_particle_filter_config.yaml"):
self.robotSystem = robotSystem
self.robotStateModel = robotSystem.robotStateModel
self.robotStateModel.connectModelChanged(self.onModelChanged)
self.options = cfUtils.loadConfig(configFilename)
self.loadDrakeModelFromFilename()
self.initializeRobotPoseTranslator()
self.initializeJointNamesList()
# keys = linkNames, wrench = 6 x 1 Torque-Force vector, all in body frame
self.externalForces = dict()
self.publishChannel = 'EXTERNAL_FORCE_TORQUE'
self.captureMode = False
self.captureModeCounter = 0
self.showContactRay = True
self.showContactFilterEstimate = True
self.addSubscribers()
self.createPlunger()
self.createMeshDataAndLocators()
self.createTwoStepEstimator(configFilename)
self.visObjectDrawTime = dict()
self.timeout = 1.5
# setup timercallback to publish, lets say at 5 hz
self.timer = TimerCallback(targetFps=25)
self.timer.callback = self.publish
self.startPublishing()
# self.startPublishing() # this now gets activated when you start linkSelection
self.visObjectCleanupTimer = TimerCallback(targetFps=1)
self.visObjectCleanupTimer.callback = self.removeStaleVisObjects
self.visObjectCleanupTimer.start()
self.initializationTime = time.time()
self.trueResidual = None
self.estimatedResidual = None
def addSubscribers(self):
# lcmUtils.addSubscriber('CONTACT_FILTER_POINT_ESTIMATE', cpf_lcmtypes.contact_filter_estimate_t, self.onContactEstimate)
lcmUtils.addSubscriber(
'RESIDUAL_OBSERVER_STATE',
robotlocomotion_lcmtypes.residual_observer_state_t,
self.onResidualObserverState)
# lcmUtils.addSubscriber("CONTACT_FILTER_BODY_WRENCH_ESTIMATE", cpf_lcmtypes.contact_filter_body_wrench_estimate_t, self.onActiveLinkContactEstimate)
# lcmUtils.addSubscriber("EXTERNAL_FORCE_TORQUE", lcmdrake.lcmt_external_force_torque(), self.onActiveLinkContactEstimate)
def createMeshDataAndLocators(self):
self.linkMeshData = dict()
drakeModelLinkNames = self.robotStateModel.model.getLinkNames()
for linkName in drakeModelLinkNames:
linkName = str(linkName)
data = dict()
polyData = vtk.vtkPolyData()
self.robotStateModel.model.getLinkModelMesh(linkName, polyData)
transform = self.robotStateModel.getLinkFrame(linkName)
data['linkName'] = linkName
data['polyData'] = polyData
data['transform'] = transformUtils.copyFrame(
self.robotStateModel.getLinkFrame(linkName))
if (polyData.GetNumberOfCells() == 0):
print linkName + " mesh has no cells, not building a locator for it"
continue
data['locator'] = self.buildCellLocator(polyData)
self.linkMeshData[linkName] = data
def createTwoStepEstimator(self, configFilename):
self.twoStepEstimator = TwoStepEstimator(
self.robotStateModel, self.robotSystem.robotStateJointController,
self.linkMeshData, configFilename)
# either get the EST_ROBOT_STATE utime or just use the wall clock
def getUtime(self):
msg = self.robotSystem.robotStateJointController.lastRobotStateMessage
utime = 0
if msg is not None:
utime = msg.utime
else:
utime = utimeUtil.getUtime() / 5.0 # slow down time by factor of 5
return utime
def loadDrakeModelFromFilename(self, filename=None):
urdf_filename = self.options['robot']['urdf']
floatingBaseTypeString = self.options['robot']['floatingBaseType']
self.drakeModel = PythonDrakeModel(floatingBaseTypeString,
urdf_filename)
def initializeJointNamesList(self):
jointNamesTuple = self.drakeModel.model.getJointNames()
jointNames = []
for idx, val in enumerate(jointNamesTuple):
jointNames.append(str(val))
self.jointNames = jointNames
def initializeRobotPoseTranslator(self):
self.robotPoseTranslator = cfUtils.RobotPoseTranslator(
self.robotSystem.robotStateModel.model, self.drakeModel.model)
# linkName is a string, wrench is an np.array
def addForce(self,
linkName,
wrench=None,
forceDirection=None,
forceMagnitude=None,
forceLocation=None,
inWorldFrame=False):
linkName = str(linkName) # getting a weird u in front otherwise
d = dict()
# need at least one of wrench, or forceDirection and forceMagnitude
assert (wrench is not None) or ((forceDirection is not None) and
(forceMagnitude is not None) and
(forceLocation is not None))
if self.captureMode:
self.captureModeCounter += 1
key = linkName + "_" + str(self.captureModeCounter)
else:
key = linkName
# check to see if a force on this body already exists, if so then use that as the forceMagnitude
if self.externalForces.has_key(key):
forceMagnitude = self.externalForces[key]['forceMagnitude']
visName = key + ' external force'
om.removeFromObjectModel(om.findObjectByName(visName))
if wrench is not None:
if inWorldFrame:
raise ValueError(
'do not support specifying wrench in world frame')
d['wrench'] = wrench
d['forceLocation'] = np.array([0, 0, 0])
d['forceDirection'] = wrench[3:] / np.linalg.norm(wrench[3:])
d['forceMagnitude'] = np.linalg.norm(wrench[3:])
d['isWrench'] = True
d['linkName'] = linkName
else:
if inWorldFrame:
linkToWorld = self.robotStateModel.getLinkFrame(linkName)
worldToLink = linkToWorld.GetLinearInverse()
forceLocation = np.array(
worldToLink.TransformPoint(forceLocation))
forceDirection = np.array(
worldToLink.TransformDoubleVector(forceDirection))
# this should all be in link frame
forceDirection = forceDirection / np.linalg.norm(forceDirection)
d['forceDirection'] = forceDirection
d['forceMagnitude'] = forceMagnitude
d['forceLocation'] = forceLocation
d['isWrench'] = False
d['linkName'] = linkName
d['time'] = time.time()
self.externalForces[key] = d
self.updateContactWrench(key)
self.drawForces()
def computeWrench(self, linkName, forceDirection, forceMagnitude,
forceLocation):
outputFrame = vtk.vtkTransform()
wrenchFrame = vtk.vtkTransform()
wrenchFrame.Translate(forceLocation)
forceMomentTransform = transformUtils.forceMomentTransformation(
wrenchFrame, outputFrame)
wrench = np.zeros(6)
wrench[3:] = forceMagnitude * forceDirection
wrenchTransformed = np.dot(forceMomentTransform, wrench)
return wrenchTransformed
# WARNING: make sure you call doKinematics before you get here
def computeSingleContactPointResidual(self, linkName, wrench):
linkId = self.drakeModel.model.findLinkID(linkName)
geometricJacobian = self.drakeModel.geometricJacobian(
0, linkId, linkId, 0, False)
singleContactResidual = np.dot(geometricJacobian.transpose(), wrench)
return singleContactResidual
def removeForce(self, key, callFromFrameObj=False):
if not self.externalForces.has_key(key):
return
visObjectName = key + ' external force'
self.externalForces.pop(key, None)
if not callFromFrameObj:
om.removeFromObjectModel(om.findObjectByName(visObjectName))
def removeAllForces(self):
keyList = list(self.externalForces.keys())
for key in keyList:
self.removeForce(key)
# remove forces from dict that haven't been refreshed in at least self.timeout seconds
def removeStaleExternalForces(self):
keysToRemove = []
for key, value in self.externalForces.iteritems():
elapsed = time.time() - value['time']
if elapsed > self.timeout:
keysToRemove.append(key)
for key in keysToRemove:
self.removeForce(key)
def removeStaleVisObjects(self):
keysToRemove = []
for key, val in self.visObjectDrawTime.iteritems():
elapsed = time.time() - val
if elapsed > self.timeout:
keysToRemove.append(key)
for key in keysToRemove:
om.removeFromObjectModel(om.findObjectByName(key))
del self.visObjectDrawTime[key]
# be careful here if director and this use different models
# for example if we are FIXED base and director has ROLLPITCHYAW
def getCurrentPose(self):
q_director = self.robotSystem.robotStateJointController.q
q = self.robotPoseTranslator.translateDirectorPoseToRobotPose(
q_director)
return q
def publish(self):
# if len(self.externalForces) == 0:
# return
tol = 1e-3
numExternalForces = 0
msg = cpf_lcmtypes.external_force_torque_t()
msgMultipleContactLocations = cpf_lcmtypes.multiple_contact_location_t(
)
trueResidual = np.zeros((self.drakeModel.numJoints, ))
# make sure we call doKinematics before we do all the geometricJacobian stuff
if self.options['debug']['publishTrueResidual']:
q = self.getCurrentPose()
self.drakeModel.model.setJointPositions(q)
# alternatively can just do setJointPositions
# self.drakeModel.model.setJointPositions(q)
for key, val in self.externalForces.iteritems():
# don't publish it if the force is very small
if np.linalg.norm(val['wrench']) < tol:
continue
numExternalForces += 1
msg.body_names.append(val['linkName'])
msg.tx.append(val['wrench'][0])
msg.ty.append(val['wrench'][1])
msg.tz.append(val['wrench'][2])
msg.fx.append(val['wrench'][3])
msg.fy.append(val['wrench'][4])
msg.fz.append(val['wrench'][5])
linkName = val['linkName']
linkFrame = self.robotStateModel.getLinkFrame(linkName)
contactLocationInWorld = linkFrame.TransformPoint(
val['forceLocation'])
contactNormalInWorld = linkFrame.TransformVector(
val['forceDirection'])
force = val['forceMagnitude'] * val['forceDirection']
forceInWorld = linkFrame.TransformPoint(force)
msgContactLocation = cpf_lcmtypes.single_contact_filter_estimate_t(
)
msgContactLocation.body_name = linkName
msgContactLocation.contact_position = val['forceLocation']
msgContactLocation.contact_force = force
msgContactLocation.contact_normal = val['forceDirection']
msgContactLocation.contact_position_in_world = contactLocationInWorld
msgContactLocation.contact_force_in_world = forceInWorld
msgContactLocation.contact_normal_in_world = contactNormalInWorld
msgMultipleContactLocations.contacts.append(msgContactLocation)
# compute the true residual if we are asked to publish it
if self.options['debug']['publishTrueResidual']:
singleContactResidual = self.computeSingleContactPointResidual(
val['linkName'], val['wrench'])
trueResidual += singleContactResidual
# this message goes to the simulator
msg.num_external_forces = numExternalForces
lcmUtils.publish(self.publishChannel, msg)
# this message is for analysis
msgMultipleContactLocations.num_contacts = numExternalForces
lcmUtils.publish("EXTERNAL_CONTACT_LOCATION",
msgMultipleContactLocations)
# this message is for debugging
if self.options['debug']['publishTrueResidual']:
self.trueResidual = trueResidual
residualMsg = robotlocomotion_lcmtypes.residual_observer_state_t()
residualMsg.utime = self.getUtime()
residualMsg.num_joints = len(self.jointNames)
residualMsg.joint_name = self.jointNames
residualMsg.residual = trueResidual
# these are just placeholders so lcm doesn't complain
# we are not actually populating them
residualMsg.gravity = 0 * trueResidual
residualMsg.internal_torque = 0 * trueResidual
residualMsg.foot_contact_torque = 0 * trueResidual
lcmUtils.publish("RESIDUAL_ACTUAL", residualMsg)
if self.options['twoStepEstimator']['computeEstimate']:
twoStepEstimateData = self.computeTwoStepEstimate()
# if twoStepEstimateData is None it means that some criterion
# wasn't satisfied and we didn't actually perform the estimation
if twoStepEstimateData is not None:
self.publishTwoStepEstimateData(twoStepEstimateData,
msgMultipleContactLocations)
if self.options['twoStepEstimator']['visualize']:
self.visualizeTwoStepEstimate(twoStepEstimateData)
def visualizeTwoStepEstimate(self, data):
for linkName, singleContactData in data.iteritems():
self.visualizeTwoStepEstimateSingleContact(singleContactData)
def visualizeTwoStepEstimateSingleContact(self, data):
# draw the contact ray if that option is set
if self.options['twoStepEstimator']['showContactRay']:
d = DebugData()
d.addLine(data['contactRay']['rayOriginInWorld'],
data['contactRay']['rayEndInWorld'],
radius=0.005)
color = data['contactRay']['color']
visName = data['contactRay']['visObjectName']
obj = vis.updatePolyData(d.getPolyData(), visName, color=color)
self.visObjectDrawTime[visName] = time.time()
# draw the actual contact point if it exists
if data['pt'] is not None:
visName = data['linkName'] + " active link estimated external force"
self.drawForce(visName,
data['linkName'],
data['pt'],
data['force'],
color=[1, 0, 0])
self.visObjectDrawTime[visName] = time.time()
def publishTwoStepEstimateData(self, twoStepEstimateData,
actualContactLocationsMsg):
msg = cpf_lcmtypes.actual_and_estimated_contact_locations_t()
msg.utime = self.getUtime()
msg.actual_contact_location = actualContactLocationsMsg
estMsg = cpf_lcmtypes.multiple_contact_location_t()
estMsg.num_contacts = len(twoStepEstimateData)
for linkName, data in twoStepEstimateData.iteritems():
tmpMsg = cpf_lcmtypes.single_contact_filter_estimate_t()
tmpMsg.body_name = linkName
tmpMsg.contact_normal = data['force']
if data['pt'] is None:
tmpMsg.utime = -1 # signifies that no intersection was found
else:
tmpMsg.contact_position = data['contactLocation']
tmpMsg.contact_position_in_world = data[
'contactLocationInWorld']
tmpMsg.contact_force = data['force']
tmpMsg.contact_force_in_world = data['forceInWorld']
estMsg.contacts.append(tmpMsg)
msg.estimated_contact_location = estMsg
lcmUtils.publish(self.options['twoStepEstimator']['publishChannel'],
msg)
def startPublishing(self):
self.captureMode = False
self.captureModeCounter = 0
self.removeAllForces()
self.timer.start()
def stopPublishing(self):
print "stopping publishing"
self.timer.stop()
def startCaptureMode(self):
self.stopPublishing()
print "starting capture mode"
self.removeAllForces()
self.captureMode = True
self.captureModeCounter = 0
def onResidualObserverState(self, msg):
msgJointNames = msg.joint_name
msgData = msg.residual
residual = self.drakeModel.extractDataFromMessage(
msgJointNames, msgData)
self.estimatedResidual = residual
def onContactEstimate(self, msg):
if not self.showContactFilterEstimate:
return
name = 'estimated external force'
for i in xrange(0, msg.num_contact_points):
subMsg = msg.single_contact_estimate[i]
forceLocation = np.array(subMsg.contact_position)
force = np.array(subMsg.contact_force)
eps = 0.5
name = 'estimated external force ' + str(i)
if np.linalg.norm(force) < eps:
# om.removeFromObjectModel(om.findObjectByName(name))
return
self.drawForce(name,
subMsg.body_name,
forceLocation,
force,
color=[0, 0, 1])
self.visObjectDrawTime[name] = time.time()
def drawForce(self, name, linkName, forceLocation, force, color, key=None):
forceDirection = force / np.linalg.norm(force)
# om.removeFromObjectModel(om.findObjectByName(name))
linkToWorld = self.robotStateModel.getLinkFrame(linkName)
forceLocationInWorld = np.array(
linkToWorld.TransformPoint(forceLocation))
forceDirectionInWorld = np.array(
linkToWorld.TransformDoubleVector(forceDirection))
# point = forceLocationInWorld - 0.1*forceDirectionInWorld
# d = DebugData()
# # d.addArrow(point, forceLocationInWorld, headRadius=0.025, tubeRadius=0.005, color=color)
# d.addSphere(forceLocationInWorld, radius=0.01)
# d.addLine(point, forceLocationInWorld, radius=0.005)
transformForVis = transformUtils.getTransformFromOriginAndNormal(
forceLocationInWorld, forceDirectionInWorld)
obj = vis.updatePolyData(self.plungerPolyData, name, color=color)
obj.actor.SetUserTransform(transformForVis)
if key is not None and om.findObjectByName(name) is not None:
obj.addProperty('magnitude',
self.externalForces[key]['forceMagnitude'])
obj.addProperty('linkName', linkName)
obj.addProperty('key', key)
obj.connectRemovedFromObjectModel(self.removeForceFromFrameObject)
obj.properties.connectPropertyChanged(
functools.partial(self.onPropertyChanged, obj))
return obj
# connect this with an on model changed
def drawForces(self):
if len(self.externalForces) == 0:
return
for key, val in self.externalForces.iteritems():
linkName = val['linkName']
name = key + ' external force'
#Green is for a force, red is for a wrench
color = [0, 1, 0]
if val['isWrench']:
color = [1, 0, 0]
# linkToWorld = self.robotStateModel.getLinkFrame(linkName)
# forceLocationInWorld = np.array(linkToWorld.TransformPoint(val['forceLocation']))
# forceDirectionInWorld = np.array(linkToWorld.TransformDoubleVector(val['forceDirection']))
# point = forceLocationInWorld - 0.1*forceDirectionInWorld
# d = DebugData()
# # d.addArrow(point, forceLocationInWorld, headRadius=0.025, tubeRadius=0.005, color=color)
# d.addSphere(forceLocationInWorld, radius=0.01)
# d.addLine(point, forceLocationInWorld, radius=0.005)
obj = self.drawForce(name,
linkName,
val['forceLocation'],
val['forceDirection'],
color,
key=key)
def onModelChanged(self, model):
self.drawForces()
def onPropertyChanged(self, frameObj, propertySet, propertyName):
if propertyName != 'magnitude':
return
key = frameObj.getProperty('key')
linkName = frameObj.getProperty('linkName')
magnitude = frameObj.getProperty('magnitude')
if magnitude < 0:
print "you must specify a positive magnitude"
print "external forces can only PUSH, NOT PULL"
return
self.externalForces[key]['forceMagnitude'] = magnitude
self.updateContactWrench(key)
def updateContactWrench(self, key):
if not self.externalForces.has_key(key):
return
val = self.externalForces[key]
# if it was specified as a wrench, then don't overwrite it
if val['isWrench']:
return
val['wrench'] = self.computeWrench(val['linkName'],
val['forceDirection'],
val['forceMagnitude'],
val['forceLocation'])
def removeForceFromFrameObject(self, tree_, frameObj):
key = frameObj.getProperty('key')
self.removeForce(key, callFromFrameObj=True)
def computeTwoStepEstimate(self):
residual = None
if self.options['debug']['useTrueResidual']:
residual = self.trueResidual
else:
residual = self.estimatedResidual
# this means we haven't gotten any data yet
# so just return an empty dict which means no data
if residual is None:
return None
if self.options['noise']['addNoise']:
residualSize = np.size(residual)
residual = residual + np.random.normal(
scale=self.options['noise']['stddev'], size=residualSize)
if self.options['twoStepEstimator']['provideLinkContactInfo']:
# only do this if we are using fake residual
linksWithContactForce = []
for key, val in self.externalForces.iteritems():
if val['forceMagnitude'] > 0.1:
linksWithContactForce.append(key)
else:
linksWithContactForce = None
return self.twoStepEstimator.computeTwoStepEstimate(
residual, linksWithContactForce)
def printForces(self):
for key in self.externalForces.keys():
print key
# deprecated
def saveForceLocationsToFileOld(self,
filename=None,
verbose=False,
overwrite=False):
spartan_source_dir = os.getenv('SPARTAN_SOURCE_DIR')
if filename is None:
fullFilename = spartan_source_dir + self.options['data'][
'contactCells']
else:
fullFilename = spartan_source_dir + \
"/src/ContactParticleFilter/config/" + filename
print "saving initial particle locations to ", filename
if os.path.isfile(fullFilename) and not overwrite:
print "FILE ALREADY EXISTS, set the overwrite flag to true to overwrite"
return
fileObject = open(fullFilePath, 'w')
for key, val in self.externalForces.iteritems():
line = str(val['linkName']) + ","
for i in range(0, 3):
line += str(val['forceLocation'][i]) + ","
for i in range(0, 3):
line += str(val['forceDirection'][i]) + ","
line += "\n"
if verbose:
print line
fileObject.write(line)
fileObject.close()
def saveForceLocationsToFile(self,
filename=None,
verbose=False,
overwrite=False):
spartan_source_dir = os.getenv('SPARTAN_SOURCE_DIR')
if filename is None:
fullFilename = spartan_source_dir + self.options['data'][
'contactCells']
else:
fullFilename = spartan_source_dir + \
"/src/ContactParticleFilter/config/" + filename
print "saving initial particle locations to ", fullFilename
if os.path.isfile(fullFilename) and not overwrite:
print "FILE ALREADY EXISTS, set the overwrite flag to true to overwrite"
return
ioUtils.saveDataToFile(fullFilename,
self.externalForces,
overwrite=overwrite)
def addForcesFromFile(self, filename=None):
self.startCaptureMode()
spartan_source_dir = os.getenv('SPARTAN_SOURCE_DIR')
if filename is None:
fullFilename = spartan_source_dir + self.options['data'][
'initialParticleLocations']
else:
fullFilename = spartan_source_dir + \
"/src/ContactParticleFilter/config/" + filename
dataDict = ioUtils.readDataFromFile(fullFilename)
for key, val in dataDict.iteritems():
linkName = val['linkName']
forceLocation = val['forceLocation']
forceDirection = val['forceDirection']
self.addForce(linkName,
wrench=None,
forceDirection=forceDirection,
forceMagnitude=0.0,
forceLocation=forceLocation,
inWorldFrame=False)
def createPlunger(self):
forceLocationInWorld = np.array([0, 0, 0])
forceDirectionInWorld = np.array([0, 0, 1])
point = forceLocationInWorld - 0.1 * forceDirectionInWorld
color = [1, 0, 0]
d = DebugData()
# d.addArrow(point, forceLocationInWorld, headRadius=0.025, tubeRadius=0.005, color=color)
d.addSphere(forceLocationInWorld, radius=0.01)
d.addLine(point, forceLocationInWorld, radius=0.005)
self.plungerPolyData = d.getPolyData()
@staticmethod
def buildCellLocator(polyData):
print "building cell locator"
loc = vtk.vtkCellLocator()
loc.SetDataSet(polyData)
loc.BuildLocator()
return loc
@staticmethod
def raycast(locator, rayOrigin, rayEnd):
tolerance = 0.0 # intersection tolerance
pt = [0.0, 0.0, 0.0] # data coordinate where intersection occurs
lineT = vtk.mutable(
0.0
) # parametric distance along line segment where intersection occurs
pcoords = [
0.0, 0.0, 0.0
] # parametric location within cell (triangle) where intersection occurs
subId = vtk.mutable(0) # sub id of cell intersection
result = locator.IntersectWithLine(rayOrigin, rayEnd, tolerance, lineT,
pt, pcoords, subId)
return pt if result else None
def visualizeMesh(self, linkName):
if linkName not in self.linkMeshData:
print "I can't find a mesh corresponding to " + linkName
return
vis.showPolyData(self.linkMeshData[linkName]['polyData'],
linkName + ' mesh')
# these are test methods
def setupTest(self):
w = np.array([1, 2, 3, 4, 5, 6])
self.addForce('pelvis', wrench=w)
self.startPublishing()
def test1(self):
forceDirection = np.array([0, 0, 1])
forceMagnitude = 100
forceLocation = np.array([0, 0, 0])
linkName = 'pelvis'
self.addForce(linkName,
forceDirection=forceDirection,
forceMagnitude=forceMagnitude,
forceLocation=forceLocation)
self.drawForces()
def test2(self):
wrench = np.array([0, 0, 0, 0, 0, 100])
linkName = 'pelvis'
self.addForce(linkName, wrench=wrench)
def constructTestFrames(self):
T = vtk.vtkTransform()
S = vtk.vtkTransform()
S.Translate([1, 2, 0])
FM = transformUtils.forceMomentTransformation(S, T)
print FM
return T, S, FM
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.0
self.randomSeed = 5
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()
self.initial_XVelocity = 0.0
self.initial_YVelocity = 0.0
self.running_sim = True
self.currentIdx = 0;
if autoInitialize:
self.initialize()
def initializeOptions(self):
self.options = dict()
self.options['World'] = dict()
self.options['World']['obstaclesInnerFraction'] = 0.98
self.options['World']['randomSeed'] = 40
self.options['World']['percentObsDensity'] = 15.0
self.options['World']['nonRandomWorld'] = True
self.options['World']['circleRadius'] = 0.6
self.circleRadius = self.options['World']['circleRadius']
self.options['World']['scale'] = 2.0
self.options['Car'] = dict()
self.options['Car']['velocity'] = 20
self.options['dt'] = 0.05
self.options['runTime'] = dict()
self.options['runTime']['defaultControllerTime'] = 100
def setDefaultOptions(self):
defaultOptions = dict()
defaultOptions['World'] = dict()
defaultOptions['World']['obstaclesInnerFraction'] = 0.98
defaultOptions['World']['randomSeed'] = 40
defaultOptions['World']['percentObsDensity'] = 30
defaultOptions['World']['nonRandomWorld'] = True
defaultOptions['World']['circleRadius'] = 1.75
defaultOptions['World']['scale'] = 2.5
defaultOptions['Car'] = dict()
defaultOptions['Car']['velocity'] = 20
defaultOptions['dt'] = 0.05
defaultOptions['runTime'] = dict()
defaultOptions['runTime']['defaultControllerTime'] = 100
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.Controller = ControllerObj()
self.Car = CarPlant(controller=self.Controller,
velocity=self.options['Car']['velocity'])
self.Controller.initializeVelocity(self.Car.v)
self.ActionSet = ActionSetObj()
# 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)
#will need to set locator for purple trajectories
#self.Sensor.setLocator(self.locator)
self.frame = self.robot.getChildFrame()
self.frame.setProperty('Scale', 3)
#self.frame.setProperty('Visible', False)
#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.Car.setFrame(self.frame)
print "Finished initialization"
def rankTrajectories(self):
# access the trajectories
x_traj = self.ActionSet.p_x_trajectories
y_traj = self.ActionSet.p_y_trajectories
final_x_positions = x_traj[:,-1]
final_y_positions = y_traj[:,-1]
#access the global goal
global_goal_x = self.globalGoal.global_goal_x
global_goal_y = self.globalGoal.global_goal_y
ranks_with_indices = []
def getKey(item):
return item[1]
final_distances_squared = np.zeros((len(final_x_positions),len(final_y_positions)))
for i in xrange(len(final_x_positions)):
for j in xrange(len(final_y_positions)):
distance = (final_x_positions[i] - global_goal_x)**2 + (final_y_positions[j] - global_goal_y)**2
ranks_with_indices.append(((i,j), distance))
sorted_ranks_with_indices = sorted(ranks_with_indices,key=getKey)
return sorted_ranks_with_indices
def CheckIfCollisionFreeTrajectoryIndex(self, traj_index):
# accessing the right trajectory
x_trajectory_to_check = self.ActionSet.p_x_trajectories[traj_index[0]]
y_trajectory_to_check = self.ActionSet.p_y_trajectories[traj_index[1]]
#check if anything is too close to the center of the circles
#print self.world.list_of_circles
for i in range(len(x_trajectory_to_check)):
point = ( x_trajectory_to_check[i], y_trajectory_to_check[i] )
if not self.CheckIfCollisionFreePoint(point):
#print "trajectory wasn't free"
return False
return True
def CheckIfCollisionFreePoint(self, point):
for circle_center in self.world.list_of_circles:
#print "circle centers first"
#print circle_center[0], circle_center[1]
#print self.circleRadius
#print point[0], point[1]
#print (circle_center[0] - point[0])**2 + (circle_center[1]-point[1])**2
if ( (circle_center[0] - point[0])**2 + (circle_center[1]-point[1])**2 < self.circleRadius**2):
#print "I found a point in a circle"
return False
if point[0] > self.world.Xmax:
#print "I would have gone past top wall"
return False
if point[0] < self.world.Xmin:
#print "I would have gone below bottom wall"
return False
if point[1] > self.world.Ymax:
#print "I would have gone to the right of the side wall"
return False
if point[1] < self.world.Ymin:
#print "I would have gone to the left of the side wall"
return False
return True
def runSingleSimulation(self, controllerType='default', simulationCutoff=None):
self.Car.setCarState(0.0,0.0,self.initial_XVelocity,self.initial_YVelocity)
self.setRobotFrameState(0.0,0.0,0.0)
currentCarState = np.copy(self.Car.state)
nextCarState = np.copy(self.Car.state)
self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2])
# 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,:] = currentCarState
x = self.stateOverTime[idx,0]
y = self.stateOverTime[idx,1]
self.setRobotFrameState(x,y,0.0)
# self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2])
if controllerType not in self.colorMap.keys():
print
raise ValueError("controller of type " + controllerType + " not supported")
# compute all trajectories from action set
self.ActionSet.computeAllPositions(currentCarState[0], currentCarState[1],currentCarState[2],currentCarState[3])
sorted_ranks_with_indices = self.rankTrajectories()
#method
#inputs: trajectories, global goal
#outputs: sorted list of indexes by desirability global goal
#method
#input: trajectory
#output: is collision free or not
for traj in sorted_ranks_with_indices:
if self.CheckIfCollisionFreeTrajectoryIndex(traj[0]):
traj_to_use_index = traj[0]
break
traj_to_use_index = traj[0]
x_index_to_use = traj_to_use_index[0]
y_index_to_use = traj_to_use_index[1]
x_accel = self.ActionSet.a_x[x_index_to_use]
y_accel = self.ActionSet.a_y[y_index_to_use]
controlInput = [x_accel, y_accel]
self.controlInputData[idx] = controlInput
nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt)
x = nextCarState[0]
y = nextCarState[1]
theta = nextCarState[2]
self.setRobotFrameState(x,y,theta)
#bookkeeping
currentCarState = nextCarState
self.counter+=1
# break if we are in collision
if self.counter >= simulationCutoff:
break
# fill in the last state by hand
self.stateOverTime[self.counter,:] = currentCarState
# 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, 4))
self.controlInputData = np.zeros((maxNumTimesteps+1,2))
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.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.Car.setCarState(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]
theta = 0 #always forward
self.Car.setCarState(x,y,self.initial_XVelocity,self.initial_YVelocity)
self.setRobotFrameState(x,y,theta)
if not self.checkInCollision():
break
self.firstRandomCollisionFreeInitialState_x = x
self.firstRandomCollisionFreeInitialState_y = y
return x,y,0,0
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)
self.max_velocity = 20.0
sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
sliderXVelocity.connect('valueChanged(int)', self.onXVelocityChanged)
sliderXVelocity.setMaximum(self.max_velocity)
sliderXVelocity.setMinimum(-self.max_velocity)
l.addWidget(sliderXVelocity)
sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
sliderYVelocity.connect('valueChanged(int)', self.onYVelocityChanged)
sliderYVelocity.setMaximum(self.max_velocity)
sliderYVelocity.setMinimum(-self.max_velocity)
l.addWidget(sliderYVelocity)
randomGlobalGoalButton = QtGui.QPushButton('Initialize Random Global Goal')
randomGlobalGoalButton.connect('clicked()', self.onRandomGlobalGoalButton)
l.addWidget(randomGlobalGoalButton)
drawActionSetButton = QtGui.QPushButton('Draw Action Set')
drawActionSetButton.connect('clicked()', self.onDrawActionSetButton)
l.addWidget(drawActionSetButton)
runSimButton = QtGui.QPushButton('Simulate')
runSimButton.connect('clicked()', self.onRunSimButton)
l.addWidget(runSimButton)
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)
self.view.orientationMarkerWidget().Off()
l.addWidget(panel)
w.showMaximized()
# need to connect frames with DrawActionSet
self.frame.connectFrameModified(self.updateDrawActionSet)
self.updateDrawActionSet(self.frame)
# 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()
cameracontrolpanel.CameraControlPanel(self.view).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.running_sim = True
self.onRandomGlobalGoalButton()
self.counter = 1
self.runBatchSimulation()
self.running_sim = False
print "my state over time is", self.stateOverTime
if launchApp:
self.setupPlayback()
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):
return False
def updateDrawActionSet(self, frame):
origin = np.array(frame.transform.GetPosition())
if not self.running_sim:
self.ActionSet.computeAllPositions(self.stateOverTime[self.currentIdx,0], self.stateOverTime[self.currentIdx,1], self.stateOverTime[self.currentIdx,2],self.stateOverTime[self.currentIdx,3])
#self.ActionSet.drawActionSetFinal()
self.ActionSet.drawActionSetFull()
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 onXVelocityChanged(self, value):
self.initial_XVelocity = value
print "initial x velocity changed to ", value
def onYVelocityChanged(self, value):
self.initial_YVelocity = -value
print "initial y velocity changed to ", -value
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)
self.currentIdx = idx
x,y, xdot, ydot = self.stateOverTime[idx]
self.setRobotFrameState(x,y,0)
self.sliderMovedByPlayTimer = False
def onRandomGlobalGoalButton(self):
print "random global goal button pressed"
self.globalGoal = World.buildGlobalGoal(scale=self.options['World']['scale'])
def onDrawActionSetButton(self):
print "drawing action set"
#self.ActionSet.computeFinalPositions(self.XVelocity_drawing,self.YVelocity_drawing)
self.ActionSet.computeAllPositions(self.Car.state[0], self.Car.state[1], self.initial_XVelocity, self.initial_YVelocity)
#self.ActionSet.drawActionSetFinal()
self.ActionSet.drawActionSetFull()
def onRunSimButton(self):
self.running_sim = True
self.runBatchSimulation()
print "my state over time is", self.stateOverTime
self.running_sim = False
#self.saveToFile("latest")
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['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.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 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 = 0.4
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()
self.XVelocity_drawing = 0.0
self.YVelocity_drawing = 0.0
def initializeOptions(self):
self.options = dict()
self.options["World"] = dict()
self.options["World"]["obstaclesInnerFraction"] = 0.98
self.options["World"]["randomSeed"] = 40
self.options["World"]["percentObsDensity"] = 0.0
self.options["World"]["nonRandomWorld"] = True
self.options["World"]["circleRadius"] = 1.0
self.options["World"]["scale"] = 1
self.options["Sensor"] = dict()
self.options["Sensor"]["rayLength"] = 10
self.options["Sensor"]["numRays"] = 50
self.options["Car"] = dict()
self.options["Car"]["velocity"] = 4.0
self.options["dt"] = 0.05
self.options["runTime"] = dict()
self.options["runTime"]["defaultControllerTime"] = 100
def setDefaultOptions(self):
defaultOptions = dict()
defaultOptions["World"] = dict()
defaultOptions["World"]["obstaclesInnerFraction"] = 0.98
defaultOptions["World"]["randomSeed"] = 40
defaultOptions["World"]["percentObsDensity"] = 30
defaultOptions["World"]["nonRandomWorld"] = True
defaultOptions["World"]["circleRadius"] = 1.75
defaultOptions["World"]["scale"] = 2.5
defaultOptions["Sensor"] = dict()
defaultOptions["Sensor"]["rayLength"] = 10
defaultOptions["Sensor"]["numRays"] = 51
defaultOptions["Car"] = dict()
defaultOptions["Car"]["velocity"] = 20
defaultOptions["dt"] = 0.05
defaultOptions["runTime"] = dict()
defaultOptions["runTime"]["defaultControllerTime"] = 100
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.SensorApproximator = SensorApproximatorObj(
numRays=self.options["Sensor"]["numRays"], circleRadius=self.options["World"]["circleRadius"]
)
self.Controller = ControllerObj(self.Sensor, self.SensorApproximator)
self.Car = CarPlant(controller=self.Controller, velocity=self.options["Car"]["velocity"])
self.Controller.initializeVelocity(self.Car.v)
self.ActionSet = ActionSetObj()
# create the things needed for simulation
om.removeFromObjectModel(om.findObjectByName("world"))
self.world = World.buildLineSegmentTestWorld(
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.frame = self.robot.getChildFrame()
self.frame.setProperty("Scale", 3)
# self.frame.setProperty('Visible', False)
# 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.Car.setFrame(self.frame)
print "Finished initialization"
def runSingleSimulation(self, controllerType="default", simulationCutoff=None):
# self.setRandomCollisionFreeInitialState()
self.setInitialStateAtZero()
currentCarState = np.copy(self.Car.state)
nextCarState = np.copy(self.Car.state)
self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2])
firstRaycast = self.Sensor.raycastAll(self.frame)
firstRaycastLocations = self.Sensor.raycastAllLocations(self.frame)
# self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
# self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
# self.Sensor.setLocator(self.LineSegmentLocator)
nextRaycast = np.zeros(self.Sensor.numRays)
# record the reward data
runData = dict()
startIdx = self.counter
thisRunIndex = 0
NMaxSteps = 100
while self.counter < self.numTimesteps - 1:
thisRunIndex += 1
idx = self.counter
currentTime = self.t[idx]
self.stateOverTime[idx, :] = currentCarState
x = self.stateOverTime[idx, 0]
y = self.stateOverTime[idx, 1]
self.setRobotFrameState(x, y, 0.0)
# 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"]:
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)
print "NEXTCARSTATE is ", nextCarState
x = nextCarState[0]
y = nextCarState[1]
self.setRobotFrameState(x, y, 0.0)
nextRaycast = self.Sensor.raycastAll(self.frame)
# Compute the next control input
S_next = (nextCarState, nextRaycast)
if controllerType in ["default", "defaultRandom"]:
nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(
nextCarState, currentTime, self.frame, raycastDistance=firstRaycast, randomize=False
)
# bookkeeping
currentCarState = nextCarState
currentRaycast = nextRaycast
self.counter += 1
# break if we are in collision
if self.checkInCollision(nextRaycast):
if self.verbose:
print "Had a collision, terminating simulation"
break
if thisRunIndex > NMaxSteps:
print "was safe during N steps"
break
if self.counter >= simulationCutoff:
break
# fill in the last state by hand
self.stateOverTime[self.counter, :] = currentCarState
self.raycastData[self.counter, :] = currentRaycast
# this just makes sure we don't get stuck in an infinite loop.
if startIdx == self.counter:
self.counter += 1
return runData
def setNumpyRandomSeed(self, seed=1):
np.random.seed(seed)
def runBatchSimulation(self, endTime=None, dt=0.05):
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.Car.setCarState(x, y, 0.0, 0.0)
self.setRobotFrameState(x, y, theta)
print "In loop"
if not self.checkInCollision():
break
return x, y, theta
def setInitialStateAtZero(self):
x = 0.0
y = 0.0
theta = 0.0
self.Car.setCarState(x, y, 0.0, 0.0)
self.setRobotFrameState(x, y, theta)
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.Car.setCarState(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)
self.max_velocity = 20.0
sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
sliderXVelocity.connect("valueChanged(int)", self.onXVelocityChanged)
sliderXVelocity.setMaximum(self.max_velocity)
sliderXVelocity.setMinimum(-self.max_velocity)
l.addWidget(sliderXVelocity)
sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
sliderYVelocity.connect("valueChanged(int)", self.onYVelocityChanged)
sliderYVelocity.setMaximum(self.max_velocity)
sliderYVelocity.setMinimum(-self.max_velocity)
l.addWidget(sliderYVelocity)
firstRaycast = np.ones((self.Sensor.numRays, 1)) * 10.0 + np.random.randn(self.Sensor.numRays, 1) * 1.0
self.drawFirstIntersections(self.frame, firstRaycast)
randomGlobalGoalButton = QtGui.QPushButton("Initialize Random Global Goal")
randomGlobalGoalButton.connect("clicked()", self.onRandomGlobalGoalButton)
l.addWidget(randomGlobalGoalButton)
randomObstaclesButton = QtGui.QPushButton("Initialize Random Obstacles")
randomObstaclesButton.connect("clicked()", self.onRandomObstaclesButton)
l.addWidget(randomObstaclesButton)
buildWorldFromRandomObstaclesButton = QtGui.QPushButton("Generate Polygon World")
buildWorldFromRandomObstaclesButton.connect("clicked()", self.onBuildWorldFromRandomObstacles)
l.addWidget(buildWorldFromRandomObstaclesButton)
findLocalGoalButton = QtGui.QPushButton("Find Local Goal (Heuristic)")
findLocalGoalButton.connect("clicked()", self.onFindLocalGoalButton)
l.addWidget(findLocalGoalButton)
drawActionSetButton = QtGui.QPushButton("Draw Action Set")
drawActionSetButton.connect("clicked()", self.onDrawActionSetButton)
l.addWidget(drawActionSetButton)
runSimButton = QtGui.QPushButton("Simulate")
runSimButton.connect("clicked()", self.onRunSimButton)
l.addWidget(runSimButton)
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
# slider4 = QtGui.QSlider(QtCore.Qt.Horizontal)
# slider4.setMaximum(self.sliderMax)
# l.addWidget(slider4)
# slider5 = QtGui.QSlider(QtCore.Qt.Horizontal)
# slider5.setMaximum(self.sliderMax)
# l.addWidget(slider5)
# slider5 = QtGui.QSlider(QtCore.Qt.Horizontal)
# slider5.setMaximum(self.sliderMax)
# l.addWidget(slider5)
# slider6 = QtGui.QSlider(QtCore.Qt.Horizontal)
# slider6.setMaximum(self.sliderMax)
# l.addWidget(slider6)
# slider7 = QtGui.QSlider(QtCore.Qt.Horizontal)
# slider7.setMaximum(self.sliderMax)
# l.addWidget(slider7)
l.addWidget(playButton)
l.addWidget(slider)
w = QtGui.QWidget()
l = QtGui.QVBoxLayout(w)
l.addWidget(self.view)
self.view.orientationMarkerWidget().Off()
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()
cameracontrolpanel.CameraControlPanel(self.view).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.onRandomObstaclesButton()
self.app.start()
def drawFirstIntersections(self, frame, firstRaycast):
origin = np.array(frame.transform.GetPosition())
d = DebugData()
firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, firstRaycast)
for i in xrange(self.Sensor.numRays):
endpoint = firstRaycastLocations[i, :]
if firstRaycast[i] >= self.Sensor.rayLength - 0.1:
d.addLine(origin, endpoint, color=[0, 1, 0])
else:
d.addLine(origin, endpoint, color=[1, 0, 0])
vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255")
self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
self.locator = self.LineSegmentLocator
self.Sensor.setLocator(self.LineSegmentLocator)
def updateDrawIntersection(self, frame, locator="None"):
if locator == "None":
locator = self.locator
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 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(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
# if raycastDistance[(len(raycastDistance)+1)/2] < 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, xdot, ydot = self.stateOverTime[idx]
self.setRobotFrameState(x, y, 0)
self.sliderMovedByPlayTimer = False
def onXVelocityChanged(self, value):
self.XVelocity_drawing = value
self.onDrawActionSetButton()
print "x velocity changed to ", value
def onYVelocityChanged(self, value):
print value
self.YVelocity_drawing = -value
self.onDrawActionSetButton()
print "y velocity changed to ", -value
def onShowSensorsButton(self):
print "I pressed the show sensors button"
self.setInitialStateAtZero()
firstRaycast = np.ones((self.Sensor.numRays, 1)) * 10.0 + np.random.randn(self.Sensor.numRays, 1) * 1.0
self.drawFirstIntersections(self.frame, firstRaycast)
def onRandomObstaclesButton(self):
print "random obstacles button pressed"
self.setInitialStateAtZero()
self.world = World.buildLineSegmentTestWorld(
percentObsDensity=8.0,
circleRadius=self.options["World"]["circleRadius"],
nonRandom=False,
scale=self.options["World"]["scale"],
randomSeed=self.options["World"]["randomSeed"],
obstaclesInnerFraction=self.options["World"]["obstaclesInnerFraction"],
)
self.locator = World.buildCellLocator(self.world.visObj.polyData)
self.Sensor.setLocator(self.locator)
self.updateDrawIntersection(self.frame, locator=self.locator)
def onRandomGlobalGoalButton(self):
print "random global goal button pressed"
self.globalGoal = World.buildGlobalGoal()
def onBuildWorldFromRandomObstacles(self):
distances = self.Sensor.raycastAll(self.frame)
firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, distances)
self.polygon_initial_distances = distances
self.polygon_initial_raycastLocations = firstRaycastLocations
self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
self.locator = self.LineSegmentLocator
self.Sensor.setLocator(self.LineSegmentLocator)
self.updateDrawIntersection(self.frame)
def onFindLocalGoalButton(self):
print "find local goal button pressed"
local_goal = self.findLocalGoal()
print local_goal, "is my local goal"
self.localGoal = World.placeLocalGoal(local_goal)
def findLocalGoal(self):
biggest_gap_width = 0
biggest_gap_start_index = 0
current_gap_width = 0
current_gap_start_index = 0
for index, value in enumerate(self.polygon_initial_distances):
# if still in a gap
if value == self.Sensor.rayLength:
current_gap_width += 1
# else terminate counting for this gap
else:
if current_gap_width > biggest_gap_width:
biggest_gap_width = current_gap_width
biggest_gap_start_index = current_gap_start_index
current_gap_width = 0
current_gap_start_index = index + 1
if current_gap_width > biggest_gap_width:
biggest_gap_width = current_gap_width
biggest_gap_start_index = current_gap_start_index
middle_index_of_gap = biggest_gap_start_index + biggest_gap_width / 2
return self.polygon_initial_raycastLocations[middle_index_of_gap, :]
def onDrawActionSetButton(self):
print "drawing action set"
# self.ActionSet.computeFinalPositions(self.XVelocity_drawing,self.YVelocity_drawing)
self.ActionSet.computeAllPositions(self.XVelocity_drawing, self.YVelocity_drawing)
# self.ActionSet.drawActionSetFinal()
self.ActionSet.drawActionSetFull()
def onRunSimButton(self):
self.runBatchSimulation()
self.saveToFile("latest")
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()
def run(self, launchApp=True):
self.counter = 1
# 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, self.dt)
maxNumTimesteps = np.size(self.t)
self.stateOverTime = np.zeros((maxNumTimesteps + 1, 4))
self.raycastData = np.zeros((maxNumTimesteps + 1, self.Sensor.numRays))
self.controlInputData = np.zeros((maxNumTimesteps + 1, 2))
self.numTimesteps = maxNumTimesteps
# self.runBatchSimulation()
if launchApp:
self.setupPlayback()
@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 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.0
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()
self.initial_XVelocity = 0.0
self.initial_YVelocity = 0.0
self.running_sim = True
self.currentIdx = 0;
if autoInitialize:
self.initialize()
def initializeOptions(self):
self.options = dict()
self.options['World'] = dict()
self.options['World']['obstaclesInnerFraction'] = 0.98
self.options['World']['randomSeed'] = 40
self.options['World']['percentObsDensity'] = 25.0
self.options['World']['nonRandomWorld'] = True
self.options['World']['circleRadius'] = 0.6
self.circleRadius = self.options['World']['circleRadius']
self.options['World']['scale'] = 2.0
self.options['Sensor'] = dict()
self.options['Sensor']['rayLength'] = 10
self.options['Sensor']['numRays'] = 40
self.options['Car'] = dict()
self.options['Car']['velocity'] = 20
self.options['dt'] = 0.05
self.options['runTime'] = dict()
self.options['runTime']['defaultControllerTime'] = 100
def initializeColorMap(self):
self.colorMap = dict()
self.colorMap['default'] = [0,1,0]
def initialize(self):
self.dt = self.options['dt']
self.controllerTypeOrder = ['default']
self.Controller = ControllerObj()
self.Sensor = SensorObj(rayLength=self.options['Sensor']['rayLength'],
numRays=self.options['Sensor']['numRays'])
self.Car = CarPlant(controller=self.Controller,
velocity=self.options['Car']['velocity'])
self.Controller.initializeVelocity(self.Car.v)
self.ActionSet = ActionSetObj()
# 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)
#will need to set locator for purple trajectories
#self.Sensor.setLocator(self.locator)
self.frame = self.robot.getChildFrame()
self.frame.setProperty('Scale', 3)
#self.frame.setProperty('Visible', False)
#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.Car.setFrame(self.frame)
print "Finished initialization"
def terminalEuclideanCostForTrajectories(self):
# access the trajectories
x_traj = self.ActionSet.p_x_trajectories
y_traj = self.ActionSet.p_y_trajectories
final_x_positions = x_traj[:,-1]
final_y_positions = y_traj[:,-1]
#access the global goal
global_goal_x = self.globalGoal.global_goal_x
global_goal_y = self.globalGoal.global_goal_y
euclideans_vector = []
# def getKey(item):
# return item[1]
for i in xrange(self.ActionSet.num_x_bins):
for j in xrange(self.ActionSet.num_y_bins):
distance = np.sqrt((final_x_positions[i] - global_goal_x)**2 + (final_y_positions[j] - global_goal_y)**2)
euclideans_vector.append(distance)
#sorted_ranks_with_indices = sorted(ranks_with_indices,key=getKey)
return np.array(euclideans_vector)
def CheckIfCollisionFreeTrajectoryIndex(self, traj_index):
# accessing the right trajectory
x_trajectory_to_check = self.ActionSet.p_x_trajectories[traj_index[0]]
y_trajectory_to_check = self.ActionSet.p_y_trajectories[traj_index[1]]
#check if anything is too close to the center of the circles
#print self.world.list_of_circles
for i in range(len(x_trajectory_to_check)):
point = ( x_trajectory_to_check[i], y_trajectory_to_check[i] )
if not self.CheckIfCollisionFreePoint(point):
#print "trajectory wasn't free"
return False
return True
def CheckIfCollisionFreePoint(self, point):
for circle_center in self.world.list_of_circles:
#print "circle centers first"
#print circle_center[0], circle_center[1]
#print self.circleRadius
#print point[0], point[1]
#print (circle_center[0] - point[0])**2 + (circle_center[1]-point[1])**2
if ( (circle_center[0] - point[0])**2 + (circle_center[1]-point[1])**2 < self.circleRadius**2):
#print "I found a point in a circle"
return False
if point[0] > self.world.Xmax:
#print "I would have gone past top wall"
return False
if point[0] < self.world.Xmin:
#print "I would have gone below bottom wall"
return False
if point[1] > self.world.Ymax:
#print "I would have gone to the right of the side wall"
return False
if point[1] < self.world.Ymin:
#print "I would have gone to the left of the side wall"
return False
return True
def computeProbabilitiesOfCollisionAllTrajectories(self, currentRaycastIntersectionLocations, speed_allowed_matrix):
probability_vector = []
indices_vector = []
for x_index in xrange(self.ActionSet.num_x_bins):
for y_index in xrange(self.ActionSet.num_y_bins):
if not speed_allowed_matrix[x_index, y_index]:
probability_vector.append(1.0)
else:
probability_of_collision = self.computeProbabilityOfCollisionOneTrajectory(x_index, y_index, currentRaycastIntersectionLocations)
probability_vector.append(probability_of_collision)
indices_vector.append([x_index, y_index])
return np.array(probability_vector), indices_vector
def computeProbabilityOfCollisionOneTrajectory(self, x_index, y_index, currentRaycastIntersectionLocations):
probability_no_collision = 1
for time_step_index, time_step_value in enumerate(self.ActionSet.t_vector):
for obstacle_center in currentRaycastIntersectionLocations:
probability_of_collision_step_k_obstacle_j = self.computeProbabilityOfCollisionOneStepOneObstacle(x_index, y_index, time_step_index, time_step_value, obstacle_center)
probability_no_collision_step_k_obstacle_j = 1 - probability_of_collision_step_k_obstacle_j
probability_no_collision = probability_no_collision * probability_no_collision_step_k_obstacle_j
return float(1.0 - probability_no_collision)
def computeProbabilityOfCollisionOneStepOneObstacle(self, x_index, y_index, time_step_index, time_step_value, obstacle_center):
volume = 4.18
Sigma_sensor = np.zeros((3,3))
np.fill_diagonal(Sigma_sensor, self.sigma_sensor)
variance_x = (2.5 + abs(self.current_initial_velocity_x*0.1))*time_step_value+0.01
variance_y = (2.5 + abs(self.current_initial_velocity_y*0.1))*time_step_value+0.01
variance_z = (1.5)*time_step_value+0.01
Sigma_robot = np.zeros((3,3))
np.fill_diagonal(Sigma_sensor, [variance_x, variance_y, variance_z])
Sigma_C = Sigma_robot + Sigma_sensor
denominator = np.sqrt(np.linalg.det(2*np.pi*Sigma_C))
x = self.ActionSet.p_x_trajectories[x_index, time_step_index]
y = self.ActionSet.p_y_trajectories[y_index, time_step_index]
z = 0
x_robot = np.array([x, y, z])
obstacle_center = np.array(obstacle_center)
exponent = - 0.5 * np.matrix((x_robot - obstacle_center)) * np.matrix(np.linalg.inv(Sigma_C)) * np.matrix((x_robot - obstacle_center)).T
return volume / denominator * np.exp(exponent)
def computeJerkVector(self,controlInput):
jerk_vector = []
for x_index in xrange(self.ActionSet.num_x_bins):
for y_index in xrange(self.ActionSet.num_y_bins):
last_x_accel = controlInput[0]
last_y_accel = controlInput[1]
this_x_accel = self.ActionSet.a_x[x_index]
this_y_accel = self.ActionSet.a_y[y_index]
jerk = np.sqrt( (last_x_accel - this_x_accel)**2 + (last_y_accel - this_y_accel)**2 )
jerk_vector.append(jerk)
return np.array(jerk_vector)
def identifySpeedAllowedTrajectories(self):
speed_allowed_matrix = np.ones((self.ActionSet.num_x_bins, self.ActionSet.num_y_bins))
# for x_index in xrange(self.ActionSet.num_x_bins):
# for y_index in xrange(self.ActionSet.num_y_bins):
# this_x_accel = self.ActionSet.a_x[x_index]
# this_y_accel = self.ActionSet.a_y[y_index]
# if np.sqrt( (self.current_initial_velocity_x - this_x_accel)**2 + (self.current_initial_velocity_y - this_y_accel)**2) < self.speed_max:
# speed_allowed_matrix[x_index,y_index] = 1
return speed_allowed_matrix
def runSingleSimulation(self, controllerType='default', simulationCutoff=None):
self.Car.setCarState(0.0,0.0,self.initial_XVelocity,self.initial_YVelocity)
self.setRobotFrameState(0.0,0.0,0.0)
currentCarState = np.copy(self.Car.state)
nextCarState = np.copy(self.Car.state)
self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2])
self.Sensor.setLocator(self.locator)
firstRaycast = self.Sensor.raycastAll(self.frame)
firstRaycastLocations = self.Sensor.raycastAllLocations(self.frame)
nextRaycast = np.zeros(self.Sensor.numRays)
# record the reward data
runData = dict()
startIdx = self.counter
controlInput = [0,0]
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]
self.setRobotFrameState(x,y,0.0)
# self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2])
currentRaycastIntersectionLocations = self.Sensor.raycastLocationsOnlyOfIntersections(self.frame)
if self.checkInCollision(currentCarState[0], currentCarState[1], currentRaycastIntersectionLocations):
break
if controllerType not in self.colorMap.keys():
print
raise ValueError("controller of type " + controllerType + " not supported")
# compute all trajectories from action set
self.ActionSet.computeAllPositions(currentCarState[0], currentCarState[1], currentCarState[2],currentCarState[3])
self.current_initial_velocity_x = currentCarState[2]
self.current_initial_velocity_y = currentCarState[3]
speed_allowed_matrix = self.identifySpeedAllowedTrajectories()
probability_vector, indices_list = self.computeProbabilitiesOfCollisionAllTrajectories(currentRaycastIntersectionLocations, speed_allowed_matrix)
euclideans_vector = self.terminalEuclideanCostForTrajectories()
jerk_vector = self.computeJerkVector(controlInput)
# k_collision_cost = 10
# k_terminal_cost = 1
# k_jerk = 0.1
# sum_vector = probability_vector*k_collision_cost + euclideans_vector/np.max(euclideans_vector)*k_terminal_cost + k_jerk*jerk_vector/np.max(jerk_vector)
# min_action_index_in_vector = np.argmin(sum_vector)
# x_index_to_use = indices_list[min_action_index_in_vector][0]
# y_index_to_use = indices_list[min_action_index_in_vector][1]
# convert to probability no collision
probability_vector = np.ones(len(probability_vector)) - probability_vector
# convert distances to amount of progress
current_distance = np.sqrt((currentCarState[0] - self.globalGoal.global_goal_x)**2 + (currentCarState[1] - self.globalGoal.global_goal_y)**2)
#print "euclideans vector", euclideans_vector
euclidean_progress_vector = np.ones(len(euclideans_vector))*current_distance - euclideans_vector
#print "euclidean progress vector", euclidean_progress_vector
reward_vector = euclidean_progress_vector - 0.1*jerk_vector/np.max(jerk_vector)
#print "reward_vector", reward_vector
expected_reward = np.multiply(probability_vector, reward_vector)
max_action_index_in_vector = np.argmax(expected_reward)
x_index_to_use = indices_list[max_action_index_in_vector][0]
y_index_to_use = indices_list[max_action_index_in_vector][1]
self.actionIndicesOverTime[idx,:] = [x_index_to_use, y_index_to_use]
x_accel = self.ActionSet.a_x[x_index_to_use]
y_accel = self.ActionSet.a_y[y_index_to_use]
controlInput = [x_accel, y_accel]
self.controlInputData[idx] = controlInput
nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt)
x = nextCarState[0]
y = nextCarState[1]
theta = nextCarState[2]
self.setRobotFrameState(x,y,theta)
#bookkeeping
currentCarState = nextCarState
self.counter+=1
# break if we are in collision
if self.counter >= simulationCutoff:
break
# fill in the last state by hand
self.stateOverTime[self.counter,:] = currentCarState
self.actionIndicesOverTime[self.counter,:] = [x_index_to_use, y_index_to_use]
# 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, 4))
self.actionIndicesOverTime = np.zeros((maxNumTimesteps+1, 2))
self.controlInputData = np.zeros((maxNumTimesteps+1,2))
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.actionIndicesOverTime = self.actionIndicesOverTime[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.Car.setCarState(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]
theta = 0 #always forward
self.Car.setCarState(x,y,self.initial_XVelocity,self.initial_YVelocity)
self.setRobotFrameState(x,y,theta)
if not self.checkInCollision():
break
self.firstRandomCollisionFreeInitialState_x = x
self.firstRandomCollisionFreeInitialState_y = y
return x,y,0,0
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)
self.max_velocity = 20.0
sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
sliderXVelocity.connect('valueChanged(int)', self.onXVelocityChanged)
sliderXVelocity.setMaximum(self.max_velocity)
sliderXVelocity.setMinimum(-self.max_velocity)
l.addWidget(sliderXVelocity)
sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
sliderYVelocity.connect('valueChanged(int)', self.onYVelocityChanged)
sliderYVelocity.setMaximum(self.max_velocity)
sliderYVelocity.setMinimum(-self.max_velocity)
l.addWidget(sliderYVelocity)
randomGlobalGoalButton = QtGui.QPushButton('Initialize Random Global Goal')
randomGlobalGoalButton.connect('clicked()', self.onRandomGlobalGoalButton)
l.addWidget(randomGlobalGoalButton)
drawActionSetButton = QtGui.QPushButton('Draw Action Set')
drawActionSetButton.connect('clicked()', self.onDrawActionSetButton)
l.addWidget(drawActionSetButton)
runSimButton = QtGui.QPushButton('Simulate')
runSimButton.connect('clicked()', self.onRunSimButton)
l.addWidget(runSimButton)
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)
toggleSensorNoiseButton = QtGui.QPushButton('Toggle Sensor Noise')
toggleSensorNoiseButton.connect('clicked()', self.onToggleSensorNoise)
l.addWidget(toggleSensorNoiseButton)
showObstaclesButton = QtGui.QPushButton('Show Obsatacles')
showObstaclesButton.connect('clicked()', self.onShowObstacles)
l.addWidget(showObstaclesButton)
w = QtGui.QWidget()
l = QtGui.QVBoxLayout(w)
l.addWidget(self.view)
self.view.orientationMarkerWidget().Off()
l.addWidget(panel)
w.showMaximized()
# need to connect frames with DrawActionSet
self.frame.connectFrameModified(self.updateDrawActionSet)
self.updateDrawActionSet(self.frame)
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()
cameracontrolpanel.CameraControlPanel(self.view).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.sphere_toggle = False
self.sigma_sensor = 0.5
self.speed_max = 10.0
self.running_sim = True
self.onRandomGlobalGoalButton()
self.counter = 1
self.runBatchSimulation()
self.running_sim = False
if launchApp:
self.setupPlayback()
def onToggleSensorNoise(self):
self.sphere_toggle = not self.sphere_toggle
def onShowObstacles(self):
print "time to show obstacles"
A = 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'], alpha=1.0)
def drawFirstIntersections(self, frame, firstRaycast):
origin = np.array(frame.transform.GetPosition())
d = DebugData()
firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, firstRaycast)
for i in xrange(self.Sensor.numRays):
endpoint = firstRaycastLocations[i,:]
if firstRaycast[i] >= self.Sensor.rayLength - 0.1:
d.addLine(origin, endpoint, color=[0,1,0])
else:
d.addLine(origin, endpoint, color=[1,0,0])
vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
self.locator = self.LineSegmentLocator
self.Sensor.setLocator(self.LineSegmentLocator)
def updateDrawIntersection(self, frame, locator="None"):
if locator=="None":
locator = self.locator
if self.sphere_toggle:
sphere_radius=self.sigma_sensor
else:
sphere_radius=0.0
origin = np.array(frame.transform.GetPosition())
#print "origin is now at", origin
d = DebugData()
d_sphere=DebugData()
sliderIdx = self.slider.value
controllerType = self.getControllerTypeFromCounter(sliderIdx)
colorMaxRange = self.colorMap[controllerType]
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(locator, origin, origin + rayTransformed*self.Sensor.rayLength)
if intersection is not None:
d.addLine(origin, intersection, color=[1,0,0])
d_sphere.addSphere(intersection, radius=sphere_radius, color=[1,0,0])
else:
d.addLine(origin, origin+rayTransformed*self.Sensor.rayLength, color=colorMaxRange)
d_sphere.addSphere(origin, radius=0.0, color=[0,1,0])
vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
vis.updatePolyData(d_sphere.getPolyData(), 'spheres', colorByName='RGB255', alpha=0.3)
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, x, y, raycastLocations):
return False
# carState = [x,y,0]
# for raycastLocation in raycastLocations:
# if np.linalg.norm(carState - raycastLocation) < 0.3:
# return True
# return False
def updateDrawActionSet(self, frame):
origin = np.array(frame.transform.GetPosition())
if not self.running_sim:
self.ActionSet.computeAllPositions(self.stateOverTime[self.currentIdx,0], self.stateOverTime[self.currentIdx,1], self.stateOverTime[self.currentIdx,2],self.stateOverTime[self.currentIdx,3])
#self.ActionSet.drawActionSetFinal()
self.ActionSet.drawActionSetFull()
self.drawChosenFunnel()
def drawChosenFunnel(self):
velocity_initial_x = self.stateOverTime[self.currentIdx,2]
velocity_initial_y = self.stateOverTime[self.currentIdx,3]
variance_x = 2.5 + abs(velocity_initial_x*0.1)
variance_y = 2.5 + abs(velocity_initial_y*0.1)
variance_z = 1.5
x_index = self.actionIndicesOverTime[self.currentIdx,0]
y_index = self.actionIndicesOverTime[self.currentIdx,1]
for i in xrange(0,10):
time = 0.5/10.0*i
x_center = self.ActionSet.p_x_trajectories[x_index, i]
y_center = self.ActionSet.p_y_trajectories[y_index, i]
z_center = 0.0
World.buildEllipse(i, [x_center,y_center,z_center], variance_x*time, variance_y*time, variance_z*time, alpha=0.3)
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 onXVelocityChanged(self, value):
self.initial_XVelocity = value
print "initial x velocity changed to ", value
def onYVelocityChanged(self, value):
self.initial_YVelocity = -value
print "initial y velocity changed to ", -value
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)
self.currentIdx = idx
x,y, xdot, ydot = self.stateOverTime[idx]
self.setRobotFrameState(x,y,0)
self.sliderMovedByPlayTimer = False
def onRandomGlobalGoalButton(self):
print "random global goal button pressed"
self.globalGoal = World.buildGlobalGoal(scale=self.options['World']['scale'])
def onDrawActionSetButton(self):
print "drawing action set"
#self.ActionSet.computeFinalPositions(self.XVelocity_drawing,self.YVelocity_drawing)
self.ActionSet.computeAllPositions(self.Car.state[0], self.Car.state[1], self.initial_XVelocity, self.initial_YVelocity)
#self.ActionSet.drawActionSetFinal()
self.ActionSet.drawActionSetFull()
def onRunSimButton(self):
self.running_sim = True
self.runBatchSimulation()
self.running_sim = False
#self.saveToFile("latest")
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['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.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 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 = 0.4
self.randomSeed = 5
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()
self.XVelocity_drawing = 0.0
self.YVelocity_drawing = 0.0
def initializeOptions(self):
self.options = dict()
self.options['World'] = dict()
self.options['World']['obstaclesInnerFraction'] = 0.98
self.options['World']['randomSeed'] = 40
self.options['World']['percentObsDensity'] = 0.0
self.options['World']['nonRandomWorld'] = True
self.options['World']['circleRadius'] = 1.0
self.options['World']['scale'] = 1
self.options['Car'] = dict()
self.options['Car']['velocity'] = 4.0
self.options['dt'] = 0.05
self.options['runTime'] = dict()
self.options['runTime']['defaultControllerTime'] = 100
def setDefaultOptions(self):
defaultOptions = dict()
defaultOptions['World'] = dict()
defaultOptions['World']['obstaclesInnerFraction'] = 0.98
defaultOptions['World']['randomSeed'] = 40
defaultOptions['World']['percentObsDensity'] = 30
defaultOptions['World']['nonRandomWorld'] = True
defaultOptions['World']['circleRadius'] = 1.75
defaultOptions['World']['scale'] = 2.5
defaultOptions['Car'] = dict()
defaultOptions['Car']['velocity'] = 20
defaultOptions['dt'] = 0.05
defaultOptions['runTime'] = dict()
defaultOptions['runTime']['defaultControllerTime'] = 100
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.Controller = ControllerObj()
self.Car = CarPlant(controller=self.Controller,
velocity=self.options['Car']['velocity'])
self.Controller.initializeVelocity(self.Car.v)
self.ActionSet = ActionSetObj()
# create the things needed for simulation
om.removeFromObjectModel(om.findObjectByName('world'))
self.world = World.buildLineSegmentTestWorld(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.frame = self.robot.getChildFrame()
self.frame.setProperty('Scale', 3)
#self.frame.setProperty('Visible', False)
#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.Car.setFrame(self.frame)
print "Finished initialization"
def runSingleSimulation(self, controllerType='default', simulationCutoff=None):
#self.setRandomCollisionFreeInitialState()
self.setInitialStateAtZero()
currentCarState = np.copy(self.Car.state)
nextCarState = np.copy(self.Car.state)
self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2])
# DELETED FIRST RAYCAST AND RAYCAST LOCATIONS
# record the reward data
runData = dict()
startIdx = self.counter
thisRunIndex = 0
NMaxSteps = 100
while (self.counter < self.numTimesteps - 1):
thisRunIndex += 1
idx = self.counter
currentTime = self.t[idx]
self.stateOverTime[idx,:] = currentCarState
x = self.stateOverTime[idx,0]
y = self.stateOverTime[idx,1]
self.setRobotFrameState(x,y,0.0)
# self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2])
if controllerType in ["default"]:
controlInput, controlInputIdx = self.Controller.computeControlInput(currentCarState,
currentTime, self.frame,
randomize=False)
self.controlInputData[idx] = controlInput
nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt)
print "NEXTCARSTATE is ", nextCarState
x = nextCarState[0]
y = nextCarState[1]
self.setRobotFrameState(x,y,0.0)
if controllerType in ["default", "defaultRandom"]:
nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(nextCarState,
currentTime, self.frame,
randomize=False)
#bookkeeping
self.counter+=1
# break if we are in collision
if self.counter >= simulationCutoff:
break
# fill in the last state by hand
self.stateOverTime[self.counter,:] = currentCarState
# 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):
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.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.Car.setCarState(x,y,0.0,0.0)
self.setRobotFrameState(x,y,theta)
print "In loop"
if not self.checkInCollision():
break
return x,y,theta
def setInitialStateAtZero(self):
x = 0.0
y = 0.0
theta = 0.0
self.Car.setCarState(x,y,0.0,0.0)
self.setRobotFrameState(x,y,theta)
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.Car.setCarState(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)
self.max_velocity = 20.0
sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
sliderXVelocity.connect('valueChanged(int)', self.onXVelocityChanged)
sliderXVelocity.setMaximum(self.max_velocity)
sliderXVelocity.setMinimum(-self.max_velocity)
l.addWidget(sliderXVelocity)
sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal)
sliderYVelocity.connect('valueChanged(int)', self.onYVelocityChanged)
sliderYVelocity.setMaximum(self.max_velocity)
sliderYVelocity.setMinimum(-self.max_velocity)
l.addWidget(sliderYVelocity)
randomGlobalGoalButton = QtGui.QPushButton('Initialize Random Global Goal')
randomGlobalGoalButton.connect('clicked()', self.onRandomGlobalGoalButton)
l.addWidget(randomGlobalGoalButton)
randomObstaclesButton = QtGui.QPushButton('Initialize Random Obstacles')
randomObstaclesButton.connect('clicked()', self.onRandomObstaclesButton)
l.addWidget(randomObstaclesButton)
drawActionSetButton = QtGui.QPushButton('Draw Action Set')
drawActionSetButton.connect('clicked()', self.onDrawActionSetButton)
l.addWidget(drawActionSetButton)
runSimButton = QtGui.QPushButton('Simulate')
runSimButton.connect('clicked()', self.onRunSimButton)
l.addWidget(runSimButton)
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
# slider4 = QtGui.QSlider(QtCore.Qt.Horizontal)
# slider4.setMaximum(self.sliderMax)
# l.addWidget(slider4)
# slider5 = QtGui.QSlider(QtCore.Qt.Horizontal)
# slider5.setMaximum(self.sliderMax)
# l.addWidget(slider5)
# slider5 = QtGui.QSlider(QtCore.Qt.Horizontal)
# slider5.setMaximum(self.sliderMax)
# l.addWidget(slider5)
# slider6 = QtGui.QSlider(QtCore.Qt.Horizontal)
# slider6.setMaximum(self.sliderMax)
# l.addWidget(slider6)
# slider7 = QtGui.QSlider(QtCore.Qt.Horizontal)
# slider7.setMaximum(self.sliderMax)
# l.addWidget(slider7)
l.addWidget(playButton)
l.addWidget(slider)
w = QtGui.QWidget()
l = QtGui.QVBoxLayout(w)
l.addWidget(self.view)
self.view.orientationMarkerWidget().Off()
l.addWidget(panel)
w.showMaximized()
# We need to connectFrameModified with updateDrawActionSet
# 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()
cameracontrolpanel.CameraControlPanel(self.view).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.onRandomObstaclesButton()
self.app.start()
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):
# We need to re-evaluate this
return False
# if raycastDistance[(len(raycastDistance)+1)/2] < 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, xdot, ydot = self.stateOverTime[idx]
self.setRobotFrameState(x,y,0)
self.sliderMovedByPlayTimer = False
def onXVelocityChanged(self, value):
self.XVelocity_drawing = value
self.onDrawActionSetButton()
print "x velocity changed to ", value
def onYVelocityChanged(self, value):
print value
self.YVelocity_drawing = -value
self.onDrawActionSetButton()
print "y velocity changed to ", -value
def onRandomObstaclesButton(self):
print "random obstacles button pressed"
self.setInitialStateAtZero()
self.world = World.buildLineSegmentTestWorld(percentObsDensity=8.0,
circleRadius=self.options['World']['circleRadius'],
nonRandom=False,
scale=self.options['World']['scale'],
randomSeed=self.options['World']['randomSeed'],
obstaclesInnerFraction=self.options['World']['obstaclesInnerFraction'])
self.locator = World.buildCellLocator(self.world.visObj.polyData)
def onRandomGlobalGoalButton(self):
print "random global goal button pressed"
self.globalGoal = World.buildGlobalGoal()
def onDrawActionSetButton(self):
print "drawing action set"
#self.ActionSet.computeFinalPositions(self.XVelocity_drawing,self.YVelocity_drawing)
self.ActionSet.computeAllPositions(self.XVelocity_drawing,self.YVelocity_drawing)
#self.ActionSet.drawActionSetFinal()
self.ActionSet.drawActionSetFull()
def onRunSimButton(self):
self.runBatchSimulation()
self.saveToFile("latest")
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['controlInputData'] = self.controlInputData
my_shelf['numTimesteps'] = self.numTimesteps
my_shelf['idxDict'] = self.idxDict
my_shelf['counter'] = self.counter
my_shelf.close()
def run(self, launchApp=True):
self.counter = 1
# 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, self.dt)
maxNumTimesteps = np.size(self.t)
self.stateOverTime = np.zeros((maxNumTimesteps+1, 4))
self.controlInputData = np.zeros((maxNumTimesteps+1,2))
self.numTimesteps = maxNumTimesteps
self.runBatchSimulation()
if launchApp:
self.setupPlayback()
@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 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 CameraTrackerManager(object):
def __init__(self):
self.target = None
self.targetFrame = None
self.trackerClass = None
self.camera = None
self.view = None
self.timer = TimerCallback()
self.timer.callback = self.updateTimer
self.addTrackers()
self.initTracker()
def updateTimer(self):
tNow = time.time()
dt = tNow - self.tLast
if dt < self.timer.elapsed/2.0:
return
self.update()
def setView(self, view):
self.view = view
self.camera = view.camera()
def setTarget(self, obj):
if obj == self.target:
return
self.disableActiveTracker()
if not obj:
return
self.target = obj
self.targetFrame = obj.getChildFrame()
self.callbackId = self.targetFrame.connectFrameModified(self.onTargetFrameModified)
self.initTracker()
def disableActiveTracker(self):
if self.targetFrame:
self.targetFrame.disconnectFrameModified(self.callbackId)
self.target = None
self.targetFrame = None
self.trackerClass = None
self.initTracker()
def update(self):
tNow = time.time()
dt = tNow - self.tLast
self.tLast = tNow
if self.activeTracker:
self.activeTracker.dt = dt
self.activeTracker.update()
def reset(self):
self.tLast = time.time()
if self.activeTracker:
self.activeTracker.reset()
def getModeActions(self):
if self.activeTracker:
return self.activeTracker.actions
return []
def onModeAction(self, actionName):
if self.activeTracker:
self.activeTracker.onAction(actionName)
def getModeProperties(self):
if self.activeTracker:
return self.activeTracker.properties
return None
def onTargetFrameModified(self, frame):
self.update()
def initTracker(self):
self.timer.stop()
self.activeTracker = self.trackerClass(self.view, self.targetFrame) if self.trackerClass else None
self.reset()
self.update()
if self.activeTracker:
minimumUpdateRate = self.activeTracker.getMinimumUpdateRate()
if minimumUpdateRate > 0:
self.timer.targetFps = minimumUpdateRate
self.timer.start()
def addTrackers(self):
self.trackers = OrderedDict([
['Off', None],
['Position', PositionTracker],
['Position & Orientation', PositionOrientationTracker],
['Smooth Follow', SmoothFollowTracker],
['Look At', LookAtTracker],
['Orbit', OrbitTracker],
])
def setTrackerMode(self, modeName):
assert modeName in self.trackers
self.trackerClass = self.trackers[modeName]
self.initTracker()
class PointCloudSource(vis.PolyDataItem):
_requiredProviderClass = perceptionmeta.PointCloudSourceMeta
def __init__(
self, name, robotStateJointController, callbackFunc=None, provider=None
):
vis.PolyDataItem.__init__(self, name, vtk.vtkPolyData(), view=None)
self.firstData = True
self.robotStateJointController = robotStateJointController
self.timer = TimerCallback()
self.timer.callback = self.showPointCloud
self.timer.start()
self.callbackFunc = callbackFunc
if provider:
self.setProvider(provider)
else:
self.provider = None
def setProvider(self, provider):
"""
Set the provider for this cloud. This completes the initialisation of the object and displays the cloud by
pulling data from the provider
:param provider: An instantiation of the PointCloudSourceMeta abstract class
:return:
"""
if not issubclass(provider.__class__, self._requiredProviderClass):
raise TypeError(
"Attempted to set {} provider to {}, but it was not a"
" subclass of {} as is required.".format(
self.__class__,
provider.__class__,
self._requiredProviderClass.__class__,
)
)
self.provider = provider
self.provider.set_consumer(self)
self.addProperty("Updates Enabled", True)
self.addProperty(
"Number of Point Clouds",
10,
attributes=om.PropertyAttributes(
decimals=0, minimum=1, maximum=100, singleStep=1, hidden=False
),
)
if self.getProperty("Visible"):
self.provider.start()
def _onPropertyChanged(self, propertySet, propertyName):
vis.PolyDataItem._onPropertyChanged(self, propertySet, propertyName)
if propertyName == "Visible" or propertyName == "Updates Enabled":
if self.getProperty(propertyName):
self.timer.start()
if self.provider:
self.provider.start()
else:
self.timer.stop()
if self.provider:
self.provider.stop()
elif propertyName == "Number of Point Clouds":
numberOfPointCloud = self.getProperty(propertyName)
self.provider.set_num_pointclouds(numberOfPointCloud)
if self.provider:
self.provider._on_property_changed(propertySet, propertyName)
@CheckProvider
def getPointCloud(self):
polyData = vtk.vtkPolyData()
self.provider.get_point_cloud(polyData)
if polyData.GetNumberOfPoints() == 0:
return None
else:
return polyData
@CheckProvider
def resetTime(self):
self.provider.reset_time()
@CheckProvider
def showPointCloud(self):
polyData = vtk.vtkPolyData()
self.provider.get_point_cloud(polyData, True)
if polyData.GetNumberOfPoints() == 0:
return
bodyHeight = self.robotStateJointController.q[2]
self.setRangeMap("z", [bodyHeight - 0.5, bodyHeight + 0.5])
if self.callbackFunc:
self.callbackFunc()
# update view
self.setPolyData(polyData)
if self.firstData:
self.firstData = False
colorList = self.properties.getPropertyAttribute("Color By", "enumNames")
zIndex = colorList.index("z") if "z" in colorList else 0
self.properties.setProperty("Color By", zIndex)
class DepthImagePointCloudSource(vis.PolyDataItem):
_requiredProviderClass = perceptionmeta.DepthImageSourceMeta
def __init__(
self, name, cameraName, imageManager, robotStateJointController, provider=None
):
vis.PolyDataItem.__init__(self, name, vtk.vtkPolyData(), view=None)
self.robotStateJointController = robotStateJointController
self.addProperty("Camera name", cameraName)
self.addProperty(
"Decimation",
1,
attributes=om.PropertyAttributes(enumNames=["1", "2", "4", "8", "16"]),
)
self.addProperty(
"Remove Size",
1000,
attributes=om.PropertyAttributes(
decimals=0, minimum=0, maximum=100000.0, singleStep=1000
),
)
self.addProperty(
"Target FPS",
5.0,
attributes=om.PropertyAttributes(
decimals=1, minimum=0.1, maximum=30.0, singleStep=0.1
),
)
self.addProperty(
"Max Range",
5.0,
attributes=om.PropertyAttributes(
decimals=2, minimum=0.0, maximum=30.0, singleStep=0.25
),
)
self.imageManager = imageManager
self.cameraName = cameraName
self.firstData = True
self.timer = TimerCallback()
self.timer.callback = self.update
self.lastUtime = 0
self.lastDataReceivedTime = 0
if provider:
self.setProvider(provider)
else:
self.provider = None
def setProvider(self, provider):
"""
Set the provider for this depth image cloud. This completes the initialisation of the object and displays the
cloud by pulling data from the provider
:param provider: An instantiation of the DepthImageSourceMeta abstract class
:return:
"""
if not issubclass(provider.__class__, self._requiredProviderClass):
raise TypeError(
"Attempted to set {} provider to {}, but it was not a"
" subclass of {} as is required.".format(
self.__class__,
provider.__class__,
self._requiredProviderClass.__class__,
)
)
self.provider = provider
self.provider.set_consumer(self)
decimation = int(self.properties.getPropertyEnumValue("Decimation"))
removeSize = int(self.properties.getProperty("Remove Size"))
rangeThreshold = float(self.properties.getProperty("Max Range"))
self.addProperty("Remove Stale Data", False)
self.addProperty(
"Stale Data Timeout",
5.0,
attributes=om.PropertyAttributes(
decimals=1, minimum=0.1, maximum=30.0, singleStep=0.1
),
)
self.provider.set_decimate(int(decimation))
self.provider.set_remove_size(removeSize)
self.provider.set_range_threshold(rangeThreshold)
self.setProperty("Visible", True)
self.lastDataReceivedTime = time.time()
def _onPropertyChanged(self, propertySet, propertyName):
vis.PolyDataItem._onPropertyChanged(self, propertySet, propertyName)
if propertyName == "Visible":
if self.getProperty(propertyName):
self.timer.start()
if self.provider:
self.provider.start()
else:
self.timer.stop()
if self.provider:
self.provider.stop()
if propertyName in ("Decimation", "Remove outliers", "Max Range"):
self.lastUtime = 0
if propertyName == "Decimation":
decimate = self.getPropertyEnumValue(propertyName)
self.provider.set_decimate(int(decimate))
elif propertyName == "Remove Size":
remove_size = self.getProperty(propertyName)
self.provider.set_remove_size(remove_size)
elif propertyName == "Max Range":
max_range = self.getProperty(propertyName)
self.provider.set_range_threshold(max_range)
@CheckProvider
def getPointCloud(self):
polyData = vtk.vtkPolyData()
self.provider.get_point_cloud(polyData)
if polyData.GetNumberOfPoints() == 0:
return None
else:
return polyData
def onRemoveFromObjectModel(self):
vis.PolyDataItem.onRemoveFromObjectModel(self)
self.timer.stop()
@CheckProvider
def resetTime(self):
self.provider.reset_time()
@CheckProvider
def update(self):
# utime = self.imageManager.queue.getCurrentImageTime(self.cameraName)
utime = self.provider.get_sec() * 1e6 + round(self.provider.get_nsec() * 1e-3)
if utime == self.lastUtime:
if self.getProperty("Remove Stale Data") and (
(time.time() - self.lastDataReceivedTime)
> self.getProperty("Stale Data Timeout")
):
if self.polyData.GetNumberOfPoints() > 0:
self.setPolyData(vtk.vtkPolyData())
return
if utime < self.lastUtime:
temp = 0 # dummy
elif utime - self.lastUtime < 1e6 / self.getProperty("Target FPS"):
return
polyData = vtk.vtkPolyData()
new_data = self.provider.get_point_cloud(polyData, True)
if polyData.GetNumberOfPoints() == 0:
return
# currently disabled
# bodyToLocal = vtk.vtkTransform()
# self.imageManager.queue.getTransform('body', 'local', utime, bodyToLocal)
# bodyHeight = bodyToLocal.GetPosition()[2]
bodyHeight = self.robotStateJointController.q[2]
self.setRangeMap("z", [bodyHeight - 0.5, bodyHeight + 0.5])
self.setPolyData(polyData)
if self.firstData:
self.firstData = False
colorList = self.properties.getPropertyAttribute("Color By", "enumNames")
zIndex = colorList.index("z") if "z" in colorList else 0
self.properties.setProperty("Color By", zIndex)
self.lastDataReceivedTime = time.time()
self.lastUtime = utime
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 and min(plan.plan_info) >= 0)
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('Color Mode', 1 if self.playbackRobotModelUseTextures else 0)
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 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', lcmbotcore.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 AsyncTaskQueue(object):
QUEUE_STARTED_SIGNAL = 'QUEUE_STARTED_SIGNAL'
QUEUE_STOPPED_SIGNAL = 'QUEUE_STOPPED_SIGNAL'
TASK_STARTED_SIGNAL = 'TASK_STARTED_SIGNAL'
TASK_ENDED_SIGNAL = 'TASK_ENDED_SIGNAL'
TASK_PAUSED_SIGNAL = 'TASK_PAUSED_SIGNAL'
TASK_FAILED_SIGNAL = 'TASK_FAILED_SIGNAL'
TASK_EXCEPTION_SIGNAL = 'TASK_EXCEPTION_SIGNAL'
class PauseException(Exception):
pass
class FailException(Exception):
pass
def __init__(self):
self.tasks = []
self.generators = []
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.callbackLoop
self.callbacks = callbacks.CallbackRegistry([self.QUEUE_STARTED_SIGNAL,
self.QUEUE_STOPPED_SIGNAL,
self.TASK_STARTED_SIGNAL,
self.TASK_ENDED_SIGNAL,
self.TASK_PAUSED_SIGNAL,
self.TASK_FAILED_SIGNAL,
self.TASK_EXCEPTION_SIGNAL])
self.currentTask = None
self.isRunning = False
def reset(self):
assert not self.isRunning
assert not self.generators
self.tasks = []
def start(self):
self.isRunning = True
self.callbacks.process(self.QUEUE_STARTED_SIGNAL, self)
self.timer.start()
def stop(self):
self.isRunning = False
self.currentTask = None
self.generators = []
self.timer.stop()
self.callbacks.process(self.QUEUE_STOPPED_SIGNAL, self)
def wrapGenerator(self, generator):
def generatorWrapper():
return generator
return generatorWrapper
def addTask(self, task):
if isinstance(task, types.GeneratorType):
task = self.wrapGenerator(task)
assert hasattr(task, '__call__')
self.tasks.append(task)
def callbackLoop(self):
try:
for i in xrange(10):
self.doWork()
if not self.tasks:
self.stop()
except AsyncTaskQueue.PauseException:
assert self.currentTask
self.callbacks.process(self.TASK_PAUSED_SIGNAL, self, self.currentTask)
self.stop()
except AsyncTaskQueue.FailException:
assert self.currentTask
self.callbacks.process(self.TASK_FAILED_SIGNAL, self, self.currentTask)
self.stop()
except:
assert self.currentTask
self.callbacks.process(self.TASK_EXCEPTION_SIGNAL, self, self.currentTask)
self.stop()
raise
return self.isRunning
def popTask(self):
assert not self.isRunning
assert not self.currentTask
if self.tasks:
self.tasks.pop(0)
def completePreviousTask(self):
assert self.currentTask
self.tasks.remove(self.currentTask)
self.callbacks.process(self.TASK_ENDED_SIGNAL, self, self.currentTask)
self.currentTask = None
def startNextTask(self):
self.currentTask = self.tasks[0]
self.callbacks.process(self.TASK_STARTED_SIGNAL, self, self.currentTask)
result = self.currentTask()
if isinstance(result, types.GeneratorType):
self.generators.insert(0, result)
def doWork(self):
if self.generators:
self.handleGenerator(self.generators[0])
else:
if self.currentTask:
self.completePreviousTask()
if self.tasks:
self.startNextTask()
def handleGenerator(self, generator):
try:
result = generator.next()
except StopIteration:
self.generators.remove(generator)
else:
if isinstance(result, types.GeneratorType):
self.generators.insert(0, result)
def connectQueueStarted(self, func):
return self.callbacks.connect(self.QUEUE_STARTED_SIGNAL, func)
def disconnectQueueStarted(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectQueueStopped(self, func):
return self.callbacks.connect(self.QUEUE_STOPPED_SIGNAL, func)
def disconnectQueueStopped(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskStarted(self, func):
return self.callbacks.connect(self.TASK_STARTED_SIGNAL, func)
def disconnectTaskStarted(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskEnded(self, func):
return self.callbacks.connect(self.TASK_ENDED_SIGNAL, func)
def disconnectTaskEnded(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskPaused(self, func):
return self.callbacks.connect(self.TASK_PAUSED_SIGNAL, func)
def disconnectTaskPaused(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskFailed(self, func):
return self.callbacks.connect(self.TASK_FAILED_SIGNAL, func)
def disconnectTaskFailed(self, callbackId):
self.callbacks.disconnect(callbackId)
def connectTaskException(self, func):
return self.callbacks.connect(self.TASK_EXCEPTION_SIGNAL, func)
def disconnectTaskException(self, callbackId):
self.callbacks.disconnect(callbackId)
class RosGridMap(vis.PolyDataItem):
_requiredProviderClass = perceptionmeta.RosGridMapMeta
def __init__(
self, robotStateJointController, name, callbackFunc=None, provider=None
):
vis.PolyDataItem.__init__(self, name, vtk.vtkPolyData(), view=None)
self.firstData = True
self.robotStateJointController = robotStateJointController
self.timer = TimerCallback()
self.timer.callback = self.showMap
self.timer.start()
self.callbackFunc = callbackFunc
if provider:
self.setProvider(provider)
else:
self.provider = None
def setProvider(self, provider):
"""
Set the provider for this grid map. This completes the initialisation of the object and displays the grid map
by pulling data from the provider
:param provider: An instantiation of the RosGridMapMeta abstract class
:return:
"""
if not issubclass(provider.__class__, self._requiredProviderClass):
raise TypeError(
"Attempted to set {} provider to {}, but it was not a"
" subclass of {} as is required.".format(
self.__class__,
provider.__class__,
self._requiredProviderClass.__class__,
)
)
self.provider = provider
self.provider.set_consumer(self)
if self.getProperty("Visible"):
self.provider.start()
def _onPropertyChanged(self, propertySet, propertyName):
vis.PolyDataItem._onPropertyChanged(self, propertySet, propertyName)
if propertyName == "Visible":
if self.getProperty(propertyName):
self.timer.start()
if self.provider:
self.provider.start()
else:
self.timer.stop()
if self.provider:
self.provider.stop()
elif propertyName == "Color By":
color = self.getPropertyEnumValue(propertyName)
self.provider.set_color_layer(color)
# only_new_data = False because the poly_date need to be redraw with the new color layer
self.showMap(only_new_data=False)
self._updateColorBy()
if self.provider:
self.provider._on_property_changed(propertySet, propertyName)
@CheckProvider
def showMap(self, only_new_data=True):
polyData = vtk.vtkPolyData()
self.provider.get_mesh(polyData, only_new_data)
if polyData.GetNumberOfPoints() == 0:
return
bodyHeight = self.robotStateJointController.q[2]
self.setRangeMap("z", [bodyHeight - 0.5, bodyHeight])
if self.callbackFunc:
self.callbackFunc()
# update view
self.setPolyData(polyData)
if self.firstData:
self.firstData = False
colorList = self.properties.getPropertyAttribute("Color By", "enumNames")
zIndex = colorList.index("z") if "z" in colorList else 0
self.properties.setProperty("Color By", zIndex)
@CheckProvider
def resetTime(self):
self.provider.reset_time()
@CheckProvider
def getPointCloud(self):
polyData = vtk.vtkPolyData()
self.provider.get_point_cloud(polyData)
if polyData.GetNumberOfPoints() == 0:
return None
else:
return polyData
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.useDevelopmentColors()
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 = False
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
interpolationVisible = True
elif viewMode == 'frames':
playbackVisible = False
samplesVisible = True
interpolationVisible = True
elif viewMode == 'hidden':
playbackVisible = False
samplesVisible = False
interpolationVisible = False
else:
raise Exception('Unexpected view mode')
self.ui.samplesLabel.setVisible(samplesVisible)
self.ui.samplesSpinBox.setVisible(samplesVisible)
self.ui.interpolationLabel.setVisible(interpolationVisible)
self.ui.interpolationCombo.setVisible(interpolationVisible)
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)
self.hidePlan()
if self.plan:
if viewMode == 'continuous' and self.autoPlay:
self.startAnimation()
elif viewMode == 'frames':
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
and min(plan.plan_info) >= 0)
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(
'Color Mode', 1 if self.playbackRobotModelUseTextures else 0)
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()
self.ui.playbackSlider.value = 0
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 MarkerSource(vis.PolyDataItem):
_requiredProviderClass = perceptionmeta.MarkerSourceMeta
def __init__(self, name, callbackFunc=None, provider=None):
vis.PolyDataItem.__init__(self, name, vtk.vtkPolyData(), view=None)
self.timer = TimerCallback()
self.timer.callback = self.showData
self.timer.start()
self.callbackFunc = callbackFunc
self.resetColor = True
if provider:
self.setProvider(provider)
else:
self.provider = None
def setProvider(self, provider):
"""
Set the provider for this marker. This completes the initialisation of the object and displays the markers
by pulling data from the provider
:param provider: An instantiation of the MarkerSourceMeta abstract class
:return:
"""
if not issubclass(provider.__class__, self._requiredProviderClass):
raise TypeError(
"Attempted to set {} provider to {}, but it was not a"
" subclass of {} as is required.".format(
self.__class__,
provider.__class__,
self._requiredProviderClass.__class__,
)
)
self.provider = provider
self.provider.set_consumer(self)
if self.getProperty("Visible"):
self.provider.start()
def _onPropertyChanged(self, propertySet, propertyName):
vis.PolyDataItem._onPropertyChanged(self, propertySet, propertyName)
if propertyName == "Visible" or propertyName == "Subscribe":
if self.getProperty(propertyName):
self.timer.start()
if self.provider:
self.provider.start()
else:
self.timer.stop()
if self.provider:
self.provider.stop()
if self.provider:
self.provider._on_property_changed(propertySet, propertyName)
@CheckProvider
def resetTime(self):
self.provider.reset_time()
@CheckProvider
def showData(self):
polyData = vtk.vtkPolyData()
self.provider.get_mesh(polyData)
if polyData.GetNumberOfPoints() == 0:
# if an empty message is received, we will reset the default color when the next message is received
self.resetColor = True
if self.callbackFunc:
self.callbackFunc()
# update view
self.setPolyData(polyData)
if self.resetColor and polyData.GetNumberOfPoints() != 0:
self.resetColor = False
colorList = self.properties.getPropertyAttribute("Color By", "enumNames")
index = colorList.index("color") if "color" in colorList else 0
self.properties.setProperty("Color By", index)
class CameraTrackerManager(object):
def __init__(self):
self.target = None
self.targetFrame = None
self.trackerClass = None
self.camera = None
self.view = None
self.timer = TimerCallback()
self.timer.callback = self.updateTimer
self.addTrackers()
self.initTracker()
def updateTimer(self):
tNow = time.time()
dt = tNow - self.tLast
if dt < self.timer.elapsed / 2.0:
return
self.update()
def setView(self, view):
self.view = view
self.camera = view.camera()
def setTarget(self, target):
"""
target should be an instance of TargetFrameConverter or
any object that provides a method getTargetFrame().
"""
if target == self.target:
return
self.disableActiveTracker()
if not target:
return
self.target = target
self.targetFrame = target.getTargetFrame()
self.callbackId = self.targetFrame.connectFrameModified(self.onTargetFrameModified)
self.initTracker()
def disableActiveTracker(self):
if self.targetFrame:
self.targetFrame.disconnectFrameModified(self.callbackId)
self.target = None
self.targetFrame = None
self.initTracker()
def update(self):
tNow = time.time()
dt = tNow - self.tLast
self.tLast = tNow
if self.activeTracker:
self.activeTracker.dt = dt
self.activeTracker.update()
def reset(self):
self.tLast = time.time()
if self.activeTracker:
self.activeTracker.reset()
def getModeActions(self):
if self.activeTracker:
return self.activeTracker.actions
return []
def onModeAction(self, actionName):
if self.activeTracker:
self.activeTracker.onAction(actionName)
def getModeProperties(self):
if self.activeTracker:
return self.activeTracker.properties
return None
def onTargetFrameModified(self, frame):
self.update()
def initTracker(self):
self.timer.stop()
self.activeTracker = (
self.trackerClass(self.view, self.targetFrame) if (self.trackerClass and self.targetFrame) else None
)
self.reset()
self.update()
if self.activeTracker:
minimumUpdateRate = self.activeTracker.getMinimumUpdateRate()
if minimumUpdateRate > 0:
self.timer.targetFps = minimumUpdateRate
self.timer.start()
def addTrackers(self):
self.trackers = OrderedDict(
[
["Off", None],
["Position", PositionTracker],
["Position & Orientation", PositionOrientationTracker],
["Smooth Follow", SmoothFollowTracker],
["Look At", LookAtTracker],
["Orbit", OrbitTracker],
]
)
def setTrackerMode(self, modeName):
assert modeName in self.trackers
self.trackerClass = self.trackers[modeName]
self.initTracker()
class MarkerArraySource(vis.PolyDataItemList):
_requiredProviderClass = perceptionmeta.MarkerArraySourceMeta
def __init__(self, name, singlePolyData=False, callbackFunc=None, provider=None):
vis.PolyDataItemList.__init__(self, name, "color")
# if singlePolyData is True, it means that all the markers received are merged into a single one
self.singlePolyData = singlePolyData
self.timer = TimerCallback()
self.timer.callback = self.showData
self.timer.start()
self.callbackFunc = callbackFunc
# self.topicName = topicName
if provider:
self.setProvider(provider)
else:
self.provider = None
def setProvider(self, provider):
"""
Set the provider for this marker array. This completes the initialisation of the object and displays the marker
array by pulling data from the provider
:param provider: An instantiation of the MarkerArraySourceMeta abstract class
:return:
"""
if not issubclass(provider.__class__, self._requiredProviderClass):
raise TypeError(
"Attempted to set {} provider to {}, but it was not a"
" subclass of {} as is required.".format(
self.__class__,
provider.__class__,
self._requiredProviderClass.__class__,
)
)
self.provider = provider
self.provider.set_consumer(self)
if self.getProperty("Visible"):
self.provider.start()
def _onPropertyChanged(self, propertySet, propertyName):
vis.PolyDataItemList._onPropertyChanged(self, propertySet, propertyName)
if propertyName == "Visible" or propertyName == "Subscribe":
if self.getProperty(propertyName):
self.timer.start()
if self.provider:
self.provider.start()
else:
self.timer.stop()
if self.provider:
self.provider.stop()
if self.provider:
self.provider._on_property_changed(propertySet, propertyName)
def resetTime(self):
self.provider.reset_time()
@CheckProvider
def showData(self):
numPoly = self.provider.get_number_of_mesh()
polyDataList = []
if self.singlePolyData:
polyData = vtk.vtkPolyData()
self.provider.get_mesh(polyData)
if polyData.GetNumberOfPoints() > 0:
polyDataList.append(polyData)
else:
for i in range(0, numPoly):
polyData = vtk.vtkPolyData()
self.provider.get_mesh(polyData, i)
polyDataList.append(polyData)
if self.callbackFunc:
self.callbackFunc()
self.setPolyData(polyDataList)
