Example #1
0
def toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False):

    obj, _ = vis.findPickedObject(displayPoint, view)

    if not obj:
        return False

    name = obj.getProperty('Name')

    if name in ('footstep widget', 'footstep widget frame'):
        om.removeFromObjectModel(om.findObjectByName('footstep widget'))
        return True

    match = re.match('^step (\d+)$', name)
    if not match:
        return False

    stepIndex = int(match.group(1))

    existingWidget = om.findObjectByName('footstep widget')
    if existingWidget:
        previousStep = existingWidget.stepIndex
        print 'have existing widget for step:', stepIndex

        om.removeFromObjectModel(existingWidget)
        if previousStep == stepIndex:
            print 'returning because widget was for selected step'
            return True


    footMesh = shallowCopy(obj.polyData)
    footFrame = transformUtils.copyFrame(obj.getChildFrame().transform)

    if useHorizontalWidget:
        rpy = [0.0, 0.0, transformUtils.rollPitchYawFromTransform(footFrame)[2]]
        footFrame = transformUtils.frameFromPositionAndRPY(footFrame.GetPosition(), np.degrees(rpy))

    footObj = vis.showPolyData(footMesh, 'footstep widget', parent='planning', alpha=0.2)
    footObj.stepIndex = stepIndex
    frameObj = vis.showFrame(footFrame, 'footstep widget frame', parent=footObj, scale=0.2)
    footObj.actor.SetUserTransform(frameObj.transform)
    footObj.setProperty('Color', obj.getProperty('Color'))
    frameObj.setProperty('Edit', True)

    rep = frameObj.widget.GetRepresentation()
    rep.SetTranslateAxisEnabled(2, False)
    rep.SetRotateAxisEnabled(0, False)
    rep.SetRotateAxisEnabled(1, False)
    frameObj.widget.HandleRotationEnabledOff()

    walkGoal = om.findObjectByName('walking goal')
    if walkGoal:
        walkGoal.setProperty('Edit', False)


    def onFootWidgetChanged(frame):
        footstepsDriver.onStepModified(stepIndex - 1, frame)

    frameObj.connectFrameModified(onFootWidgetChanged)
    return True
Example #2
0
def placeHandModel(displayPoint, view, side='left'):

    obj, _ = vis.findPickedObject(displayPoint, view)
    if isinstance(obj, vis.FrameItem):
        _, handFrame = handFactory.placeHandModelWithTransform(obj.transform, view, side=side, parent=obj.parent())
        handFrame.frameSync = vis.FrameSync()
        handFrame.frameSync.addFrame(obj)
        handFrame.frameSync.addFrame(handFrame, ignoreIncoming=True)
        return

    pickedPoint, prop, _, normal = vis.pickPoint(displayPoint, view, pickType='cells', tolerance=0.0, returnNormal=True)

    obj = vis.getObjectByProp(prop)
    if not obj:
        return

    yaxis = -normal
    zaxis = [0,0,1]
    xaxis = np.cross(yaxis, zaxis)
    xaxis /= np.linalg.norm(xaxis)
    zaxis = np.cross(xaxis, yaxis)
    zaxis /= np.linalg.norm(zaxis)

    t = transformUtils.getTransformFromAxes(-zaxis, yaxis, xaxis)
    t.PostMultiply()
    t.Translate(pickedPoint)
    handObj, handFrame = handFactory.placeHandModelWithTransform(t, view, side=side, parent=obj)

    syncFrame = getChildFrame(obj)
    if syncFrame:
        handFrame.frameSync = vis.FrameSync()
        handFrame.frameSync.addFrame(handFrame, ignoreIncoming=True)
        handFrame.frameSync.addFrame(syncFrame)
Example #3
0
    def onViewDoubleClicked(self, displayPoint):

        obj, pickedPoint = vis.findPickedObject(displayPoint, self.view)

        if pickedPoint is None or not obj:
            return

        imageName = obj.getProperty('Name')
        imageUtime = self.imageManager.getUtime(imageName)

        cameraToLocal = vtk.vtkTransform()
        self.imageManager.queue.getTransform(imageName, 'local', imageUtime,
                                             cameraToLocal)

        utorsoToLocal = vtk.vtkTransform()
        self.imageManager.queue.getTransform('utorso', 'local', imageUtime,
                                             utorsoToLocal)

        p = range(3)
        utorsoToLocal.TransformPoint(pickedPoint, p)

        ray = np.array(p) - np.array(cameraToLocal.GetPosition())
        ray /= np.linalg.norm(ray)

        if self.rayCallback:
            self.rayCallback(np.array(cameraToLocal.GetPosition()), ray)
Example #4
0
    def selectLink(self, displayPoint, view):

        if not self.enabled():
            return False

        robotModel, _ = vis.findPickedObject(displayPoint, view)

        try:
            robotModel.model.getLinkNameForMesh
        except AttributeError:
            return False

        model = robotModel.model

        pickedPoint, _, polyData = vis.pickProp(displayPoint, view)

        linkName = model.getLinkNameForMesh(polyData)
        if not linkName:
            return False

        fadeValue = 1.0 if linkName == self.selectedLink else 0.05

        for name in model.getLinkNames():
            linkColor = model.getLinkColor(name)
            linkColor.setAlphaF(fadeValue)
            model.setLinkColor(name, linkColor)

        if linkName == self.selectedLink:
            self.selectedLink = None
            vis.hideCaptionWidget()
            om.removeFromObjectModel(
                om.findObjectByName('selected link frame'))

        else:
            self.selectedLink = linkName
            linkColor = model.getLinkColor(self.selectedLink)
            linkColor.setAlphaF(1.0)
            model.setLinkColor(self.selectedLink, linkColor)
            vis.showCaptionWidget(robotModel.getLinkFrame(
                self.selectedLink).GetPosition(),
                                  self.selectedLink,
                                  view=view)
            vis.updateFrame(robotModel.getLinkFrame(self.selectedLink),
                            'selected link frame',
                            scale=0.2,
                            parent=robotModel)

        return True
Example #5
0
def toggleFrameWidget(displayPoint, view):

    obj, _ = vis.findPickedObject(displayPoint, view)

    if not isinstance(obj, vis.FrameItem):
        obj = getChildFrame(obj)

    if not obj:
        return False

    edit = not obj.getProperty('Edit')
    obj.setProperty('Edit', edit)

    parent = obj.parent()
    if getChildFrame(parent) == obj:
        parent.setProperty('Alpha', 0.5 if edit else 1.0)

    return True
Example #6
0
    def selectLink(self, displayPoint, view):

        if not self.enabled():
            return False

        robotModel, _ = vis.findPickedObject(displayPoint, view)

        try:
            robotModel.model.getLinkNameForMesh
        except AttributeError:
            return False

        model = robotModel.model

        pickedPoint, _, polyData = vis.pickProp(displayPoint, view)

        linkName = model.getLinkNameForMesh(polyData)
        if not linkName:
            return False

        fadeValue = 1.0 if linkName == self.selectedLink else 0.05

        for name in model.getLinkNames():
            linkColor = model.getLinkColor(name)
            linkColor.setAlphaF(fadeValue)
            model.setLinkColor(name, linkColor)

        if linkName == self.selectedLink:
            self.selectedLink = None
            vis.hideCaptionWidget()
            om.removeFromObjectModel(om.findObjectByName('selected link frame'))

        else:
            self.selectedLink = linkName
            linkColor = model.getLinkColor(self.selectedLink)
            linkColor.setAlphaF(1.0)
            model.setLinkColor(self.selectedLink, linkColor)
            vis.showCaptionWidget(robotModel.getLinkFrame(self.selectedLink).GetPosition(), self.selectedLink, view=view)
            vis.updateFrame(robotModel.getLinkFrame(self.selectedLink), 'selected link frame', scale=0.2, parent=robotModel)

        return True
Example #7
0
    def onViewDoubleClicked(self, displayPoint):

        obj, pickedPoint = vis.findPickedObject(displayPoint, self.view)

        if pickedPoint is None or not obj:
            return

        imageName = obj.getProperty('Name')
        imageUtime = self.imageManager.getUtime(imageName)

        cameraToLocal = vtk.vtkTransform()
        self.imageManager.queue.getTransform(imageName, 'local', imageUtime, cameraToLocal)

        utorsoToLocal = vtk.vtkTransform()
        self.imageManager.queue.getTransform('utorso', 'local', imageUtime, utorsoToLocal)

        p = range(3)
        utorsoToLocal.TransformPoint(pickedPoint, p)

        ray = np.array(p) - np.array(cameraToLocal.GetPosition())
        ray /= np.linalg.norm(ray)

        if self.rayCallback:
            self.rayCallback(np.array(cameraToLocal.GetPosition()), ray)
Example #8
0
def showRightClickMenu(displayPoint, view):

    pickedObj, pickedPoint = vis.findPickedObject(displayPoint, view)
    if not pickedObj:
        return

    objectName = pickedObj.getProperty('Name')
    if objectName == 'grid':
        return

    displayPoint = displayPoint[0], view.height - displayPoint[1]

    globalPos = view.mapToGlobal(QtCore.QPoint(*displayPoint))

    menu = QtGui.QMenu(view)

    widgetAction = QtGui.QWidgetAction(menu)
    label = QtGui.QLabel('<b>%s</b>' % objectName)
    label.setContentsMargins(9,9,6,6)
    widgetAction.setDefaultWidget(label)
    menu.addAction(widgetAction)
    menu.addSeparator()


    propertiesPanel = PythonQt.dd.ddPropertiesPanel()
    propertiesPanel.setBrowserModeToWidget()
    propertyset.PropertyPanelHelper.addPropertiesToPanel(pickedObj.properties, propertiesPanel)

    def onPropertyChanged(prop):
        om.PropertyPanelHelper.setPropertyFromPanel(prop, propertiesPanel, pickedObj.properties)
    propertiesPanel.connect('propertyValueChanged(QtVariantProperty*)', onPropertyChanged)

    propertiesMenu = menu.addMenu('Properties')
    propertiesWidgetAction = QtGui.QWidgetAction(propertiesMenu)
    propertiesWidgetAction.setDefaultWidget(propertiesPanel)
    propertiesMenu.addAction(propertiesWidgetAction)


    def onDelete():
        om.removeFromObjectModel(pickedObj)

    def onHide():
        pickedObj.setProperty('Visible', False)

    def onSelect():
        om.setActiveObject(pickedObj)

    reachFrame = getAsFrame(pickedObj)
    collisionParent = getCollisionParent(pickedObj)
    def onReachLeft():
        reachToFrame(reachFrame, 'left', collisionParent)
    def onReachRight():
        reachToFrame(reachFrame, 'right', collisionParent)

    def flipHandSide():
        for obj in [pickedObj] + pickedObj.children():
            if not isGraspSeed(obj):
                continue
            side = 'right' if obj.side == 'left' else 'left'
            obj.side = side
            color = [1.0, 1.0, 0.0]
            if side == 'right':
                color = [0.33, 1.0, 0.0]
            obj.setProperty('Color', color)

    def flipHandThumb():
        handFrame = pickedObj.children()[0]
        t = transformUtils.copyFrame(handFrame.transform)
        t.PreMultiply()
        t.RotateY(180)
        handFrame.copyFrame(t)
        pickedObj._renderAllViews()

    def onSplineLeft():
        splinewidget.planner.newSpline(pickedObj, 'left')
    def onSplineRight():
        splinewidget.planner.newSpline(pickedObj, 'right')


    def getPointCloud(obj):
        try:
            obj = obj.model.polyDataObj
        except AttributeError:
            pass
        try:
            obj.polyData
        except AttributeError:
            return None
        if obj and obj.polyData.GetNumberOfPoints():# and (obj.polyData.GetNumberOfCells() == obj.polyData.GetNumberOfVerts()):
            return obj


    pointCloudObj = getPointCloud(pickedObj)
    affordanceObj = pickedObj if isinstance(pickedObj, affordanceitems.AffordanceItem) else None

    def onSegmentGround():
        groundPoints, scenePoints =  segmentation.removeGround(pointCloudObj.polyData)
        vis.showPolyData(groundPoints, 'ground points', color=[0,1,0], parent='segmentation')
        vis.showPolyData(scenePoints, 'scene points', color=[1,0,1], parent='segmentation')
        pickedObj.setProperty('Visible', False)


    def onCopyPointCloud():
        global lastRandomColor
        polyData = vtk.vtkPolyData()
        polyData.DeepCopy(pointCloudObj.polyData)
        
        if pointCloudObj.getChildFrame():
            polyData = segmentation.transformPolyData(polyData, pointCloudObj.getChildFrame().transform)
        polyData = segmentation.addCoordArraysToPolyData(polyData)

        # generate random color, and average with a common color to make them generally similar
        lastRandomColor = lastRandomColor + 0.1 + 0.1*random.random()
        rgb = colorsys.hls_to_rgb(lastRandomColor, 0.7, 1.0)
        obj = vis.showPolyData(polyData, pointCloudObj.getProperty('Name') + ' copy', color=rgb, parent='point clouds')

        t = vtk.vtkTransform()
        t.PostMultiply()
        t.Translate(filterUtils.computeCentroid(polyData))
        segmentation.makeMovable(obj, t)
        om.setActiveObject(obj)
        pickedObj.setProperty('Visible', False)

    def onMergeIntoPointCloud():
        allPointClouds = om.findObjectByName('point clouds')
        if allPointClouds:
            allPointClouds = [i.getProperty('Name') for i in allPointClouds.children()]
        sel =  QtGui.QInputDialog.getItem(None, "Point Cloud Merging", "Pick point cloud to merge into:", allPointClouds, current=0, editable=False)
        sel = om.findObjectByName(sel)

        # Make a copy of each in same frame
        polyDataInto = vtk.vtkPolyData()
        polyDataInto.ShallowCopy(sel.polyData)
        if sel.getChildFrame():
            polyDataInto = segmentation.transformPolyData(polyDataInto, sel.getChildFrame().transform)

        polyDataFrom = vtk.vtkPolyData()
        polyDataFrom.DeepCopy(pointCloudObj.polyData)
        if pointCloudObj.getChildFrame():
            polyDataFrom = segmentation.transformPolyData(polyDataFrom, pointCloudObj.getChildFrame().transform)

        # Actual merge
        append = filterUtils.appendPolyData([polyDataFrom, polyDataInto])
        if sel.getChildFrame():
            polyDataInto = segmentation.transformPolyData(polyDataInto, sel.getChildFrame().transform.GetInverse())

        # resample
        append = segmentationroutines.applyVoxelGrid(append, 0.01)
        append = segmentation.addCoordArraysToPolyData(append)

        # Recenter the frame
        sel.setPolyData(append)
        t = vtk.vtkTransform()
        t.PostMultiply()
        t.Translate(filterUtils.computeCentroid(append))
        segmentation.makeMovable(sel, t)

        # Hide the old one
        if pointCloudObj.getProperty('Name') in allPointClouds:
            pointCloudObj.setProperty('Visible', False)


    def onSegmentTableScene():
        data = segmentation.segmentTableScene(pointCloudObj.polyData, pickedPoint)
        vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')

    def onSegmentDrillAlignedWithTable():
        segmentation.segmentDrillAlignedWithTable(pickedPoint, pointCloudObj.polyData)

    def onCachePickedPoint():
        ''' Cache the Picked Point for general purpose use'''
        global lastCachedPickedPoint
        lastCachedPickedPoint = pickedPoint
        #data = segmentation.segmentTableScene(pointCloudObj.polyData, pickedPoint)
        #vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')


    def onLocalPlaneFit():
        planePoints, normal = segmentation.applyLocalPlaneFit(pointCloudObj.polyData, pickedPoint, searchRadius=0.1, searchRadiusEnd=0.2)
        obj = vis.showPolyData(planePoints, 'local plane fit', color=[0,1,0])
        obj.setProperty('Point Size', 7)

        fields = segmentation.makePolyDataFields(obj.polyData)

        pose = transformUtils.poseFromTransform(fields.frame)
        desc = dict(classname='BoxAffordanceItem', Name='local plane', Dimensions=list(fields.dims), pose=pose)
        box = segmentation.affordanceManager.newAffordanceFromDescription(desc)

    def onOrientToMajorPlane():
        polyData, planeFrame = segmentation.orientToMajorPlane(pointCloudObj.polyData, pickedPoint=pickedPoint)
        pointCloudObj.setPolyData(polyData)


    def onDiskGlyph():
        result = segmentation.applyDiskGlyphs(pointCloudObj.polyData)
        obj = vis.showPolyData(result, 'disks', color=[0.8,0.8,0.8])
        om.setActiveObject(obj)
        pickedObj.setProperty('Visible', False)

    def onArrowGlyph():
        result = segmentation.applyArrowGlyphs(pointCloudObj.polyData)
        obj = vis.showPolyData(result, 'disks')

    def onSegmentationEditor():
        segmentationpanel.activateSegmentationMode(pointCloudObj.polyData)

    def addNewFrame():
        t = transformUtils.copyFrame(affordanceObj.getChildFrame().transform)
        t.PostMultiply()
        t.Translate(np.array(pickedPoint) - np.array(t.GetPosition()))
        newFrame = vis.showFrame(t, '%s frame %d' % (affordanceObj.getProperty('Name'), len(affordanceObj.children())), scale=0.2, parent=affordanceObj)
        affordanceObj.getChildFrame().getFrameSync().addFrame(newFrame, ignoreIncoming=True)

    def copyAffordance():
        desc = dict(affordanceObj.getDescription())
        del desc['uuid']
        desc['Name'] = desc['Name'] + ' copy'
        aff = robotSystem.affordanceManager.newAffordanceFromDescription(desc)
        aff.getChildFrame().setProperty('Edit', True)

    def onPromoteToAffordance():
        affObj = affordanceitems.MeshAffordanceItem.promotePolyDataItem(pickedObj)
        robotSystem.affordanceManager.registerAffordance(affObj)

    actions = [
      (None, None),
      ('Hide', onHide),
      ('Delete', onDelete),
      ('Select', onSelect)
      ]


    if affordanceObj:
        actions.extend([
            ('Copy affordance', copyAffordance),
            ('Add new frame', addNewFrame),
        ])

    elif type(pickedObj) == vis.PolyDataItem:
        actions.extend([
            ('Promote to Affordance', onPromoteToAffordance),
        ])

    if isGraspSeed(pickedObj):
        actions.extend([
            (None, None),
            ('Flip Side', flipHandSide),
            ('Flip Thumb', flipHandThumb),
        ])

    if reachFrame is not None:
        actions.extend([
            (None, None),
            ('Reach Left', onReachLeft),
            ('Reach Right', onReachRight),
            #('Spline Left', onSplineLeft),
            #('Spline Right', onSplineRight),
            ])

    if pointCloudObj:
        actions.extend([
            (None, None),
            ('Copy Pointcloud', onCopyPointCloud),
            ('Merge Pointcloud Into', onMergeIntoPointCloud),
            ('Segment Ground', onSegmentGround),
            ('Segment Table', onSegmentTableScene),
            ('Segment Drill Aligned', onSegmentDrillAlignedWithTable),
            ('Local Plane Fit', onLocalPlaneFit),
            ('Orient with Horizontal', onOrientToMajorPlane),
            ('Arrow Glyph', onArrowGlyph),
            ('Disk Glyph', onDiskGlyph),
            ('Cache Pick Point', onCachePickedPoint),
            (None, None),
            ('Open Segmentation Editor', onSegmentationEditor)
            ])

    for actionName, func in actions:
        if not actionName:
            menu.addSeparator()
        else:
            action = menu.addAction(actionName)
            action.connect('triggered()', func)


    selectedAction = menu.popup(globalPos)