Пример #1
0
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)
Пример #2
0
 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)
Пример #3
0
    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()
Пример #4
0
    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()
Пример #5
0
    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)
Пример #6
0
 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')
Пример #7
0
    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)
Пример #8
0
    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()
Пример #9
0
def init():
    mainWindow = app.getMainWindow()

    mainWindow.connect('fileOpen()', onFileOpen)
    mainWindow.connect('fileSaveData()', onFileSaveData)
    mainWindow.connect('fileExportUrdf()', onFileExportUrdf)
    mainWindow.connect('openOnlineHelp()', onOpenOnlineHelp)
Пример #10
0
    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()
Пример #11
0
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)
Пример #12
0
 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)
Пример #13
0
 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()
Пример #14
0
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()
Пример #15
0
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)
Пример #16
0
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)
Пример #17
0
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()
Пример #18
0
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


###############################################################################

Пример #19
0
 def chooseDirectory(self):
     return QtGui.QFileDialog.getExistingDirectory(app.getMainWindow(), "Choose directory...", self.movieOutputDirectory())
Пример #20
0
 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)
Пример #21
0
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)
Пример #22
0
def init():
    mainWindow = app.getMainWindow()

    mainWindow.connect('fileOpen()', onFileOpen)
    mainWindow.connect('fileSaveData()', onFileSaveData)
    mainWindow.connect('fileExportUrdf()', onFileExportUrdf)
Пример #23
0
    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
Пример #24
0
 def chooseDirectory(self):
     return QtGui.QFileDialog.getExistingDirectory(
         app.getMainWindow(), "Choose directory...",
         self.movieOutputDirectory())
Пример #25
0
 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')
Пример #26
0
 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)
Пример #27
0
    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