예제 #1
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)
예제 #2
0
    def _remove_subscriber(self):
        if (self._subscriber is None):
            return

        lcmUtils.removeSubscriber(self._subscriber)
        om.removeFromObjectModel(om.findObjectByName(self._folder_name))
        self._subscriber = None
예제 #3
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")
예제 #4
0
def toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False):

    obj, _ = vis.findPickedObject(displayPoint, view)

    if not obj:
        return False

    name = obj.getProperty('Name')

    if name in ('footstep widget', 'footstep widget frame'):
        om.removeFromObjectModel(om.findObjectByName('footstep widget'))
        return True

    match = re.match('^step (\d+)$', name)
    if not match:
        return False

    stepIndex = int(match.group(1))

    existingWidget = om.findObjectByName('footstep widget')
    if existingWidget:
        previousStep = existingWidget.stepIndex
        print 'have existing widget for step:', stepIndex

        om.removeFromObjectModel(existingWidget)
        if previousStep == stepIndex:
            print 'returning because widget was for selected step'
            return True


    footMesh = shallowCopy(obj.polyData)
    footFrame = transformUtils.copyFrame(obj.getChildFrame().transform)

    if useHorizontalWidget:
        rpy = [0.0, 0.0, transformUtils.rollPitchYawFromTransform(footFrame)[2]]
        footFrame = transformUtils.frameFromPositionAndRPY(footFrame.GetPosition(), np.degrees(rpy))

    footObj = vis.showPolyData(footMesh, 'footstep widget', parent='planning', alpha=0.2)
    footObj.stepIndex = stepIndex
    frameObj = vis.showFrame(footFrame, 'footstep widget frame', parent=footObj, scale=0.2)
    footObj.actor.SetUserTransform(frameObj.transform)
    footObj.setProperty('Color', obj.getProperty('Color'))
    frameObj.setProperty('Edit', True)

    rep = frameObj.widget.GetRepresentation()
    rep.SetTranslateAxisEnabled(2, False)
    rep.SetRotateAxisEnabled(0, False)
    rep.SetRotateAxisEnabled(1, False)
    frameObj.widget.HandleRotationEnabledOff()

    walkGoal = om.findObjectByName('walking goal')
    if walkGoal:
        walkGoal.setProperty('Edit', False)


    def onFootWidgetChanged(frame):
        footstepsDriver.onStepModified(stepIndex - 1, frame)

    frameObj.connectFrameModified(onFootWidgetChanged)
    return True
예제 #5
0
    def run(self):

        polyData = self.getPointCloud()
        annotation = self.getAnnotationInput()
        annotationPoint = annotation.annotationPoints[0]
        planePoints, normal = segmentation.applyLocalPlaneFit(polyData, annotationPoint, searchRadius=0.1, searchRadiusEnd=0.2)
        viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()

        if np.dot(normal, viewDirection) < 0:
            normal = -normal

        xaxis = normal
        zaxis = [0, 0, 1]
        yaxis = np.cross(zaxis, xaxis)
        xaxis = np.cross(yaxis, zaxis)
        xaxis /= np.linalg.norm(xaxis)
        yaxis /= np.linalg.norm(yaxis)

        t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
        t.PostMultiply()
        t.Translate(annotationPoint)

        polyData = annotation.polyData
        polyData = segmentation.transformPolyData(polyData, t.GetLinearInverse())

        annotation.setProperty('Visible', False)
        om.removeFromObjectModel(om.findObjectByName('wall'))
        obj = vis.showPolyData(polyData, 'wall')
        obj.actor.SetUserTransform(t)
        vis.showFrame(t, 'wall frame', scale=0.2, parent=obj)
예제 #6
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
예제 #7
0
    def _onPropertyChanged(self, propertySet, propertyName):
        PolyDataItem._onPropertyChanged(self, propertySet, propertyName)

        if propertyName == 'Scale':
            scale = self.getProperty(propertyName)
            self.rep.SetWorldSize(scale)
            self._updateAxesGeometry()
        elif propertyName == 'Edit':
            view = app.getCurrentRenderView()
            if view not in self.views:
                view = self.views[0]
            self.widget.SetInteractor(view.renderWindow().GetInteractor())

            self.widget.SetEnabled(self.getProperty(propertyName))
            isEditing = self.getProperty(propertyName)
            if isEditing:
                frameupdater.registerFrame(self)
        elif propertyName == 'Trace':
            trace = self.getProperty(propertyName)
            if trace and not self.traceData:
                self.traceData = FrameTraceVisualizer(self)
            elif not trace and self.traceData:
                om.removeFromObjectModel(self.traceData.getTraceData())
                self.traceData = None
        elif propertyName == 'Tube':
            self.properties.setPropertyAttribute('Tube Width', 'hidden', not self.getProperty(propertyName))
            self._updateAxesGeometry()
예제 #8
0
    def promotePolyDataItem(cls, obj):
        parent = obj.parent()
        view = obj.views[0]
        name = obj.getProperty('Name')
        polyData = obj.polyData
        props = obj.properties._properties
        childFrame = obj.getChildFrame()
        if childFrame:
            t = transformUtils.copyFrame(childFrame.transform)
        else:
            t = vtk.vtkTransform()
            t.PostMultiply()
            t.Translate(filterUtils.computeCentroid(polyData))
            polyData = filterUtils.transformPolyData(polyData, t.GetLinearInverse())

        children = [c for c in obj.children() if c is not childFrame]

        meshId = cls.getMeshManager().add(polyData)

        om.removeFromObjectModel(obj)
        obj = MeshAffordanceItem(name, view)
        obj.setProperty('Filename', meshId)
        om.addToObjectModel(obj, parentObj=parent)
        frame = vis.addChildFrame(obj)
        frame.copyFrame(t)

        for child in children:
            om.addToObjectModel(child, parentObj=obj)

        obj.syncProperties(props)
        return obj
예제 #9
0
    def computeGraspFrameSamplesBoard(self):

        affordanceFrame = self.getAffordanceFrame()

        additionalOffset = 0.0
        yoffset = 0.5*self.affordance.params['ywidth'] + additionalOffset
        xoffset = 0.5*self.affordance.params['xwidth'] + additionalOffset

        frames = [
          [[0.0, yoffset, 0.0], [0.0, 90, 180.0]],
          [[0.0, yoffset, 0.0], [0.0, -90, 180.0]],

          [[0.0, -yoffset, 0.0], [0.0, 90, 0.0]],
          [[0.0, -yoffset, 0.0], [0.0, -90, 0.0]],

          [[xoffset, 0.0, 0.0], [-90, -90, 180.0]],
          [[xoffset, 0.0, 0.0], [90, 90, 180.0]],

          [[-xoffset, 0.0, 0.0], [90, -90, 180.0]],
          [[-xoffset, 0.0, 0.0], [-90, 90, 180.0]],
          ]

        for i, frame in enumerate(frames):
            pos, rpy = frame
            t = transformUtils.frameFromPositionAndRPY(pos, rpy)
            t.Concatenate(affordanceFrame.transform)
            name = 'sample grasp frame %d' % i
            om.removeFromObjectModel(self.findAffordanceChild(name))
            vis.showFrame(copyFrame(t), name, parent=self.affordance, visible=False, scale=0.2)
예제 #10
0
    def addGeometry(self, path, geomItems):
        folder = self.getPathFolder(path)
        ancestors = findPathToAncestor(folder, self.getRootFolder())
        geomTransform = vtk.vtkTransform()
        for item in reversed(ancestors):
            if not hasattr(item, "transform"):
                item.transform = vtk.vtkTransform()
                item.transform.PostMultiply()
            geomTransform.Concatenate(item.transform)

        for geom in geomItems:
            existing_item = self.getItemByPath(path + ["geometry"])
            item = geom.polyDataItem
            if existing_item is not None:
                for prop in existing_item.propertyNames():
                    item.setProperty(prop, existing_item.getProperty(prop))
                om.removeFromObjectModel(existing_item)
            else:
                item.setProperty("Point Size", 2)
                for colorBy in ["rgb", "intensity"]:
                    try:
                        item.setProperty("Color By", colorBy)
                    except ValueError:
                        pass
                    else:
                        break

            item.addToView(self.view)
            om.addToObjectModel(item, parentObj=folder)
            item.actor.SetUserTransform(geomTransform)

        return path
예제 #11
0
    def projectDrillDemoInCamera():
        q = om.findObjectByName('camera overlay')
        om.removeFromObjectModel(q)

        imageView = cameraview.views['CAMERA_LEFT']
        imageView.imageActor.SetOpacity(.2)

        drawFrameInCamera(drillDemo.drill.frame.transform, 'drill frame',visible=False)

        tf = transformUtils.copyFrame( drillDemo.drill.frame.transform )
        tf.PreMultiply()
        tf.Concatenate( drillDemo.drill.drillToButtonTransform )
        drawFrameInCamera(tf, 'drill button')


        tf2 = transformUtils.copyFrame( tf )
        tf2.PreMultiply()
        tf2.Concatenate( transformUtils.frameFromPositionAndRPY( [0,0,0] , [180,0,0] ) )
        drawFrameInCamera(tf2, 'drill button flip')

        drawObjectInCamera('drill',visible=False)

        drawObjectInCamera('sensed pointer tip')
        obj = om.findObjectByName('sensed pointer tip frame')
        if (obj is not None):
            drawFrameInCamera(obj.transform, 'sensed pointer tip frame',visible=False)

        #drawObjectInCamera('left robotiq',visible=False)
        #drawObjectInCamera('right pointer',visible=False)

        v = imageView.view
        v.render()
예제 #12
0
    def initPlanning(self, robotSystem):

        from director import objectmodel as om
        from director import planningutils
        from director import roboturdf
        from director import ikplanner


        directorConfig = robotSystem.directorConfig

        ikRobotModel, ikJointController = roboturdf.loadRobotModel('ik model', urdfFile=directorConfig['urdfConfig']['ik'], parent=None)
        om.removeFromObjectModel(ikRobotModel)
        ikJointController.addPose('q_end', ikJointController.getPose('q_nom'))
        ikJointController.addPose('q_start', ikJointController.getPose('q_nom'))

        handFactory = roboturdf.HandFactory(robotSystem.robotStateModel)
        handModels = []

        for side in ['left', 'right']:
            if side in handFactory.defaultHandTypes:
                handModels.append(handFactory.getLoader(side))

        ikPlanner = ikplanner.IKPlanner(ikRobotModel, ikJointController, handModels)

        planningUtils = planningutils.PlanningUtils(robotSystem.robotStateModel, robotSystem.robotStateJointController)

        return FieldContainer(
            ikRobotModel=ikRobotModel,
            ikJointController=ikJointController,
            handFactory=handFactory,
            handModels=handModels,
            ikPlanner=ikPlanner,
            planningUtils=planningUtils
            )
 def add_target(self, target):
     data = DebugData()
     center = [target[0], target[1], 1]
     axis = [0, 0, 1]  # Upright cylinder.
     data.addCylinder(center, axis, 2, 3)
     om.removeFromObjectModel(om.findObjectByName("target"))
     self._add_polydata(data.getPolyData(), "target", [0, 0.8, 0])
예제 #14
0
 def resetTargetPath(self):
     for obj in om.getObjects():
         if obj.getProperty('Name') == 'target frame desired':
             om.removeFromObjectModel(obj)
     for obj in om.getObjects():
         if obj.getProperty('Name') == 'target frame desired path':
             om.removeFromObjectModel(obj)
예제 #15
0
def testAffordanceToUrdf():

    affs = [func() for func in newSphere, newBox, newCylinder, newCapsule, newMesh]
    print affordanceurdf.urdfStringFromAffordances(affs)

    for aff in affs:
        om.removeFromObjectModel(aff)
예제 #16
0
    def onExecClicked(self):
        self.commitFootstepPlan(self.lastFootstepPlan)

        om.removeFromObjectModel(om.findObjectByName('footstep widget'))
        walkGoal = om.findObjectByName('walking goal')
        if walkGoal:
            walkGoal.setProperty('Edit', False)
        self.execButton.setEnabled(False)
 def removeHandFrames(self):
     sides = ['left', 'right']
     for side in sides:
         linkName = self.ikPlanner.getHandLink(side)
         frameName = '%s constraint frame' % linkName
         frame = om.findObjectByName(frameName)
         if frame:
             om.removeFromObjectModel(frame)
예제 #18
0
    def remove_subscriber(self):
        if self._sub is None:
            return

        lcmUtils.removeSubscriber(self._sub)
        self._sub = None
        om.removeFromObjectModel(om.findObjectByName(self._folder_name))
        print self._name + " subscriber removed."
예제 #19
0
 def _remove_subscriber(self):
     if (self._subscriber is None):
         return
     lcmUtils.removeSubscriber(self._subscriber)
     for frame_channel in self._frame_channels:
         frame_channel.remove_folder()
     self._frame_channels.clear()
     self._subscriber = None
     om.removeFromObjectModel(self._get_folder())
예제 #20
0
    def ungraspAffordance(self, affordanceName):
        try:
            del self.frameSyncs[affordanceName]
            del self.attachedAffordances[affordanceName]
        except KeyError:
            pass

        if not self.frameSyncs:
            om.removeFromObjectModel(om.findObjectByName('l_hand frame'))
            om.removeFromObjectModel(om.findObjectByName('r_hand frame'))
예제 #21
0
 def clear_visualization(self):
     """
     Clears the visualization
     :return:
     :rtype:
     """
     container_name = "ROS"
     self._vis_container = om.getOrCreateContainer(container_name)
     om.removeFromObjectModel(self._vis_container)
     self._vis_container = om.getOrCreateContainer(container_name)
예제 #22
0
def testAffordanceToUrdf():

    affs = [
        func()
        for func in (newSphere, newBox, newCylinder, newCapsule, newMesh)
    ]
    print(affordanceurdf.urdfStringFromAffordances(affs))

    for aff in affs:
        om.removeFromObjectModel(aff)
예제 #23
0
 def _clear_visualization(self):
     """
     Delete the 'object manipulation' vis container, create a new one with the same name
     :return:
     :rtype:
     """
     self._vis_container = om.getOrCreateContainer("object_manipulation")
     om.removeFromObjectModel(self._vis_container)
     self._vis_container = om.getOrCreateContainer("object_manipulation")
     self._vis_dict = dict()
 def set_enabled(self, enable, clear=True):
     print("DeformableVisualizer set_enabled", enable)
     self._enabled = enable
     if enable:
         self.add_subscriber()
     else:
         self.remove_subscriber()
         # Removes the folder completely and resets the known meshes.
         om.removeFromObjectModel(om.findObjectByName(self._folder_name))
         self._poly_item_list = []
예제 #25
0
def spawnBox():

    t = transformUtils.frameFromPositionAndRPY([0.5, 0.0, 0.5], [0, 0, 0])

    om.removeFromObjectModel(om.findObjectByName('box'))
    obj = makeBox()
    obj.setProperty('Dimensions', [0.06, 0.04, 0.12])
    #obj.setProperty('Surface Mode', 'Wireframe')
    obj.setProperty('Color', [1, 0, 0])
    obj.getChildFrame().copyFrame(t)
예제 #26
0
    def selectCandidate(self, selectedObj, candidates):

        if self.properties.getProperty('Delete candidates'):
            for obj in candidates:
                if obj != selectedObj:
                    om.removeFromObjectModel(obj)

        newName = self.properties.getProperty('New name')
        if newName:
            selectedObj.rename(newName)
예제 #27
0
 def set_enabled(self, enable):
     self._enabled = enable
     if enable:
         self.add_subscriber()
         self.configure_action.setEnabled(True)
     else:
         self.remove_subscriber()
         self.configure_action.setEnabled(False)
         # Removes the folder completely.
         om.removeFromObjectModel(om.findObjectByName(self._folder_name))
예제 #28
0
    def removeStaleVisObjects(self):
        keysToRemove = []
        for key, val in self.visObjectDrawTime.iteritems():
            elapsed = time.time() - val
            if elapsed > self.timeout:
                keysToRemove.append(key)

        for key in keysToRemove:
            om.removeFromObjectModel(om.findObjectByName(key))
            del self.visObjectDrawTime[key]
예제 #29
0
    def ungraspAffordance(self, affordanceName):
        try:
            del self.frameSyncs[affordanceName]
            del self.attachedAffordances[affordanceName]
        except KeyError:
            pass

        if not self.frameSyncs:
            om.removeFromObjectModel(om.findObjectByName('l_hand frame'))
            om.removeFromObjectModel(om.findObjectByName('r_hand frame'))
예제 #30
0
 def clear_visualation(self):
     """
     Clear the vis container
     :return:
     :rtype:
     """
     container_name = "Category Manipulation"
     c = om.getOrCreateContainer(container_name)
     om.removeFromObjectModel(c)
     self._vis_container = om.getOrCreateContainer(container_name)
예제 #31
0
    def drawModel(self, model):

        modelFolder = om.getOrCreateContainer(model.name,
                                              parentObj=self.getRootFolder())
        markerFolder = om.getOrCreateContainer('markers',
                                               parentObj=modelFolder)
        modelName = model.name

        markerObjects = markerFolder.children()

        if len(markerObjects) != model.nummarkers:
            for obj in markerObjects:
                om.removeFromObjectModel(obj)

            modelColor = vis.getRandomColor()
            markerObjects = self.createMarkerObjects(model.nummarkers,
                                                     markerFolder, modelName,
                                                     modelColor)
            self.models[modelName] = markerObjects

        if len(markerObjects):
            modelColor = markerObjects[0].getProperty('Color')

        for i, marker in enumerate(model.markers):
            xyz = np.array(marker.xyz) * self.unitConversion
            markerFrame = vtk.vtkTransform()
            markerFrame.Translate(xyz)
            markerObjects[i].getChildFrame().copyFrame(markerFrame)

        if self.drawEdges:
            d = DebugData()
            for m1 in model.markers:
                xyz = np.array(m1.xyz) * self.unitConversion
                for m2 in model.markers:
                    xyz2 = np.array(m2.xyz) * self.unitConversion
                    d.addLine(xyz, xyz2)
            edges = shallowCopy(d.getPolyData())
            vis.updatePolyData(edges,
                               modelName + ' edges',
                               color=modelColor,
                               parent=modelFolder)
        else:
            edgesObj = om.findObjectByName(modelName + ' edges')
            om.removeFromObjectModel(edgesObj)

        computeModelFrame = True
        if computeModelFrame:
            pos = np.array(model.segments[0].T) * self.unitConversion
            rpy = np.array(model.segments[0].A)
            modelFrame = transformUtils.frameFromPositionAndRPY(
                pos, np.degrees(rpy))
            vis.updateFrame(modelFrame,
                            modelName + ' frame',
                            parent=modelFolder,
                            scale=0.1)
예제 #32
0
    def initialize(self):

        self.dt = self.options['dt']
        self.controllerTypeOrder = [
            'defaultRandom', 'learnedRandom', 'learned', 'default'
        ]

        self.setDefaultOptions()

        self.Sensor = SensorObj(rayLength=self.options['Sensor']['rayLength'],
                                numRays=self.options['Sensor']['numRays'])
        self.Controller = ControllerObj(self.Sensor)
        self.Car = CarPlant(controller=self.Controller,
                            velocity=self.options['Car']['velocity'])
        self.Reward = Reward(
            self.Sensor,
            collisionThreshold=self.collisionThreshold,
            actionCost=self.options['Reward']['actionCost'],
            collisionPenalty=self.options['Reward']['collisionPenalty'],
            raycastCost=self.options['Reward']['raycastCost'])
        self.setSARSA()

        # create the things needed for simulation
        om.removeFromObjectModel(om.findObjectByName('world'))
        self.world = World.buildCircleWorld(
            percentObsDensity=self.options['World']['percentObsDensity'],
            circleRadius=self.options['World']['circleRadius'],
            nonRandom=self.options['World']['nonRandomWorld'],
            scale=self.options['World']['scale'],
            randomSeed=self.options['World']['randomSeed'],
            obstaclesInnerFraction=self.options['World']
            ['obstaclesInnerFraction'])

        om.removeFromObjectModel(om.findObjectByName('robot'))
        self.robot, self.frame = World.buildRobot()
        self.locator = World.buildCellLocator(self.world.visObj.polyData)
        self.Sensor.setLocator(self.locator)
        self.frame = self.robot.getChildFrame()
        self.frame.setProperty('Scale', 3)
        self.frame.setProperty('Edit', True)
        self.frame.widget.HandleRotationEnabledOff()
        rep = self.frame.widget.GetRepresentation()
        rep.SetTranslateAxisEnabled(2, False)
        rep.SetRotateAxisEnabled(0, False)
        rep.SetRotateAxisEnabled(1, False)

        self.supervisedTrainingTime = self.options['runTime'][
            'supervisedTrainingTime']
        self.learningRandomTime = self.options['runTime']['learningRandomTime']
        self.learningEvalTime = self.options['runTime']['learningEvalTime']
        self.defaultControllerTime = self.options['runTime'][
            'defaultControllerTime']

        self.Car.setFrame(self.frame)
        print "Finished initialization"
예제 #33
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)
예제 #34
0
 def reset(self, robot, frame_name):
     self._tick_count = 0
     robot.init_angle()
     self.clear_locator()
     om.removeFromObjectModel(om.findObjectByName("line1"))
     om.removeFromObjectModel(om.findObjectByName("line2"))
     self._play_count += 1
     self.set_safe_position(robot)
     print("count:", self._play_count)
     self._update_moving_object(robot, frame_name)
     robot._ctrl.save()
예제 #35
0
def setBoxGraspTarget(position, rpy, dimensions):
    rot_quat = transformUtils.rollPitchYawToQuaternion(
        [math.radians(x) for x in rpy])
    t = transformUtils.transformFromPose(position, rot_quat)

    om.removeFromObjectModel(om.findObjectByName('box'))
    obj = makeBox()
    obj.setProperty('Dimensions', dimensions)
    obj.getChildFrame().copyFrame(t)
    obj.setProperty('Surface Mode', 'Wireframe')
    obj.setProperty('Color', [1, 0, 0])
예제 #36
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])
예제 #37
0
def testCollection():

    affordanceCollection = affordanceManager.collection
    assert len(affordanceCollection.collection) == 0
    aff = newBox()

    assert len(affordanceCollection.collection) == 1
    assert aff.getProperty('uuid') in affordanceCollection.collection

    om.removeFromObjectModel(aff)

    assert len(affordanceCollection.collection) == 0
예제 #38
0
    def _init(self, size):
        items = self._getPolyDataItems()
        for item in items:
            om.removeFromObjectModel(item)

        for i in range(0, size):
            name = self.name if i == 0 else (self.name + str(i))
            item = PolyDataItem(name, vtk.vtkPolyData(), view=None)
            item.setProperty("Visible", self.getProperty("Visible"))
            for view in self.views:
                item.addToView(view)
            om.addToObjectModel(item, self)
예제 #39
0
def testCollection():

    affordanceCollection = affordanceManager.collection
    assert len(affordanceCollection.collection) == 0
    aff = newBox()

    assert len(affordanceCollection.collection) == 1
    assert aff.getProperty('uuid') in affordanceCollection.collection

    om.removeFromObjectModel(aff)

    assert len(affordanceCollection.collection) == 0
예제 #40
0
    def selectCandidate(self, selectedObj, candidates):

        if self.properties.getProperty('Delete candidates'):
            for obj in candidates:
                if obj != selectedObj:
                    om.removeFromObjectModel(obj)

        newName = self.properties.getProperty('New name')
        if newName:
            oldName = selectedObj.getProperty('Name')
            selectedObj.setProperty('Name', newName)
            for child in selectedObj.children():
                child.setProperty('Name', child.getProperty('Name').replace(oldName, newName))
예제 #41
0
    def run(self):

        polyData = self.getPointCloud()
        annotation = self.getAnnotationInput()
        annotationPoint = annotation.annotationPoints[0]

        mesh = segmentation.fitShelfItem(polyData, annotationPoint, clusterTolerance=self.properties.getProperty('Cluster tolerance'))

        annotation.setProperty('Visible', False)
        om.removeFromObjectModel(om.findObjectByName('shelf item'))
        obj = vis.showPolyData(mesh, 'shelf item', color=[0,1,0])
        t = transformUtils.frameFromPositionAndRPY(segmentation.computeCentroid(mesh), [0,0,0])
        segmentation.makeMovable(obj, t)
예제 #42
0
    def run(self):

        polyData = self.getPointCloud()
        annotation = self.getAnnotationInput()
        annotationPoint = annotation.annotationPoints[0]

        mesh = segmentation.fitShelfItem(polyData, annotationPoint, clusterTolerance=self.properties.getProperty('Cluster tolerance'))

        annotation.setProperty('Visible', False)
        om.removeFromObjectModel(om.findObjectByName('shelf item'))
        obj = vis.showPolyData(mesh, 'shelf item', color=[0,1,0])
        t = transformUtils.frameFromPositionAndRPY(segmentation.computeCentroid(mesh), [0,0,0])
        segmentation.makeMovable(obj, t)
예제 #43
0
    def selectLink(self, displayPoint, view):

        if not self.enabled():
            return False

        robotModel, _ = vis.findPickedObject(displayPoint, view)

        try:
            robotModel.model.getLinkNameForMesh
        except AttributeError:
            return False

        model = robotModel.model

        pickedPoint, _, polyData = vis.pickProp(displayPoint, view)

        linkName = model.getLinkNameForMesh(polyData)
        if not linkName:
            return False

        fadeValue = 1.0 if linkName == self.selectedLink else 0.05

        for name in model.getLinkNames():
            linkColor = model.getLinkColor(name)
            linkColor.setAlphaF(fadeValue)
            model.setLinkColor(name, linkColor)

        if linkName == self.selectedLink:
            self.selectedLink = None
            vis.hideCaptionWidget()
            om.removeFromObjectModel(
                om.findObjectByName("selected link frame"))

        else:
            self.selectedLink = linkName
            linkColor = model.getLinkColor(self.selectedLink)
            linkColor.setAlphaF(1.0)
            model.setLinkColor(self.selectedLink, linkColor)
            vis.showCaptionWidget(
                robotModel.getLinkFrame(self.selectedLink).GetPosition(),
                self.selectedLink,
                view=view,
            )
            vis.updateFrame(
                robotModel.getLinkFrame(self.selectedLink),
                "selected link frame",
                scale=0.2,
                parent=robotModel,
            )

        return True
예제 #44
0
    def initialize(self):

        self.dt = self.options["dt"]
        self.controllerTypeOrder = ["default"]

        self.setDefaultOptions()

        self.Sensor = SensorObj(
            rayLength=self.options["Sensor"]["rayLength"], numRays=self.options["Sensor"]["numRays"]
        )

        self.SensorApproximator = SensorApproximatorObj(
            numRays=self.options["Sensor"]["numRays"], circleRadius=self.options["World"]["circleRadius"]
        )

        self.Controller = ControllerObj(self.Sensor, self.SensorApproximator)

        self.Car = CarPlant(controller=self.Controller, velocity=self.options["Car"]["velocity"])

        self.Controller.initializeVelocity(self.Car.v)

        self.ActionSet = ActionSetObj()

        # create the things needed for simulation
        om.removeFromObjectModel(om.findObjectByName("world"))
        self.world = World.buildLineSegmentTestWorld(
            percentObsDensity=self.options["World"]["percentObsDensity"],
            circleRadius=self.options["World"]["circleRadius"],
            nonRandom=self.options["World"]["nonRandomWorld"],
            scale=self.options["World"]["scale"],
            randomSeed=self.options["World"]["randomSeed"],
            obstaclesInnerFraction=self.options["World"]["obstaclesInnerFraction"],
        )

        om.removeFromObjectModel(om.findObjectByName("robot"))
        self.robot, self.frame = World.buildRobot()

        self.frame = self.robot.getChildFrame()
        self.frame.setProperty("Scale", 3)
        # self.frame.setProperty('Visible', False)
        # self.frame.setProperty('Edit', True)
        self.frame.widget.HandleRotationEnabledOff()
        rep = self.frame.widget.GetRepresentation()
        rep.SetTranslateAxisEnabled(2, False)
        rep.SetRotateAxisEnabled(0, False)
        rep.SetRotateAxisEnabled(1, False)

        self.defaultControllerTime = self.options["runTime"]["defaultControllerTime"]

        self.Car.setFrame(self.frame)
        print "Finished initialization"
예제 #45
0
    def drawFrameInCamera(t, frameName='new frame',visible=True):

        v = imageView.view
        q = cameraview.imageManager.queue
        localToCameraT = vtk.vtkTransform()
        q.getTransform('local', 'CAMERA_LEFT', localToCameraT)

        res = vis.showFrame( vtk.vtkTransform() , 'temp',view=v, visible=True, scale = 0.2)
        om.removeFromObjectModel(res)
        pd = res.polyData
        pd = filterUtils.transformPolyData(pd, t)
        pd = filterUtils.transformPolyData(pd, localToCameraT)
        q.projectPoints('CAMERA_LEFT', pd )
        vis.showPolyData(pd, ('overlay ' + frameName), view=v, colorByName='Axes',parent='camera overlay',visible=visible)
예제 #46
0
    def initialize(self):

        self.dt = self.options['dt']
        self.controllerTypeOrder = ['default']

        self.setDefaultOptions()

        self.Sensor = SensorObj(rayLength=self.options['Sensor']['rayLength'],
                                numRays=self.options['Sensor']['numRays'])
        self.SensorManual = SensorObjManual(rayLength=self.options['Sensor']['rayLength'],
                                numRays=self.options['Sensor']['numRays'])

        self.SensorApproximator = SensorApproximatorObj(numRays=self.options['Sensor']['numRays'], circleRadius=self.options['World']['circleRadius'], )

        self.Controller = ControllerObj(self.Sensor, self.SensorApproximator)

        self.Car = CarPlant(controller=self.Controller,
                            velocity=self.options['Car']['velocity'])

        self.Controller.initializeVelocity(self.Car.v)



        # create the things needed for simulation
        om.removeFromObjectModel(om.findObjectByName('world'))
        self.world = World.buildLineSegmentTestWorld(percentObsDensity=self.options['World']['percentObsDensity'],
                                            circleRadius=self.options['World']['circleRadius'],
                                            nonRandom=self.options['World']['nonRandomWorld'],
                                            scale=self.options['World']['scale'],
                                            randomSeed=self.options['World']['randomSeed'],
                                            obstaclesInnerFraction=self.options['World']['obstaclesInnerFraction'])

        om.removeFromObjectModel(om.findObjectByName('robot'))
        self.robot, self.frame = World.buildRobot()
        

        self.frame = self.robot.getChildFrame()
        self.frame.setProperty('Scale', 3)
        #self.frame.setProperty('Visible', False)
        #self.frame.setProperty('Edit', True)
        self.frame.widget.HandleRotationEnabledOff()
        rep = self.frame.widget.GetRepresentation()
        rep.SetTranslateAxisEnabled(2, False)
        rep.SetRotateAxisEnabled(0, False)
        rep.SetRotateAxisEnabled(1, False)

        self.defaultControllerTime = self.options['runTime']['defaultControllerTime']

        self.Car.setFrame(self.frame)
        print "Finished initialization"
예제 #47
0
    def handle_message(self, msg):
        # Limits the rate of message handling, since redrawing is done in the
        # message handler.
        self._sub.setSpeedLimit(30)

        # Removes the folder completely.
        om.removeFromObjectModel(om.findObjectByName(self._folder_name))

        # Recreates folder.
        folder = om.getOrCreateContainer(self._folder_name)

        # A map from pair of body names to a list of contact forces
        collision_pair_to_forces = {}
        for contact in msg.contact_info:
            point = np.array([
                contact.contact_point[0], contact.contact_point[1],
                contact.contact_point[2]
            ])
            force = np.array([
                contact.contact_force[0], contact.contact_force[1],
                contact.contact_force[2]
            ])
            mag = np.linalg.norm(force)
            if mag > 1e-4:
                mag = 0.3 / mag

            key1 = (str(contact.body1_name), str(contact.body2_name))
            key2 = (str(contact.body2_name), str(contact.body1_name))

            if key1 in collision_pair_to_forces:
                collision_pair_to_forces[key1].append(
                    (point, point + mag * force))
            elif key2 in collision_pair_to_forces:
                collision_pair_to_forces[key2].append(
                    (point, point + mag * force))
            else:
                collision_pair_to_forces[key1] = [(point, point + mag * force)]

        for key, list_of_forces in collision_pair_to_forces.iteritems():
            d = DebugData()
            for force_pair in list_of_forces:
                d.addArrow(start=force_pair[0],
                           end=force_pair[1],
                           tubeRadius=0.005,
                           headRadius=0.01)

            vis.showPolyData(d.getPolyData(),
                             str(key),
                             parent=folder,
                             color=[0, 1, 0])
예제 #48
0
    def drawFrameInCamera(t, frameName='new frame',visible=True):

        v = imageView.view
        q = cameraview.imageManager.queue
        localToCameraT = vtk.vtkTransform()
        q.getTransform('local', 'CAMERA_LEFT', localToCameraT)

        res = vis.showFrame( vtk.vtkTransform() , 'temp',view=v, visible=True, scale = 0.2)
        om.removeFromObjectModel(res)
        pd = res.polyData
        pd = filterUtils.transformPolyData(pd, t)
        pd = filterUtils.transformPolyData(pd, localToCameraT)
        q.projectPoints('CAMERA_LEFT', pd )
        vis.showPolyData(pd, ('overlay ' + frameName), view=v, colorByName='Axes',parent='camera overlay',visible=visible)
예제 #49
0
    def _handle_message(self, msg):
        # Removes the folder completely. This is the clearest and easiest way
        # of doing update. Otherwise we need to store the FrameItem returned
        # by vis.showFrame, and update its transform. Also need to handle
        # enable / disable.
        om.removeFromObjectModel(om.findObjectByName(self._folder_name))

        # Recreates folder.
        folder = om.getOrCreateContainer(self._folder_name)

        for i in range(0, msg.num_links):
            name = msg.link_name[i]
            transform = transformUtils.transformFromPose(
                msg.position[i], msg.quaternion[i])
            vis.showFrame(transform, name, parent=folder, scale=0.1)
예제 #50
0
    def _handle_message(self, msg):
        # Removes the folder completely. This is the clearest and easiest way
        # of doing update. Otherwise we need to store the FrameItem returned
        # by vis.showFrame, and update its transform. Also need to handle
        # enable / disable.
        om.removeFromObjectModel(om.findObjectByName(self._folder_name))

        # Recreates folder.
        folder = om.getOrCreateContainer(self._folder_name)

        for i in range(0, msg.num_links):
            name = msg.link_name[i]
            transform = transformUtils.transformFromPose(
                msg.position[i], msg.quaternion[i])
            vis.showFrame(transform, name, parent=folder, scale=0.1);
예제 #51
0
    def _add_polydata(self, polydata, frame_name, color):
        """Adds polydata to the simulation.

        Args:
            polydata: Polydata.
            frame_name: Frame name.
            color: Color of object.

        Returns:
            Frame.
        """
        om.removeFromObjectModel(om.findObjectByName(frame_name))
        frame = vis.showPolyData(polydata, frame_name, color=color)

        vis.addChildFrame(frame)
        return frame
예제 #52
0
    def spawnTargetFrame(self):
        debugFolder = om.getOrCreateContainer('debug')
        om.removeFromObjectModel('target frame')

        handLink = str(self.robotSystem.ikPlanner.getHandLink())
        handFrame = transformUtils.copyFrame(
            self.robotSystem.robotStateModel.getLinkFrame(handLink))

        handFrame.PreMultiply()
        handFrame.Translate(0.02, 0, 0)

        self.targetFrame = vis.updateFrame(handFrame,
                                           'target frame',
                                           parent=debugFolder,
                                           scale=0.15)
        return self.targetFrame
예제 #53
0
def loadHandheldScannerMesh(affordanceManager, filename='oil_bottle.obj', name='oil_bottle', scaleDown=True):
    filename = os.path.join(getLabelFusionDataDir(),'object-meshes/handheld-scanner', filename)
    print filename
    pose = [[0,0,0],[1,0,0,0]]
    visObj = loadAffordanceModel(affordanceManager, name, filename, pose)
    t = visObj.getChildFrame().transform
    center = visObj.polyData.GetCenter()
    translation = -np.array(center)
    t.Translate(translation)
    scale = 0.001
    t.Scale(scale, scale, scale)
    polyData = filterUtils.transformPolyData(visObj.polyData, t)

    om.removeFromObjectModel(visObj)

    scaledVisObj = vis.showPolyData(polyData, name+'_small')
    vis.addChildFrame(scaledVisObj)
예제 #54
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)
예제 #55
0
파일: show_frames.py 프로젝트: xiaoxq/drake
    def _handle_message(self, msg):
        if set(self._link_name_published) != set(msg.link_name):
            # Removes the folder completely.
            # TODO(eric.cousineau): Consider only removing frames that are in
            # the set difference.
            om.removeFromObjectModel(om.findObjectByName(self._folder_name))
            self._link_name_published = msg.link_name

        # Recreates folder.
        folder = om.getOrCreateContainer(self._folder_name)

        for i in range(0, msg.num_links):
            name = msg.link_name[i]
            transform = transformUtils.transformFromPose(
                msg.position[i], msg.quaternion[i])
            # `vis.updateFrame` will either create or update the frame
            # according to its name within its parent folder.
            vis.updateFrame(transform, name, parent=folder, scale=0.1);
예제 #56
0
    def _handleMarkerSets(self, marker_sets):
        # Get the list of existing marker sets so we can track any
        # which disappear.
        remaining_set_names = set(
            [x.getProperty('Name') for x in self.marker_sets.children()])

        for marker_set in marker_sets:
            set_name = 'Marker set ' + marker_set.name
            remaining_set_names.discard(set_name)
            set_folder = om.getOrCreateContainer(set_name,
                                                 parentObj=self.marker_sets)
            marker_ids = range(marker_set.num_markers)
            self._updateMarkerCollection(marker_set.name + '.', set_folder,
                                         marker_ids, marker_set.xyz)

        for remaining_set in remaining_set_names:
            obj = om.findObjectByName(remaining_set, self.marker_sets)
            om.removeFromObjectModel(obj)
예제 #57
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()
예제 #58
0
def makePostGraspFrame(obj, graspFrameName):
    graspFrame = om.findObjectByName(graspFrameName).transform
    goalTransform = vtk.vtkTransform()
    goalTransform.Translate(0, 0, 0.10)
    # Copy the resulting matrix.  graspFrame can move around if the
    # robot's frame moves when the gripper occludes the mocap.
    goalTransform.SetMatrix(
        transformUtils.concatenateTransforms([graspFrame,
                                              goalTransform]).GetMatrix())

    goalFrameName = "postgrasp to world"
    om.removeFromObjectModel(om.findObjectByName(goalFrameName))

    goalFrame = vis.showFrame(goalTransform,
                              goalFrameName,
                              scale=0.1,
                              parent=obj,
                              visible=False)
    obj.getChildFrame().getFrameSync().addFrame(goalFrame, ignoreIncoming=True)