Beispiel #1
0
def cropToCylinder(polyData, p1, p2, radius):
    polyData = segmentation.cropToLineSegment(polyData, p1, p2)
    if polyData.GetNumberOfPoints():
        polyData = segmentation.labelDistanceToLine(polyData, p1, p2)
        polyData = segmentation.thresholdPoints(polyData, 'distance_to_line',
                                                [0.0, radius])
    return polyData
    def onNewWalkingGoal(self, walkingGoal=None):

        walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel)
        frameObj = vis.updateFrame(walkingGoal,
                                   'walking goal',
                                   parent='planning',
                                   scale=0.25)
        frameObj.setProperty('Edit', True)

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

        if self.placer:
            self.placer.stop()

        terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
        if terrain:

            pos = np.array(frameObj.transform.GetPosition())

            polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
            if polyData.GetNumberOfPoints():
                polyData = segmentation.labelDistanceToLine(
                    polyData, pos, pos + [0, 0, 1])
                polyData = segmentation.thresholdPoints(
                    polyData, 'distance_to_line', [0.0, 0.1])
                if polyData.GetNumberOfPoints():
                    pos[2] = np.nanmax(
                        vnp.getNumpyFromVtk(polyData, 'Points')[:, 2])
                    frameObj.transform.Translate(
                        pos - np.array(frameObj.transform.GetPosition()))

            d = DebugData()
            d.addSphere((0, 0, 0), radius=0.03)
            handle = vis.showPolyData(d.getPolyData(),
                                      'walking goal terrain handle',
                                      parent=frameObj,
                                      visible=True,
                                      color=[1, 1, 0])
            handle.actor.SetUserTransform(frameObj.transform)
            self.placer = PlacerWidget(app.getCurrentRenderView(), handle,
                                       terrain)

            def onFramePropertyModified(propertySet, propertyName):
                if propertyName == 'Edit':
                    if propertySet.getProperty(propertyName):
                        self.placer.start()
                    else:
                        self.placer.stop()

            frameObj.properties.connectPropertyChanged(onFramePropertyModified)
            onFramePropertyModified(frameObj, 'Edit')

        frameObj.connectFrameModified(self.onWalkingGoalModified)
        self.onWalkingGoalModified(frameObj)
    def __init__(self, uid, view, seed_pose, irisDriver, existing_region=None):

        d = DebugData()
        self.uid = uid
        vis.PolyDataItem.__init__(self, "IRIS region {:d}".format(uid), d.getPolyData(), view)
        self.transform = seed_pose
        d.addSphere((0,0,0), radius=0.02)
        self.seedObj = vis.showPolyData(d.getPolyData(), 'region seed', parent=om.getOrCreateContainer('IRIS region seeds'))
        self.seedObj.actor.SetUserTransform(self.transform)
        self.frameObj = vis.showFrame(self.transform, 'region seed frame',
                                      scale=0.2,
                                      visible=False,
                                      parent=self.seedObj)
        self.frameObj.setProperty('Edit', True)

        self.frameObj.widget.HandleRotationEnabledOff()

        terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
        if terrain:
            rep = self.frameObj.widget.GetRepresentation()
            rep.SetTranslateAxisEnabled(2, False)
            rep.SetRotateAxisEnabled(0, False)
            rep.SetRotateAxisEnabled(1, False)

            pos = np.array(self.frameObj.transform.GetPosition())
            polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
            if polyData.GetNumberOfPoints():
                polyData = segmentation.labelDistanceToLine(polyData, pos, pos+[0,0,1])
                polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, 0.1])
                if polyData.GetNumberOfPoints():
                    pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, 'Points')[:,2])
                    self.frameObj.transform.Translate(pos - np.array(self.frameObj.transform.GetPosition()))

            self.placer = PlacerWidget(view, self.seedObj, terrain)
            self.placer.start()
        else:
            self.frameObj.setProperty('Edit', True)
            self.frameObj.setProperty('Visible', True)


        self.driver = irisDriver
        self.safe_region = None
        self.addProperty('Visible', True)
        self.addProperty('Enabled for Walking', True)
        self.addProperty('Alpha', 1.0)
        self.addProperty('Color', QtGui.QColor(200,200,20))

        self.frameObj.connectFrameModified(self.onFrameModified)
        if existing_region is None:
            self.onFrameModified(self.frameObj)
        else:
            self.setRegion(existing_region)

        self.setProperty('Alpha', 0.5)
        self.setProperty('Color', QtGui.QColor(220,220,220))
    def onNewWalkingGoal(self, walkingGoal=None):

        walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel)
        frameObj = vis.updateFrame(walkingGoal, 'walking goal', parent='planning', scale=0.25)
        frameObj.setProperty('Edit', True)


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

        if self.placer:
            self.placer.stop()

        terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
        if terrain:

            pos = np.array(frameObj.transform.GetPosition())

            polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
            if polyData.GetNumberOfPoints():
                polyData = segmentation.labelDistanceToLine(polyData, pos, pos+[0,0,1])
                polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, 0.1])
                if polyData.GetNumberOfPoints():
                    pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, 'Points')[:,2])
                    frameObj.transform.Translate(pos - np.array(frameObj.transform.GetPosition()))

            d = DebugData()
            d.addSphere((0,0,0), radius=0.03)
            handle = vis.showPolyData(d.getPolyData(), 'walking goal terrain handle', parent=frameObj, visible=True, color=[1,1,0])
            handle.actor.SetUserTransform(frameObj.transform)
            self.placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain)

            def onFramePropertyModified(propertySet, propertyName):
                if propertyName == 'Edit':
                    if propertySet.getProperty(propertyName):
                        self.placer.start()
                    else:
                        self.placer.stop()

            frameObj.properties.connectPropertyChanged(onFramePropertyModified)
            onFramePropertyModified(frameObj, 'Edit')

        frameObj.connectFrameModified(self.onWalkingGoalModified)
        self.onWalkingGoalModified(frameObj)
Beispiel #5
0
    def __init__(self, uid, view, seed_pose, irisDriver, existing_region=None):

        d = DebugData()
        self.uid = uid
        vis.PolyDataItem.__init__(self, "IRIS region {:d}".format(uid),
                                  d.getPolyData(), view)
        self.transform = seed_pose
        d.addSphere((0, 0, 0), radius=0.02)
        self.seedObj = vis.showPolyData(
            d.getPolyData(),
            'region seed',
            parent=om.getOrCreateContainer('IRIS region seeds'))
        self.seedObj.actor.SetUserTransform(self.transform)
        self.frameObj = vis.showFrame(self.transform,
                                      'region seed frame',
                                      scale=0.2,
                                      visible=False,
                                      parent=self.seedObj)
        self.frameObj.setProperty('Edit', True)

        self.frameObj.widget.HandleRotationEnabledOff()

        terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
        if terrain:
            rep = self.frameObj.widget.GetRepresentation()
            rep.SetTranslateAxisEnabled(2, False)
            rep.SetRotateAxisEnabled(0, False)
            rep.SetRotateAxisEnabled(1, False)

            pos = np.array(self.frameObj.transform.GetPosition())
            polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
            if polyData.GetNumberOfPoints():
                polyData = segmentation.labelDistanceToLine(
                    polyData, pos, pos + [0, 0, 1])
                polyData = segmentation.thresholdPoints(
                    polyData, 'distance_to_line', [0.0, 0.1])
                if polyData.GetNumberOfPoints():
                    pos[2] = np.nanmax(
                        vnp.getNumpyFromVtk(polyData, 'Points')[:, 2])
                    self.frameObj.transform.Translate(
                        pos - np.array(self.frameObj.transform.GetPosition()))

            self.placer = PlacerWidget(view, self.seedObj, terrain)
            self.placer.start()
        else:
            self.frameObj.setProperty('Edit', True)
            self.frameObj.setProperty('Visible', True)

        self.driver = irisDriver
        self.safe_region = None
        self.addProperty('Visible', True)
        self.addProperty('Enabled for Walking', True)
        self.addProperty('Alpha', 1.0)
        self.addProperty('Color', QtGui.QColor(200, 200, 20))

        self.frameObj.connectFrameModified(self.onFrameModified)
        if existing_region is None:
            self.onFrameModified(self.frameObj)
        else:
            self.setRegion(existing_region)

        self.setProperty('Alpha', 0.5)
        self.setProperty('Color', QtGui.QColor(220, 220, 220))
    def newWalkingGoal(self, displayPoint, view):

        # put walking goal at robot's base
        mainLink = drcargs.getRobotConfig(self.robotName)["pelvisLink"]
        footFrame = self.robotModel.getLinkFrame(mainLink)

        if not footFrame:
            print(
                "ERROR: The link '{}' provided for the key 'pelvisLink' in the configuration file does not exist in "
                "the robot's URDF. Cannot place walking goal.".format(mainLink)
            )
            return

        worldPt1, worldPt2 = vis.getRayFromDisplayPoint(view, displayPoint)
        groundOrigin = footFrame.GetPosition()
        groundNormal = [0.0, 0.0, 1.0]
        selectedGroundPoint = [0.0, 0.0, 0.0]

        t = vtk.mutable(0.0)
        vtk.vtkPlane.IntersectWithLine(
            worldPt1, worldPt2, groundNormal, groundOrigin, t, selectedGroundPoint
        )

        walkingTarget = transformUtils.frameFromPositionAndRPY(
            selectedGroundPoint, np.array(footFrame.GetOrientation())
        )

        frameObj = vis.updateFrame(
            walkingTarget,
            self.robotName + " walking goal",
            parent="planning",
            scale=0.25,
        )
        frameObj.setProperty("Edit", True)

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

        terrain = om.findObjectByName("HEIGHT_MAP_SCENE")
        if terrain:

            pos = np.array(frameObj.transform.GetPosition())

            polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
            if polyData.GetNumberOfPoints():
                polyData = segmentation.labelDistanceToLine(
                    polyData, pos, pos + [0, 0, 1]
                )
                polyData = segmentation.thresholdPoints(
                    polyData, "distance_to_line", [0.0, 0.1]
                )
                if polyData.GetNumberOfPoints():
                    pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, "Points")[:, 2])
                    frameObj.transform.Translate(
                        pos - np.array(frameObj.transform.GetPosition())
                    )

            d = DebugData()
            d.addSphere((0, 0, 0), radius=0.03)
            handle = vis.showPolyData(
                d.getPolyData(),
                "walking goal terrain handle " + self.robotName,
                parent=frameObj,
                visible=True,
                color=[1, 1, 0],
            )
            handle.actor.SetUserTransform(frameObj.transform)
            placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain)

            def onFramePropertyModified(propertySet, propertyName):
                if propertyName == "Edit":
                    if propertySet.getProperty(propertyName):
                        placer.start()
                    else:
                        placer.stop()

            frameObj.properties.connectPropertyChanged(onFramePropertyModified)
            onFramePropertyModified(frameObj, "Edit")

        frameObj.connectFrameModified(self.onWalkingGoalModified)