Python DebugData.getPolyData方法代码示例

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import getPolyData [as 别名]
def setupStrings():
    d = DebugData()
    poles = [[-0.36, 0.5, 1.5], [-0.36, -0.5, 1.5], [0, 0.5, 1.5], [0, -0.5, 1.5], [0.36, 0.5, 1.5], [0.36, -0.5, 1.5]]
    for pole in poles:
        d.addCylinder(pole, [0, 0, 1], 3, radius=0.021)
    vis.updatePolyData(d.getPolyData(), "poles")

    d = DebugData()
    strings = [
        [-0.36, 0.5, 0.09, 0, -0.5, 1.77],
        [-0.36, 0.5, 0.74, -0.36, -0.5, 0.95],
        [-0.36, 0.5, 1.12, -0.36, -0.5, 1.68],
        [-0.36, 0.5, 1.33, 0.36, -0.5, 2.29],
        [-0.36, 0.5, 1.6, 0.36, -0.5, 1.62],
        [-0.36, 0.5, 1.74, 0.36, -0.5, 1.93],
        [-0.36, 0.5, 2.15, -0.36, -0.5, 1.46],
        [0, 0.5, 0.765, 0, -0.5, 0.795],
        [0, 0.5, 1.15, 0.36, -0.5, 1.15],
        [0, 0.5, 1.28, -0.36, -0.5, 0.11],
        [0, 0.5, 1.42, 0, -0.5, 1.42],
        [0, 0.5, 1.78, 0.36, -0.5, 0.12],
        [0, 0.5, 2.05, -0.36, -0.5, 1.835],
        [0.36, 0.5, 0.8, -0.36, -0.5, 1.11],
        [0.36, 0.5, 1.16, -0.36, -0.5, 1.47],
        [0.36, 0.5, 1.61, 0.36, -0.5, 1.19],
        [0.36, 0.5, 2.0, 0.36, -0.5, 2.1],
        [-0.36, 0.3, 0, -0.36, 0.3, 2.01],
        [0, -0.34, 0, 0, -0.34, 1.42],
        [0.36, 0, 0, 0.36, 0, 2.05],
    ]
    for string in strings:
        p1 = string[:3]
        p2 = string[3:]
        d.addLine(p1, p2, radius=0.001, color=[255, 0, 0])
    vis.updatePolyData(d.getPolyData(), "strings")

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

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

    origin = np.array(frame.transform.GetPosition())
    rayLength = 5

    for i in range(0,numRays):
        ray = rays[:,i]
        rayTransformed = np.array(frame.transform.TransformNormal(ray))
        intersection = computeIntersection(locator, origin, origin + rayTransformed*rayLength)
        name = 'ray intersection ' + str(i)

        if intersection is not None:
            om.removeFromObjectModel(om.findObjectByName(name))
            d = DebugData()
            d.addLine(origin, intersection)
            color = [1,0,0]
            # d.addSphere(intersection, radius=0.04)
            vis.updatePolyData(d.getPolyData(), name, color=color)
        else:
            om.removeFromObjectModel(om.findObjectByName(name))
            d = DebugData()
            d.addLine(origin, origin+rayTransformed*rayLength)
            color = [0,1,0]
            # d.addSphere(intersection, radius=0.04)
            vis.updatePolyData(d.getPolyData(), name, color=color)

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import getPolyData [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 getPolyData [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 getPolyData [as 别名]
    def updateDrawIntersection(self, frame):

        origin = np.array(frame.transform.GetPosition())
        # print "origin is now at", origin
        d = DebugData()

        sliderIdx = self.slider.value

        controllerType = self.getControllerTypeFromCounter(sliderIdx)
        colorMaxRange = self.colorMap[controllerType]

        # if the QValue was empty then color it green
        if self.emptyQValue[sliderIdx]:
            colorMaxRange = [1, 1, 0]  # this is yellow

        for i in xrange(self.Sensor.numRays):
            ray = self.Sensor.rays[:, i]
            rayTransformed = np.array(frame.transform.TransformNormal(ray))
            # print "rayTransformed is", rayTransformed
            intersection = self.Sensor.raycast(self.locator, origin, origin + rayTransformed * self.Sensor.rayLength)

            if intersection is not None:
                d.addLine(origin, intersection, color=[1, 0, 0])
            else:
                d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange)

        vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255")

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import getPolyData [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 getPolyData [as 别名]
 def setRegion(self, safe_region):
     debug = DebugData()
     pos = safe_region.point
     try:
         xy_verts = safe_region.xy_polytope()
         if xy_verts.shape[1] == 0:
             raise QhullError("No points returned")
         xyz_verts = np.vstack((xy_verts, pos[2] + 0.02 + np.zeros((1, xy_verts.shape[1]))))
         xyz_verts = np.hstack((xyz_verts, np.vstack((xy_verts, pos[2] + 0.015 + np.zeros((1, xy_verts.shape[1]))))))
         # print xyz_verts.shape
         polyData = vnp.getVtkPolyDataFromNumpyPoints(xyz_verts.T.copy())
         vol_mesh = filterUtils.computeDelaunay3D(polyData)
         for j in range(xy_verts.shape[1]):
             z = pos[2] + 0.005
             p1 = np.hstack((xy_verts[:,j], z))
             if j < xy_verts.shape[1] - 1:
                 p2 = np.hstack((xy_verts[:,j+1], z))
             else:
                 p2 = np.hstack((xy_verts[:,0], z))
             debug.addLine(p1, p2, color=[.7,.7,.7], radius=0.003)
         debug.addPolyData(vol_mesh)
         # self.setPolyData(vol_mesh)
         self.setPolyData(debug.getPolyData())
         self.safe_region = safe_region
     except QhullError:
         print "Could not generate convex hull (polytope is likely unbounded)."

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

    d = DebugData()
    d.addCone((0,0,0), (1,0,0), height=0.2, radius=0.1)
    obj = vis.showPolyData(d.getPolyData(), 'robot')
    frame = vis.addChildFrame(obj)
    return obj

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

    d = DebugData()
    d.addLine((2,-1,0), (2,1,0), radius=0.1)
    d.addLine((2,-1,0), (1,-2,0), radius=0.1)
    obj = vis.showPolyData(d.getPolyData(), 'world')
    return obj

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import getPolyData [as 别名]
def loadFeet():
    meshDir = os.path.join(app.getDRCBase(), 'software/models/atlas_v3/meshes')
    meshes = []
    for foot in ['l', 'r']:
        d = DebugData()
        d.addPolyData(io.readPolyData(os.path.join(meshDir, '%s_talus.stl' % foot)))
        d.addPolyData(io.readPolyData(os.path.join(meshDir, '%s_foot.stl' % foot)))
        meshes.append(d.getPolyData())
    return meshes

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

    d = DebugData()
    d.addArrow((0,0,0), (0,0,0.3))
    pd = d.getPolyData()
    meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)

    desc = dict(classname='MeshAffordanceItem', Name='test mesh', Filename=meshId, uuid=newUUID(), pose=((0.5,0.0,1.0), (1,0,0,0)))
    return affordanceFromDescription(desc)

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import getPolyData [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 getPolyData [as 别名]
 def updateGeometryFromProperties(self):
     radius = self.getProperty('Radius')
     circlePoints = np.linspace(0, 2*np.pi, self.getProperty('Segments')+1)
     spokes = [(0.0, np.sin(x), np.cos(x)) for x in circlePoints]
     spokes = [radius*np.array(x)/np.linalg.norm(x) for x in spokes]
     d = DebugData()
     for a, b in zip(spokes, spokes[1:]):
         d.addCapsule(center=(a+b)/2.0, axis=(b-a), length=np.linalg.norm(b-a), radius=self.getProperty('Tube Radius'))
     self.setPolyData(d.getPolyData())