Esempio n. 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)
Esempio n. 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)
Esempio n. 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()
Esempio n. 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()
Esempio n. 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)
Esempio n. 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')
Esempio n. 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)
Esempio n. 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()
Esempio n. 9
0
def init():
    mainWindow = app.getMainWindow()

    mainWindow.connect('fileOpen()', onFileOpen)
    mainWindow.connect('fileSaveData()', onFileSaveData)
    mainWindow.connect('fileExportUrdf()', onFileExportUrdf)
    mainWindow.connect('openOnlineHelp()', onOpenOnlineHelp)
Esempio n. 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()
Esempio n. 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)
Esempio n. 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)
Esempio n. 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()
Esempio n. 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()
Esempio n. 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)
Esempio n. 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)
Esempio n. 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()
Esempio n. 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


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

Esempio n. 19
0
 def chooseDirectory(self):
     return QtGui.QFileDialog.getExistingDirectory(app.getMainWindow(), "Choose directory...", self.movieOutputDirectory())
Esempio n. 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)
Esempio n. 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)
Esempio n. 22
0
def init():
    mainWindow = app.getMainWindow()

    mainWindow.connect('fileOpen()', onFileOpen)
    mainWindow.connect('fileSaveData()', onFileSaveData)
    mainWindow.connect('fileExportUrdf()', onFileExportUrdf)
Esempio n. 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
Esempio n. 24
0
 def chooseDirectory(self):
     return QtGui.QFileDialog.getExistingDirectory(
         app.getMainWindow(), "Choose directory...",
         self.movieOutputDirectory())
Esempio n. 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')
Esempio n. 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)
Esempio n. 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