Beispiel #1
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
Beispiel #2
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._updateAxesGeometry()
Beispiel #3
0
def showHandCloud(hand='left', view=None):

    view = view or app.getCurrentRenderView()
    if view is None:
        return

    assert hand in ('left', 'right')

    maps = om.findObjectByName('Map Server')
    assert maps is not None

    viewId = 52 if hand == 'left' else 53
    reader = maps.source.reader

    def getCurrentViewId():
        return reader.GetCurrentMapId(viewId)

    p = vtk.vtkPolyData()
    obj = showPolyData(p, '%s hand cloud' % hand, view=view, parent='sensors')
    obj.currentViewId = -1

    def updateCloud():
        currentViewId = getCurrentViewId()
        #print 'updateCloud: current view id:', currentViewId
        if currentViewId != obj.currentViewId:
            reader.GetDataForMapId(viewId, currentViewId, p)
            #print 'updated poly data.  %d points.' % p.GetNumberOfPoints()
            obj._renderAllViews()

    t = TimerCallback()
    t.targetFps = 1
    t.callback = updateCloud
    t.start()
    obj.updater = t
    return obj
    def onNewWalkingGoal(self, walkingGoal=None):

        walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel)
        frameObj = vis.updateFrame(walkingGoal,
                                   'walking goal',
                                   parent='planning',
                                   scale=0.25)
        frameObj.setProperty('Edit', True)

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

        if self.placer:
            self.placer.stop()

        terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
        if terrain:

            pos = np.array(frameObj.transform.GetPosition())

            polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
            if polyData.GetNumberOfPoints():
                polyData = segmentation.labelDistanceToLine(
                    polyData, pos, pos + [0, 0, 1])
                polyData = segmentation.thresholdPoints(
                    polyData, 'distance_to_line', [0.0, 0.1])
                if polyData.GetNumberOfPoints():
                    pos[2] = np.nanmax(
                        vnp.getNumpyFromVtk(polyData, 'Points')[:, 2])
                    frameObj.transform.Translate(
                        pos - np.array(frameObj.transform.GetPosition()))

            d = DebugData()
            d.addSphere((0, 0, 0), radius=0.03)
            handle = vis.showPolyData(d.getPolyData(),
                                      'walking goal terrain handle',
                                      parent=frameObj,
                                      visible=True,
                                      color=[1, 1, 0])
            handle.actor.SetUserTransform(frameObj.transform)
            self.placer = PlacerWidget(app.getCurrentRenderView(), handle,
                                       terrain)

            def onFramePropertyModified(propertySet, propertyName):
                if propertyName == 'Edit':
                    if propertySet.getProperty(propertyName):
                        self.placer.start()
                    else:
                        self.placer.stop()

            frameObj.properties.connectPropertyChanged(onFramePropertyModified)
            onFramePropertyModified(frameObj, 'Edit')

        frameObj.connectFrameModified(self.onWalkingGoalModified)
        self.onWalkingGoalModified(frameObj)
Beispiel #5
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
Beispiel #6
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
Beispiel #7
0
def pickPoint(displayPoint, view, obj=None, pickType='points', tolerance=0.01, returnNormal=False):

    assert pickType in ('points', 'cells', 'render')

    view = view or app.getCurrentRenderView()
    assert view

    if isinstance(obj, str):
        obj = om.findObjectByName(obj)
        assert obj


    if pickType == 'render':
        picker = vtk.vtkPropPicker()
    else:
        picker = vtk.vtkPointPicker() if pickType == 'points' else vtk.vtkCellPicker()
        picker.SetTolerance(tolerance)


    if obj:
        if isinstance(obj, list):
            for o in obj:
                picker.AddPickList(o.actor)
            obj = None
        else:
            picker.AddPickList(obj.actor)
        picker.PickFromListOn()

    picker.Pick(displayPoint[0], displayPoint[1], 0, view.renderer())
    pickedProp = picker.GetViewProp()
    pickedPoint = np.array(picker.GetPickPosition())
    pickedDataset = pickedProp.GetMapper().GetInput() if isinstance(pickedProp, vtk.vtkActor) else None

    pickedNormal = np.zeros(3)

    if returnNormal:
        if pickType == 'cells':
          pickedNormal = np.array(picker.GetPickNormal())
        elif pickType == 'points' and pickedDataset:
          pointId = picker.GetPointId()
          normals = pickedDataset.GetPointData().GetNormals()
          if normals:
              pickedNormal = np.array(normals.GetTuple3(pointId))


    if obj:
        if returnNormal:
            return (pickedPoint, pickedNormal) if pickedProp else (None, None)
        else:
            return pickedPoint if pickedProp else None
    else:
        return (pickedPoint, pickedProp, pickedDataset, pickedNormal) if returnNormal else (pickedPoint, pickedProp, pickedDataset)
Beispiel #8
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
Beispiel #9
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
    def onNewWalkingGoal(self, walkingGoal=None):

        walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel)
        frameObj = vis.updateFrame(walkingGoal, 'walking goal', parent='planning', scale=0.25)
        frameObj.setProperty('Edit', True)


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

        if self.placer:
            self.placer.stop()

        terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
        if terrain:

            pos = np.array(frameObj.transform.GetPosition())

            polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
            if polyData.GetNumberOfPoints():
                polyData = segmentation.labelDistanceToLine(polyData, pos, pos+[0,0,1])
                polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, 0.1])
                if polyData.GetNumberOfPoints():
                    pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, 'Points')[:,2])
                    frameObj.transform.Translate(pos - np.array(frameObj.transform.GetPosition()))

            d = DebugData()
            d.addSphere((0,0,0), radius=0.03)
            handle = vis.showPolyData(d.getPolyData(), 'walking goal terrain handle', parent=frameObj, visible=True, color=[1,1,0])
            handle.actor.SetUserTransform(frameObj.transform)
            self.placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain)

            def onFramePropertyModified(propertySet, propertyName):
                if propertyName == 'Edit':
                    if propertySet.getProperty(propertyName):
                        self.placer.start()
                    else:
                        self.placer.stop()

            frameObj.properties.connectPropertyChanged(onFramePropertyModified)
            onFramePropertyModified(frameObj, 'Edit')

        frameObj.connectFrameModified(self.onWalkingGoalModified)
        self.onWalkingGoalModified(frameObj)
Beispiel #11
0
def showCaptionWidget(position, text, view=None):

    view = view or app.getCurrentRenderView()
    assert view

    global captionWidget

    if not captionWidget:
        rep = vtk.vtkCaptionRepresentation()
        rep.SetPosition(0.2, 0.8)
        w = vtk.vtkCaptionWidget()
        w.SetInteractor(view.renderWindow().GetInteractor())
        w.SetRepresentation(rep)
        w.On()
        captionWidget = w

    rep = captionWidget.GetRepresentation()
    rep.SetAnchorPosition(position)
    rep.GetCaptionActor2D().SetCaption(text)

    a = rep.GetCaptionActor2D()

    pr = a.GetTextActor().GetTextProperty()
    pr.SetJustificationToCentered()
    pr.SetVerticalJustificationToCentered()
    pr.SetItalic(0)
    pr.SetBold(0)
    pr.SetShadow(0)
    pr.SetFontFamilyToArial()

    c2 = rep.GetPosition2Coordinate()
    c2.SetCoordinateSystemToDisplay()
    c2.SetValue(12*len(text),30)

    # disable border
    #rep.SetShowBorder(0)

    a.SetThreeDimensionalLeader(0)
    a.SetLeaderGlyphSize(0.005)

    captionWidget.On()
    captionWidget.Render()
Beispiel #12
0
    def _onPropertyChanged(self, propertySet, propertyName):

        om.ObjectModelItem._onPropertyChanged(self, propertySet, propertyName)

        if propertyName == 'Visible':
            self.actor.SetVisibility(self.getProperty(propertyName))
            self._renderAllViews()
        elif propertyName == 'Text':
            view = app.getCurrentRenderView()
            self.actor.SetInput(self.getProperty(propertyName))
        elif propertyName == 'Position':
            pos = self.getProperty(propertyName)
            self.actor.SetPosition(pos[0], pos[1])
        elif propertyName == 'Font Size':
            self.actor.GetTextProperty().SetFontSize(self.getProperty(propertyName))
        elif propertyName == 'Bold Size':
            self.actor.GetTextProperty().SetBold(self.getProperty(propertyName))
        elif propertyName == 'Italic':
            self.actor.GetTextProperty().SetItalic(self.getProperty(propertyName))


        if self.getProperty('Visible'):
            self._renderAllViews()
Beispiel #13
0
    def init(self, view=None, globalsDict=None):

        view = view or applogic.getCurrentRenderView()

        useRobotState = True
        usePerception = True
        useFootsteps = True
        useHands = True
        usePlanning = True
        useAtlasDriver = True
        useAtlasConvexHull = False
        useWidgets = False

        directorConfig = self.getDirectorConfig()
        neckPitchJoint = 'neck_ay'

        if useAtlasDriver:
            atlasDriver = atlasdriver.init(None)


        if useRobotState:
            robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot state model', view, urdfFile=directorConfig['urdfConfig']['robotState'], parent='sensors', color=roboturdf.getRobotGrayColor(), visible=True)
            robotStateJointController.setPose('EST_ROBOT_STATE', robotStateJointController.getPose('q_nom'))
            roboturdf.startModelPublisherListener([(robotStateModel, robotStateJointController)])
            robotStateJointController.addLCMUpdater('EST_ROBOT_STATE')
            segmentationroutines.SegmentationContext.initWithRobot(robotStateModel)


        if usePerception:
            multisenseDriver, mapServerSource = perception.init(view)

            def getNeckPitch():
                return robotStateJointController.q[robotstate.getDrakePoseJointNames().index( neckPitchJoint )]
            neckDriver = perception.NeckDriver(view, getNeckPitch)

            def getSpindleAngleFunction():
                if (robotStateJointController.lastRobotStateMessage):
                    if ('hokuyo_joint' in robotStateJointController.lastRobotStateMessage.joint_name):
                        index = robotStateJointController.lastRobotStateMessage.joint_name.index('hokuyo_joint')
                        return (float(robotStateJointController.lastRobotStateMessage.utime)/(1e6),
                            robotStateJointController.lastRobotStateMessage.joint_position[index])
                return (0, 0)
            spindleMonitor = perception.SpindleMonitor(getSpindleAngleFunction)
            robotStateModel.connectModelChanged(spindleMonitor.onRobotStateChanged)



        if useHands:
            rHandDriver = handdriver.RobotiqHandDriver(side='right')
            lHandDriver = handdriver.RobotiqHandDriver(side='left')


        if useFootsteps:
            footstepsDriver = footstepsdriver.FootstepsDriver(robotStateJointController)
            irisDriver = irisdriver.IRISDriver(robotStateJointController, footstepsDriver.params)
            raycastDriver = raycastdriver.RaycastDriver()

        if usePlanning:

            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'))


            if 'leftFootLink' in directorConfig:
                ikServer = ik.AsyncIKCommunicator(directorConfig['urdfConfig']['ik'], directorConfig['fixedPointFile'], directorConfig['leftFootLink'], directorConfig['rightFootLink'], directorConfig['pelvisLink'])
            else: # assume that robot has no feet e.g. fixed base arm
                ikServer = ik.AsyncIKCommunicator(directorConfig['urdfConfig']['ik'], directorConfig['fixedPointFile'], '', '', '')

            def startIkServer():
                ikServer.startServerAsync()
                ikServer.comm.writeCommandsToLogFile = True

            #startIkServer()

            playbackRobotModel, playbackJointController = roboturdf.loadRobotModel('playback model', view, urdfFile=directorConfig['urdfConfig']['playback'], parent='planning', color=roboturdf.getRobotBlueColor(), visible=False)
            teleopRobotModel, teleopJointController = roboturdf.loadRobotModel('teleop model', view, urdfFile=directorConfig['urdfConfig']['teleop'], parent='planning', color=roboturdf.getRobotBlueColor(), visible=False)

            if useAtlasConvexHull:
                chullRobotModel, chullJointController = roboturdf.loadRobotModel('convex hull atlas', view, urdfFile=urdfConfig['chull'], parent='planning',
                    color=roboturdf.getRobotOrangeColor(), visible=False)
                playbackJointController.models.append(chullRobotModel)


            planPlayback = planplayback.PlanPlayback()

            if (roboturdf.numberOfHands == 1):
                handFactory = roboturdf.HandFactory(robotStateModel)
                handModels = [handFactory.getLoader(side) for side in ['left']]
            elif (roboturdf.numberOfHands == 2):
                handFactory = roboturdf.HandFactory(robotStateModel)
                handModels = [handFactory.getLoader(side) for side in ['left', 'right']]
            else:
                handFactory = None
                handModels = []


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

            manipPlanner = robotplanlistener.ManipulationPlanDriver(ikPlanner)

            affordanceManager = affordancemanager.AffordanceObjectModelManager(view)
            affordanceitems.MeshAffordanceItem.getMeshManager().collection.sendEchoRequest()
            affordanceManager.collection.sendEchoRequest()
            segmentation.affordanceManager = affordanceManager

            plannerPub = plannerPublisher.PlannerPublisher(ikPlanner,affordanceManager)
            ikPlanner.setPublisher(plannerPub)

            # This joint angle is mapped to the Multisense panel
            neckPitchJoint = ikPlanner.neckPitchJoint

        applogic.resetCamera(viewDirection=[-1,0,0], view=view)




        if useWidgets:

            playbackPanel = playbackpanel.PlaybackPanel(planPlayback, playbackRobotModel, playbackJointController,
                                              robotStateModel, robotStateJointController, manipPlanner)

            teleopPanel = teleoppanel.TeleopPanel(robotStateModel, robotStateJointController, teleopRobotModel, teleopJointController,
                             ikPlanner, manipPlanner, playbackPanel.setPlan, playbackPanel.hidePlan)

            footstepsDriver.walkingPlanCallback = playbackPanel.setPlan
            manipPlanner.connectPlanReceived(playbackPanel.setPlan)

        robotSystemArgs = dict(locals())
        for arg in ['globalsDict', 'self']:
            del robotSystemArgs[arg]

        if globalsDict is not None:
            globalsDict.update(robotSystemArgs)

        robotSystem = FieldContainer(**robotSystemArgs)
        viewbehaviors.ViewBehaviors.addRobotBehaviors(robotSystem)
        return robotSystem
Beispiel #14
0
def startButton():
    view = app.getCurrentRenderView()
    init(view)
    _kinectSource.start()
Beispiel #15
0
def startButton():
    view = app.getCurrentRenderView()
    init(view)
    _kinectSource.start()
Beispiel #16
0
def onOpenUrdf(filename):

    model = roboturdf.openUrdf(filename, app.getCurrentRenderView())
    if not model:
        app.showErrorMessage('Failed to read urdf file: %s' % filename,
                             title='Read urdf error')
Beispiel #17
0
def onOpenOtdf(filename):
    model = otdfmodel.openOtdf(filename, app.getCurrentRenderView())
Beispiel #18
0
def onOpenUrdf(filename):

    model = roboturdf.openUrdf(filename, app.getCurrentRenderView())
    if not model:
        app.showErrorMessage('Failed to read urdf file: %s' % filename, title='Read urdf error')
Beispiel #19
0
def startButton():
    view = app.getCurrentRenderView()
    init(view)
    _pointcloudSource.start()
Beispiel #20
0
def onOpenOtdf(filename):
    model = otdfmodel.openOtdf(filename, app.getCurrentRenderView())
Beispiel #21
0
def startButton():
    view = app.getCurrentRenderView()
    init(view)
    _pointcloudSource.start()
Beispiel #22
0
    def init(self, view=None, globalsDict=None):

        view = view or applogic.getCurrentRenderView()

        useRobotState = True
        usePerception = True
        useFootsteps = True
        useHands = True
        usePlanning = True
        useAtlasDriver = True
        useAtlasConvexHull = False
        useWidgets = False

        directorConfig = self.getDirectorConfig()
        neckPitchJoint = 'neck_ay'

        if useAtlasDriver:
            atlasDriver = atlasdriver.init(None)

        if useRobotState:
            robotStateModel, robotStateJointController = roboturdf.loadRobotModel(
                'robot state model',
                view,
                urdfFile=directorConfig['urdfConfig']['robotState'],
                parent='sensors',
                color=roboturdf.getRobotGrayColor(),
                visible=True)
            robotStateJointController.setPose(
                'EST_ROBOT_STATE', robotStateJointController.getPose('q_nom'))
            roboturdf.startModelPublisherListener([
                (robotStateModel, robotStateJointController)
            ])
            robotStateJointController.addLCMUpdater('EST_ROBOT_STATE')
            segmentationroutines.SegmentationContext.initWithRobot(
                robotStateModel)

        if usePerception:
            multisenseDriver, mapServerSource = perception.init(view)

            def getNeckPitch():
                return robotStateJointController.q[
                    robotstate.getDrakePoseJointNames().index(neckPitchJoint)]

            neckDriver = perception.NeckDriver(view, getNeckPitch)

            def getSpindleAngleFunction():
                if (robotStateJointController.lastRobotStateMessage):
                    if ('hokuyo_joint' in robotStateJointController.
                            lastRobotStateMessage.joint_name):
                        index = robotStateJointController.lastRobotStateMessage.joint_name.index(
                            'hokuyo_joint')
                        return (float(robotStateJointController.
                                      lastRobotStateMessage.utime) / (1e6),
                                robotStateJointController.
                                lastRobotStateMessage.joint_position[index])
                return (0, 0)

            spindleMonitor = perception.SpindleMonitor(getSpindleAngleFunction)
            robotStateModel.connectModelChanged(
                spindleMonitor.onRobotStateChanged)

        if useHands:
            rHandDriver = handdriver.RobotiqHandDriver(side='right')
            lHandDriver = handdriver.RobotiqHandDriver(side='left')

        if useFootsteps:
            footstepsDriver = footstepsdriver.FootstepsDriver(
                robotStateJointController)
            irisDriver = irisdriver.IRISDriver(robotStateJointController,
                                               footstepsDriver.params)
            raycastDriver = raycastdriver.RaycastDriver()

        if usePlanning:

            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'))

            if 'leftFootLink' in directorConfig:
                ikServer = ik.AsyncIKCommunicator(
                    directorConfig['urdfConfig']['ik'],
                    directorConfig['fixedPointFile'],
                    directorConfig['leftFootLink'],
                    directorConfig['rightFootLink'],
                    directorConfig['pelvisLink'])
            else:  # assume that robot has no feet e.g. fixed base arm
                ikServer = ik.AsyncIKCommunicator(
                    directorConfig['urdfConfig']['ik'],
                    directorConfig['fixedPointFile'], '', '', '')

            def startIkServer():
                ikServer.startServerAsync()
                ikServer.comm.writeCommandsToLogFile = True

            #startIkServer()

            playbackRobotModel, playbackJointController = roboturdf.loadRobotModel(
                'playback model',
                view,
                urdfFile=directorConfig['urdfConfig']['playback'],
                parent='planning',
                color=roboturdf.getRobotBlueColor(),
                visible=False)
            teleopRobotModel, teleopJointController = roboturdf.loadRobotModel(
                'teleop model',
                view,
                urdfFile=directorConfig['urdfConfig']['teleop'],
                parent='planning',
                color=roboturdf.getRobotBlueColor(),
                visible=False)

            if useAtlasConvexHull:
                chullRobotModel, chullJointController = roboturdf.loadRobotModel(
                    'convex hull atlas',
                    view,
                    urdfFile=urdfConfig['chull'],
                    parent='planning',
                    color=roboturdf.getRobotOrangeColor(),
                    visible=False)
                playbackJointController.models.append(chullRobotModel)

            planPlayback = planplayback.PlanPlayback()

            if (roboturdf.numberOfHands == 1):
                handFactory = roboturdf.HandFactory(robotStateModel)
                handModels = [handFactory.getLoader(side) for side in ['left']]
            elif (roboturdf.numberOfHands == 2):
                handFactory = roboturdf.HandFactory(robotStateModel)
                handModels = [
                    handFactory.getLoader(side) for side in ['left', 'right']
                ]
            else:
                handFactory = None
                handModels = []

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

            manipPlanner = robotplanlistener.ManipulationPlanDriver(ikPlanner)

            affordanceManager = affordancemanager.AffordanceObjectModelManager(
                view)
            affordanceitems.MeshAffordanceItem.getMeshManager(
            ).collection.sendEchoRequest()
            affordanceManager.collection.sendEchoRequest()
            segmentation.affordanceManager = affordanceManager

            plannerPub = plannerPublisher.PlannerPublisher(
                ikPlanner, affordanceManager)
            ikPlanner.setPublisher(plannerPub)

            # This joint angle is mapped to the Multisense panel
            neckPitchJoint = ikPlanner.neckPitchJoint

        applogic.resetCamera(viewDirection=[-1, 0, 0], view=view)

        if useWidgets:

            playbackPanel = playbackpanel.PlaybackPanel(
                planPlayback, playbackRobotModel, playbackJointController,
                robotStateModel, robotStateJointController, manipPlanner)

            teleopPanel = teleoppanel.TeleopPanel(
                robotStateModel, robotStateJointController, teleopRobotModel,
                teleopJointController, ikPlanner, manipPlanner,
                playbackPanel.setPlan, playbackPanel.hidePlan)

            footstepsDriver.walkingPlanCallback = playbackPanel.setPlan
            manipPlanner.connectPlanReceived(playbackPanel.setPlan)

        robotSystemArgs = dict(locals())
        for arg in ['globalsDict', 'self']:
            del robotSystemArgs[arg]

        if globalsDict is not None:
            globalsDict.update(robotSystemArgs)

        robotSystem = FieldContainer(**robotSystemArgs)
        viewbehaviors.ViewBehaviors.addRobotBehaviors(robotSystem)
        return robotSystem