def onFileSaveData():
obj = om.getActiveObject()
if not obj:
app.showErrorMessage('Please select an object', title='No object selected')
return
if isinstance(obj, otdfmodel.OtdfModelItem):
mainWindow = app.getMainWindow()
filename = QtGui.QFileDialog.getSaveFileName(mainWindow, "Save Data...", getDefaultDirectory(), 'OTDF (*.otdf)', 'OTDF (*.otdf)')
if not os.path.splitext(filename)[1]:
filename += '.otdf'
storeDefaultDirectory(filename)
otdfString = obj.parser.getUpdatedOtdf()
otdfFile = open(filename, 'w')
otdfFile.write(otdfString)
otdfFile.close()
elif hasattr(obj, 'polyData'):
mainWindow = app.getMainWindow()
fileFilters = "PLY (*.ply);;STL (*.stl);;VTP (*.vtp)";
filename = QtGui.QFileDialog.getSaveFileName(mainWindow, "Save Data...", getDefaultDirectory(), fileFilters, 'VTP (*.vtp)')
if not filename:
return
if not os.path.splitext(filename)[1]:
filename += '.vtp'
polyData = io.writePolyData(obj.polyData, filename)
else:
app.showErrorMessage('Please select an object that contains geometry data or an OTDF object', title='Invalid object selected')
return
storeDefaultDirectory(filename)
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)
def showToolbarWidget(self):
if app.getMainWindow() is None:
return
if self.toolbarWidget:
self.execButton.setEnabled(True)
return
w = QtGui.QWidget()
l = QtGui.QHBoxLayout(w)
label = QtGui.QLabel('Walk plan:')
execButton = QtGui.QPushButton('')
execButton.setIcon(QtGui.QApplication.style().standardIcon(QtGui.QStyle.SP_MediaPlay))
clearButton = QtGui.QPushButton('')
clearButton.setIcon(QtGui.QApplication.style().standardIcon(QtGui.QStyle.SP_TrashIcon))
stopButton = QtGui.QPushButton('')
stopButton.setIcon(QtGui.QApplication.style().standardIcon(QtGui.QStyle.SP_MediaStop))
l.addWidget(label)
l.addWidget(execButton)
l.addWidget(stopButton)
l.addWidget(clearButton)
l.setContentsMargins(0, 0, 0, 0)
execButton.setShortcut(QtGui.QKeySequence('Ctrl+Return'))
execButton.connect('clicked()', self.onExecClicked)
clearButton.connect('clicked()', self.onClearClicked)
stopButton.connect('clicked()', self.sendStopWalking)
self.execButton = execButton
self.stopButton = stopButton
self.toolbarWidget = app.getMainWindow().toolBar().addWidget(w)
self.execButton.show()
def showToolbarWidget(self):
if app.getMainWindow() is None:
return
if self.toolbarWidget:
self.execButton.setEnabled(True)
return
w = QtGui.QWidget()
l = QtGui.QHBoxLayout(w)
label = QtGui.QLabel('Walk plan:')
execButton = QtGui.QPushButton('')
execButton.setIcon(QtGui.QApplication.style().standardIcon(QtGui.QStyle.SP_MediaPlay))
clearButton = QtGui.QPushButton('')
clearButton.setIcon(QtGui.QApplication.style().standardIcon(QtGui.QStyle.SP_TrashIcon))
stopButton = QtGui.QPushButton('')
stopButton.setIcon(QtGui.QApplication.style().standardIcon(QtGui.QStyle.SP_MediaStop))
l.addWidget(label)
l.addWidget(execButton)
l.addWidget(stopButton)
l.addWidget(clearButton)
l.setContentsMargins(0, 0, 0, 0)
execButton.setShortcut(QtGui.QKeySequence('Ctrl+Return'))
execButton.connect('clicked()', self.onExecClicked)
clearButton.connect('clicked()', self.onClearClicked)
stopButton.connect('clicked()', self.sendStopWalking)
self.execButton = execButton
self.stopButton = stopButton
self.toolbarWidget = app.getMainWindow().toolBar().addWidget(w)
self.execButton.show()
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 onIkStartup(ikServer, startSuccess):
if startSuccess:
app.getMainWindow().statusBar().showMessage(
'Planning server started.', 2000)
else:
app.showErrorMessage(
'Error detected while starting the matlab planning server. '
'Please check the output console for more information.',
title='Error starting matlab')
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 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 init():
mainWindow = app.getMainWindow()
mainWindow.connect('fileOpen()', onFileOpen)
mainWindow.connect('fileSaveData()', onFileSaveData)
mainWindow.connect('fileExportUrdf()', onFileExportUrdf)
mainWindow.connect('openOnlineHelp()', onOpenOnlineHelp)
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 onFileSaveData():
obj = om.getActiveObject()
if not obj:
app.showErrorMessage('Please select an object',
title='No object selected')
return
if isinstance(obj, otdfmodel.OtdfModelItem):
mainWindow = app.getMainWindow()
filename = QtGui.QFileDialog.getSaveFileName(mainWindow,
"Save Data...",
getDefaultDirectory(),
'OTDF (*.otdf)',
'OTDF (*.otdf)')
if not os.path.splitext(filename)[1]:
filename += '.otdf'
storeDefaultDirectory(filename)
otdfString = obj.parser.getUpdatedOtdf()
otdfFile = open(filename, 'w')
otdfFile.write(otdfString)
otdfFile.close()
elif hasattr(obj, 'polyData'):
mainWindow = app.getMainWindow()
fileFilters = "PLY (*.ply);;STL (*.stl);;VTP (*.vtp)"
filename = QtGui.QFileDialog.getSaveFileName(mainWindow,
"Save Data...",
getDefaultDirectory(),
fileFilters,
'VTP (*.vtp)')
if not filename:
return
if not os.path.splitext(filename)[1]:
filename += '.vtp'
polyData = io.writePolyData(obj.polyData, filename)
else:
app.showErrorMessage(
'Please select an object that contains geometry data or an OTDF object',
title='Invalid object selected')
return
storeDefaultDirectory(filename)
def loadFileAsMap(self):
fileFilters = "Map Files (*.obj *.pcd *.ply *.stl *.vtk *.vtp)"
filename = QtGui.QFileDialog.getOpenFileName(
app.getMainWindow(), "Open...",
actionhandlers.getDefaultDirectory(), fileFilters)
if not filename:
return
self.mappingPanel.loadMap(filename)
def addRobotBehaviors(_robotSystem):
global robotSystem, robotModel, handFactory, footstepsDriver, neckDriver, robotLinkSelector
robotSystem = _robotSystem
robotModel = robotSystem.robotStateModel
handFactory = robotSystem.handFactory
footstepsDriver = robotSystem.footstepsDriver
neckDriver = robotSystem.neckDriver
if app.getMainWindow() is not None:
robotLinkSelector = RobotLinkSelector()
def onFileExportUrdf():
obj = om.getActiveObject()
if not obj or not isinstance(obj, otdfmodel.OtdfModelItem):
app.showErrorMessage('Please select an OTDF object', title='OTDF object not selected')
return
mainWindow = app.getMainWindow()
filename = QtGui.QFileDialog.getSaveFileName(mainWindow, "Save Data...", getDefaultDirectory(), 'URDF (*.urdf)', 'URDF (*.urdf)')
if not os.path.splitext(filename)[1]:
filename += '.urdf'
storeDefaultDirectory(filename)
urdfString = obj.parser.getUrdfFromOtdf()
urdfFile = open(filename, 'w')
urdfFile.write(urdfString)
urdfFile.close()
def onFileOpen():
mainWindow = app.getMainWindow()
fileFilters = "Data Files (*.obj *.pcd *.ply *.stl *.vtk *.vtp *.urdf *.otdf)";
filename = QtGui.QFileDialog.getOpenFileName(mainWindow, "Open...", getDefaultDirectory(), fileFilters)
if not filename:
return
storeDefaultDirectory(filename)
if filename.lower().endswith('urdf'):
onOpenUrdf(filename)
elif filename.lower().endswith('otdf'):
onOpenOtdf(filename)
else:
onOpenGeometry(filename)
def onFileOpen():
mainWindow = app.getMainWindow()
fileFilters = "Data Files (*.obj *.pcd *.ply *.stl *.vtk *.vtp *.urdf *.otdf)"
filename = QtGui.QFileDialog.getOpenFileName(mainWindow, "Open...",
getDefaultDirectory(),
fileFilters)
if not filename:
return
storeDefaultDirectory(filename)
if filename.lower().endswith('urdf'):
onOpenUrdf(filename)
elif filename.lower().endswith('otdf'):
onOpenOtdf(filename)
else:
onOpenGeometry(filename)
def onFileExportUrdf():
obj = om.getActiveObject()
if not obj or not isinstance(obj, otdfmodel.OtdfModelItem):
app.showErrorMessage('Please select an OTDF object',
title='OTDF object not selected')
return
mainWindow = app.getMainWindow()
filename = QtGui.QFileDialog.getSaveFileName(mainWindow, "Save Data...",
getDefaultDirectory(),
'URDF (*.urdf)',
'URDF (*.urdf)')
if not os.path.splitext(filename)[1]:
filename += '.urdf'
storeDefaultDirectory(filename)
urdfString = obj.parser.getUrdfFromOtdf()
urdfFile = open(filename, 'w')
urdfFile.write(urdfString)
urdfFile.close()
from ddapp.tasks import taskmanagerwidget from ddapp.tasks.descriptions import loadTaskDescriptions import drc as lcmdrc from collections import OrderedDict import functools import math import numpy as np from ddapp.debugVis import DebugData from ddapp import ioUtils as io drcargs.requireStrict() drcargs.args() app.startup(globals()) om.init(app.getMainWindow().objectTree(), app.getMainWindow().propertiesPanel()) actionhandlers.init() quit = app.quit exit = quit view = app.getDRCView() camera = view.camera() tree = app.getMainWindow().objectTree() orbit = cameracontrol.OrbitController(view) showPolyData = segmentation.showPolyData updatePolyData = segmentation.updatePolyData ###############################################################################
def chooseDirectory(self):
return QtGui.QFileDialog.getExistingDirectory(app.getMainWindow(), "Choose directory...", self.movieOutputDirectory())
def loadFileAsMap(self):
fileFilters = "Map Files (*.obj *.pcd *.ply *.stl *.vtk *.vtp)";
filename = QtGui.QFileDialog.getOpenFileName(app.getMainWindow(), "Open...", actionhandlers.getDefaultDirectory(), fileFilters)
if not filename:
return
self.mappingPanel.loadMap(filename)
from ddapp.tasks import taskmanagerwidget
from ddapp.tasks.descriptions import loadTaskDescriptions
import drc as lcmdrc
from collections import OrderedDict
import functools
import math
import numpy as np
from ddapp.debugVis import DebugData
from ddapp import ioUtils as io
drcargs.requireStrict()
drcargs.args()
app.startup(globals())
om.init(app.getMainWindow().objectTree(),
app.getMainWindow().propertiesPanel())
actionhandlers.init()
quit = app.quit
exit = quit
view = app.getDRCView()
camera = view.camera()
tree = app.getMainWindow().objectTree()
orbit = cameracontrol.OrbitController(view)
showPolyData = segmentation.showPolyData
updatePolyData = segmentation.updatePolyData
###############################################################################
robotSystem = robotsystem.create(view)
def init():
mainWindow = app.getMainWindow()
mainWindow.connect('fileOpen()', onFileOpen)
mainWindow.connect('fileSaveData()', onFileSaveData)
mainWindow.connect('fileExportUrdf()', onFileExportUrdf)
def update(self, unused):
if (om.findObjectByName('measured cop').getProperty('Visible')
and self.robotStateJointController.lastRobotStateMessage):
if self.dialogVisible == False:
self.warningButton.setVisible(True)
app.getMainWindow().statusBar().insertPermanentWidget(
0, self.warningButton)
self.dialogVisible = True
self.leftInContact = self.robotStateJointController.lastRobotStateMessage.force_torque.l_foot_force_z > 500
self.rightInContact = self.robotStateJointController.lastRobotStateMessage.force_torque.r_foot_force_z > 500
if self.rightInContact or self.leftInContact:
lFootFt = [
self.robotStateJointController.lastRobotStateMessage.
force_torque.l_foot_torque_x,
self.robotStateJointController.lastRobotStateMessage.
force_torque.l_foot_torque_y, 0.0, 0.0, 0.0,
self.robotStateJointController.lastRobotStateMessage.
force_torque.l_foot_force_z
]
rFootFt = [
self.robotStateJointController.lastRobotStateMessage.
force_torque.r_foot_torque_x,
self.robotStateJointController.lastRobotStateMessage.
force_torque.r_foot_torque_y, 0.0, 0.0, 0.0,
self.robotStateJointController.lastRobotStateMessage.
force_torque.r_foot_force_z
]
lFootTransform = self.robotStateModel.getLinkFrame(
self.robotSystem.ikPlanner.leftFootLink)
rFootTransform = self.robotStateModel.getLinkFrame(
self.robotSystem.ikPlanner.rightFootLink)
rFootOrigin = np.array(
rFootTransform.TransformPoint([0, 0, -0.07645]))
lFootOrigin = np.array(
lFootTransform.TransformPoint([0, 0,
-0.07645])) # down to sole
measured_cop = self.ddDrakeWrapper.resolveCenterOfPressure(
self.robotStateModel.model,
[self.lFootFtFrameId, self.rFootFtFrameId],
lFootFt + rFootFt, [0., 0., 1.],
(self.rightInContact * rFootOrigin +
self.leftInContact * lFootOrigin) /
(self.leftInContact + self.rightInContact))
allFootContacts = np.empty([0, 2])
if self.rightInContact:
rFootContacts = np.array([
rFootTransform.TransformPoint(contact_point)
for contact_point in self.LONG_FOOT_CONTACT_POINTS
])
allFootContacts = np.concatenate(
(allFootContacts, rFootContacts[:, 0:2]))
if self.leftInContact:
lFootContacts = np.array([
lFootTransform.TransformPoint(contact_point)
for contact_point in self.LONG_FOOT_CONTACT_POINTS
])
allFootContacts = np.concatenate(
(allFootContacts, lFootContacts[:, 0:2]))
num_pts = allFootContacts.shape[0]
dist = self.ddDrakeWrapper.drakeSignedDistanceInsideConvexHull(
num_pts, allFootContacts.reshape(num_pts * 2, 1),
measured_cop[0:2])
inSafeSupportPolygon = dist >= 0
# map dist to color -- green if inside threshold, red if not
dist = min(max(0, dist), self.DESIRED_INTERIOR_DISTANCE
) / self.DESIRED_INTERIOR_DISTANCE
# nonlinear interpolation here to try to maintain saturation
r = int(255. - 255. * dist**4)
g = int(255. * dist**0.25)
b = 0
colorStatus = [r / 255., g / 255., b / 255.]
colorStatusString = 'rgb(%d, %d, %d)' % (r, g, b)
self.warningButton.setStyleSheet("background-color:" +
colorStatusString)
d = DebugData()
d.addSphere(measured_cop[0:3], radius=0.02)
vis.updatePolyData(d.getPolyData(),
'measured cop',
view=self.view,
parent='robot state model').setProperty(
'Color', colorStatus)
elif self.dialogVisible:
app.getMainWindow().statusBar().removeWidget(self.warningButton)
self.dialogVisible = False
def chooseDirectory(self):
return QtGui.QFileDialog.getExistingDirectory(
app.getMainWindow(), "Choose directory...",
self.movieOutputDirectory())
def onIkStartup(ikServer, startSuccess):
if startSuccess:
app.getMainWindow().statusBar().showMessage('Planning server started.', 2000)
else:
app.showErrorMessage('Error detected while starting the matlab planning server. '
'Please check the output console for more information.', title='Error starting matlab')
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)
def update(self, unused):
if (om.findObjectByName('measured cop').getProperty('Visible') and self.robotStateJointController.lastRobotStateMessage):
if self.dialogVisible == False:
self.warningButton.setVisible(True)
app.getMainWindow().statusBar().insertPermanentWidget(0, self.warningButton)
self.dialogVisible = True
self.leftInContact = self.robotStateJointController.lastRobotStateMessage.force_torque.l_foot_force_z > 500
self.rightInContact = self.robotStateJointController.lastRobotStateMessage.force_torque.r_foot_force_z > 500
if self.rightInContact or self.leftInContact:
lFootFt = [self.robotStateJointController.lastRobotStateMessage.force_torque.l_foot_torque_x,
self.robotStateJointController.lastRobotStateMessage.force_torque.l_foot_torque_y,
0.0,
0.0,
0.0,
self.robotStateJointController.lastRobotStateMessage.force_torque.l_foot_force_z]
rFootFt = [self.robotStateJointController.lastRobotStateMessage.force_torque.r_foot_torque_x,
self.robotStateJointController.lastRobotStateMessage.force_torque.r_foot_torque_y,
0.0,
0.0,
0.0,
self.robotStateJointController.lastRobotStateMessage.force_torque.r_foot_force_z]
lFootTransform = self.robotStateModel.getLinkFrame( self.robotSystem.ikPlanner.leftFootLink )
rFootTransform = self.robotStateModel.getLinkFrame( self.robotSystem.ikPlanner.rightFootLink)
rFootOrigin = np.array(rFootTransform.TransformPoint([0, 0, -0.07645]))
lFootOrigin = np.array(lFootTransform.TransformPoint([0, 0, -0.07645])) # down to sole
measured_cop = self.ddDrakeWrapper.resolveCenterOfPressure(self.robotStateModel.model, [self.lFootFtFrameId, self.rFootFtFrameId],
lFootFt + rFootFt, [0., 0., 1.], (self.rightInContact*rFootOrigin+self.leftInContact*lFootOrigin)/(self.leftInContact + self.rightInContact))
allFootContacts = np.empty([0, 2])
if self.rightInContact:
rFootContacts = np.array([rFootTransform.TransformPoint(contact_point) for contact_point in self.LONG_FOOT_CONTACT_POINTS])
allFootContacts = np.concatenate((allFootContacts, rFootContacts[:, 0:2]))
if self.leftInContact:
lFootContacts = np.array([lFootTransform.TransformPoint(contact_point) for contact_point in self.LONG_FOOT_CONTACT_POINTS])
allFootContacts = np.concatenate((allFootContacts, lFootContacts[:, 0:2]))
num_pts = allFootContacts.shape[0]
dist = self.ddDrakeWrapper.drakeSignedDistanceInsideConvexHull(num_pts, allFootContacts.reshape(num_pts*2, 1), measured_cop[0:2])
inSafeSupportPolygon = dist >= 0
# map dist to color -- green if inside threshold, red if not
dist = min(max(0, dist), self.DESIRED_INTERIOR_DISTANCE) / self.DESIRED_INTERIOR_DISTANCE
# nonlinear interpolation here to try to maintain saturation
r = int(255. - 255. * dist**4 )
g = int(255. * dist**0.25 )
b = 0
colorStatus = [r/255., g/255., b/255.]
colorStatusString = 'rgb(%d, %d, %d)' % (r, g, b)
self.warningButton.setStyleSheet("background-color:"+colorStatusString)
d = DebugData()
d.addSphere(measured_cop[0:3], radius=0.02)
vis.updatePolyData(d.getPolyData(), 'measured cop', view=self.view, parent='robot state model').setProperty('Color', colorStatus)
elif self.dialogVisible:
app.getMainWindow().statusBar().removeWidget(self.warningButton)
self.dialogVisible = False