def __init__(self, channel='OPTITRACK_FRAMES', name='Optitrack Visualier'):
self.name = name
self.channel = channel
self.subscriber = None
self.optitrackToWorld = transformUtils.frameFromPositionAndRPY([0,0,0],[90,0,90])
self.subscriber = None
self.fpsCounter = FPSCounter()
self.fpsCounter.printToConsole = False
def __init__(self, view, imageWidget):
self.view = view
self.imageWidget = imageWidget
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(":/ui/ddScreenGrabber.ui")
assert uifile.open(uifile.ReadOnly)
self.frameCount = 0
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.ui.lockViewSizeCheck.connect("clicked()", self.onLockViewSize)
self.ui.screenshotOutputBrowseButton.connect(
"clicked()", self.onChooseScreenshotOutputDir
)
self.ui.movieOutputBrowseButton.connect(
"clicked()", self.onChooseMovieOutputDir
)
self.ui.saveScreenshotButton.connect("clicked()", self.onSaveScreenshot)
self.ui.recordMovieButton.connect("clicked()", self.onRecordMovie)
self.ui.viewSizeCombo.connect(
"currentIndexChanged(const QString&)", self.updateViewSize
)
self.ui.viewHeightSpin.connect("valueChanged(int)", self.onViewSizeChanged)
self.ui.viewWidthSpin.connect("valueChanged(int)", self.onViewSizeChanged)
self.ui.imageWidgetWidthSpin.connect(
"valueChanged(int)", self.onimageWidgetWidthChanged
)
self.ui.showMainImageCheck.connect(
"clicked()", self.onShowMainImageCheckChanged
)
self.updateViewSize()
self.onLockViewSize()
self.recordTimer = QtCore.QTimer()
self.recordTimer.connect("timeout()", self.onRecordTimer)
self.fpsCounter = FPSCounter()
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.ui.scrollArea.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect("handleEvent(QObject*, QEvent*)", self.onEvent)
def __init__(self, view):
self.view = view
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddScreenGrabber.ui')
assert uifile.open(uifile.ReadOnly)
self.frameCount = 0
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.ui.syncFrameRateToDrawMessageCheck.connect('clicked()', self.onSyncFrameRateToDrawMessages)
self.ui.lockViewSizeCheck.connect('clicked()', self.onLockViewSize)
self.ui.screenshotOutputBrowseButton.connect('clicked()', self.onChooseScreenshotOutputDir)
self.ui.movieOutputBrowseButton.connect('clicked()', self.onChooseMovieOutputDir)
self.ui.saveScreenshotButton.connect('clicked()', self.onSaveScreenshot)
self.ui.recordMovieButton.connect('clicked()', self.onRecordMovie)
self.ui.viewSizeCombo.connect('currentIndexChanged(const QString&)', self.updateViewSize)
self.ui.viewHeightSpin.connect('valueChanged(int)', self.onViewSizeChanged)
self.ui.viewWidthSpin.connect('valueChanged(int)', self.onViewSizeChanged)
self.updateViewSize()
self.onLockViewSize()
self.recordTimer = QtCore.QTimer()
self.recordTimer.connect('timeout()', self.onRecordTimer)
self.fpsCounter = FPSCounter()
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.ui.scrollArea.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.onEvent)
self.subscribers = lcmUtils.addSubscriber('DRAKE_VIEWER_DRAW', lcmrl.viewer_draw_t, self.onViewerDraw)
def subscribe(self,
topic_name,
message_type,
message_function=None,
call_on_thread=False):
def on_message(msg):
self.msgs[topic_name] = msg
self.counters[topic_name].tick()
if call_on_thread and message_function:
message_function(topic_name, msg)
else:
self.pending_msgs[topic_name] = msg
self.counters[topic_name] = FPSCounter()
self.callbacks[topic_name] = message_function
self.subs[topic_name] = rospy.Subscriber(topic_name, message_type,
on_message)
def __init__(self, view):
self.view = view
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(":/ui/ddScreenGrabber.ui")
assert uifile.open(uifile.ReadOnly)
self.frameCount = 0
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.ui.lockViewSizeCheck.connect("clicked()", self.onLockViewSize)
self.ui.screenshotOutputBrowseButton.connect("clicked()", self.onChooseScreenshotOutputDir)
self.ui.movieOutputBrowseButton.connect("clicked()", self.onChooseMovieOutputDir)
self.ui.saveScreenshotButton.connect("clicked()", self.onSaveScreenshot)
self.ui.recordMovieButton.connect("clicked()", self.onRecordMovie)
self.ui.viewSizeCombo.connect("currentIndexChanged(const QString&)", self.updateViewSize)
self.ui.viewHeightSpin.connect("valueChanged(int)", self.onViewSizeChanged)
self.ui.viewWidthSpin.connect("valueChanged(int)", self.onViewSizeChanged)
self.updateViewSize()
self.onLockViewSize()
self.recordTimer = QtCore.QTimer()
self.recordTimer.connect("timeout()", self.onRecordTimer)
self.fpsCounter = FPSCounter()
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.ui.scrollArea.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect("handleEvent(QObject*, QEvent*)", self.onEvent)
def _setup(self):
self.counter = FPSCounter()
self.counter.printToConsole = True
self.ikServer = None
class PyDrakePlannerPublisher(plannerPublisher.PlannerPublisher):
def _setup(self):
self.counter = FPSCounter()
self.counter.printToConsole = True
self.ikServer = None
def _setupLocalServer(self):
if self.ikServer is not None:
return
initArgs = FieldContainer(
urdfFile=self.ikPlanner.robotModel.getProperty('Filename'),
packagePaths=roboturdf.getPackagePaths()
)
self.ikServer = PyDrakeIkServer()
self.ikServer.initInstance(initArgs)
def testEncodeDecode(self, fields):
encoded = json.dumps(fields, cls=ikconstraintencoder.ConstraintEncoder)
decoded = json.loads(encoded, object_hook=ikconstraintencoder.ConstraintDecoder)
del decoded['class']
fields = FieldContainer(**decoded)
del fields.options['class']
fields.options = ikparameters.IkParameters(**fields.options)
constraints = []
for c in fields.constraints:
objClass = getattr(ikconstraints, c['class'])
del c['class']
obj = objClass()
constraints.append(obj)
for attr, value in c.iteritems():
if isinstance(value, dict) and 'position' in value and 'quaternion' in value:
value = transformUtils.transformFromPose(value['position'], value['quaternion'])
setattr(obj, attr, value)
fields.constraints = constraints
return fields
def makePlanMessage(self, poses, poseTimes, info, fields):
from director import robotstate
states = [robotstate.drakePoseToRobotState(pose) for pose in poses]
for i, state in enumerate(states):
state.utime = poseTimes[i]*1e6
msg = robot_plan_t()
msg.utime = fields.utime
msg.robot_name = 'robot'
msg.num_states = len(states)
msg.plan = states
msg.plan_info = [info]*len(states)
msg.num_bytes = 0
msg.matlab_data = []
msg.num_grasp_transitions = 0
msg.left_arm_control_type = msg.NONE
msg.right_arm_control_type = msg.NONE
msg.left_leg_control_type = msg.NONE
msg.right_leg_control_type = msg.NONE
return msg
def processIK(self, constraints, ikParameters, positionCosts, nominalPoseName="", seedPoseName=""):
fields = self.setupFields(constraints, ikParameters, positionCosts, nominalPoseName, seedPoseName)
fields = self.testEncodeDecode(fields)
endPose, info = self.ikServer.runIk(fields)
self.counter.tick()
return endPose, info
def processTraj(self, constraints, ikParameters, positionCosts, nominalPoseName="", seedPoseName="", endPoseName=""):
fields = self.setupFields(constraints, ikParameters, positionCosts, nominalPoseName, seedPoseName, endPoseName)
fields = self.testEncodeDecode(fields)
poses, poseTimes, info = self.ikServer.runIkTraj(fields)
plan = self.makePlanMessage(poses, poseTimes, info, fields)
lcmUtils.publish('CANDIDATE_MANIP_PLAN', plan)
return plan, info
class ScreenGrabberPanel(object):
def __init__(self, view):
self.view = view
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddScreenGrabber.ui')
assert uifile.open(uifile.ReadOnly)
self.frameCount = 0
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.ui.lockViewSizeCheck.connect('clicked()', self.onLockViewSize)
self.ui.screenshotOutputBrowseButton.connect(
'clicked()', self.onChooseScreenshotOutputDir)
self.ui.movieOutputBrowseButton.connect('clicked()',
self.onChooseMovieOutputDir)
self.ui.saveScreenshotButton.connect('clicked()',
self.onSaveScreenshot)
self.ui.recordMovieButton.connect('clicked()', self.onRecordMovie)
self.ui.viewSizeCombo.connect('currentIndexChanged(const QString&)',
self.updateViewSize)
self.ui.viewHeightSpin.connect('valueChanged(int)',
self.onViewSizeChanged)
self.ui.viewWidthSpin.connect('valueChanged(int)',
self.onViewSizeChanged)
self.updateViewSize()
self.onLockViewSize()
self.recordTimer = QtCore.QTimer()
self.recordTimer.connect('timeout()', self.onRecordTimer)
self.fpsCounter = FPSCounter()
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.ui.scrollArea.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)',
self.onEvent)
def onEvent(self, obj, event):
minSize = self.ui.scrollArea.widget().minimumSizeHint.width(
) + self.ui.scrollArea.verticalScrollBar().width
self.ui.scrollArea.setMinimumWidth(minSize)
def dateTimeString(self):
return datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H:%M:%S')
def movieOutputDirectory(self):
return os.path.expanduser(self.ui.movieOutputDirectory.text)
def screenshotOutputDirectory(self):
return os.path.expanduser(self.ui.screenshotOutputDirectory.text)
def captureRate(self):
return self.ui.captureRateSpin.value
def chooseDirectory(self):
return QtGui.QFileDialog.getExistingDirectory(
app.getMainWindow(), "Choose directory...",
self.movieOutputDirectory())
def ensureDirectoryIsWritable(self, dirname):
if not os.path.isdir(dirname):
try:
os.makedirs(dirname)
except OSError:
app.showErrorMessage('Error creating directory: %s' % dirname)
return False
if not os.access(dirname, os.W_OK | os.X_OK):
app.showErrorMessage('Directory is not writable: %s' % dirname)
return False
return True
def onChooseScreenshotOutputDir(self):
newDir = self.chooseDirectory()
if newDir:
self.ui.screenshotOutputDirectory.text = newDir
def onChooseMovieOutputDir(self):
newDir = self.chooseDirectory()
if newDir:
self.ui.movieOutputDirectory.text = newDir
def onSaveScreenshot(self):
outDir = self.screenshotOutputDirectory()
if not self.ensureDirectoryIsWritable(outDir):
return
filename = os.path.join(outDir,
'Screenshot-' + self.dateTimeString() + '.png')
saveScreenshot(self.view, filename)
app.getMainWindow().statusBar().showMessage('Saved: ' + filename, 2000)
def nextMovieFileName(self):
filename = os.path.join(self.movieOutputDirectory(),
'frame_%07d.tiff' % self.frameCount)
self.frameCount += 1
return filename
def updateRecordingStats(self):
isRecordMode = self.ui.recordMovieButton.checked
currentRate = 0.0
writeQueue = 0.0
self.ui.currentRateValueLabel.setText('%.1f' % currentRate)
self.ui.writeQueueValueLabel.setText('%.1f' % currentRate)
def isRecordMode(self):
return self.ui.recordMovieButton.checked
def updateRecordingButtons(self):
isRecordMode = self.isRecordMode()
self.ui.movieOutputDirectory.setEnabled(not isRecordMode)
self.ui.movieOutputBrowseButton.setEnabled(not isRecordMode)
self.ui.captureRateSpin.setEnabled(not isRecordMode)
self.ui.captureRateLabel.setEnabled(not isRecordMode)
self.ui.moveOutputDirectoryLabel.setEnabled(not isRecordMode)
self.ui.currentRateLabel.setEnabled(isRecordMode)
self.ui.currentRateValueLabel.setEnabled(isRecordMode)
self.ui.writeQueueLabel.setEnabled(isRecordMode)
self.ui.writeQueueValueLabel.setEnabled(isRecordMode)
self.updateRecordingStats()
def onRecordMovie(self):
# Enforce even width number, otherwise avconv will fail
_width = (self.view.width if self.view.width %
2 == 0 else self.view.width + 1)
_height = (self.view.height if self.view.height %
2 == 0 else self.view.height + 1)
self.view.setFixedSize(_width, _height)
if self.isRecordMode():
self.startRecording()
else:
self.stopRecording()
self.updateRecordingButtons()
def startRecording(self):
self.frameCount = 0
if not self.ensureDirectoryIsWritable(self.movieOutputDirectory()):
self.ui.recordMovieButton.checked = False
return
existingFiles = glob.glob(
os.path.join(self.movieOutputDirectory(), '*.tiff'))
if len(existingFiles):
choice = QtGui.QMessageBox.question(
app.getMainWindow(), 'Continue?',
'There are existing image files in the output directory. They will be deleted prior to recording. Continue?',
QtGui.QMessageBox.Yes | QtGui.QMessageBox.No,
QtGui.QMessageBox.No)
if choice == QtGui.QMessageBox.No:
self.ui.recordMovieButton.checked = False
return
for fileToRemove in existingFiles:
os.remove(fileToRemove)
self.fpsCounter.tick()
self.startT = time.time()
interval = int(round(1000.0 / self.captureRate()))
self.recordTimer.setInterval(interval)
self.recordTimer.start()
def stopRecording(self):
self.recordTimer.stop()
if self.frameCount > 0:
self.showEncodingDialog()
def showEncodingDialog(self):
msg = 'Recorded %d frames. For encoding, use this command line:\n\n\n' % self.frameCount
msg += ' cd "%s"\n\n' % self.movieOutputDirectory()
msg += ' avconv -r %d -i frame_%%07d.tiff \\\n' % self.captureRate()
msg += ' -vcodec libx264 \\\n'
msg += ' -preset slow \\\n'
msg += ' -crf 18 \\\n'
msg += ' output.mp4\n\n\n'
app.showInfoMessage(msg, title='Recording Stopped')
def updateViewSize(self):
current = str(self.ui.viewSizeCombo.currentText)
useCustom = (current == 'Custom')
self.ui.viewWidthSpin.setEnabled(useCustom)
self.ui.viewHeightSpin.setEnabled(useCustom)
if useCustom:
return
else:
viewSize = [
int(value) for value in current.split(' ')[0].split('x')
]
self.ui.viewWidthSpin.value = viewSize[0]
self.ui.viewHeightSpin.value = viewSize[1]
def onViewSizeChanged(self):
self.onLockViewSize()
def viewSize(self):
return self.ui.viewWidthSpin.value, self.ui.viewHeightSpin.value
def lockViewSize(self):
self.view.setFixedSize(*self.viewSize())
self.ui.viewSizeFrame.setEnabled(True)
def unlockViewSize(self):
self.ui.viewSizeFrame.setEnabled(False)
qtwidgetMaxViewSize = 16777215
self.view.setFixedSize(qtwidgetMaxViewSize, qtwidgetMaxViewSize)
def onLockViewSize(self):
if self.ui.lockViewSizeCheck.checked:
self.lockViewSize()
else:
self.unlockViewSize()
def onRecordTimer(self):
saveScreenshot(self.view, self.nextMovieFileName(), shouldRender=False)
self.fpsCounter.tick()
tNow = time.time()
if tNow - self.startT > 1.0:
self.startT = tNow
self.ui.currentRateValueLabel.text = '%.1f' % self.fpsCounter.getAverageFPS(
)
class ScreenGrabberPanel(object):
def __init__(self, view):
self.view = view
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(":/ui/ddScreenGrabber.ui")
assert uifile.open(uifile.ReadOnly)
self.frameCount = 0
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.ui.lockViewSizeCheck.connect("clicked()", self.onLockViewSize)
self.ui.screenshotOutputBrowseButton.connect("clicked()", self.onChooseScreenshotOutputDir)
self.ui.movieOutputBrowseButton.connect("clicked()", self.onChooseMovieOutputDir)
self.ui.saveScreenshotButton.connect("clicked()", self.onSaveScreenshot)
self.ui.recordMovieButton.connect("clicked()", self.onRecordMovie)
self.ui.viewSizeCombo.connect("currentIndexChanged(const QString&)", self.updateViewSize)
self.ui.viewHeightSpin.connect("valueChanged(int)", self.onViewSizeChanged)
self.ui.viewWidthSpin.connect("valueChanged(int)", self.onViewSizeChanged)
self.updateViewSize()
self.onLockViewSize()
self.recordTimer = QtCore.QTimer()
self.recordTimer.connect("timeout()", self.onRecordTimer)
self.fpsCounter = FPSCounter()
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.ui.scrollArea.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect("handleEvent(QObject*, QEvent*)", self.onEvent)
def onEvent(self, obj, event):
minSize = self.ui.scrollArea.widget().minimumSizeHint.width() + self.ui.scrollArea.verticalScrollBar().width
self.ui.scrollArea.setMinimumWidth(minSize)
def dateTimeString(self):
return datetime.datetime.fromtimestamp(time.time()).strftime("%Y-%m-%d_%H:%M:%S")
def movieOutputDirectory(self):
return os.path.expanduser(self.ui.movieOutputDirectory.text)
def screenshotOutputDirectory(self):
return os.path.expanduser(self.ui.screenshotOutputDirectory.text)
def captureRate(self):
return self.ui.captureRateSpin.value
def chooseDirectory(self):
return QtGui.QFileDialog.getExistingDirectory(
app.getMainWindow(), "Choose directory...", self.movieOutputDirectory()
)
def ensureDirectoryIsWritable(self, dirname):
if not os.path.isdir(dirname):
try:
os.makedirs(dirname)
except OSError:
app.showErrorMessage("Error creating directory: %s" % dirname)
return False
if not os.access(dirname, os.W_OK | os.X_OK):
app.showErrorMessage("Directory is not writable: %s" % dirname)
return False
return True
def onChooseScreenshotOutputDir(self):
newDir = self.chooseDirectory()
if newDir:
self.ui.screenshotOutputDirectory.text = newDir
def onChooseMovieOutputDir(self):
newDir = self.chooseDirectory()
if newDir:
self.ui.movieOutputDirectory.text = newDir
def onSaveScreenshot(self):
outDir = self.screenshotOutputDirectory()
if not self.ensureDirectoryIsWritable(outDir):
return
filename = os.path.join(outDir, "Screenshot-" + self.dateTimeString() + ".png")
saveScreenshot(self.view, filename)
app.getMainWindow().statusBar().showMessage("Saved: " + filename, 2000)
def nextMovieFileName(self):
filename = os.path.join(self.movieOutputDirectory(), "frame_%07d.tiff" % self.frameCount)
self.frameCount += 1
return filename
def updateRecordingStats(self):
isRecordMode = self.ui.recordMovieButton.checked
currentRate = 0.0
writeQueue = 0.0
self.ui.currentRateValueLabel.setText("%.1f" % currentRate)
self.ui.writeQueueValueLabel.setText("%.1f" % currentRate)
def isRecordMode(self):
return self.ui.recordMovieButton.checked
def updateRecordingButtons(self):
isRecordMode = self.isRecordMode()
self.ui.movieOutputDirectory.setEnabled(not isRecordMode)
self.ui.movieOutputBrowseButton.setEnabled(not isRecordMode)
self.ui.captureRateSpin.setEnabled(not isRecordMode)
self.ui.captureRateLabel.setEnabled(not isRecordMode)
self.ui.moveOutputDirectoryLabel.setEnabled(not isRecordMode)
self.ui.currentRateLabel.setEnabled(isRecordMode)
self.ui.currentRateValueLabel.setEnabled(isRecordMode)
self.ui.writeQueueLabel.setEnabled(isRecordMode)
self.ui.writeQueueValueLabel.setEnabled(isRecordMode)
self.updateRecordingStats()
def onRecordMovie(self):
# Enforce even width number, otherwise avconv will fail
_width = self.view.width if self.view.width % 2 == 0 else self.view.width + 1
_height = self.view.height if self.view.height % 2 == 0 else self.view.height + 1
self.view.setFixedSize(_width, _height)
if self.isRecordMode():
self.startRecording()
else:
self.stopRecording()
self.updateRecordingButtons()
def startRecording(self):
self.frameCount = 0
if not self.ensureDirectoryIsWritable(self.movieOutputDirectory()):
self.ui.recordMovieButton.checked = False
return
existingFiles = glob.glob(os.path.join(self.movieOutputDirectory(), "*.tiff"))
if len(existingFiles):
choice = QtGui.QMessageBox.question(
app.getMainWindow(),
"Continue?",
"There are existing image files in the output directory. They will be deleted prior to recording. Continue?",
QtGui.QMessageBox.Yes | QtGui.QMessageBox.No,
QtGui.QMessageBox.No,
)
if choice == QtGui.QMessageBox.No:
self.ui.recordMovieButton.checked = False
return
for fileToRemove in existingFiles:
os.remove(fileToRemove)
self.fpsCounter.tick()
self.startT = time.time()
interval = int(round(1000.0 / self.captureRate()))
self.recordTimer.setInterval(interval)
self.recordTimer.start()
def stopRecording(self):
self.recordTimer.stop()
if self.frameCount > 0:
self.showEncodingDialog()
def showEncodingDialog(self):
msg = "Recorded %d frames. For encoding, use this command line:\n\n\n" % self.frameCount
msg += ' cd "%s"\n\n' % self.movieOutputDirectory()
msg += " avconv -r %d -i frame_%%07d.tiff \\\n" % self.captureRate()
msg += " -vcodec libx264 \\\n"
msg += " -preset slow \\\n"
msg += " -crf 18 \\\n"
msg += " output.mp4\n\n\n"
app.showInfoMessage(msg, title="Recording Stopped")
def updateViewSize(self):
current = str(self.ui.viewSizeCombo.currentText)
useCustom = current == "Custom"
self.ui.viewWidthSpin.setEnabled(useCustom)
self.ui.viewHeightSpin.setEnabled(useCustom)
if useCustom:
return
else:
viewSize = [int(value) for value in current.split(" ")[0].split("x")]
self.ui.viewWidthSpin.value = viewSize[0]
self.ui.viewHeightSpin.value = viewSize[1]
def onViewSizeChanged(self):
self.onLockViewSize()
def viewSize(self):
return self.ui.viewWidthSpin.value, self.ui.viewHeightSpin.value
def lockViewSize(self):
self.view.setFixedSize(*self.viewSize())
self.ui.viewSizeFrame.setEnabled(True)
def unlockViewSize(self):
self.ui.viewSizeFrame.setEnabled(False)
qtwidgetMaxViewSize = 16777215
self.view.setFixedSize(qtwidgetMaxViewSize, qtwidgetMaxViewSize)
def onLockViewSize(self):
if self.ui.lockViewSizeCheck.checked:
self.lockViewSize()
else:
self.unlockViewSize()
def onRecordTimer(self):
saveScreenshot(self.view, self.nextMovieFileName(), shouldRender=False)
self.fpsCounter.tick()
tNow = time.time()
if tNow - self.startT > 1.0:
self.startT = tNow
self.ui.currentRateValueLabel.text = "%.1f" % self.fpsCounter.getAverageFPS()
def _setup(self):
self.counter = FPSCounter()
self.counter.printToConsole = True
self.ikServer = None
class PyDrakePlannerPublisher(plannerPublisher.PlannerPublisher):
def _setup(self):
self.counter = FPSCounter()
self.counter.printToConsole = True
self.ikServer = None
def _setupLocalServer(self):
if self.ikServer is not None:
return
initArgs = FieldContainer(
urdfFile=self.ikPlanner.robotModel.getProperty('Filename'),
packagePaths=roboturdf.getPackagePaths())
self.ikServer = PyDrakeIkServer()
self.ikServer.initInstance(initArgs)
def testEncodeDecode(self, fields):
encoded = json.dumps(fields, cls=ikconstraintencoder.ConstraintEncoder)
decoded = json.loads(encoded,
object_hook=ikconstraintencoder.ConstraintDecoder)
del decoded['class']
fields = FieldContainer(**decoded)
del fields.options['class']
fields.options = ikparameters.IkParameters(**fields.options)
constraints = []
for c in fields.constraints:
objClass = getattr(ikconstraints, c['class'])
del c['class']
obj = objClass()
constraints.append(obj)
for attr, value in c.iteritems():
if isinstance(
value, dict
) and 'position' in value and 'quaternion' in value:
value = transformUtils.transformFromPose(
value['position'], value['quaternion'])
setattr(obj, attr, value)
fields.constraints = constraints
return fields
def makePlanMessage(self, poses, poseTimes, info, fields):
from director import robotstate
states = [robotstate.drakePoseToRobotState(pose) for pose in poses]
for i, state in enumerate(states):
state.utime = poseTimes[i] * 1e6
msg = robot_plan_t()
msg.utime = fields.utime
msg.robot_name = 'robot'
msg.num_states = len(states)
msg.plan = states
msg.plan_info = [info] * len(states)
msg.num_bytes = 0
msg.matlab_data = []
msg.num_grasp_transitions = 0
msg.left_arm_control_type = msg.NONE
msg.right_arm_control_type = msg.NONE
msg.left_leg_control_type = msg.NONE
msg.right_leg_control_type = msg.NONE
return msg
def processIK(self,
constraints,
ikParameters,
positionCosts,
nominalPoseName="",
seedPoseName=""):
fields = self.setupFields(constraints, ikParameters, positionCosts,
nominalPoseName, seedPoseName)
fields = self.testEncodeDecode(fields)
endPose, info = self.ikServer.runIk(fields)
self.counter.tick()
return endPose, info
def processTraj(self,
constraints,
ikParameters,
positionCosts,
nominalPoseName="",
seedPoseName="",
endPoseName=""):
fields = self.setupFields(constraints, ikParameters, positionCosts,
nominalPoseName, seedPoseName, endPoseName)
fields = self.testEncodeDecode(fields)
poses, poseTimes, info = self.ikServer.runIkTraj(fields)
plan = self.makePlanMessage(poses, poseTimes, info, fields)
lcmUtils.publish('CANDIDATE_MANIP_PLAN', plan)
return plan, info
def _setup(self):
self.counter = FPSCounter()
self.counter.printToConsole = True
class OptitrackVisualizer(object):
def __init__(self, channel='OPTITRACK_FRAMES', name='Optitrack Visualier'):
self.name = name
self.channel = channel
self.subscriber = None
self.optitrackToWorld = transformUtils.frameFromPositionAndRPY([0,0,0],[90,0,90])
self.subscriber = None
self.fpsCounter = FPSCounter()
self.fpsCounter.printToConsole = False
def _addSubscriber(self):
assert self.subscriber is None
self.subscriber = lcmUtils.addSubscriber(self.channel, optitrack_frame_t, self.onMessage)
self.subscriber.setSpeedLimit(100)
def _removeSubscriber(self):
assert self.subscriber is not None
lcmUtils.removeSubscriber(self.subscriber)
self.subscriber = None
def isEnabled(self):
return self.subscriber is not None
def setEnabled(self, enabled):
if enabled and not self.isEnabled():
self._addSubscriber()
elif not enabled and self.isEnabled():
self._removeSubscriber()
self.removeRootFolder()
def enable(self):
self.setEnabled(True)
def disable(self):
self.setEnabled(False)
def getRootFolder(self):
folder = om.getOrCreateContainer(self.name)
return folder
def removeRootFolder(self):
om.removeFromObjectModel(self.getRootFolder())
def _makeMarkers(self, points, radius=0.01):
d = DebugData()
for p in points:
d.addSphere(p, radius=radius, resolution=8)
return d.getPolyData()
def _handleRigidBodies(self, bodies):
if not bodies:
return
folder = self.getRootFolder()
for body in bodies:
name = 'Body ' + str(body.id)
x,y,z,w = body.quat
quat = (w,x,y,z)
bodyToOptitrack = transformUtils.transformFromPose(body.xyz, quat)
bodyToWorld = transformUtils.concatenateTransforms((bodyToOptitrack, self.optitrackToWorld))
obj = folder.findChild(name)
if not obj:
geometry = self._makeMarkers(body.marker_xyz)
geometry = filterUtils.transformPolyData(geometry, bodyToOptitrack.GetLinearInverse())
obj = vis.showPolyData(geometry, name, parent=folder, color=[1,0,0])
frameObj = vis.addChildFrame(obj)
frameObj.setProperty('Scale', 0.2)
frameObj.setProperty('Visible', True)
obj.getChildFrame().copyFrame(bodyToWorld)
def onMessage(self, msg):
self.fpsCounter.tick()
self.lastMessage = msg
self._handleRigidBodies(msg.rigid_bodies)
def _setup(self):
self.counter = FPSCounter()
self.counter.printToConsole = True
class PyDrakePlannerPublisher(plannerPublisher.PlannerPublisher):
def _setup(self):
self.counter = FPSCounter()
self.counter.printToConsole = True
# disabled for now
#self._setupLocalServer()
def _setupLocalServer(self):
initArgs = FieldContainer(
urdfFile=self.ikPlanner.robotModel.getProperty('Filename'),
packagePaths=roboturdf.getPackagePaths()
)
self.ikServer = PyDrakeIkServer()
self.ikServer.initInstance(initArgs)
def testEncodeDecode(self, fields):
encoded = json.dumps(fields, cls=ikconstraintencoder.ConstraintEncoder)
decoded = json.loads(encoded, object_hook=ikconstraintencoder.ConstraintDecoder)
del decoded['class']
fields = FieldContainer(**decoded)
del fields.options['class']
fields.options = ikparameters.IkParameters(**fields.options)
constraints = []
for c in fields.constraints:
objClass = getattr(ikconstraints, c['class'])
del c['class']
obj = objClass()
constraints.append(obj)
for attr, value in c.iteritems():
if isinstance(value, dict) and 'position' in value and 'quaternion' in value:
value = transformUtils.transformFromPose(value['position'], value['quaternion'])
setattr(obj, attr, value)
fields.constraints = constraints
return fields
def processIK(self, constraints, ikParameters, positionCosts, nominalPoseName="", seedPoseName=""):
fields = self.setupFields(constraints, ikParameters, positionCosts, nominalPoseName, seedPoseName)
fields = self.testEncodeDecode(fields)
endPose, info = self.ikServer.runIk(fields)
self.counter.tick()
return endPose, info
def processTraj(self, constraints, ikParameters, positionCosts, nominalPoseName="", seedPoseName="", endPoseName=""):
fields = self.setupFields(constraints, ikParameters, positionCosts, nominalPoseName, seedPoseName, endPoseName)
fields = self.testEncodeDecode(fields)
plan, info = self.ikServer.runIkTraj(fields)
return plan, info
