Example #1
0
class StateListener(object):
    def __init__(self):
        self.subscriber = None
        self.transforms = []
        self.timer = SimpleTimer()
        self.paused = True

    def onBDIPose(self, m):
        t = transformUtils.transformFromPose(m.pos, m.orientation)
        if self.timer.elapsed() > 1.0:
            self.transforms.append((self.timer.now(), t))
            self.timer.reset()

    def describe(self):

        print '----------'
        print '%d transforms' % len(self.transforms)
        if not self.transforms:
            return

        time0, transform0 = self.transforms[0]
        o0 = np.array(transform0.GetOrientation())
        p0 = np.array(transform0.GetPosition())
        for timeN, transformN in self.transforms:
            oN = np.array(transformN.GetOrientation())
            pN = np.array(transformN.GetPosition())
            oD = oN - o0
            pD = pN - p0
            print '%.2f: [%.3f, %.3f, %.3f]  [%.3f, %.3f, %.3f]  ' % (
                (timeN - time0), oD[0], oD[1], oD[2], pD[0], pD[1], pD[2])

    def init(self):
        self.subscriber = lcmUtils.addSubscriber('POSE_BODY',
                                                 lcmbotcore.pose_t,
                                                 self.onBDIPose)
Example #2
0
    def __init__(self, planPlayback, playbackRobotModel,
                 playbackJointController, robotStateModel,
                 robotStateJointController, manipPlanner):

        self.planPlayback = planPlayback
        self.playbackRobotModel = playbackRobotModel
        self.playbackJointController = playbackJointController
        self.robotStateModel = robotStateModel
        self.robotStateJointController = robotStateJointController
        self.manipPlanner = manipPlanner
        manipPlanner.connectPlanCommitted(self.onPlanCommitted)
        manipPlanner.connectUseSupports(self.updateButtonColor)

        self.autoPlay = True
        self.useOperationColors()

        self.planFramesObj = None
        self.plan = None
        self.poseInterpolator = None
        self.startTime = 0.0
        self.endTime = 1.0
        self.animationTimer = TimerCallback()
        self.animationTimer.targetFps = 60
        self.animationTimer.callback = self.updateAnimation
        self.animationClock = SimpleTimer()

        loader = QtUiTools.QUiLoader()
        uifile = QtCore.QFile(':/ui/ddPlaybackPanel.ui')
        assert uifile.open(uifile.ReadOnly)

        self.widget = loader.load(uifile)
        uifile.close()

        self.ui = WidgetDict(self.widget.children())

        self.ui.viewModeCombo.connect('currentIndexChanged(const QString&)',
                                      self.viewModeChanged)
        self.ui.playbackSpeedCombo.connect(
            'currentIndexChanged(const QString&)', self.playbackSpeedChanged)
        self.ui.interpolationCombo.connect(
            'currentIndexChanged(const QString&)', self.interpolationChanged)

        self.ui.samplesSpinBox.connect('valueChanged(int)',
                                       self.numberOfSamplesChanged)
        self.ui.playbackSlider.connect('valueChanged(int)',
                                       self.playbackSliderValueChanged)

        self.ui.animateButton.connect('clicked()', self.animateClicked)
        self.ui.hideButton.connect('clicked()', self.hideClicked)
        self.ui.executeButton.connect('clicked()', self.executeClicked)
        self.ui.executeButton.setShortcut(QtGui.QKeySequence('Ctrl+Return'))
        self.ui.stopButton.connect('clicked()', self.stopClicked)

        self.ui.executeButton.setContextMenuPolicy(QtCore.Qt.CustomContextMenu)
        self.ui.executeButton.connect(
            'customContextMenuRequested(const QPoint&)',
            self.showExecuteContextMenu)

        self.setPlan(None)
        self.hideClicked()
Example #3
0
    def waitForResult(self, timeout=None):

        t = SimpleTimer()

        while self.isAlive():

            result = self.checkForResult()
            if result is not None:
                return result

            if timeout is not None and t.elapsed() > timeout:
                return None
Example #4
0
    def waitForResult(self, timeout=None):

        t = SimpleTimer()

        while self.isAlive():

            result = self.checkForResult()
            if result is not None:
                return result

            if timeout is not None and t.elapsed() > timeout:
                return None
Example #5
0
    def run(self):

        delayTime = self.properties.getProperty('Delay time')
        t = SimpleTimer()

        while True:

            elapsed = t.elapsed()
            if elapsed >= delayTime:
                break

            self.statusMessage = 'Waiting %.1f seconds' % (delayTime - elapsed)
            yield
Example #6
0
    def run(self):

        delayTime = self.properties.getProperty('Delay time')
        t = SimpleTimer()

        while True:

            elapsed = t.elapsed()
            if elapsed >= delayTime:
                break

            self.statusMessage = 'Waiting %.1f seconds' % (delayTime - elapsed)
            yield
Example #7
0
    def playPoses(self, poseTimes, poses, jointController):

        f = self.getPoseInterpolator(poseTimes, poses)

        timer = SimpleTimer()

        def updateAnimation():

            tNow = timer.elapsed() * self.playbackSpeed

            if tNow > poseTimes[-1]:
                pose = poses[-1]
                jointController.setPose('plan_playback', pose)

                if self.animationCallback:
                    self.animationCallback()

                return False

            pose = f(tNow)
            jointController.setPose('plan_playback', pose)

            if self.animationCallback:
                self.animationCallback()

        self.animationTimer = TimerCallback()
        self.animationTimer.targetFps = 60
        self.animationTimer.callback = updateAnimation
        self.animationTimer.start()
        updateAnimation()
Example #8
0
    def run(self):
        def getMsg():
            return robotSystem.atlasDriver.lastControllerStatusMessage

        def isExecuting():
            return getMsg(
            ).execution_status == lcmdrc.plan_status_t.EXECUTION_STATUS_EXECUTING

        # wait for first status message
        while not getMsg():
            yield

        if isExecuting():
            raise Exception(
                'error, invoked during plan execution and cannot guarantee safety.'
            )

        t = SimpleTimer()
        lastPlanStartTime = getMsg().last_plan_start_utime

        # wait for next plan to begin
        self.statusMessage = 'Waiting for %s to begin...' % self.getTypeLabel()
        while getMsg().last_plan_start_utime == lastPlanStartTime:

            if t.elapsed() > self.properties.getProperty('Timeout'):
                yield self.promptUserForPlanRecommit()
                t.reset()
                self.recommitPlan()
            else:
                yield

        # wait for execution
        self.statusMessage = 'Waiting for %s execution...' % self.getTypeLabel(
        )
        while getMsg(
        ).execution_status == lcmdrc.plan_status_t.EXECUTION_STATUS_EXECUTING:
            if getMsg().plan_type != self.getType():
                raise Exception('error, unexpected execution plan type: %s' %
                                getMsg().plan_type)
            yield

        self.statusMessage = 'Waiting for recent robot state...'
        while robotSystem.robotStateJointController.lastRobotStateMessage.utime < getMsg(
        ).last_plan_start_utime:
            yield
Example #9
0
class JointControlTestRamp(TimerCallback):

    def __init__(self, jointController):
        TimerCallback.__init__(self)
        self.controller = jointController
        self.testTime = 2.0

    def testJoint(self, jointId):
        self.jointId = jointId
        self.testTimer = SimpleTimer()
        self.start()

    def tick(self):

        if self.testTimer.elapsed() > self.testTime:
            self.stop()
            return

        jointPosition = math.sin( (self.testTimer.elapsed() / self.testTime) * math.pi) * math.pi
        self.controller.setJointPosition(self.jointId, math.degrees(jointPosition))
Example #10
0
class JointControlTestRamp(TimerCallback):
    def __init__(self, jointController):
        TimerCallback.__init__(self)
        self.controller = jointController
        self.testTime = 2.0

    def testJoint(self, jointId):
        self.jointId = jointId
        self.testTimer = SimpleTimer()
        self.start()

    def tick(self):

        if self.testTimer.elapsed() > self.testTime:
            self.stop()
            return

        jointPosition = math.sin(
            (self.testTimer.elapsed() / self.testTime) * math.pi) * math.pi
        self.controller.setJointPosition(self.jointId,
                                         math.degrees(jointPosition))
Example #11
0
class StateListener(object):
    def __init__(self):
        self.subscriber = None
        self.transforms = []
        self.timer = SimpleTimer()
        self.paused = True

    def onBDIPose(self, m):
        t = transformUtils.transformFromPose(m.pos, m.orientation)
        if self.timer.elapsed() > 1.0:
            self.transforms.append((self.timer.now(), t))
            self.timer.reset()

    def describe(self):

        print "----------"
        print "%d transforms" % len(self.transforms)
        if not self.transforms:
            return

        time0, transform0 = self.transforms[0]
        o0 = np.array(transform0.GetOrientation())
        p0 = np.array(transform0.GetPosition())
        for timeN, transformN in self.transforms:
            oN = np.array(transformN.GetOrientation())
            pN = np.array(transformN.GetPosition())
            oD = oN - o0
            pD = pN - p0
            print "%.2f: [%.3f, %.3f, %.3f]  [%.3f, %.3f, %.3f]  " % (
                (timeN - time0),
                oD[0],
                oD[1],
                oD[2],
                pD[0],
                pD[1],
                pD[2],
            )

    def init(self):
        self.subscriber = lcmUtils.addSubscriber("POSE_BODY", lcmbotcore.pose_t, self.onBDIPose)
Example #12
0
    def __init__(self, planPlayback, playbackRobotModel, playbackJointController, robotStateModel, robotStateJointController, manipPlanner):

        self.planPlayback = planPlayback
        self.playbackRobotModel = playbackRobotModel
        self.playbackJointController = playbackJointController
        self.robotStateModel = robotStateModel
        self.robotStateJointController = robotStateJointController
        self.manipPlanner = manipPlanner
        manipPlanner.connectPlanCommitted(self.onPlanCommitted)
        manipPlanner.connectUseSupports(self.updateButtonColor)

        self.autoPlay = True
        self.useOperationColors()

        self.planFramesObj = None
        self.plan = None
        self.poseInterpolator = None
        self.startTime = 0.0
        self.endTime = 1.0
        self.animationTimer = TimerCallback()
        self.animationTimer.targetFps = 60
        self.animationTimer.callback = self.updateAnimation
        self.animationClock = SimpleTimer()

        loader = QtUiTools.QUiLoader()
        uifile = QtCore.QFile(':/ui/ddPlaybackPanel.ui')
        assert uifile.open(uifile.ReadOnly)

        self.widget = loader.load(uifile)
        uifile.close()

        self.ui = WidgetDict(self.widget.children())

        self.ui.viewModeCombo.connect('currentIndexChanged(const QString&)', self.viewModeChanged)
        self.ui.playbackSpeedCombo.connect('currentIndexChanged(const QString&)', self.playbackSpeedChanged)
        self.ui.interpolationCombo.connect('currentIndexChanged(const QString&)', self.interpolationChanged)

        self.ui.samplesSpinBox.connect('valueChanged(int)', self.numberOfSamplesChanged)
        self.ui.playbackSlider.connect('valueChanged(int)', self.playbackSliderValueChanged)

        self.ui.animateButton.connect('clicked()', self.animateClicked)
        self.ui.hideButton.connect('clicked()', self.hideClicked)
        self.ui.executeButton.connect('clicked()', self.executeClicked)
        self.ui.executeButton.setShortcut(QtGui.QKeySequence('Ctrl+Return'))
        self.ui.stopButton.connect('clicked()', self.stopClicked)

        self.ui.executeButton.setContextMenuPolicy(QtCore.Qt.CustomContextMenu)
        self.ui.executeButton.connect('customContextMenuRequested(const QPoint&)', self.showExecuteContextMenu)


        self.setPlan(None)
        self.hideClicked()
Example #13
0
    def run(self):

        def getMsg():
            return robotSystem.atlasDriver.lastControllerStatusMessage

        def isExecuting():
            return getMsg().execution_status == lcmdrc.plan_status_t.EXECUTION_STATUS_EXECUTING

        # wait for first status message
        while not getMsg():
            yield

        if isExecuting():
            raise Exception('error, invoked during plan execution and cannot guarantee safety.')

        t = SimpleTimer()
        lastPlanStartTime = getMsg().last_plan_start_utime

        # wait for next plan to begin
        self.statusMessage = 'Waiting for %s to begin...' % self.getTypeLabel()
        while getMsg().last_plan_start_utime == lastPlanStartTime:

            if t.elapsed() > self.properties.getProperty('Timeout'):
                yield self.promptUserForPlanRecommit()
                t.reset()
                self.recommitPlan()
            else:
                yield

        # wait for execution
        self.statusMessage = 'Waiting for %s execution...' % self.getTypeLabel()
        while getMsg().execution_status == lcmdrc.plan_status_t.EXECUTION_STATUS_EXECUTING:
            if getMsg().plan_type != self.getType():
                raise Exception('error, unexpected execution plan type: %s' % getMsg().plan_type)
            yield

        self.statusMessage = 'Waiting for recent robot state...'
        while robotSystem.robotStateJointController.lastRobotStateMessage.utime < getMsg().last_plan_start_utime:
            yield
Example #14
0
    def __init__(self):

        self.lastAtlasStatusMessage = None
        self.lastControllerStatusMessage = None
        self.lastAtlasBatteryDataMessage = None
        self.lastAtlasElectricArmStatusMessage = None
        self.lastControllerRateMessage = None
        self.maxPressureHistory = deque([0.0], 10)
        self.averageRecentMaxPressure = 0.0
        self._setupSubscriptions()
        self.timer = SimpleTimer()

        self.sentStandUtime = None
        self.startupStage = 0
        self._behaviorMap = None
        self._controllerStatusMap = None
Example #15
0
    def updateState(self):

        t = SimpleTimer()
        self.manager.updateExistingLoggerProcesses()

        activeLogFiles = self.manager.getActiveLogFilenames()
        self.numProcesses = len(self.manager.getActiveLoggerPids())
        self.numLogFiles = len(activeLogFiles)

        if self.numLogFiles == 1:
            self.lastActiveLogFile = activeLogFiles[0]

        if self.numProcesses == 0:
            self.button.text = 'start logger'
        elif self.numProcesses == 1:
            self.button.text = 'stop logger'
        elif self.numProcesses > 1:
            self.button.text = 'stop all loggers'

        statusDescription = 'active' if self.numProcesses else 'last'
        logFileDescription = self.lastActiveLogFile or '<unknown>'
        self.button.setToolTip('%s log file: %s' %
                               (statusDescription, logFileDescription))
Example #16
0
    def __call__(self):

        t = SimpleTimer()
        while t.elapsed() < self.delayTimeInSeconds:
            yield
Example #17
0
 def __init__(self):
     self.subscriber = None
     self.transforms = []
     self.timer = SimpleTimer()
     self.paused = True
Example #18
0
class PlaybackPanel(object):

    def __init__(self, planPlayback, playbackRobotModel, playbackJointController, robotStateModel, robotStateJointController, manipPlanner):

        self.planPlayback = planPlayback
        self.playbackRobotModel = playbackRobotModel
        self.playbackJointController = playbackJointController
        self.robotStateModel = robotStateModel
        self.robotStateJointController = robotStateJointController
        self.manipPlanner = manipPlanner
        manipPlanner.connectPlanCommitted(self.onPlanCommitted)
        manipPlanner.connectUseSupports(self.updateButtonColor)

        self.autoPlay = True
        self.useOperationColors()

        self.planFramesObj = None
        self.plan = None
        self.poseInterpolator = None
        self.startTime = 0.0
        self.endTime = 1.0
        self.animationTimer = TimerCallback()
        self.animationTimer.targetFps = 60
        self.animationTimer.callback = self.updateAnimation
        self.animationClock = SimpleTimer()

        loader = QtUiTools.QUiLoader()
        uifile = QtCore.QFile(':/ui/ddPlaybackPanel.ui')
        assert uifile.open(uifile.ReadOnly)

        self.widget = loader.load(uifile)
        uifile.close()

        self.ui = WidgetDict(self.widget.children())

        self.ui.viewModeCombo.connect('currentIndexChanged(const QString&)', self.viewModeChanged)
        self.ui.playbackSpeedCombo.connect('currentIndexChanged(const QString&)', self.playbackSpeedChanged)
        self.ui.interpolationCombo.connect('currentIndexChanged(const QString&)', self.interpolationChanged)

        self.ui.samplesSpinBox.connect('valueChanged(int)', self.numberOfSamplesChanged)
        self.ui.playbackSlider.connect('valueChanged(int)', self.playbackSliderValueChanged)

        self.ui.animateButton.connect('clicked()', self.animateClicked)
        self.ui.hideButton.connect('clicked()', self.hideClicked)
        self.ui.executeButton.connect('clicked()', self.executeClicked)
        self.ui.executeButton.setShortcut(QtGui.QKeySequence('Ctrl+Return'))
        self.ui.stopButton.connect('clicked()', self.stopClicked)

        self.ui.executeButton.setContextMenuPolicy(QtCore.Qt.CustomContextMenu)
        self.ui.executeButton.connect('customContextMenuRequested(const QPoint&)', self.showExecuteContextMenu)


        self.setPlan(None)
        self.hideClicked()


    def useDevelopmentColors(self):
        self.robotStateModelDisplayAlpha = 0.1
        self.playbackRobotModelUseTextures = True
        self.playbackRobotModelDisplayAlpha = 1

    def useOperationColors(self):
        self.robotStateModelDisplayAlpha = 1
        self.playbackRobotModelUseTextures = False
        self.playbackRobotModelDisplayAlpha = 0.5
    def showExecuteContextMenu(self, clickPosition):

        globalPos = self.ui.executeButton.mapToGlobal(clickPosition)

        menu = QtGui.QMenu()
        menu.addAction('Visualization Only')

        if not self.isPlanFeasible():
            menu.addSeparator()
            if self.isPlanAPlanWithSupports():
                menu.addAction('Execute infeasible plan with supports')
            else:
                menu.addAction('Execute infeasible plan')
        elif self.isPlanAPlanWithSupports():
            menu.addSeparator()
            menu.addAction('Execute plan with supports')

        selectedAction = menu.exec_(globalPos)
        if not selectedAction:
            return

        if selectedAction.text == 'Visualization Only':
            self.executePlan(visOnly=True)
        elif selectedAction.text == 'Execute infeasible plan':
            self.executePlan(overrideInfeasibleCheck=True)
        elif selectedAction.text == 'Execute plan with supports':
            self.executePlan(overrideSupportsCheck=True)
        elif selectedAction.text == 'Execute infeasible plan with supports':
            self.executePlan(overrideInfeasibleCheck=True, overrideSupportsCheck=True)


    def getViewMode(self):
        return str(self.ui.viewModeCombo.currentText)

    def setViewMode(self, mode):
        '''
        Set the mode of the view widget. input arg: 'continous', 'frames', 'hidden'
        e.g. can hide all plan playback with 'hidden'
        '''
        self.ui.viewModeCombo.setCurrentIndex(self.ui.viewModeCombo.findText(mode))

    def getPlaybackSpeed(self):
      s = str(self.ui.playbackSpeedCombo.currentText).replace('x', '')
      if '/' in s:
          n, d = s.split('/')
          return float(n)/float(d)
      return float(s)

    def getInterpolationMethod(self):
        return str(self.ui.interpolationCombo.currentText)

    def getNumberOfSamples(self):
        return self.ui.samplesSpinBox.value

    def viewModeChanged(self):
        viewMode = self.getViewMode()
        if viewMode == 'continuous':
            playbackVisible = True
            samplesVisible = False
        else:
            playbackVisible = False
            samplesVisible = True

        self.ui.samplesLabel.setEnabled(samplesVisible)
        self.ui.samplesSpinBox.setEnabled(samplesVisible)

        self.ui.playbackSpeedLabel.setVisible(playbackVisible)
        self.ui.playbackSpeedCombo.setVisible(playbackVisible)
        self.ui.playbackSlider.setEnabled(playbackVisible)
        self.ui.animateButton.setVisible(playbackVisible)
        self.ui.timeLabel.setVisible(playbackVisible)

        if self.plan:

            if self.getViewMode() == 'continuous' and self.autoPlay:
                self.startAnimation()
            else:
                self.ui.playbackSlider.value = 0
                self.stopAnimation()
                self.updatePlanFrames()


    def playbackSpeedChanged(self):
        self.planPlayback.playbackSpeed = self.getPlaybackSpeed()


    def getPlaybackTime(self):
        sliderValue = self.ui.playbackSlider.value
        return (sliderValue / 1000.0) * self.endTime

    def updateTimeLabel(self):
        playbackTime = self.getPlaybackTime()
        self.ui.timeLabel.text = 'Time: %.2f s' % playbackTime

    def playbackSliderValueChanged(self):
        self.updateTimeLabel()
        self.showPoseAtTime(self.getPlaybackTime())

    def interpolationChanged(self):

        methods = {'linear' : 'slinear',
                   'cubic spline' : 'cubic',
                   'pchip' : 'pchip' }
        self.planPlayback.interpolationMethod = methods[self.getInterpolationMethod()]
        self.poseInterpolator = self.planPlayback.getPoseInterpolatorFromPlan(self.plan)
        self.updatePlanFrames()

    def numberOfSamplesChanged(self):
        self.updatePlanFrames()

    def animateClicked(self):
        self.startAnimation()

    def hideClicked(self):

        if self.ui.hideButton.text == 'hide':
            self.ui.playbackFrame.setEnabled(False)
            self.hidePlan()
            self.ui.hideButton.text = 'show'
            self.ui.executeButton.setEnabled(False)

            if not self.plan:
                self.ui.hideButton.setEnabled(False)
        else:
            self.ui.playbackFrame.setEnabled(True)
            self.ui.hideButton.text = 'hide'
            self.ui.hideButton.setEnabled(True)
            self.ui.executeButton.setEnabled(True)

            self.viewModeChanged()

        self.updateButtonColor()


    def executeClicked(self):
        self.executePlan()


    def executePlan(self, visOnly=False, overrideInfeasibleCheck=False, overrideSupportsCheck=False):
        if visOnly:
            _, poses = self.planPlayback.getPlanPoses(self.plan)
            self.onPlanCommitted(self.plan)
            self.robotStateJointController.setPose('EST_ROBOT_STATE', poses[-1])
        else:
            if (self.isPlanFeasible() or overrideInfeasibleCheck) and (not self.isPlanAPlanWithSupports() or overrideSupportsCheck):
                self.manipPlanner.commitManipPlan(self.plan)


    def onPlanCommitted(self, plan):
        self.setPlan(None)
        self.hideClicked()


    def stopClicked(self):
        self.stopAnimation()
        self.manipPlanner.sendPlanPause()


    def isPlanFeasible(self):
        plan = robotstate.asRobotPlan(self.plan)
        return plan is not None and max(plan.plan_info) < 10

    def getPlanInfo(self, plan):
        plan = robotstate.asRobotPlan(self.plan)
        return max(plan.plan_info)


    def isPlanAPlanWithSupports(self):
        return hasattr(self.plan, 'support_sequence') or self.manipPlanner.publishPlansWithSupports

    def updatePlanFrames(self):

        if self.getViewMode() != 'frames':
            return

        numberOfSamples = self.getNumberOfSamples()

        meshes = self.planPlayback.getPlanPoseMeshes(self.plan, self.playbackJointController, self.playbackRobotModel, numberOfSamples)
        d = DebugData()

        startColor = [0.8, 0.8, 0.8]
        endColor = [85/255.0, 255/255.0, 255/255.0]
        colorFunc = scipy.interpolate.interp1d([0, numberOfSamples-1], [startColor, endColor], axis=0, kind='slinear')

        for i, mesh in reversed(list(enumerate(meshes))):
            d.addPolyData(mesh, color=colorFunc(i))

        pd = d.getPolyData()
        clean = vtk.vtkCleanPolyData()
        clean.SetInput(pd)
        clean.Update()
        pd = clean.GetOutput()

        self.planFramesObj = vis.updatePolyData(d.getPolyData(), 'robot plan', alpha=1.0, visible=False, colorByName='RGB255', parent='planning')
        self.showPlanFrames()


    def showPlanFrames(self):
        self.planFramesObj.setProperty('Visible', True)
        self.robotStateModel.setProperty('Visible', False)
        self.playbackRobotModel.setProperty('Visible', False)


    def startAnimation(self):
        self.showPlaybackModel()
        self.stopAnimation()
        self.ui.playbackSlider.value = 0
        self.animationClock.reset()
        self.animationTimer.start()
        self.updateAnimation()


    def stopAnimation(self):
        self.animationTimer.stop()

    def showPlaybackModel(self):
        self.robotStateModel.setProperty('Visible', True)
        self.playbackRobotModel.setProperty('Visible', True)
        self.playbackRobotModel.setProperty('Textures', self.playbackRobotModelUseTextures)
        self.robotStateModel.setProperty('Alpha', self.robotStateModelDisplayAlpha)
        self.playbackRobotModel.setProperty('Alpha', self.playbackRobotModelDisplayAlpha)
        if self.planFramesObj:
            self.planFramesObj.setProperty('Visible', False)

    def hidePlan(self):
        self.stopAnimation()
        if self.planFramesObj:
            self.planFramesObj.setProperty('Visible', False)
        if self.playbackRobotModel:
            self.playbackRobotModel.setProperty('Visible', False)

        self.robotStateModel.setProperty('Visible', True)
        self.robotStateModel.setProperty('Alpha', 1.0)


    def showPoseAtTime(self, time):
        pose = self.poseInterpolator(time)
        self.playbackJointController.setPose('plan_playback', pose)


    def updateAnimation(self):

        tNow = self.animationClock.elapsed() * self.planPlayback.playbackSpeed
        if tNow > self.endTime:
            tNow = self.endTime

        sliderValue = int(1000.0 * tNow / self.endTime)

        self.ui.playbackSlider.blockSignals(True)
        self.ui.playbackSlider.value = sliderValue
        self.ui.playbackSlider.blockSignals(False)
        self.updateTimeLabel()

        self.showPoseAtTime(tNow)
        return tNow < self.endTime


    def updateButtonColor(self):
        if self.ui.executeButton.enabled and self.plan and not self.isPlanFeasible():
            styleSheet = 'background-color:red'
        elif self.ui.executeButton.enabled and self.plan and self.isPlanAPlanWithSupports():
            styleSheet = 'background-color:orange'
        else:
            styleSheet = ''

        self.ui.executeButton.setStyleSheet(styleSheet)


    def setPlan(self, plan):

        self.ui.playbackSlider.value = 0
        self.ui.timeLabel.text = 'Time: 0.00 s'
        self.ui.planNameLabel.text = ''
        self.plan = plan
        self.endTime = 1.0
        self.updateButtonColor()

        if not self.plan:
            return

        planText = 'Plan: %d.  %.2f seconds' % (plan.utime, self.planPlayback.getPlanElapsedTime(plan))
        self.ui.planNameLabel.text = planText

        self.startTime = 0.0
        self.endTime = self.planPlayback.getPlanElapsedTime(plan)
        self.interpolationChanged()
        info = self.getPlanInfo(plan)
        app.displaySnoptInfo(info)

        if self.ui.hideButton.text == 'show':
            self.hideClicked()
        else:
            self.viewModeChanged()

        self.updateButtonColor()

        if self.autoPlay and self.widget.parent() is not None:
            self.widget.parent().show()
Example #19
0
 def testJoint(self, jointId):
     self.jointId = jointId
     self.testTimer = SimpleTimer()
     self.start()
Example #20
0
class PlaybackPanel(object):
    def __init__(self, planPlayback, playbackRobotModel,
                 playbackJointController, robotStateModel,
                 robotStateJointController, manipPlanner):

        self.planPlayback = planPlayback
        self.playbackRobotModel = playbackRobotModel
        self.playbackJointController = playbackJointController
        self.robotStateModel = robotStateModel
        self.robotStateJointController = robotStateJointController
        self.manipPlanner = manipPlanner
        manipPlanner.connectPlanCommitted(self.onPlanCommitted)
        manipPlanner.connectUseSupports(self.updateButtonColor)

        self.autoPlay = True
        self.useOperationColors()

        self.planFramesObj = None
        self.plan = None
        self.poseInterpolator = None
        self.startTime = 0.0
        self.endTime = 1.0
        self.animationTimer = TimerCallback()
        self.animationTimer.targetFps = 60
        self.animationTimer.callback = self.updateAnimation
        self.animationClock = SimpleTimer()

        loader = QtUiTools.QUiLoader()
        uifile = QtCore.QFile(':/ui/ddPlaybackPanel.ui')
        assert uifile.open(uifile.ReadOnly)

        self.widget = loader.load(uifile)
        uifile.close()

        self.ui = WidgetDict(self.widget.children())

        self.ui.viewModeCombo.connect('currentIndexChanged(const QString&)',
                                      self.viewModeChanged)
        self.ui.playbackSpeedCombo.connect(
            'currentIndexChanged(const QString&)', self.playbackSpeedChanged)
        self.ui.interpolationCombo.connect(
            'currentIndexChanged(const QString&)', self.interpolationChanged)

        self.ui.samplesSpinBox.connect('valueChanged(int)',
                                       self.numberOfSamplesChanged)
        self.ui.playbackSlider.connect('valueChanged(int)',
                                       self.playbackSliderValueChanged)

        self.ui.animateButton.connect('clicked()', self.animateClicked)
        self.ui.hideButton.connect('clicked()', self.hideClicked)
        self.ui.executeButton.connect('clicked()', self.executeClicked)
        self.ui.executeButton.setShortcut(QtGui.QKeySequence('Ctrl+Return'))
        self.ui.stopButton.connect('clicked()', self.stopClicked)

        self.ui.executeButton.setContextMenuPolicy(QtCore.Qt.CustomContextMenu)
        self.ui.executeButton.connect(
            'customContextMenuRequested(const QPoint&)',
            self.showExecuteContextMenu)

        self.setPlan(None)
        self.hideClicked()

    def useDevelopmentColors(self):
        self.robotStateModelDisplayAlpha = 0.1
        self.playbackRobotModelUseTextures = True
        self.playbackRobotModelDisplayAlpha = 1

    def useOperationColors(self):
        self.robotStateModelDisplayAlpha = 1
        self.playbackRobotModelUseTextures = False
        self.playbackRobotModelDisplayAlpha = 0.5

    def showExecuteContextMenu(self, clickPosition):

        globalPos = self.ui.executeButton.mapToGlobal(clickPosition)

        menu = QtGui.QMenu()
        menu.addAction('Visualization Only')

        if not self.isPlanFeasible():
            menu.addSeparator()
            if self.isPlanAPlanWithSupports():
                menu.addAction('Execute infeasible plan with supports')
            else:
                menu.addAction('Execute infeasible plan')
        elif self.isPlanAPlanWithSupports():
            menu.addSeparator()
            menu.addAction('Execute plan with supports')

        selectedAction = menu.exec_(globalPos)
        if not selectedAction:
            return

        if selectedAction.text == 'Visualization Only':
            self.executePlan(visOnly=True)
        elif selectedAction.text == 'Execute infeasible plan':
            self.executePlan(overrideInfeasibleCheck=True)
        elif selectedAction.text == 'Execute plan with supports':
            self.executePlan(overrideSupportsCheck=True)
        elif selectedAction.text == 'Execute infeasible plan with supports':
            self.executePlan(overrideInfeasibleCheck=True,
                             overrideSupportsCheck=True)

    def getViewMode(self):
        return str(self.ui.viewModeCombo.currentText)

    def setViewMode(self, mode):
        '''
        Set the mode of the view widget. input arg: 'continous', 'frames', 'hidden'
        e.g. can hide all plan playback with 'hidden'
        '''
        self.ui.viewModeCombo.setCurrentIndex(
            self.ui.viewModeCombo.findText(mode))

    def getPlaybackSpeed(self):
        s = str(self.ui.playbackSpeedCombo.currentText).replace('x', '')
        if '/' in s:
            n, d = s.split('/')
            return float(n) / float(d)
        return float(s)

    def getInterpolationMethod(self):
        return str(self.ui.interpolationCombo.currentText)

    def getNumberOfSamples(self):
        return self.ui.samplesSpinBox.value

    def viewModeChanged(self):
        viewMode = self.getViewMode()
        if viewMode == 'continuous':
            playbackVisible = True
            samplesVisible = False
        else:
            playbackVisible = False
            samplesVisible = True

        self.ui.samplesLabel.setEnabled(samplesVisible)
        self.ui.samplesSpinBox.setEnabled(samplesVisible)

        self.ui.playbackSpeedLabel.setVisible(playbackVisible)
        self.ui.playbackSpeedCombo.setVisible(playbackVisible)
        self.ui.playbackSlider.setEnabled(playbackVisible)
        self.ui.animateButton.setVisible(playbackVisible)
        self.ui.timeLabel.setVisible(playbackVisible)

        if self.plan:

            if self.getViewMode() == 'continuous' and self.autoPlay:
                self.startAnimation()
            else:
                self.ui.playbackSlider.value = 0
                self.stopAnimation()
                self.updatePlanFrames()

    def playbackSpeedChanged(self):
        self.planPlayback.playbackSpeed = self.getPlaybackSpeed()

    def getPlaybackTime(self):
        sliderValue = self.ui.playbackSlider.value
        return (sliderValue / 1000.0) * self.endTime

    def updateTimeLabel(self):
        playbackTime = self.getPlaybackTime()
        self.ui.timeLabel.text = 'Time: %.2f s' % playbackTime

    def playbackSliderValueChanged(self):
        self.updateTimeLabel()
        self.showPoseAtTime(self.getPlaybackTime())

    def interpolationChanged(self):

        methods = {
            'linear': 'slinear',
            'cubic spline': 'cubic',
            'pchip': 'pchip'
        }
        self.planPlayback.interpolationMethod = methods[
            self.getInterpolationMethod()]
        self.poseInterpolator = self.planPlayback.getPoseInterpolatorFromPlan(
            self.plan)
        self.updatePlanFrames()

    def numberOfSamplesChanged(self):
        self.updatePlanFrames()

    def animateClicked(self):
        self.startAnimation()

    def hideClicked(self):

        if self.ui.hideButton.text == 'hide':
            self.ui.playbackFrame.setEnabled(False)
            self.hidePlan()
            self.ui.hideButton.text = 'show'
            self.ui.executeButton.setEnabled(False)

            if not self.plan:
                self.ui.hideButton.setEnabled(False)
        else:
            self.ui.playbackFrame.setEnabled(True)
            self.ui.hideButton.text = 'hide'
            self.ui.hideButton.setEnabled(True)
            self.ui.executeButton.setEnabled(True)

            self.viewModeChanged()

        self.updateButtonColor()

    def executeClicked(self):
        self.executePlan()

    def executePlan(self,
                    visOnly=False,
                    overrideInfeasibleCheck=False,
                    overrideSupportsCheck=False):
        if visOnly:
            _, poses = self.planPlayback.getPlanPoses(self.plan)
            self.onPlanCommitted(self.plan)
            self.robotStateJointController.setPose('EST_ROBOT_STATE',
                                                   poses[-1])
        else:
            if (self.isPlanFeasible() or overrideInfeasibleCheck) and (
                    not self.isPlanAPlanWithSupports()
                    or overrideSupportsCheck):
                self.manipPlanner.commitManipPlan(self.plan)

    def onPlanCommitted(self, plan):
        self.setPlan(None)
        self.hideClicked()

    def stopClicked(self):
        self.stopAnimation()
        self.manipPlanner.sendPlanPause()

    def isPlanFeasible(self):
        plan = robotstate.asRobotPlan(self.plan)
        return plan is not None and max(plan.plan_info) < 10

    def getPlanInfo(self, plan):
        plan = robotstate.asRobotPlan(self.plan)
        return max(plan.plan_info)

    def isPlanAPlanWithSupports(self):
        return hasattr(
            self.plan,
            'support_sequence') or self.manipPlanner.publishPlansWithSupports

    def updatePlanFrames(self):

        if self.getViewMode() != 'frames':
            return

        numberOfSamples = self.getNumberOfSamples()

        meshes = self.planPlayback.getPlanPoseMeshes(
            self.plan, self.playbackJointController, self.playbackRobotModel,
            numberOfSamples)
        d = DebugData()

        startColor = [0.8, 0.8, 0.8]
        endColor = [85 / 255.0, 255 / 255.0, 255 / 255.0]
        colorFunc = scipy.interpolate.interp1d([0, numberOfSamples - 1],
                                               [startColor, endColor],
                                               axis=0,
                                               kind='slinear')

        for i, mesh in reversed(list(enumerate(meshes))):
            d.addPolyData(mesh, color=colorFunc(i))

        pd = d.getPolyData()
        clean = vtk.vtkCleanPolyData()
        clean.SetInput(pd)
        clean.Update()
        pd = clean.GetOutput()

        self.planFramesObj = vis.updatePolyData(d.getPolyData(),
                                                'robot plan',
                                                alpha=1.0,
                                                visible=False,
                                                colorByName='RGB255',
                                                parent='planning')
        self.showPlanFrames()

    def showPlanFrames(self):
        self.planFramesObj.setProperty('Visible', True)
        self.robotStateModel.setProperty('Visible', False)
        self.playbackRobotModel.setProperty('Visible', False)

    def startAnimation(self):
        self.showPlaybackModel()
        self.stopAnimation()
        self.ui.playbackSlider.value = 0
        self.animationClock.reset()
        self.animationTimer.start()
        self.updateAnimation()

    def stopAnimation(self):
        self.animationTimer.stop()

    def showPlaybackModel(self):
        self.robotStateModel.setProperty('Visible', True)
        self.playbackRobotModel.setProperty('Visible', True)
        self.playbackRobotModel.setProperty('Textures',
                                            self.playbackRobotModelUseTextures)
        self.robotStateModel.setProperty('Alpha',
                                         self.robotStateModelDisplayAlpha)
        self.playbackRobotModel.setProperty(
            'Alpha', self.playbackRobotModelDisplayAlpha)
        if self.planFramesObj:
            self.planFramesObj.setProperty('Visible', False)

    def hidePlan(self):
        self.stopAnimation()
        if self.planFramesObj:
            self.planFramesObj.setProperty('Visible', False)
        if self.playbackRobotModel:
            self.playbackRobotModel.setProperty('Visible', False)

        self.robotStateModel.setProperty('Visible', True)
        self.robotStateModel.setProperty('Alpha', 1.0)

    def showPoseAtTime(self, time):
        pose = self.poseInterpolator(time)
        self.playbackJointController.setPose('plan_playback', pose)

    def updateAnimation(self):

        tNow = self.animationClock.elapsed() * self.planPlayback.playbackSpeed
        if tNow > self.endTime:
            tNow = self.endTime

        sliderValue = int(1000.0 * tNow / self.endTime)

        self.ui.playbackSlider.blockSignals(True)
        self.ui.playbackSlider.value = sliderValue
        self.ui.playbackSlider.blockSignals(False)
        self.updateTimeLabel()

        self.showPoseAtTime(tNow)
        return tNow < self.endTime

    def updateButtonColor(self):
        if self.ui.executeButton.enabled and self.plan and not self.isPlanFeasible(
        ):
            styleSheet = 'background-color:red'
        elif self.ui.executeButton.enabled and self.plan and self.isPlanAPlanWithSupports(
        ):
            styleSheet = 'background-color:orange'
        else:
            styleSheet = ''

        self.ui.executeButton.setStyleSheet(styleSheet)

    def setPlan(self, plan):

        self.ui.playbackSlider.value = 0
        self.ui.timeLabel.text = 'Time: 0.00 s'
        self.ui.planNameLabel.text = ''
        self.plan = plan
        self.endTime = 1.0
        self.updateButtonColor()

        if not self.plan:
            return

        planText = 'Plan: %d.  %.2f seconds' % (
            plan.utime, self.planPlayback.getPlanElapsedTime(plan))
        self.ui.planNameLabel.text = planText

        self.startTime = 0.0
        self.endTime = self.planPlayback.getPlanElapsedTime(plan)
        self.interpolationChanged()
        info = self.getPlanInfo(plan)
        app.displaySnoptInfo(info)

        if self.ui.hideButton.text == 'show':
            self.hideClicked()
        else:
            self.viewModeChanged()

        self.updateButtonColor()

        if self.autoPlay and self.widget.parent() is not None:
            self.widget.parent().show()
Example #21
0
 def __init__(self):
     self.subscriber = None
     self.transforms = []
     self.timer = SimpleTimer()
     self.paused = True
Example #22
0
 def delay(self, delayTimeInSeconds):
     yield
     t = SimpleTimer()
     while t.elapsed() < delayTimeInSeconds:
         yield
Example #23
0
 def delay(self, delayTimeInSeconds):
     yield
     t = SimpleTimer()
     while t.elapsed() < delayTimeInSeconds:
         yield
Example #24
0
 def testJoint(self, jointId):
     self.jointId = jointId
     self.testTimer = SimpleTimer()
     self.start()