示例#1
0
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
示例#2
0
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)
def setupStrings():
    d = DebugData()
    poles = [[-.36,.5,1.5],[-.36,-.5,1.5],[0,.5,1.5],[0,-.5,1.5],[.36,.5,1.5],[.36,-.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 = [
        [-.36, .5, .09, 0, -.5, 1.77],
        [-.36, .5, .74, -.36, -.5, .95],
        [-.36, .5, 1.12, -.36, -.5, 1.68],
        [-.36, .5, 1.33, .36, -.5, 2.29],
        [-.36, .5, 1.6, .36, -.5, 1.62],
        [-.36, .5, 1.74, .36, -.5, 1.93],
        [-.36, .5, 2.15, -.36, -.5, 1.46],
        [0, .5, .765, 0, -.5, .795],
        [0, .5, 1.15, .36, -.5, 1.15],
        [0, .5, 1.28, -.36, -.5, .11],
        [0, .5, 1.42, 0, -.5, 1.42],
        [0, .5, 1.78, .36, -.5, .12],
        [0, .5, 2.05, -.36, -.5, 1.835],
        [.36, .5, .8, -.36, -.5, 1.11],
        [.36, .5, 1.16, -.36, -.5, 1.47],
        [.36, .5, 1.61, .36, -.5, 1.19],
        [.36, .5, 2.0, .36, -.5, 2.1],
        [-.36, .3, 0, -.36, .3, 2.01],
        [0, -.34, 0, 0, -.34, 1.42],
        [.36, 0, 0, .36, 0, 2.05],
    ]
    for string in strings:
        p1 = string[:3]
        p2 = string[3:]
        d.addLine(p1,p2,radius=.001,color=[255,0,0])
    vis.updatePolyData(d.getPolyData(), 'strings')
示例#4
0
    def buildDonutWorld():
        print "building donut world"

        lineRadius = 0.2
        hallwayWidth = 10
        numSegments = 6
        circleRadius = 50
        d = DebugData()

        angleList = np.linspace(0, 2 * np.pi, numSegments + 1)
        for i in range(numSegments):
            theta = angleList[i]
            theta_next = angleList[i + 1]

            for radius in [circleRadius, circleRadius + hallwayWidth]:
                lineStart = [radius * np.cos(theta), radius * np.sin(theta), 0]
                lineEnd = [
                    radius * np.cos(theta_next), radius * np.sin(theta_next), 0
                ]
                d.addLine(lineStart, lineEnd, radius=lineRadius)

        obj = vis.showPolyData(d.getPolyData(), 'world')
        world = World()
        world.visObj = obj
        return world
示例#5
0
    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()
示例#6
0
    def testFindWalls(self):
        raycastDistance = self.sensor.raycastAllFromCurrentFrameLocation()

        wallsFound = self.findWalls(raycastDistance, addNoise=True)

        carTransform = om.findObjectByName('robot frame').transform
        d = DebugData()

        for wallData in wallsFound:
            intercept = wallData['ransacFit'][0]
            slope = wallData['ransacFit'][1]
            wallDirectionInCarFrame = np.array([1.0, slope, 0.0])
            wallPointInCarFrame = np.array([0.0, intercept, 0.0])

            # now need to transform these to world frame in order to plot them.
            wallPointWorldFrame = np.array(
                carTransform.TransformPoint(wallPointInCarFrame))
            wallDirectionWorldFrame = np.array(
                carTransform.TransformVector(wallDirectionInCarFrame))
            wallDirectionWorldFrame = 1 / np.linalg.norm(
                wallDirectionWorldFrame) * wallDirectionWorldFrame
            lineLength = 15.0
            lineOrigin = wallPointWorldFrame - lineLength * wallDirectionWorldFrame
            lineEnd = wallPointWorldFrame + lineLength * wallDirectionWorldFrame

            d.addLine(lineOrigin, lineEnd, radius=0.3, color=[0, 0, 1])

        vis.updatePolyData(d.getPolyData(),
                           'line estimate',
                           colorByName='RGB255')
        return wallsFound
示例#7
0
    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()
示例#8
0
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)
示例#9
0
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()
示例#10
0
    def __init__(self, robotSystem, view):

        self.robotStateModel = robotSystem.robotStateModel
        self.robotStateJointController = robotSystem.robotStateJointController
        self.robotSystem = robotSystem
        self.lFootFtFrameId = self.robotStateModel.model.findLinkID(
            self.robotSystem.ikPlanner.leftFootLink)
        self.rFootFtFrameId = self.robotStateModel.model.findLinkID(
            self.robotSystem.ikPlanner.rightFootLink)
        self.leftInContact = 0
        self.rightInContact = 0
        self.view = view
        self.ddDrakeWrapper = PythonQt.dd.ddDrakeWrapper()

        self.warningButton = QtGui.QLabel('COP Warning')
        self.warningButton.setStyleSheet("background-color:white")
        self.dialogVisible = False

        d = DebugData()
        vis.updatePolyData(d.getPolyData(),
                           'measured cop',
                           view=self.view,
                           parent='robot state model')
        om.findObjectByName('measured cop').setProperty('Visible', False)

        self.robotStateModel.connectModelChanged(self.update)
示例#11
0
    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
示例#12
0
 def updateGeometryFromProperties(self):
     d = DebugData()
     length = self.getProperty('Length')
     d.addCapsule(center=(0, 0, 0),
                  axis=(0, 0, 1),
                  length=self.getProperty('Length'),
                  radius=self.getProperty('Radius'))
     self.setPolyData(d.getPolyData())
示例#13
0
    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)
示例#14
0
    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, boundaryType="Square")
        #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
示例#15
0
    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())
示例#16
0
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)
示例#17
0
def rayDebug(position, ray):
    d = DebugData()
    d.addLine(position, position + ray * 5.0)
    drcView = app.getViewManager().findView('DRC View')
    obj = vis.updatePolyData(d.getPolyData(),
                             'camera ray',
                             view=drcView,
                             color=[0, 1, 0])
    obj.actor.GetProperty().SetLineWidth(2)
示例#18
0
 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())
示例#19
0
    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())
示例#20
0
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)
示例#21
0
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)
示例#22
0
    def onSpawnMesh(self):

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

        pose = transformUtils.poseFromTransform(self.getSpawnFrame())
        desc = dict(classname='MeshAffordanceItem', Name='mesh', Filename=meshId, uuid=newUUID(), pose=pose)
        return self.affordanceFromDescription(desc)
示例#23
0
    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)
示例#24
0
 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())
示例#25
0
    def onSpawnMesh(self):

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

        pose = transformUtils.poseFromTransform(self.getSpawnFrame())
        desc = dict(classname='MeshAffordanceItem', Name='mesh', Filename=meshId, uuid=newUUID(), pose=pose)
        return self.affordanceManager.newAffordanceFromDescription(desc)
示例#26
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))
示例#27
0
    def draw(self):

        d = DebugData()

        points = [p if p is not None else self.hoverPos for p in self.points]

        # draw points
        for p in points:
            if p is not None:
                d.addSphere(p, radius=0.008)

        if self.drawLines:
            # draw lines
            for a, b in zip(points, points[1:]):
                if b is not None:
                    d.addLine(a, b)

            # connect end points
            if points[-1] is not None:
                d.addLine(points[0], points[-1])

        self.annotationObj = vis.updatePolyData(d.getPolyData(),
                                                self.annotationName,
                                                parent=self.annotationFolder)
        self.annotationObj.setProperty('Color', [1, 0, 0])
        self.annotationObj.actor.SetPickable(False)
示例#28
0
文件: world.py 项目: vtjeng/DirectSim
    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
示例#29
0
def loadFootMeshes():
    meshes = []
    for i in  range(0,2):
        d = DebugData()

        for footMeshFile in _footMeshFiles[i]:
          d.addPolyData(ioUtils.readPolyData( footMeshFile , computeNormals=True))

        t = vtk.vtkTransform()
        t.Scale(0.98, 0.98, 0.98)
        pd = filterUtils.transformPolyData(d.getPolyData(), t)
        meshes.append(pd)
    return meshes
示例#30
0
    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()
示例#31
0
 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())
示例#32
0
def loadFootMeshes():
    meshes = []
    for i in  range(0,2):
        d = DebugData()

        for footMeshFile in _footMeshFiles[i]:
          d.addPolyData(ioUtils.readPolyData( footMeshFile , computeNormals=True))

        t = vtk.vtkTransform()
        t.Scale(0.98, 0.98, 0.98)
        pd = filterUtils.transformPolyData(d.getPolyData(), t)
        meshes.append(pd)
    return meshes
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")
示例#34
0
    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')
示例#35
0
    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)
    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
示例#37
0
    def getCameraFrustumGeometry(self, rayLength):

        camPos, rays = self.getCameraFrustumRays()

        rays = [rayLength*r for r in rays]

        d = DebugData()
        d.addLine(camPos, camPos+rays[0])
        d.addLine(camPos, camPos+rays[1])
        d.addLine(camPos, camPos+rays[2])
        d.addLine(camPos, camPos+rays[3])
        d.addLine(camPos+rays[0], camPos+rays[1])
        d.addLine(camPos+rays[1], camPos+rays[2])
        d.addLine(camPos+rays[2], camPos+rays[3])
        d.addLine(camPos+rays[3], camPos+rays[0])
        return d.getPolyData()
    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)
示例#39
0
    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()
示例#40
0
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)
示例#41
0
    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()
示例#42
0
    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)
示例#43
0
 def buildSimpleWorld():
     print "this is a test"
     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
示例#44
0
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
示例#45
0
 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)."
    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')
示例#47
0
def drawPolytope(msg):
    _id = msg.id;
    if msg.remove:
        om.removeFromObjectModel(om.findObjectByName('IRIS polytopes'))
        return
    if msg.highlighted:
        color = [.8,.2,.2]
    else:
        color = [.8,.8,.8]
    name = 'polytope {:d}'.format(_id)
    obj = om.findObjectByName(name)
    if obj:
        om.removeFromObjectModel(obj)
    V = np.array(msg.V)
    polyData = vnp.numpyToPolyData(V.T.copy())
    vol_mesh = filterUtils.computeDelaunay3D(polyData)
    debug = DebugData()
    debug.addPolyData(vol_mesh)
    vis.showPolyData(debug.getPolyData(), name, color=color, alpha=0.4, parent='IRIS polytopes')
示例#48
0
 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')
def drawPolytope(msg):
    _id = msg.id
    if msg.remove:
        om.removeFromObjectModel(om.findObjectByName("IRIS polytopes"))
        return
    if msg.highlighted:
        color = [0.8, 0.2, 0.2]
    else:
        color = [0.8, 0.8, 0.8]
    name = "polytope {:d}".format(_id)
    obj = om.findObjectByName(name)
    if obj:
        om.removeFromObjectModel(obj)
    V = np.array(msg.V)
    polyData = vnp.numpyToPolyData(V.T.copy())
    vol_mesh = filterUtils.computeDelaunay3D(polyData)
    debug = DebugData()
    debug.addPolyData(vol_mesh)
    vis.showPolyData(debug.getPolyData(), name, color=color, alpha=0.4, parent="IRIS polytopes")
    def buildStickWorld(percentObsDensity):
        print "building stick world"

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

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

        # draw random stick obstacles
        obsLength = 2.0


        for i in xrange(numObstacles):
            firstX = worldXmin + np.random.rand()*(worldXmax-worldXmin)
            firstY = worldYmin + np.random.rand()*(worldYmax-worldYmin)
            firstEndpt = (firstX,firstY,0)
            
            randTheta = np.random.rand() * 2.0*np.pi
            secondEndpt = (firstX+obsLength*np.cos(randTheta), firstY+obsLength*np.sin(randTheta), 0)

            d.addLine(firstEndpt, secondEndpt, radius=0.2)


        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
    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")
示例#52
0
 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)."
示例#53
0
    def __init__(self, robotSystem, view):

        self.robotStateModel = robotSystem.robotStateModel
        self.robotStateJointController = robotSystem.robotStateJointController
        self.robotSystem = robotSystem
        self.lFootFtFrameId = self.robotStateModel.model.findLinkID( self.robotSystem.ikPlanner.leftFootLink)
        self.rFootFtFrameId = self.robotStateModel.model.findLinkID( self.robotSystem.ikPlanner.rightFootLink)
        self.leftInContact = 0
        self.rightInContact = 0
        self.view = view
        self.ddDrakeWrapper = PythonQt.dd.ddDrakeWrapper()

        self.warningButton = QtGui.QLabel('COP Warning')
        self.warningButton.setStyleSheet("background-color:white")
        self.dialogVisible = False

        d = DebugData()
        vis.updatePolyData(d.getPolyData(), 'measured cop', view=self.view, parent='robot state model')
        om.findObjectByName('measured cop').setProperty('Visible', False)

        
        self.robotStateModel.connectModelChanged(self.update)
示例#54
0
    def updateGeometryFromProperties(self):
        filename = self.getProperty('Filename')

        if not filename:
            polyData = vtk.vtkPolyData()
        else:
            polyData = self.getMeshManager().get(filename)

        if not polyData:

            if not os.path.isabs(filename):
                filename = os.path.join(ddapp.getDRCBaseDir(), filename)

            if os.path.isfile(filename):
                polyData = ioUtils.readPolyData(filename)
            else:
                # use axes as a placeholder mesh
                d = DebugData()
                d.addFrame(vtk.vtkTransform(), scale=0.1, tubeRadius=0.005)
                polyData = d.getPolyData()

        self.setPolyData(polyData)