Python DebugData.addSphere方法代码示例

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addSphere [as 别名]
def main():
    global app, view, nav_data

    nav_data = np.array([[0, 0, 0]])

    lcmUtils.addSubscriber(".*_NAV$", node_nav_t, handleNavData)

    app = ConsoleApp()

    app.setupGlobals(globals())
    app.showPythonConsole()

    view = app.createView()
    view.show()

    global d
    d = DebugData()
    d.addLine([0,0,0], [1,0,0], color=[0,1,0])
    d.addSphere((0,0,0), radius=0.02, color=[1,0,0])

    #vis.showPolyData(d.getPolyData(), 'my debug geometry', colorByName='RGB255')

    startSwarmVisualization()

    app.start()

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addSphere [as 别名]
    def draw(self):

        d = DebugData()

        points = list(self.points)
        if self.hoverPos is not None:
            points.append(self.hoverPos)

        # draw points
        for p in points:
            d.addSphere(p, radius=5)

        if self.drawLines and len(points) > 1:
            for a, b in zip(points, points[1:]):
                d.addLine(a, b)

            # connect end points
            # d.addLine(points[0], points[-1])

        if self.annotationObj:
            self.annotationObj.setPolyData(d.getPolyData())
        else:
            self.annotationObj = vis.updatePolyData(d.getPolyData(), 'annotation', parent='segmentation', color=[1,0,0], view=self.view)
            self.annotationObj.addToView(self.view)
            self.annotationObj.actor.SetPickable(False)
            self.annotationObj.actor.GetProperty().SetLineWidth(2)

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addSphere [as 别名]
def drawCenterOfMass(model):
    stanceFrame = footstepsDriver.getFeetMidPoint(model)
    com = list(model.model.getCenterOfMass())
    com[2] = stanceFrame.GetPosition()[2]
    d = DebugData()
    d.addSphere(com, radius=0.015)
    obj = vis.updatePolyData(d.getPolyData(), 'COM %s' % model.getProperty('Name'), color=[1,0,0], visible=False, parent=model)

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addSphere [as 别名]
    def drawContactPts(self, obj, footstep, **kwargs):
        leftPoints, rightPoints = FootstepsDriver.getContactPts(footstep.params.support_contact_groups)
        contact_pts = rightPoints if footstep.is_right_foot else leftPoints

        d = DebugData()
        for pt in contact_pts:
            d.addSphere(pt, radius=0.01)
        d_obj = vis.showPolyData(d.getPolyData(), "contact points", parent=obj, **kwargs)
        d_obj.actor.SetUserTransform(obj.actor.GetUserTransform())

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addSphere [as 别名]
 def drawContactPts(self, obj, footstep, **kwargs):
     if footstep.is_right_foot:
         _, contact_pts = FootstepsDriver.getContactPts()
     else:
         contact_pts, _ = FootstepsDriver.getContactPts()
     d = DebugData()
     for pt in contact_pts:
         d.addSphere(pt, radius=0.01)
     d_obj = vis.showPolyData(d.getPolyData(), "contact points", parent=obj, **kwargs)
     d_obj.actor.SetUserTransform(obj.actor.GetUserTransform())

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addSphere [as 别名]
def showBoardDebug(affs=None):
    referenceFrame = vtk.vtkTransform()
    referenceFrame.Translate(0, 0, 5.0)
    affs = affs or om.getObjects()
    for obj in affs:
        if isinstance(obj, BlockAffordanceItem):
            d = DebugData()
            d.addSphere(computeClosestCorner(obj, referenceFrame), radius=0.015)
            showPolyData(d.getPolyData(), 'closest corner', parent='board debug', visible=True)
            showFrame(computeGroundFrame(obj, referenceFrame), 'ground frame', parent='board debug', visible=True)

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addSphere [as 别名]
    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))

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addSphere [as 别名]
    def createSpheres(self, sensorValues):
        d = DebugData()

        for key in sensorValues.keys():
            frame, pos, rpy = self.sensorLocations[key]

            t = transformUtils.frameFromPositionAndRPY(pos, rpy)
            t.PostMultiply()
            t.Concatenate(self.frames[frame])
            d.addSphere(t.GetPosition(),
                        radius=0.005,
                        color=self.getColor(sensorValues[key], key),
                        resolution=8)
        vis.updatePolyData(d.getPolyData(), self.name, colorByName='RGB255')

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addSphere [as 别名]
    def update(self):

        t = self.frameProvider.getFrame('SCAN')

        p1 = [0.0, 0.0, 0.0]
        p2 = [2.0, 0.0, 0.0]

        p1 = t.TransformPoint(p1)
        p2 = t.TransformPoint(p2)

        d = DebugData()
        d.addSphere(p1, radius=0.01, color=[0,1,0])
        d.addLine(p1, p2, color=[0,1,0])
        self.setPolyData(d.getPolyData())

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addSphere [as 别名]
    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)

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addSphere [as 别名]
    def onMouseMove(self, displayPoint, modifiers=None):

        om.removeFromObjectModel(om.findObjectByName('link selection'))
        self.linkName = None
        self.pickedPoint = None


        selection = self.getSelection(displayPoint)
        if selection is None:
            return

        pickedPoint, linkName, normal = selection

        d = DebugData()
        d.addSphere(pickedPoint, radius=0.01)
        d.addLine(pickedPoint, np.array(pickedPoint) + 0.1 * np.array(normal), radius=0.005)
        obj = vis.updatePolyData(d.getPolyData(), 'link selection', color=[0,1,0])
        obj.actor.SetPickable(False)

        self.linkName = linkName
        self.pickedPoint = pickedPoint

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addSphere [as 别名]
    def createPolyDataFromPrimitive(geom):

        if geom.type == lcmdrake.lcmt_viewer_geometry_data.BOX:
            d = DebugData()
            d.addCube(dimensions=geom.float_data[0:3], center=[0,0,0])
            return d.getPolyData()

        elif geom.type == lcmdrake.lcmt_viewer_geometry_data.SPHERE:
            d = DebugData()
            d.addSphere(center=(0,0,0), radius=geom.float_data[0])
            return d.getPolyData()

        elif geom.type == lcmdrake.lcmt_viewer_geometry_data.CYLINDER:
            d = DebugData()
            d.addCylinder(center=(0,0,0), axis=(0,0,1), radius=geom.float_data[0], length=geom.float_data[1])
            return d.getPolyData()

        elif geom.type == lcmdrake.lcmt_viewer_geometry_data.CAPSULE:
            d = DebugData()
            radius = geom.float_data[0]
            length = geom.float_data[1]
            d.addCylinder(center=(0,0,0), axis=(0,0,1), radius=radius, length=length)
            d.addSphere(center=(0,0,length/2.0), radius=radius)
            d.addSphere(center=(0,0,-length/2.0), radius=radius)
            return d.getPolyData()

        raise Exception('Unsupported geometry type: %s' % geom.type)

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addSphere [as 别名]
    def debugDrawFootPoints(model):
        pts_left, pts_right = FootstepsDriver.getContactPts()
        d = DebugData()

        for linkName in [_leftFootLink, _rightFootLink]:

            t = model.getLinkFrame(linkName)
            d.addFrame(t, scale=0.2)

            if (linkName is _leftFootLink):
                pts = pts_left
            else:
                pts = pts_right

            footMidPoint = np.mean(pts, axis=0)
            for p in pts.tolist() + [footMidPoint.tolist()]:
                t.TransformPoint(p, p)
                d.addSphere(p, radius=0.015)

        midpt = FootstepsDriver.getFeetMidPoint(model)
        d.addFrame(midpt, scale=0.2)
        vis.showPolyData(d.getPolyData(), 'foot points debug', parent='debug', colorByName='RGB255')

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addSphere [as 别名]
 def updateGeometryFromProperties(self):
     d = DebugData()
     d.addSphere((0,0,0), self.getProperty('Radius'))
     self.setPolyData(d.getPolyData())

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addSphere [as 别名]
from ddapp.consoleapp import ConsoleApp
from ddapp.debugVis import DebugData
import ddapp.visualization as vis

# initialize application components
app = ConsoleApp()
view = app.createView()
view.showMaximized()

# create a sphere primitive
d = DebugData()
d.addSphere(center=(0,0,0), radius=0.5)

# show the sphere in the visualization window
sphere = vis.showPolyData(d.getPolyData(), 'sphere')
sphere.setProperty('Color', [0,1,0])

# start the application
app.start()