Python DebugData.addLine方法代码示例

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addLine [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 addLine [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 addLine [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 addLine [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 addLine [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 addLine [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 addLine [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 addLine [as 别名]
    def drawTargetPath(self):
        path = DebugData()
        for i in range(1,len(self.targetPath)):
          p0 = self.targetPath[i-1].GetPosition()
          p1 = self.targetPath[i].GetPosition()
          path.addLine ( np.array( p0 ) , np.array(  p1 ), radius= 0.005)

        pathMesh = path.getPolyData()
        self.targetPathMesh = vis.showPolyData(pathMesh, 'target frame desired path', color=[0.0, 0.3, 1.0], parent=self.targetAffordance, alpha=0.6)
        self.targetPathMesh.actor.SetUserTransform(self.targetFrame)

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addLine [as 别名]
    def buildWarehouseWorld(percentObsDensity, nonRandom=False, circleRadius=0.1, scale=None, randomSeed=5,
                         obstaclesInnerFraction=1.0):

        if nonRandom:
            np.random.seed(randomSeed)

        d = DebugData()
        worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d, scale=scale, boundaryType="Warehouse")

        numObstacles = 8
 
        obsLength = 2.0

        worldLength = worldXmax - worldXmin

        print worldXmin
        print worldXmax

        obstacleZone = [worldXmin + 0.2 * worldLength, worldXmax - 0.2 * worldLength ]

        print obstacleZone

        obstacleLength = obstacleZone[1] - obstacleZone[0]

        incrementSize = obstacleLength * 1.0 / numObstacles

        print incrementSize

        leftOrRight = -1.0
        for i in xrange(numObstacles):
            
            firstX = obstacleZone[0] + incrementSize * i
            leftOrRight = leftOrRight * -1.0

            firstEndpt = (firstX, leftOrRight * worldYmax,0.0)
            secondEndpt = (firstX, 0.0, 0.0)

            #d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
            d.addLine(firstEndpt, secondEndpt, radius=circleRadius)

        obj = vis.showPolyData(d.getPolyData(), 'world')

        world = World()
        world.visObj = obj
        world.Xmax = worldXmax
        world.Xmin = worldXmin
        world.Ymax = worldYmax
        world.Ymin = worldYmin
        world.numObstacles = numObstacles
        world.percentObsDensity = percentObsDensity

        return world

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

        center = self.displayPointToImagePoint(displayPoint, restrictToImageDimensions=False)
        center = np.array(center)

        d = DebugData()
        d.addLine(center + [0, -3000, 0], center + [0, 3000, 0])
        d.addLine(center + [-3000, 0, 0], center + [3000, 0, 0])
        self.cursorObj = vis.updatePolyData(d.getPolyData(), 'cursor', alpha=0.5, view=self.view)
        self.cursorObj.addToView(self.view)
        self.cursorObj.actor.SetUseBounds(False)
        self.cursorObj.actor.SetPickable(False)
        self.view.render()

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addLine [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 addLine [as 别名]
    def buildCircleWorld(percentObsDensity, nonRandom=False, circleRadius=3, scale=None, randomSeed=5,
                         obstaclesInnerFraction=1.0):
        #print "building circle world"

        if nonRandom:
            np.random.seed(randomSeed)

        d = DebugData()
        worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d, scale=scale)
        #print "boundaries done"

        worldArea = (worldXmax-worldXmin)*(worldYmax-worldYmin)
        #print worldArea
        obsScalingFactor = 1.0/12.0
        maxNumObstacles = obsScalingFactor * worldArea
        
        numObstacles = int(obstaclesInnerFraction**2 * percentObsDensity/100.0 * maxNumObstacles)
        #print numObstacles

        # draw random stick obstacles
        obsLength = 2.0

        obsXmin = worldXmin + (1-obstaclesInnerFraction)/2.0*(worldXmax - worldXmin)
        obsXmax = worldXmax - (1-obstaclesInnerFraction)/2.0*(worldXmax - worldXmin)
        obsYmin = worldYmin + (1-obstaclesInnerFraction)/2.0*(worldYmax - worldYmin)
        obsYmax = worldYmax - (1-obstaclesInnerFraction)/2.0*(worldYmax - worldYmin)

        for i in xrange(numObstacles):
            firstX = obsXmin + np.random.rand()*(obsXmax-obsXmin)
            firstY = obsYmin + np.random.rand()*(obsYmax-obsYmin)
            firstEndpt = (firstX,firstY,+0.2)
            secondEndpt = (firstX,firstY,-0.2)

            #d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
            d.addLine(firstEndpt, secondEndpt, radius=circleRadius)


        obj = vis.showPolyData(d.getPolyData(), 'world')

        world = World()
        world.visObj = obj
        world.Xmax = worldXmax
        world.Xmin = worldXmin
        world.Ymax = worldYmax
        world.Ymin = worldYmin
        world.numObstacles = numObstacles
        world.percentObsDensity = percentObsDensity

        return world

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addLine [as 别名]
 def doFTDraw(self, unusedrobotstate):
     frames = []
     fts = []
     vis_names = []
     if (hasattr(self.robotStateJointController, 'lastRobotStateMessage') and 
         self.robotStateJointController.lastRobotStateMessage):
         if self.draw_right:
             rft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_force +
               self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_torque)
             rft[0] = -1.*rft[0] # right FT looks to be rotated 180deg around the z axis
             rft[1] = -1.*rft[1]
             rft[3] = -1.*rft[3]
             rft[4] = -1.*rft[4]
             rft -= self.ft_right_bias
             fts.append(rft)
             frames.append(self.robotStateModel.getLinkFrame('r_hand_force_torque'))
             vis_names.append('ft_right')
         if self.draw_left:
             lft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_force +
               self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_torque)
             lft -= self.ft_left_bias
             fts.append(lft)
             frames.append(self.robotStateModel.getLinkFrame('l_hand_force_torque'))
             vis_names.append('ft_left')
         
     for i in range(0, len(frames)):
         frame = frames[i]
         ft = fts[i]
         offset = [0., 0., 0.]
         p1 = frame.TransformPoint(offset)
         scale = 1.0/25.0 # todo add slider for this?
         scalet = 1.0 / 2.5 
         p2f = frame.TransformPoint(offset + ft[0:3]*scale)
         p2t = frame.TransformPoint(offset + ft[3:6])
         normalt = (np.array(p2t) - np.array(p1))
         normalt = normalt /  np.linalg.norm(normalt)
         d = DebugData()
         # force
         if np.linalg.norm(np.array(p2f) - np.array(p1)) < 0.1:
             d.addLine(p1, p2f, color=[1.0, 0.0, 0.0])
         else:
             d.addArrow(p1, p2f, color=[1.,0.,0.])
         # torque
         if self.draw_torque:
             d.addCircle(p1, normalt, scalet*np.linalg.norm(ft[3:6]))
         # frame (largely for debug)
         vis.updateFrame(frame, vis_names[i]+'frame', view=self.view, parent='wristft', visible=False, scale=0.2)
         vis.updatePolyData(d.getPolyData(), vis_names[i], view=self.view, parent='wristft')

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

        radius = self.properties.getProperty('Radius')
        thickness = 0.03

        folder = om.getOrCreateContainer('affordances')
        frame = self.computeValveFrame()
        d = DebugData()
        d.addLine(np.array([0, 0, -thickness/2.0]), np.array([0, 0, thickness/2.0]), radius=radius)
        mesh = d.getPolyData()
        params = dict(radius=radius, length=thickness, xwidth=radius, ywidth=radius, zwidth=thickness, otdf_type='steering_cyl', friendly_name='valve')

        affordance = vis.showPolyData(mesh, 'valve', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder, alpha=1.0)
        frame = vis.showFrame(frame, 'valve frame', parent=affordance, visible=False, scale=radius)
        affordance.actor.SetUserTransform(frame.transform)
        affordance.setAffordanceParams(params)
        affordance.updateParamsFromActorTransform()

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

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

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


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

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