예제 #1
0
 def makeMarker(i):
     obj = vis.showPolyData(shallowCopy(geom),
                            modelName + ' marker %d' % i,
                            color=modelColor,
                            parent=modelFolder)
     vis.addChildFrame(obj)
     return obj
예제 #2
0
def show(data, offset=[0,0,0]):
    polyData = data.getPolyData()
    obj = vis.showPolyData(polyData, 'data', colorByName='RGB255')
    t = vtk.vtkTransform()
    t.Translate(offset)
    vis.addChildFrame(obj).copyFrame(t)
    return obj
예제 #3
0
 def onOpenVrml(self, filename):
     meshes, color = ioUtils.readVrml(filename)
     folder = om.getOrCreateContainer(os.path.basename(filename), parentObj=om.getOrCreateContainer("files"))
     for i, pair in enumerate(zip(meshes, color)):
         mesh, color = pair
         obj = vis.showPolyData(mesh, "mesh %d" % i, color=color, parent=folder)
         vis.addChildFrame(obj)
예제 #4
0
 def onOpenVrml(self, filename):
     meshes, color = ioUtils.readVrml(filename)
     folder = om.getOrCreateContainer(os.path.basename(filename), parentObj=self.getRootFolder())
     for i, pair in enumerate(zip(meshes, color)):
         mesh, color = pair
         obj = vis.showPolyData(mesh, 'mesh %d' % i, color=color, parent=folder)
         vis.addChildFrame(obj)
예제 #5
0
def centerObject(visObj):
    polyData = filterUtils.transformPolyData(visObj.polyData, visObj.actor.GetUserTransform())
    name = visObj.getProperty('Name')

    om.removeFromObjectModel(visObj)
    newVisObj = vis.showPolyData(polyData, name)
    vis.addChildFrame(newVisObj)
예제 #6
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)
예제 #7
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)
예제 #8
0
    def loadReconstructedPointCloud(self, filename=None):
        if filename is None:
            sourceDir = spartanUtils.getSpartanSourceDir()
            filename = os.path.join(sourceDir, 'logs', 'test',
                                    'reconstructed_pointcloud.vtp')

        polyData = ioUtils.readPolyData(filename)
        self.pointcloud = vis.showPolyData(polyData,
                                           'reconstructed pointcloud',
                                           colorByName='RGB')
        vis.addChildFrame(self.pointcloud)
예제 #9
0
    def addLinkGeometry(self, geom, linkName, linkFolder):
        geom.polyDataItem.addToView(self.view)
        om.addToObjectModel(geom.polyDataItem, parentObj=linkFolder)
        vis.addChildFrame(geom.polyDataItem)

        if linkName == 'world':
            #geom.polyDataItem.actor.SetUseBounds(False)
            #geom.polyDataItem.actor.GetProperty().LightingOff()
            geom.polyDataItem.actor.GetProperty().SetSpecular(0.0)
        else:
            geom.polyDataItem.actor.GetProperty().SetSpecular(0.9)
            geom.polyDataItem.actor.GetProperty().SetSpecularPower(20)
예제 #10
0
    def addLinkGeometry(self, geom, linkName, linkFolder):
        geom.polyDataItem.addToView(self.view)
        om.addToObjectModel(geom.polyDataItem, parentObj=linkFolder)
        vis.addChildFrame(geom.polyDataItem)

        if linkName == 'world':
            #geom.polyDataItem.actor.SetUseBounds(False)
            #geom.polyDataItem.actor.GetProperty().LightingOff()
            geom.polyDataItem.actor.GetProperty().SetSpecular(0.0)
        else:
            geom.polyDataItem.actor.GetProperty().SetSpecular(0.9)
            geom.polyDataItem.actor.GetProperty().SetSpecularPower(20)
예제 #11
0
    def openGeometry(self, filename):

        if filename.lower().endswith('wrl'):
            self.onOpenVrml(filename)
            return

        polyData = ioUtils.readPolyData(filename)

        if not polyData or not polyData.GetNumberOfPoints():
            self.app.showErrorMessage('Failed to read any data from file: %s' % filename, title='Reader error')
            return

        obj = vis.showPolyData(polyData, os.path.basename(filename), parent=self.getRootFolder())
        vis.addChildFrame(obj)
    def spawnCropBox(self, dims=None):
        if dims is None:
            dim_x = CONFIG['crop_box']['dimensions']['x']
            dim_y = CONFIG['crop_box']['dimensions']['y']
            dim_z = CONFIG['crop_box']['dimensions']['z']
            dims = [dim_x, dim_y, dim_z]

        transform = director_utils.transformFromPose(CONFIG['crop_box']['transform'])
        d = DebugData()
        d.addCube(dims, [0,0,0], color=[0,1,0])
        self.cube_vis = vis.updatePolyData(d.getPolyData(), 'Crop Cube', colorByName='RGB255')
        vis.addChildFrame(self.cube_vis)
        self.cube_vis.getChildFrame().copyFrame(transform)
        self.cube_vis.setProperty('Alpha', 0.3)
예제 #13
0
    def _handleRigidBodies(self, bodies):

        if not bodies:
            return

        folder = self.getRootFolder()

        for body in bodies:
            name = 'Body ' + str(body.id)

            x,y,z,w = body.quat
            quat = (w,x,y,z)
            bodyToOptitrack = transformUtils.transformFromPose(body.xyz, quat)

            bodyToWorld = transformUtils.concatenateTransforms((bodyToOptitrack, self.optitrackToWorld))


            obj = folder.findChild(name)
            if not obj:
                geometry = self._makeMarkers(body.marker_xyz)
                geometry = filterUtils.transformPolyData(geometry, bodyToOptitrack.GetLinearInverse())
                obj = vis.showPolyData(geometry, name, parent=folder, color=[1,0,0])
                frameObj = vis.addChildFrame(obj)
                frameObj.setProperty('Scale', 0.2)
                frameObj.setProperty('Visible', True)

            obj.getChildFrame().copyFrame(bodyToWorld)
예제 #14
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
    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
예제 #16
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
예제 #17
0
def buildRobot(x=0,y=0):

    d = DebugData()
    d.addCone((x,y,0), (1,0,0), height=0.2, radius=0.1)
    obj = vis.showPolyData(d.getPolyData(), 'robot')
    frame = vis.addChildFrame(obj)
    return obj
예제 #18
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
예제 #19
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
예제 #20
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)
예제 #21
0
    def spawnCropBox(self, dims=None):
        if dims is None:
            dim_x = CONFIG['crop_box']['dimensions']['x']
            dim_y = CONFIG['crop_box']['dimensions']['y']
            dim_z = CONFIG['crop_box']['dimensions']['z']
            dims = [dim_x, dim_y, dim_z]

        transform = director_utils.transformFromPose(
            CONFIG['crop_box']['transform'])
        d = DebugData()
        d.addCube(dims, [0, 0, 0], color=[0, 1, 0])
        self.cube_vis = vis.updatePolyData(d.getPolyData(),
                                           'Crop Cube',
                                           colorByName='RGB255')
        vis.addChildFrame(self.cube_vis)
        self.cube_vis.getChildFrame().copyFrame(transform)
        self.cube_vis.setProperty('Alpha', 0.3)
예제 #22
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"))
     frame = vis.addChildFrame(aff)
     frame.setProperty("Deletable", False)
     aff.loadDescription(desc, copyMode=aff.COPY_MODE_ALL)
     self.registerAffordance(aff, notify=False)
예제 #23
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"))
     frame = vis.addChildFrame(aff)
     frame.setProperty("Deletable", False)
     aff.loadDescription(desc, copyMode=aff.COPY_MODE_ALL)
     self.registerAffordance(aff, notify=False)
예제 #24
0
    def setTarget(self, obj):

        self.onAbortPick()

        if obj and not hasattr(obj, 'getChildFrame'):
            obj = None

        if obj:
            vis.addChildFrame(obj)
            obj.connectRemovedFromObjectModel(self.onObjectRemoved)

        self.trackerManager.setTarget(obj)

        name = self.getObjectShortName(obj) if obj else 'None'
        self.ui.targetNameLabel.setText(name)
        self.ui.controlFrame.setEnabled(obj is not None)

        if not obj:
            self.ui.trackModeCombo.setCurrentIndex(0)
예제 #25
0
    def load_object_model(self):
        """
        Load the object model poly data
        :return:
        :rtype:
        """
        model_filename = os.path.join(spartan_utils.get_data_dir(),
                                      "pdc/templates/mugs/mug.stl")
        print("model_filename", model_filename)
        self._poly_data = ioUtils.readPolyData(model_filename)
        self._object = vis.updatePolyData(self._poly_data,
                                          "Object",
                                          parent=self._vis_container)
        vis.addChildFrame(self._object)

        pos = [0.34301733, 0.00805546, 0.92509635]
        quat = [0.17589242, 0.98340013, -0.04305877, 0.01148846]

        t = transformUtils.transformFromPose(pos, quat)
        self._object.getChildFrame().copyFrame(t)
예제 #26
0
    def load_mug_rack(self):
        """
        Loads a mug rack and side table poly data
        :return:
        :rtype:
        """
        pdc_data_dir = os.path.join(spartan_utils.get_data_dir(), 'pdc')
        rack_ply_file = os.path.join(pdc_data_dir,
                                     self._mug_rack_config["mesh"])
        rack_poly_data = ioUtils.readPolyData(rack_ply_file)
        self._mug_rack = vis.showPolyData(rack_poly_data,
                                          'Mug Rack',
                                          parent=self._vis_container)
        # rack_target_position

        target_pose_name = "left_side_table"
        target_pose = director_utils.transformFromPose(
            self._mug_rack_config['poses'][target_pose_name])

        vis.addChildFrame(self._mug_rack)
        self._mug_rack.getChildFrame().copyFrame(target_pose)
예제 #27
0
def initCameraFrustumVisualizer(depthScanner):

    cameraObj = vis.showPolyData(vtk.vtkPolyData(), 'camera', parent=depthScanner.parentFolder, view=depthScanner.pointCloudView)
    cameraFrame = vis.addChildFrame(cameraObj)
    cameraFrame.setProperty('Scale', 50)
    cameraObj.setProperty('Visible', False)
    pointCloudToCamera = transformUtils.frameFromPositionAndRPY((0,0,0,), (-90, 90, 0)).GetLinearInverse()

    def updateCameraMesh():
        scale = cameraObj.getChildFrame().getProperty('Scale') * 10.0
        rayLength = scale
        d = DebugData()
        d.addCube(dimensions=[0.04, 0.08, 0.06], center=[-0.02, 0.0, 0.0], color=[1,0.5,0])
        d.addLine([0.0, 0.0, 0.0], [0.01, 0.0, 0.0], radius=0.023, color=[1,0.5,0])
        cameraModelMesh = d.getPolyData()

        t = vtk.vtkTransform()
        t.Scale(scale, scale, scale)
        cameraModelMesh = filterUtils.transformPolyData(cameraModelMesh, t)

        cameraMesh = getCameraFrustumMesh(depthScanner.view, rayLength)
        cameraMesh = filterUtils.transformPolyData(cameraMesh, getCameraTransform(depthScanner.view.camera()).GetLinearInverse())
        cameraMesh = filterUtils.appendPolyData([cameraMesh, cameraModelMesh])
        cameraObj.setPolyData(cameraMesh)

    def onCameraModified():
        cameraToWorld = getCameraTransform(depthScanner.view.camera())
        depthScanner.pointCloudObj.actor.SetUserTransform(transformUtils.concatenateTransforms([pointCloudToCamera, cameraToWorld]))
        cameraFrame.copyFrame(cameraToWorld)

    def enableFrustum():
        updateCameraMesh()
        cameraObj.setProperty('Visible', True)
        onCameraModified()
        depthScanner._updateFunc = onCameraModified
        applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView, True)
        applogic.resetCamera(viewDirection=[1, 1, -0.4], view=depthScanner.pointCloudView)
        depthScanner.pointCloudView.camera().SetViewUp([0, 0, 1])

    def disableFrustum():
        cameraObj.setProperty('Visible', False)
        depthScanner.pointCloudObj.actor.SetUserTransform(None)
        depthScanner._updateFunc = None
        applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView, False)
        applogic.resetCamera(viewDirection=[0, 0, -1], view=depthScanner.pointCloudView)
        depthScanner.pointCloudView.camera().SetViewUp([0, 1, 0])

    return FieldContainer(
        updateCameraMesh=updateCameraMesh,
        onCameraModified=onCameraModified,
        enableFrustum=enableFrustum,
        disableFrustum=disableFrustum
        )
예제 #28
0
    def load_mug_platform(self):
        """
        Load the mug platform
        :return:
        :rtype:
        """

        pdc_data_dir = os.path.join(spartan_utils.get_data_dir(), 'pdc')
        platform_ply_file = os.path.join(pdc_data_dir,
                                         self._mug_platform_config['mesh'])

        platform_poly_data = ioUtils.readPolyData(platform_ply_file)
        self._mug_platform = vis.showPolyData(platform_poly_data,
                                              'Platform',
                                              parent=self._vis_container)

        target_pose_name = "left_side_table"
        target_pose = director_utils.transformFromPose(
            self._mug_platform_config['poses'][target_pose_name])

        vis.addChildFrame(self._mug_platform)
        self._mug_platform.getChildFrame().copyFrame(target_pose)
예제 #29
0
    def buildRobot(x=0,y=0):
        #print "building robot"
        polyData = ioUtils.readPolyData('crazyflie.obj')
        
        scale = 0.005
        t = vtk.vtkTransform()
        t.RotateX(90)
        t.Scale(scale, scale, scale)
        polyData = filterUtils.transformPolyData(polyData, t)

        #d = DebugData()
        #d.addCone((x,y,0), (1,0,0), height=0.2, radius=0.1)
        #polyData = d.getPolyData()

        obj = vis.showPolyData(polyData, 'robot')
        robotFrame = vis.addChildFrame(obj)
        return obj, robotFrame
예제 #30
0
    def buildRobot(x=0, y=0):
        #print "building robot"
        polyData = ioUtils.readPolyData('celica.obj')

        scale = 0.04
        t = vtk.vtkTransform()
        t.RotateZ(90)
        t.Scale(scale, scale, scale)
        polyData = filterUtils.transformPolyData(polyData, t)

        #d = DebugData()
        #d.addCone((x,y,0), (1,0,0), height=0.2, radius=0.1)
        #polyData = d.getPolyData()

        obj = vis.showPolyData(polyData, 'robot')
        robotFrame = vis.addChildFrame(obj)
        return obj, robotFrame
예제 #31
0
 def makeMarker(i):
     obj = vis.showPolyData(
         shallowCopy(geom), modelName + " marker %d" % i, color=modelColor, parent=modelFolder
     )
     vis.addChildFrame(obj)
     return obj
예제 #32
0
 def __init__(self, obj):
     if obj is not None:
         vis.addChildFrame(obj)
         self.targetFrame = obj.getChildFrame()
     else:
         self.targetFrame = None
예제 #33
0
def initCameraFrustumVisualizer(depthScanner):

    cameraObj = vis.showPolyData(vtk.vtkPolyData(),
                                 'camera',
                                 parent=depthScanner.parentFolder,
                                 view=depthScanner.pointCloudView)
    cameraFrame = vis.addChildFrame(cameraObj)
    cameraFrame.setProperty('Scale', 50)
    cameraObj.setProperty('Visible', False)
    pointCloudToCamera = transformUtils.frameFromPositionAndRPY((
        0,
        0,
        0,
    ), (-90, 90, 0)).GetLinearInverse()

    def updateCameraMesh():
        scale = cameraObj.getChildFrame().getProperty('Scale') * 10.0
        rayLength = scale
        d = DebugData()
        d.addCube(dimensions=[0.04, 0.08, 0.06],
                  center=[-0.02, 0.0, 0.0],
                  color=[1, 0.5, 0])
        d.addLine([0.0, 0.0, 0.0], [0.01, 0.0, 0.0],
                  radius=0.023,
                  color=[1, 0.5, 0])
        cameraModelMesh = d.getPolyData()

        t = vtk.vtkTransform()
        t.Scale(scale, scale, scale)
        cameraModelMesh = filterUtils.transformPolyData(cameraModelMesh, t)

        cameraMesh = getCameraFrustumMesh(depthScanner.view, rayLength)
        cameraMesh = filterUtils.transformPolyData(
            cameraMesh,
            getCameraTransform(depthScanner.view.camera()).GetLinearInverse())
        cameraMesh = filterUtils.appendPolyData([cameraMesh, cameraModelMesh])
        cameraObj.setPolyData(cameraMesh)

    def onCameraModified():
        cameraToWorld = getCameraTransform(depthScanner.view.camera())
        depthScanner.pointCloudObj.actor.SetUserTransform(
            transformUtils.concatenateTransforms(
                [pointCloudToCamera, cameraToWorld]))
        cameraFrame.copyFrame(cameraToWorld)

    def enableFrustum():
        updateCameraMesh()
        cameraObj.setProperty('Visible', True)
        onCameraModified()
        depthScanner._updateFunc = onCameraModified
        applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView, True)
        applogic.resetCamera(viewDirection=[1, 1, -0.4],
                             view=depthScanner.pointCloudView)
        depthScanner.pointCloudView.camera().SetViewUp([0, 0, 1])

    def disableFrustum():
        cameraObj.setProperty('Visible', False)
        depthScanner.pointCloudObj.actor.SetUserTransform(None)
        depthScanner._updateFunc = None
        applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView,
                                             False)
        applogic.resetCamera(viewDirection=[0, 0, -1],
                             view=depthScanner.pointCloudView)
        depthScanner.pointCloudView.camera().SetViewUp([0, 1, 0])

    return FieldContainer(updateCameraMesh=updateCameraMesh,
                          onCameraModified=onCameraModified,
                          enableFrustum=enableFrustum,
                          disableFrustum=disableFrustum)
예제 #34
0
 def __init__(self, obj):
     if obj is not None:
         vis.addChildFrame(obj)
         self.targetFrame = obj.getChildFrame()
     else:
         self.targetFrame = None
예제 #35
0
        self.obj.getChildFrame().copyFrame(t)
        self.view.render()




app = ConsoleApp()

app.setupGlobals(globals())


view = app.createView()


obj = vis.showPolyData(makeRobotPolyData(), 'robot', color=[0.7, 0.7, 0.8])
vis.addChildFrame(obj)


animator = AnimateRobot(om.findObjectByName('robot'), view)
animator.start()

panel = cameracontrolpanel.CameraControlPanel(view)
panel.widget.show()




def addWidgetToDock(widget, window, dockArea):
    dock = QtGui.QDockWidget()
    dock.setWidget(widget)
    dock.setWindowTitle(widget.windowTitle)
예제 #36
0
    def _handleRigidBodies(self, rigid_bodies):
        # Get the list of existing rigid bodies so we can track any
        # which disappear.
        remaining_body_names = set(
            [x.getProperty('Name') for x in self.rigid_bodies.children()])
        bodies_added = False
        bodies_removed = False

        for body in rigid_bodies:
            body_name = 'Body ' + str(body.id)
            for desc in self.data_descriptions.rigid_bodies:
                if desc.id == body.id:
                    body_name = desc.name
            if body_name in remaining_body_names:
                body_obj = om.findObjectByName(body_name,
                                               parent=self.rigid_bodies)
            else:
                bodies_added = True
                # The use of a box here is arbitrary.
                body_obj = affordanceitems.BoxAffordanceItem(
                    body_name, applogic.getCurrentRenderView())
                om.addToObjectModel(body_obj, parentObj=self.rigid_bodies)
                vis.addChildFrame(body_obj).setProperty('Deletable', False)
                body_obj.setProperty('Surface Mode', 'Wireframe')
                body_obj.setProperty('Color', [1, 0, 0])
            remaining_body_names.discard(body_name)

            x, y, z, w = body.quat
            quat = (w, x, y, z)
            objToOptitrack = transformUtils.transformFromPose(body.xyz, quat)

            # Dimension our box based on a bounding across all of our
            # markers.
            all_xyz = body.marker_xyz + [body.xyz]
            all_xyz = [(xyz[0] - body.xyz[0], xyz[1] - body.xyz[1],
                        xyz[2] - body.xyz[2]) for xyz in all_xyz]
            marker_transforms = [
                transformUtils.transformFromPose(xyz, (1, 0, 0, 0))
                for xyz in all_xyz
            ]
            inv_transform = transformUtils.transformFromPose((0, 0, 0),
                                                             (w, -x, -y, -z))
            marker_transforms = [
                transformUtils.concatenateTransforms([t, inv_transform])
                for t in marker_transforms
            ]
            all_xyz = [t.GetPosition() for t in marker_transforms]
            (min_x, min_y, min_z) = (min(xyz[0] for xyz in all_xyz),
                                     min(xyz[1] for xyz in all_xyz),
                                     min(xyz[2] for xyz in all_xyz))
            (max_x, max_y, max_z) = (max(xyz[0] for xyz in all_xyz),
                                     max(xyz[1] for xyz in all_xyz),
                                     max(xyz[2] for xyz in all_xyz))
            dimensions = (max(0.01, max_x - min_x), max(0.01, max_y - min_y),
                          max(0.01, max_z - min_z))
            #print "max, min", (max_x, max_y, max_z), (min_x, min_y, min_z)
            body_obj.setProperty('Dimensions', dimensions)
            objToWorld = transformUtils.concatenateTransforms(
                [objToOptitrack, self.optitrackToWorld])
            body_obj.getChildFrame().copyFrame(objToWorld)

            folder = om.getOrCreateContainer('Models', parentObj=body_obj)
            self._updateMarkerCollection(
                body_name + '.', folder, body.marker_ids,
                body.marker_xyz)  #, base_transform=transform)

        if len(remaining_body_names):
            bodies_removed = True

        for remaining_body in remaining_body_names:
            obj = om.findObjectByName(remaining_body, self.rigid_bodies)
            om.removeFromObjectModel(obj)

        if bodies_added or bodies_removed:
            self.callbacks.process(
                'RIGID_BODY_LIST_CHANGED',
                sorted([
                    x.getProperty('Name')
                    for x in self.rigid_bodies.children()
                ]))
예제 #37
0
        self.obj.getChildFrame().copyFrame(t)
        self.view.render()




app = ConsoleApp()

app.setupGlobals(globals())


view = app.createView()


obj = vis.showPolyData(makeRobotPolyData(), 'robot', color=[0.7, 0.7, 0.8])
vis.addChildFrame(obj)


animator = AnimateRobot(om.findObjectByName('robot'), view)
animator.start()

panel = cameracontrolpanel.CameraControlPanel(view)
panel.widget.show()




def addWidgetToDock(widget, window, dockArea):
    dock = QtGui.QDockWidget()
    dock.setWidget(widget)
    dock.setWindowTitle(widget.windowTitle)