Exemplo n.º 1
0
def processSnippet():

    obj = om.getOrCreateContainer('continuous')
    om.getOrCreateContainer('cont debug', obj)

    if (continuouswalkingDemo.processContinuousStereo):
        polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/block_snippet_stereo.vtp'))
        polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
    else:
        polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/block_snippet.vtp'))


    vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='continuous')
    standingFootName = 'l_foot'

    standingFootFrame = robotStateModel.getLinkFrame(standingFootName)
    vis.updateFrame(standingFootFrame, standingFootName, parent='continuous', visible=False)

    # Step 2: find all the surfaces in front of the robot (about 0.75sec)
    clusters = segmentation.findHorizontalSurfaces(polyData)
    if (clusters is None):
        print "No cluster found, stop walking now!"
        return

    # Step 3: find the corners of the minimum bounding rectangles
    blocks,groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame)

    footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame)

    cwdemo.drawFittedSteps(footsteps)
Exemplo n.º 2
0
    def showFusedMaps(self):
        om.removeFromObjectModel(om.findObjectByName('stereo'))
        om.getOrCreateContainer('stereo')

        q = imageManager.queue
        cameraToLocalNow = vtk.vtkTransform()
        utime = q.getCurrentImageTime('CAMERA_TSDF')

        q.getTransform('CAMERA_LEFT', 'local', utime, cameraToLocalNow)
        cameraToLocalFusedNow = vtk.vtkTransform()
        q.getTransform('CAMERA_LEFT_ALT', 'local', utime,
                       cameraToLocalFusedNow)

        for i in range(len(self.pointClouds)):

            fusedNowToLocalNow = vtk.vtkTransform()
            fusedNowToLocalNow.PreMultiply()
            fusedNowToLocalNow.Concatenate(cameraToLocalNow)
            fusedNowToLocalNow.Concatenate(
                cameraToLocalFusedNow.GetLinearInverse())

            fusedTransform = vtk.vtkTransform()
            fusedTransform.PreMultiply()
            fusedTransform.Concatenate(fusedNowToLocalNow)
            fusedTransform.Concatenate(self.cameraToLocalFusedTransforms[i])

            pd = filterUtils.transformPolyData(self.pointClouds[i],
                                               fusedTransform)
            vis.showFrame(fusedTransform, ('cloud frame ' + str(i)),
                          visible=True,
                          scale=0.2,
                          parent='stereo')
            vis.showPolyData(pd, ('stereo ' + str(i)),
                             parent='stereo',
                             colorByName='rgb_colors')
Exemplo n.º 3
0
def getFootstepsFolder():
    obj = om.findObjectByName('footstep plan')
    if obj is None:
        obj = om.getOrCreateContainer('footstep plan', parentObj=om.getOrCreateContainer('planning'))
        obj.setIcon(om.Icons.Feet)
        om.collapse(obj)
    return obj
Exemplo n.º 4
0
def getDebugFolder():
    obj = om.findObjectByName('debug')
    if obj is None:
        obj = om.getOrCreateContainer('debug',
                                      om.getOrCreateContainer('segmentation'))
        om.collapse(obj)
    return obj
Exemplo n.º 5
0
 def onHideBDIButton(self):
     self.driver.showBDIPlan = False
     self.driver.bdiRobotModel.setProperty('Visible', False)
     folder = om.getOrCreateContainer("BDI footstep plan")
     om.removeFromObjectModel(folder)
     folder = om.getOrCreateContainer("BDI adj footstep plan")
     om.removeFromObjectModel(folder)
Exemplo n.º 6
0
 def onHideBDIButton(self):
     self.driver.showBDIPlan = False
     self.driver.bdiRobotModel.setProperty('Visible', False)
     folder = om.getOrCreateContainer("BDI footstep plan")
     om.removeFromObjectModel(folder)
     folder = om.getOrCreateContainer("BDI adj footstep plan")
     om.removeFromObjectModel(folder)
Exemplo n.º 7
0
def getFootstepsFolder():
    obj = om.findObjectByName('footstep plan')
    if obj is None:
        obj = om.getOrCreateContainer('footstep plan', parentObj=om.getOrCreateContainer('planning'))
        obj.setIcon(om.Icons.Feet)
        om.collapse(obj)
    return obj
Exemplo n.º 8
0
    def showFusedMaps(self):
        om.removeFromObjectModel(om.findObjectByName('stereo'))
        om.getOrCreateContainer('stereo')

        q = imageManager.queue
        cameraToLocalNow = vtk.vtkTransform()
        utime = q.getCurrentImageTime('CAMERA_TSDF')

        q.getTransform('CAMERA_LEFT','local', utime,cameraToLocalNow)
        cameraToLocalFusedNow = vtk.vtkTransform()
        q.getTransform('CAMERA_LEFT_ALT','local', utime,cameraToLocalFusedNow)

        for i in range(len(self.pointClouds)):

            fusedNowToLocalNow = vtk.vtkTransform()
            fusedNowToLocalNow.PreMultiply()
            fusedNowToLocalNow.Concatenate( cameraToLocalNow)
            fusedNowToLocalNow.Concatenate( cameraToLocalFusedNow.GetLinearInverse() )


            fusedTransform = vtk.vtkTransform()
            fusedTransform.PreMultiply()
            fusedTransform.Concatenate( fusedNowToLocalNow)
            fusedTransform.Concatenate( self.cameraToLocalFusedTransforms[i] )

            pd = filterUtils.transformPolyData(self.pointClouds[i], fusedTransform)
            vis.showFrame(fusedTransform, ('cloud frame ' + str(i)), visible=True, scale=0.2, parent='stereo')
            vis.showPolyData(pd, ('stereo ' + str(i)), parent='stereo', colorByName='rgb_colors')
Exemplo n.º 9
0
    def drawBDIFootstepPlanAdjusted(self):
        if (self.bdi_plan_adjusted is None):
            return

        folder = om.getOrCreateContainer('BDI adj footstep plan')
        om.removeFromObjectModel(folder)

        folder = om.getOrCreateContainer('BDI adj footstep plan')
        folder.setIcon(om.Icons.Feet)
        om.collapse(folder)
        self.drawFootstepPlan(self.bdi_plan_adjusted, folder, [1.0, 1.0, 0.0] , [0.0, 1.0, 1.0])
Exemplo n.º 10
0
    def drawBDIFootstepPlanAdjusted(self):
        if (self.bdi_plan_adjusted is None):
            return

        folder = om.getOrCreateContainer('BDI adj footstep plan')
        om.removeFromObjectModel(folder)

        folder = om.getOrCreateContainer('BDI adj footstep plan')
        folder.setIcon(om.Icons.Feet)
        om.collapse(folder)
        self.drawFootstepPlan(self.bdi_plan_adjusted, folder, [1.0, 1.0, 0.0] , [0.0, 1.0, 1.0])
Exemplo n.º 11
0
    def showHandSamples(self, numberOfSamples=15):

        om.removeFromObjectModel(om.findObjectByName('sampled hands'))
        handFolder = om.getOrCreateContainer('sampled hands', parentObj=om.getOrCreateContainer('debug'))

        for i in xrange(numberOfSamples):
            t = self.splineInterp(i/float(numberOfSamples-1))
            handObj, f = self.handFactory.placeHandModelWithTransform(t, self.view, side=self.side, name='sample %d' % i, parent=handFolder)
            handObj.setProperty('Alpha', 0.3)

        handFolder.setProperty('Visible', False)
Exemplo n.º 12
0
def getTerrainSlicesFolder():
    obj = om.findObjectByName('terrain slices')
    if obj is None:
        obj = om.getOrCreateContainer('terrain slices', parentObj=getFootstepsFolder())
        obj.setProperty('Visible', False)
        om.collapse(obj)
    return obj
Exemplo n.º 13
0
def _addPlanItem(plan, name, itemClass):
    assert plan is not None
    item = itemClass(name)
    item.plan = plan
    om.removeFromObjectModel(om.findObjectByName(name))
    om.addToObjectModel(item, om.getOrCreateContainer('segmentation'))
    return item
Exemplo n.º 14
0
def _addPlanItem(plan, name, itemClass):
        assert plan is not None
        item = itemClass(name)
        item.plan = plan
        om.removeFromObjectModel(om.findObjectByName(name))
        om.addToObjectModel(item, om.getOrCreateContainer('segmentation'))
        return item
Exemplo n.º 15
0
def showPolyData(polyData, name, color=None, colorByName=None, colorByRange=None, alpha=1.0, visible=True, view=None, parent='segmentation', cls=None):

    view = view or app.getCurrentRenderView()
    assert view

    cls = cls or PolyDataItem
    item = cls(name, polyData, view)

    if isinstance(parent, str):
        parentObj = om.getOrCreateContainer(parent)
    else:
        parentObj = parent

    om.addToObjectModel(item, parentObj)
    item.setProperty('Visible', visible)
    item.setProperty('Alpha', alpha)

    if colorByName and colorByName not in item.getArrayNames():
        print 'showPolyData(colorByName=%s): array not found' % colorByName
        colorByName = None

    if colorByName:
        item.setProperty('Color By', colorByName)
        item.colorBy(colorByName, colorByRange)

    else:
        color = [1.0, 1.0, 1.0] if color is None else color
        item.setProperty('Color', [float(c) for c in color])
        item.colorBy(None)

    return item
Exemplo n.º 16
0
    def newPolyData(self, name, view, parent=None):
        polyData = self.getNewHandPolyData()

        if isinstance(parent, str):
            parent = om.getOrCreateContainer(parent)

        color = [1.0, 1.0, 0.0]
        if self.side == 'right':
            color = [0.33, 1.0, 0.0]
        obj = vis.showPolyData(polyData,
                               name,
                               view=view,
                               color=color,
                               visible=False,
                               parent=parent)
        obj.side = self.side
        frame = vtk.vtkTransform()
        frame.PostMultiply()
        obj.actor.SetUserTransform(frame)
        frameObj = vis.showFrame(frame,
                                 '%s frame' % name,
                                 view=view,
                                 scale=0.2,
                                 visible=False,
                                 parent=obj)
        return obj
Exemplo n.º 17
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()
Exemplo n.º 18
0
def getBDIAdjustedFootstepsFolder():
    obj = om.findObjectByName('BDI adj footstep plan')
    if obj is None:
        obj = om.getOrCreateContainer('BDI adj footstep plan')
        obj.setIcon(om.Icons.Feet)
        om.collapse(obj)
    return obj
Exemplo n.º 19
0
    def planSequenceRoomMap(self):
        self.graspingHand = 'left'
        self.targetSweepType = 'orientation'
        self.graspToHandLinkFrame = self.ikPlanner.newGraspToHandFrame(
            self.graspingHand)
        self.planFromCurrentRobotState = False
        self.plans = []
        self.currentYawDegrees = 60
        self.ikPlanner.ikServer.maxDegreesPerSecond = 10

        self.nextPosition = [0, 0, 0]
        self.targetPath = []
        self.resetTargetPath()
        self.fromTop = True

        self.mapFolder = om.getOrCreateContainer('room mapping')
        om.collapse(self.mapFolder)

        # taskQueue doesnt support a while loop:
        #while (self.currentYawDegrees >= -90):
        #    self.getRoomSweepFrames()
        #    self.planRoomReach()# move to next start point
        #    self.planRoomSweep()        # reach down/up
        #    self.currentYawDegrees = self.currentYawDegrees - 30
        #    self.fromTop = not self.fromTop

        self.getRoomSweepFrames()
        self.planRoomReach()  # move to next start point
        self.planRoomSweep()  # reach down/up
        self.moveRoomSweepOnwards()

        self.getRoomSweepFrames()
        self.planRoomReach()  # move to next start point
        self.planRoomSweep()  # reach down/up
        self.moveRoomSweepOnwards()

        self.getRoomSweepFrames()
        self.planRoomReach()  # move to next start point
        self.planRoomSweep()  # reach down/up
        self.moveRoomSweepOnwards()

        self.getRoomSweepFrames()
        self.planRoomReach()  # move to next start point
        self.planRoomSweep()  # reach down/up
        self.moveRoomSweepOnwards()

        self.getRoomSweepFrames()
        self.planRoomReach()  # move to next start point
        self.planRoomSweep()  # reach down/up
        self.moveRoomSweepOnwards()

        self.getRoomSweepFrames()
        self.planRoomReach()  # move to next start point
        self.planRoomSweep()  # reach down/up
        self.moveRoomSweepOnwards()

        self.getRoomSweepFrames()
        self.planRoomReach()  # move to next start point
        self.planRoomSweep()  # reach down/up
        self.moveRoomSweepOnwards()
Exemplo n.º 20
0
def loadRobotModel(name, view=None, parent='planning', urdfFile=None, color=None, visible=True):

    if not urdfFile:
        urdfFile = urdfConfig['default']

    if isinstance(parent, str):
        parent = om.getOrCreateContainer(parent)

    model = loadRobotModelFromFile(urdfFile)
    if not model:
        raise Exception('Error loading robot model from file: %s' % urdfFile)

    obj = RobotModelItem(model)
    om.addToObjectModel(obj, parent)

    obj.setProperty('Visible', visible)
    obj.setProperty('Name', name)
    obj.setProperty('Color', color or getRobotGrayColor())
    if view is not None:
        obj.addToView(view)

    jointController = jointcontrol.JointController([obj], fixedPointFile)
    jointController.setNominalPose()

    return obj, jointController
Exemplo n.º 21
0
def loadRobotModel(name, view=None, parent='planning', urdfFile=None, color=None, visible=True):

    if not urdfFile:
        urdfFile = urdfConfig['default']

    if isinstance(parent, str):
        parent = om.getOrCreateContainer(parent)

    model = loadRobotModelFromFile(urdfFile)
    if not model:
        raise Exception('Error loading robot model from file: %s' % urdfFile)

    obj = RobotModelItem(model)
    om.addToObjectModel(obj, parent)

    obj.setProperty('Visible', visible)
    obj.setProperty('Name', name)
    obj.setProperty('Color', color or getRobotGrayColor())
    if view is not None:
        obj.addToView(view)

    jointController = jointcontrol.JointController([obj], fixedPointFile)
    jointController.setNominalPose()

    return obj, jointController
Exemplo n.º 22
0
def getBDIAdjustedFootstepsFolder():
    obj = om.findObjectByName('BDI adj footstep plan')
    if obj is None:
        obj = om.getOrCreateContainer('BDI adj footstep plan')
        obj.setIcon(om.Icons.Feet)
        om.collapse(obj)
    return obj
Exemplo n.º 23
0
    def newPolyData(self, name, view, parent=None):
        self.handModel.model.setJointPositions(
            np.zeros(self.handModel.model.numberOfJoints()))
        polyData = vtk.vtkPolyData()
        self.handModel.model.getModelMesh(polyData)
        polyData = filterUtils.transformPolyData(polyData, self.modelToPalm)

        if isinstance(parent, str):
            parent = om.getOrCreateContainer(parent)

        color = [1.0, 1.0, 0.0]
        if self.side == 'right':
            color = [0.33, 1.0, 0.0]
        obj = vis.showPolyData(polyData,
                               name,
                               view=view,
                               color=color,
                               visible=False,
                               parent=parent)
        obj.side = self.side
        frame = vtk.vtkTransform()
        frame.PostMultiply()
        obj.actor.SetUserTransform(frame)
        frameObj = vis.showFrame(frame,
                                 '%s frame' % name,
                                 view=view,
                                 scale=0.2,
                                 visible=False,
                                 parent=obj)
        return obj
Exemplo n.º 24
0
def init(view):
    global _multisenseItem
    global multisenseDriver


    m = MultiSenseSource(view)
    m.start()
    multisenseDriver = m

    sensorsFolder = om.getOrCreateContainer('sensors')

    _multisenseItem = MultisenseItem(m)
    om.addToObjectModel(_multisenseItem, sensorsFolder)

    useMapServer = hasattr(drc, 'vtkMapServerSource')
    if useMapServer:
        mapServerSource = MapServerSource(view, callbackFunc=view.render)
        mapsServerContainer = om.ObjectModelItem('Map Server', icon=om.Icons.Robot)
        mapsServerContainer.source = mapServerSource
        om.addToObjectModel(mapsServerContainer, parentObj=sensorsFolder)
        mapServerSource.start()
    else:
        mapServerSource = None


    spindleDebug = SpindleAxisDebug(multisenseDriver)
    spindleDebug.addToView(view)
    om.addToObjectModel(spindleDebug, sensorsFolder)


    return multisenseDriver, mapServerSource
Exemplo n.º 25
0
def getTerrainSlicesFolder():
    obj = om.findObjectByName('terrain slices')
    if obj is None:
        obj = om.getOrCreateContainer('terrain slices', parentObj=getFootstepsFolder())
        obj.setProperty('Visible', False)
        om.collapse(obj)
    return obj
Exemplo n.º 26
0
    def makeSplineGraspConstraints(self, ikPlanner, positionTolerance=0.03, angleToleranceInDegrees=15):

        params = self.computeHandleParameterization()
        segments = zip(params, params[1:])
        #times = [np.linspace(segment[0], segment[1], 6) for segment in segments]
        #times = [[0.0, 0.3, 0.5], np.linspace(params[-2], params[-1], 6)]

        times = np.linspace(params[-2], params[-1], 6)
        times = np.hstack(times)
        times = np.unique(times)

        frames = []
        for t in times:
            frames.append(self.splineInterp(t))


        folder = om.getOrCreateContainer('constraint spline samples', parentObj=om.getOrCreateContainer('debug'))
        for f in frames:
            vis.showFrame(f, 'frame', scale=0.1)


        side = self.side
        graspToPalm = vtk.vtkTransform()
        graspToHand = ikPlanner.newGraspToHandFrame(side, graspToPalm)

        constraints = []

        for f, t in zip(frames[:-1], times[:-1]):
            graspToWorld = f
            p, q = ikPlanner.createPositionOrientationGraspConstraints(side, graspToWorld, graspToHand)

            p.lowerBound = np.tile(-positionTolerance, 3)
            p.upperBound = np.tile(positionTolerance, 3)
            q.angleToleranceInDegrees = angleToleranceInDegrees

            if t >= params[-2]:
                q.angleToleranceInDegrees = 0
                p.lowerBound = np.tile(-0.0, 3)
                p.upperBound = np.tile(0.0, 3)
                constraints.append(q)

            p.tspan = [t, t]
            q.tspan = [t, t]
            constraints.append(p)


        return constraints
Exemplo n.º 27
0
    def planSequenceRoomMap(self):
        self.graspingHand = 'left'
        self.targetSweepType = 'orientation'
        self.graspToHandLinkFrame = self.ikPlanner.newGraspToHandFrame(self.graspingHand)
        self.planFromCurrentRobotState = False
        self.plans = []
        self.currentYawDegrees = 60
        self.ikPlanner.ikServer.maxDegreesPerSecond = 10

        self.nextPosition =[0,0,0]
        self.targetPath = []
        self.resetTargetPath()
        self.fromTop = True

        self.mapFolder=om.getOrCreateContainer('room mapping')
        om.collapse(self.mapFolder)

        # taskQueue doesnt support a while loop:
        #while (self.currentYawDegrees >= -90):
        #    self.getRoomSweepFrames()
        #    self.planRoomReach()# move to next start point
        #    self.planRoomSweep()        # reach down/up
        #    self.currentYawDegrees = self.currentYawDegrees - 30
        #    self.fromTop = not self.fromTop

        self.getRoomSweepFrames()
        self.planRoomReach()# move to next start point
        self.planRoomSweep()        # reach down/up
        self.moveRoomSweepOnwards()

        self.getRoomSweepFrames()
        self.planRoomReach()# move to next start point
        self.planRoomSweep()        # reach down/up
        self.moveRoomSweepOnwards()

        self.getRoomSweepFrames()
        self.planRoomReach()# move to next start point
        self.planRoomSweep()        # reach down/up
        self.moveRoomSweepOnwards()

        self.getRoomSweepFrames()
        self.planRoomReach()# move to next start point
        self.planRoomSweep()        # reach down/up
        self.moveRoomSweepOnwards()

        self.getRoomSweepFrames()
        self.planRoomReach()# move to next start point
        self.planRoomSweep()        # reach down/up
        self.moveRoomSweepOnwards()

        self.getRoomSweepFrames()
        self.planRoomReach()# move to next start point
        self.planRoomSweep()        # reach down/up
        self.moveRoomSweepOnwards()

        self.getRoomSweepFrames()
        self.planRoomReach()# move to next start point
        self.planRoomSweep()        # reach down/up
        self.moveRoomSweepOnwards()
Exemplo n.º 28
0
 def findEndEffectors(self):
     planning = om.getOrCreateContainer('planning')
     if planning is not None:
         teleopFolder = planning.findChild('teleop plan')
         if teleopFolder is not None:
             framesFolder = teleopFolder.findChild('constraint frames')
             if framesFolder is not None:
                 self.endEffectorFrames = framesFolder.children()
Exemplo n.º 29
0
 def _loadAffordanceFromDescription(self, desc):
     className = desc['classname']
     cls = getattr(affordanceitems, className)
     aff = cls(desc['Name'], self.view)
     om.addToObjectModel(aff, parentObj=om.getOrCreateContainer('affordances'))
     vis.addChildFrame(aff)
     aff.loadDescription(desc, copyMode=aff.COPY_MODE_ALL)
     self.registerAffordance(aff, notify=False)
Exemplo n.º 30
0
 def findEndEffectors(self):
     planning = om.getOrCreateContainer('planning')
     if planning is not None:
         teleopFolder = planning.findChild('teleop plan')
         if teleopFolder is not None:
             framesFolder = teleopFolder.findChild('constraint frames')
             if framesFolder is not None:
                 self.endEffectorFrames = framesFolder.children()
Exemplo n.º 31
0
    def computeFootstepPlanSafeRegions(self, blocks, robotPose, standingFootName):

        print 'planning with safe regions.  %d blocks.' % len(blocks)

        folder = om.getOrCreateContainer('Safe terrain regions')
        om.removeFromObjectModel(folder)

        footsteps = []

        for i, block in enumerate(blocks):
            corners = block.getCorners()
            rpy = np.radians(block.cornerTransform.GetOrientation())
            #rpy = [0.0, 0.0, 0.0]
            self.convertStepToSafeRegion(corners, rpy)

        lastBlock = blocks[-1]

        goalFrame = transformUtils.copyFrame(lastBlock.cornerTransform)
        goalOffset = vtk.vtkTransform()
        goalOffset.Translate(0.3, lastBlock.rectWidth/2.0, 0.0)
        goalFrame.PreMultiply()
        goalFrame.Concatenate(goalOffset)
        goalPosition = np.array(goalFrame.GetPosition())

        if len(blocks) > 1:
            goalFrame = transformUtils.copyFrame(blocks[-2].cornerTransform)
            goalFrame.Translate(goalPosition - np.array(goalFrame.GetPosition()))

        vis.updateFrame(goalFrame, 'footstep plan goal', scale=0.2)

        request = self.footstepsPanel.driver.constructFootstepPlanRequest(robotPose, goalFrame)

        assert standingFootName in (self.ikPlanner.leftFootLink, self.ikPlanner.rightFootLink)
        if standingFootName == self.ikPlanner.rightFootLink:
            leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_RIGHT
        else:
            leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_LEFT

        request.params.leading_foot = leadingFoot
        request.params.max_forward_step = 0.5
        request.params.nom_forward_step = 0.12
        request.params.nom_step_width = 0.22
        request.params.max_num_steps = 8 #2*len(blocks)

        plan = self.footstepsPanel.driver.sendFootstepPlanRequest(request, waitForResponse=True)

        if not plan:
            return []

        #print 'received footstep plan with %d steps.' % len(plan.footsteps)

        footsteps = []
        for i, footstep in enumerate(plan.footsteps):
            footstepTransform = self.transformFromFootstep(footstep)
            footsteps.append(Footstep(footstepTransform, footstep.is_right_foot))

        return footsteps[2:]
Exemplo n.º 32
0
 def _loadAffordanceFromDescription(self, desc):
     className = desc['classname']
     cls = getattr(affordanceitems, className)
     aff = cls(desc['Name'], self.view)
     om.addToObjectModel(aff,
                         parentObj=om.getOrCreateContainer('affordances'))
     vis.addChildFrame(aff)
     aff.loadDescription(desc, copyMode=aff.COPY_MODE_ALL)
     self.registerAffordance(aff, notify=False)
Exemplo n.º 33
0
    def computeFootstepPlanSafeRegions(self, blocks, robotPose, standingFootName):

        print 'planning with safe regions.  %d blocks.' % len(blocks)

        folder = om.getOrCreateContainer('Safe terrain regions')
        om.removeFromObjectModel(folder)

        footsteps = []

        for i, block in enumerate(blocks):
            corners = block.getCorners()
            rpy = np.radians(block.cornerTransform.GetOrientation())
            #rpy = [0.0, 0.0, 0.0]
            self.convertStepToSafeRegion(corners, rpy)

        lastBlock = blocks[-1]

        goalFrame = transformUtils.copyFrame(lastBlock.cornerTransform)
        goalOffset = vtk.vtkTransform()
        goalOffset.Translate(0.3, lastBlock.rectWidth/2.0, 0.0)
        goalFrame.PreMultiply()
        goalFrame.Concatenate(goalOffset)
        goalPosition = np.array(goalFrame.GetPosition())

        if len(blocks) > 1:
            goalFrame = transformUtils.copyFrame(blocks[-2].cornerTransform)
            goalFrame.Translate(goalPosition - np.array(goalFrame.GetPosition()))

        vis.updateFrame(goalFrame, 'footstep plan goal', scale=0.2)

        request = self.footstepsPanel.driver.constructFootstepPlanRequest(robotPose, goalFrame)

        assert standingFootName in (self.ikPlanner.leftFootLink, self.ikPlanner.rightFootLink)
        if standingFootName == self.ikPlanner.rightFootLink:
            leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_RIGHT
        else:
            leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_LEFT

        request.params.leading_foot = leadingFoot
        request.params.max_forward_step = 0.5
        request.params.nom_forward_step = 0.12
        request.params.nom_step_width = 0.22
        request.params.max_num_steps = 8 #2*len(blocks)

        plan = self.footstepsPanel.driver.sendFootstepPlanRequest(request, waitForResponse=True)

        if not plan:
            return []

        #print 'received footstep plan with %d steps.' % len(plan.footsteps)

        footsteps = []
        for i, footstep in enumerate(plan.footsteps):
            footstepTransform = self.transformFromFootstep(footstep)
            footsteps.append(Footstep(footstepTransform, footstep.is_right_foot))

        return footsteps[2:]
Exemplo n.º 34
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))
Exemplo n.º 35
0
    def planTargetReach(self):
        # A single one shot gaze-constrained reach: place xyz at goal and align y-axis of hand with x-axis of goal
        worldToTargetFrame = vis.updateFrame(
            self.targetFrame,
            'gaze goal',
            visible=False,
            scale=0.2,
            parent=om.getOrCreateContainer('affordances'))

        self.initConstraintSet()
        self.addConstraintForTargetFrame(worldToTargetFrame, 1)
        self.planTrajectory()
Exemplo n.º 36
0
def init(view):
    global PointCloudQueue, _pointcloudItem, _pointcloudSource
    PointCloudQueue = PythonQt.dd.ddPointCloudLCM(lcmUtils.getGlobalLCMThread())
    PointCloudQueue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
    
    _pointcloudSource = PointCloudSource(view, PointCloudQueue)
    _pointcloudSource.start()

    sensorsFolder = om.getOrCreateContainer('sensors')

    _pointcloudItem = PointCloudItem(_pointcloudSource)
    om.addToObjectModel(_pointcloudItem, sensorsFolder)
Exemplo n.º 37
0
def init(view):
    global KinectQueue, _kinectItem, _kinectSource
    KinectQueue = PythonQt.dd.ddKinectLCM(lcmUtils.getGlobalLCMThread())
    KinectQueue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)

    _kinectSource = KinectSource(view, KinectQueue)
    _kinectSource.start()

    sensorsFolder = om.getOrCreateContainer('sensors')

    _kinectItem = KinectItem(_kinectSource)
    om.addToObjectModel(_kinectItem, sensorsFolder)
Exemplo n.º 38
0
    def run(self):

        folder = om.getOrCreateContainer('affordances')
        frame = self.computeAffordanceFrame()
        mesh = segmentation.getDrillMesh()
        params = segmentation.getDrillAffordanceParams(np.array(frame.GetPosition()), [1,0,0], [0,1,0], [0,0,1])

        affordance = vis.showPolyData(mesh, 'drill', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder)
        frame = vis.showFrame(frame, 'drill frame', parent=affordance, visible=False, scale=0.2)
        affordance.actor.SetUserTransform(frame.transform)
        affordance.setAffordanceParams(params)
        affordance.updateParamsFromActorTransform()
Exemplo n.º 39
0
def init(view):
    global KinectQueue, _kinectItem, _kinectSource
    KinectQueue = PythonQt.dd.ddKinectLCM(lcmUtils.getGlobalLCMThread())
    KinectQueue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
    
    _kinectSource = KinectSource(view, KinectQueue)
    _kinectSource.start()

    sensorsFolder = om.getOrCreateContainer('sensors')

    _kinectItem = KinectItem(_kinectSource)
    om.addToObjectModel(_kinectItem, sensorsFolder)
Exemplo n.º 40
0
    def newTerrainItem(self, tform, uid=None, region=None):
        if uid is None:
            uid = self.getNewUID()
        elif uid in self.regions:
            return self.regions[uid]

        view = app.getCurrentRenderView()
        item = TerrainRegionItem(uid, view, tform, self, region)
        parentObj = om.getOrCreateContainer('Safe terrain regions')
        om.addToObjectModel(item, parentObj)
        self.regions[uid] = item
        return item
Exemplo n.º 41
0
    def newTerrainItem(self, tform, uid=None, region=None):
        if uid is None:
            uid = self.getNewUID()
        elif uid in self.regions:
            return self.regions[uid]

        view = app.getCurrentRenderView()
        item = TerrainRegionItem(uid, view, tform, self, region)
        parentObj = om.getOrCreateContainer('Safe terrain regions')
        om.addToObjectModel(item, parentObj)
        self.regions[uid] = item
        return item
Exemplo n.º 42
0
def init(view):
    global PointCloudQueue, _pointcloudItem, _pointcloudSource
    PointCloudQueue = PythonQt.dd.ddPointCloudLCM(
        lcmUtils.getGlobalLCMThread())
    PointCloudQueue.init(lcmUtils.getGlobalLCMThread(),
                         drcargs.args().config_file)

    _pointcloudSource = PointCloudSource(view, PointCloudQueue)
    _pointcloudSource.start()

    sensorsFolder = om.getOrCreateContainer('sensors')

    _pointcloudItem = PointCloudItem(_pointcloudSource)
    om.addToObjectModel(_pointcloudItem, sensorsFolder)
Exemplo n.º 43
0
def showFrame(frame, name, view=None, parent='segmentation', scale=0.35, visible=True):

    view = view or app.getCurrentRenderView()
    assert view

    if isinstance(parent, str):
        parentObj = om.getOrCreateContainer(parent)
    else:
        parentObj = parent

    item = FrameItem(name, frame, view)
    om.addToObjectModel(item, parentObj)
    item.setProperty('Visible', visible)
    item.setProperty('Scale', scale)
    return item
Exemplo n.º 44
0
def showText(text, name, fontSize=18, position=(10, 10), parent=None, view=None):

    view = view or app.getCurrentRenderView()
    assert view

    item = TextItem(name, text, view=view)
    item.setProperty('Font Size', fontSize)
    item.setProperty('Position', list(position))

    if isinstance(parent, str):
        parentObj = om.getOrCreateContainer(parent)
    else:
        parentObj = parent

    om.addToObjectModel(item, parentObj)
    return item
Exemplo n.º 45
0
    def newPolyData(self, name, view, parent=None):
        polyData = self.getNewHandPolyData()

        if isinstance(parent, str):
            parent = om.getOrCreateContainer(parent)

        color = [1.0, 1.0, 0.0]
        if self.side == 'right':
            color = [0.33, 1.0, 0.0]
        obj = vis.showPolyData(polyData, name, view=view, color=color, visible=False, parent=parent)
        obj.side = self.side
        frame = vtk.vtkTransform()
        frame.PostMultiply()
        obj.actor.SetUserTransform(frame)
        frameObj = vis.showFrame(frame, '%s frame' % name, view=view, scale=0.2, visible=False, parent=obj)
        return obj
Exemplo n.º 46
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()
Exemplo n.º 47
0
    def spawnTargetAffordance(self):
        for obj in om.getObjects():
            if obj.getProperty('Name') == 'target':
                om.removeFromObjectModel(obj)

        targetFrame = transformUtils.frameFromPositionAndRPY([0.6, 0.2, 0.6],
                                                             [180, 0, 90])

        folder = om.getOrCreateContainer('affordances')
        z = DebugData()
        z.addLine(np.array([0, 0, 0]),
                  np.array([-self.boxLength, 0, 0]),
                  radius=0.02)  # main bar
        z.addLine(np.array([-self.boxLength, 0, 0]),
                  np.array([-self.boxLength, 0, self.boxLength]),
                  radius=0.02)  # main bar
        z.addLine(np.array([-self.boxLength, 0, self.boxLength]),
                  np.array([0, 0, self.boxLength]),
                  radius=0.02)  # main bar
        z.addLine(np.array([0, 0, self.boxLength]),
                  np.array([0, 0, 0]),
                  radius=0.02)  # main bar
        targetMesh = z.getPolyData()

        self.targetAffordance = vis.showPolyData(
            targetMesh,
            'target',
            color=[0.0, 1.0, 0.0],
            cls=affordanceitems.FrameAffordanceItem,
            parent=folder,
            alpha=0.3)
        self.targetAffordance.actor.SetUserTransform(targetFrame)
        self.targetFrame = vis.showFrame(targetFrame,
                                         'target frame',
                                         parent=self.targetAffordance,
                                         visible=False,
                                         scale=0.2)
        self.targetFrame = self.targetFrame.transform

        params = dict(length=self.boxLength,
                      otdf_type='target',
                      friendly_name='target')
        self.targetAffordance.setAffordanceParams(params)
        self.targetAffordance.updateParamsFromActorTransform()
Exemplo n.º 48
0
    def newPolyData(self, name, view, parent=None):
        self.handModel.model.setJointPositions(np.zeros(self.handModel.model.numberOfJoints()))
        polyData = vtk.vtkPolyData()
        self.handModel.model.getModelMesh(polyData)
        polyData = filterUtils.transformPolyData(polyData, self.modelToPalm)

        if isinstance(parent, str):
            parent = om.getOrCreateContainer(parent)

        color = [1.0, 1.0, 0.0]
        if self.side == 'right':
            color = [0.33, 1.0, 0.0]
        obj = vis.showPolyData(polyData, name, view=view, color=color, visible=False, parent=parent)
        obj.side = self.side
        frame = vtk.vtkTransform()
        frame.PostMultiply()
        obj.actor.SetUserTransform(frame)
        frameObj = vis.showFrame(frame, '%s frame' % name, view=view, scale=0.2, visible=False, parent=obj)
        return obj
Exemplo n.º 49
0
    def onShowMapButton(self):
        # reloads the map each time - in case its changed on disk:
        #if (self.octomap_cloud is None):
        filename = ddapp.getDRCBaseDir() + "/software/build/data/octomap.pcd"
        self.octomap_cloud = io.readPolyData(filename) # c++ object called vtkPolyData

        assert (self.octomap_cloud.GetNumberOfPoints() !=0 )

        # clip point cloud to height - doesnt work very well yet. need to know robot's height
        #self.octomap_cloud = segmentation.cropToLineSegment(self.octomap_cloud, np.array([0,0,-10]), np.array([0,0,3]) )

        # access to z values
        #points= vnp.getNumpyFromVtk(self.octomap_cloud, 'Points')
        #zvalues = points[:,2]

        # remove previous map:
        folder = om.getOrCreateContainer("segmentation")
        om.removeFromObjectModel(folder)
        vis.showPolyData(self.octomap_cloud, 'prior map', alpha=1.0, color=[0,0,0.4])
Exemplo n.º 50
0
    def onShowMapButton(self):
        # reloads the map each time - in case its changed on disk:
        #if (self.octomap_cloud is None):
        filename = ddapp.getDRCBaseDir() + "/software/build/data/octomap.pcd"
        self.octomap_cloud = io.readPolyData(filename) # c++ object called vtkPolyData

        assert (self.octomap_cloud.GetNumberOfPoints() !=0 )

        # clip point cloud to height - doesnt work very well yet. need to know robot's height
        #self.octomap_cloud = segmentation.cropToLineSegment(self.octomap_cloud, np.array([0,0,-10]), np.array([0,0,3]) )

        # access to z values
        #points= vnp.getNumpyFromVtk(self.octomap_cloud, 'Points')
        #zvalues = points[:,2]

        # remove previous map:
        folder = om.getOrCreateContainer("segmentation")
        om.removeFromObjectModel(folder)
        vis.showPolyData(self.octomap_cloud, 'prior map', alpha=1.0, color=[0,0,0.4])
Exemplo n.º 51
0
    def addCollisionObject(self, obj):
        if om.getOrCreateContainer('affordances').findChild(obj.getProperty('Name') + ' affordance'):
            return # Affordance has been created previously

        frame = obj.findChild(obj.getProperty('Name') + ' frame')
        (origin, quat) = transformUtils.poseFromTransform(frame.transform)
        (xaxis, yaxis, zaxis) = transformUtils.getAxesFromTransform(frame.transform)

        # TODO: move this into transformUtils as getAxisDimensions or so
        box = obj.findChild(obj.getProperty('Name') + ' box')
        box_np = vtkNumpy.getNumpyFromVtk(box.polyData, 'Points')
        box_min = np.amin(box_np, 0)
        box_max = np.amax(box_np, 0)
        xwidth = np.linalg.norm(box_max[0]-box_min[0])
        ywidth = np.linalg.norm(box_max[1]-box_min[1])
        zwidth = np.linalg.norm(box_max[2]-box_min[2])
        name = obj.getProperty('Name') + ' affordance'

        boxAffordance = segmentation.createBlockAffordance(origin, xaxis, yaxis, zaxis, xwidth, ywidth, zwidth, name, parent='affordances')
        boxAffordance.setSolidColor(obj.getProperty('Color'))
        boxAffordance.setProperty('Alpha', 0.3)
Exemplo n.º 52
0
    def run(self):

        folder = om.getOrCreateContainer('affordances')
        frame = self.computeAffordanceFrame()
        mesh = segmentation.getDrillMesh()
        params = segmentation.getDrillAffordanceParams(
            np.array(frame.GetPosition()), [1, 0, 0], [0, 1, 0], [0, 0, 1])

        affordance = vis.showPolyData(mesh,
                                      'drill',
                                      color=[0.0, 1.0, 0.0],
                                      cls=affordanceitems.FrameAffordanceItem,
                                      parent=folder)
        frame = vis.showFrame(frame,
                              'drill frame',
                              parent=affordance,
                              visible=False,
                              scale=0.2)
        affordance.actor.SetUserTransform(frame.transform)
        affordance.setAffordanceParams(params)
        affordance.updateParamsFromActorTransform()
Exemplo n.º 53
0
    def autonomousExecuteRoomMap(self):
        self.graspingHand = 'left'
        self.targetSweepType = 'orientation'
        self.graspToHandLinkFrame = self.ikPlanner.newGraspToHandFrame(self.graspingHand)
        self.planFromCurrentRobotState = True
        self.visOnly = False
        self.ikPlanner.ikServer.maxDegreesPerSecond = 3#5
        self.currentYawDegrees = 60
        self.fromTop = True
        self.mapFolder=om.getOrCreateContainer('room mapping')

        taskQueue = AsyncTaskQueue()
        self.addTasksToQueueSweep(taskQueue)
        self.addTasksToQueueSweep(taskQueue)
        self.addTasksToQueueSweep(taskQueue)
        self.addTasksToQueueSweep(taskQueue)
        self.addTasksToQueueSweep(taskQueue)
        self.addTasksToQueueSweep(taskQueue)
        self.addTasksToQueueSweep(taskQueue)
        taskQueue.addTask(self.printAsync('done!'))
        taskQueue.addTask(self.doneIndicator)
        return taskQueue