def __init__(self, **kwargs): self._add_fields(usePointwise=None, useCollision=None, majorIterationsLimit=None, majorOptimalityTolerance=None, majorFeasibilityTolerance=None, maxDegreesPerSecond=None, maxBaseMetersPerSecond=None, maxBaseRPYDegreesPerSecond=None, maxBackDegreesPerSecond=None, accelerationParam=None, accelerationFraction=None, maxPlanDuration=None, fixInitialState=None, numberOfAddedKnots=None, numberOfInterpolatedCollisionChecks=None, collisionMinDistance=None, rrtMaxEdgeLength=None, rrtGoalBias=None, rrtHand=None, rrtMaxNumVertices=None, rrtNSmoothingPasses=None, maxBodyTranslationSpeed=None, maxBodyRotationSpeed=None, rescaleBodyNames=None, rescaleBodyPts=None, quasiStaticShrinkFactor=None) FieldContainer.__init__(self, **kwargs)
def testEncodeDecode(self, fields): encoded = json.dumps(fields, cls=ikconstraintencoder.ConstraintEncoder) decoded = json.loads(encoded, object_hook=ikconstraintencoder.ConstraintDecoder) del decoded['class'] fields = FieldContainer(**decoded) del fields.options['class'] fields.options = ikparameters.IkParameters(**fields.options) constraints = [] for c in fields.constraints: objClass = getattr(ikconstraints, c['class']) del c['class'] obj = objClass() constraints.append(obj) for attr, value in c.iteritems(): if isinstance(value, dict) and 'position' in value and 'quaternion' in value: value = transformUtils.transformFromPose(value['position'], value['quaternion']) setattr(obj, attr, value) fields.constraints = constraints return fields
def testEncodeDecode(self, fields): encoded = json.dumps(fields, cls=ikconstraintencoder.ConstraintEncoder) decoded = json.loads(encoded, object_hook=ikconstraintencoder.ConstraintDecoder) del decoded['class'] fields = FieldContainer(**decoded) del fields.options['class'] fields.options = ikparameters.IkParameters(**fields.options) constraints = [] for c in fields.constraints: objClass = getattr(ikconstraints, c['class']) del c['class'] obj = objClass() constraints.append(obj) for attr, value in c.iteritems(): if isinstance( value, dict ) and 'position' in value and 'quaternion' in value: value = transformUtils.transformFromPose( value['position'], value['quaternion']) setattr(obj, attr, value) fields.constraints = constraints return fields
def __init__(self, **kwargs): self._add_fields( usePointwise = None, useCollision = None, majorIterationsLimit = None, majorOptimalityTolerance = None, majorFeasibilityTolerance = None, maxDegreesPerSecond = None, maxBaseMetersPerSecond = None, maxBaseRPYDegreesPerSecond = None, maxBackDegreesPerSecond = None, accelerationParam = None, accelerationFraction = None, maxPlanDuration = None, fixInitialState = None, numberOfAddedKnots = None, numberOfInterpolatedCollisionChecks = None, collisionMinDistance = None, rrtMaxEdgeLength = None, rrtGoalBias = None, rrtHand = None, rrtMaxNumVertices = None, rrtNSmoothingPasses = None, maxBodyTranslationSpeed = None, maxBodyRotationSpeed = None, rescaleBodyNames = None, rescaleBodyPts = None, quasiStaticShrinkFactor = None ) FieldContainer.__init__(self, **kwargs)
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 initMainToolBar(self, fields): from director import viewcolors app = fields.app toolBar = app.addToolBar('Main Toolbar') app.addToolBarAction(toolBar, 'Python Console', ':/images/python_logo.png', callback=app.showPythonConsole) toolBar.addSeparator() terrainModeAction = fields.app.addToolBarAction(toolBar, 'Camera Free Rotate', ':/images/camera_mode.png') lightAction = fields.app.addToolBarAction(toolBar, 'Background Light', ':/images/light_bulb_icon.png') app.addToolBarAction(toolBar, 'Reset Camera', ':/images/reset_camera.png', callback=applogic.resetCamera) def getFreeCameraMode(): return not applogic.getCameraTerrainModeEnabled(fields.view) def setFreeCameraMode(enabled): applogic.setCameraTerrainModeEnabled(fields.view, not enabled) terrainToggle = applogic.ActionToggleHelper(terrainModeAction, getFreeCameraMode, setFreeCameraMode) viewBackgroundLightHandler = viewcolors.ViewBackgroundLightHandler(fields.viewOptions, fields.gridObj, lightAction) return FieldContainer(viewBackgroundLightHandler=viewBackgroundLightHandler, terrainToggle=terrainToggle)
def initGrid(self, fields): gridObj = vis.showGrid(fields.view, parent='scene') gridObj.setProperty('Surface Mode', 'Surface with edges') gridObj.setProperty('Color', [0, 0, 0]) gridObj.setProperty('Alpha', 0.1) applogic.resetCamera(viewDirection=[-1, -1, -0.3], view=fields.view) return FieldContainer(gridObj=gridObj)
def initTeleop(self, robotSystem): from director import roboturdf from director import teleoppanel directorConfig = robotSystem.directorConfig teleopRobotModel, teleopJointController = roboturdf.loadRobotModel( 'teleop model', robotSystem.view, urdfFile=directorConfig['urdfConfig']['teleop'], parent='planning', color=roboturdf.getRobotBlueColor(), visible=False, colorMode=directorConfig['colorMode']) teleopPanel = teleoppanel.TeleopPanel( robotSystem.robotStateModel, robotSystem.robotStateJointController, teleopRobotModel, teleopJointController, robotSystem.ikPlanner, robotSystem.manipPlanner, robotSystem.affordanceManager, robotSystem.playbackPanel.setPlan, robotSystem.playbackPanel.hidePlan, robotSystem.planningUtils) return FieldContainer( teleopRobotModel=teleopRobotModel, teleopJointController=teleopJointController, teleopPanel=teleopPanel, )
def initOutputConsole(self, fields): from director import outputconsole outputConsole = outputconsole.OutputConsole() outputConsole.addToAppWindow(fields.app, visible=False) return FieldContainer(outputConsole=outputConsole)
def initGrid(self, fields): gridObj = vis.showGrid(fields.view, parent="scene") gridObj.setProperty("Surface Mode", "Surface with edges") gridObj.setProperty("Color", [0, 0, 0]) gridObj.setProperty("Alpha", 0.1) applogic.resetCamera(viewDirection=[-1, -1, -0.3], view=fields.view) return FieldContainer(gridObj=gridObj)
def initUndoRedo(self, fields): undoStack = QtGui.QUndoStack() undoView = QtGui.QUndoView(undoStack) undoView.setEmptyLabel("Start") undoView.setWindowTitle("History") undoDock = fields.app.addWidgetToDock( undoView, QtCore.Qt.LeftDockWidgetArea, visible=False ) undoAction = undoStack.createUndoAction(undoStack) redoAction = undoStack.createRedoAction(undoStack) undoAction.setShortcut(QtGui.QKeySequence("Ctrl+Z")) redoAction.setShortcut(QtGui.QKeySequence("Ctrl+Shift+Z")) fields.app.editMenu.addAction(undoAction) fields.app.editMenu.addAction(redoAction) return FieldContainer( undoDock=undoDock, undoStack=undoStack, undoView=undoView, undoAction=undoAction, redoAction=redoAction, )
def setupFields(self, constraints, ikParameters, positionCosts, nominalPoseName="", seedPoseName="", endPoseName=""): poses = ikconstraintencoder.getPlanPoses(constraints, self.ikPlanner) poses.update(self.poses) poses['q_nom'] = list(self.ikPlanner.jointController.getPose('q_nom')) fields = FieldContainer( utime=getUtime(), poses=poses, constraints=constraints, seedPose=seedPoseName, nominalPose=nominalPoseName, endPose=endPoseName, jointNames=self.jointNames, jointLimits=self.jointLimits, positionCosts=positionCosts, affordances=self.processAffordances(), options=ikParameters, ) return fields
def initHandDrivers(self, robotSystem): from director import handdriver rHandDriver = handdriver.RobotiqHandDriver(side='right') lHandDriver = handdriver.RobotiqHandDriver(side='left') return FieldContainer(rHandDriver=rHandDriver, lHandDriver=lHandDriver)
def initPlayback(self, robotSystem): from director import roboturdf from director import planplayback from director import playbackpanel from director import robotplanlistener directorConfig = robotSystem.directorConfig manipPlanner = robotplanlistener.ManipulationPlanDriver( robotSystem.ikPlanner) playbackRobotModel, playbackJointController = roboturdf.loadRobotModel( 'playback model', robotSystem.view, urdfFile=directorConfig['urdfConfig']['playback'], parent='planning', color=roboturdf.getRobotOrangeColor(), visible=False, colorMode=directorConfig['colorMode']) planPlayback = planplayback.PlanPlayback() playbackPanel = playbackpanel.PlaybackPanel( planPlayback, playbackRobotModel, playbackJointController, robotSystem.robotStateModel, robotSystem.robotStateJointController, manipPlanner) manipPlanner.connectPlanReceived(playbackPanel.setPlan) return FieldContainer(playbackRobotModel=playbackRobotModel, playbackJointController=playbackJointController, planPlayback=planPlayback, manipPlanner=manipPlanner, playbackPanel=playbackPanel)
def initRobotLinkSelector(self, robotSystem): from director import robotlinkselector robotLinkSelector = robotlinkselector.RobotLinkSelector() robotSystem.viewBehaviors.addHandler( robotSystem.viewBehaviors.LEFT_DOUBLE_CLICK_EVENT, robotLinkSelector.onLeftDoubleClick) return FieldContainer(robotLinkSelector=robotLinkSelector)
def initViewBehaviors(self, robotSystem): from director import applogic from director import robotviewbehaviors viewBehaviors = robotviewbehaviors.RobotViewBehaviors(robotSystem.view, robotSystem) applogic.resetCamera(viewDirection=[-1,0,0], view=robotSystem.view) return FieldContainer(viewBehaviors=viewBehaviors)
def initGlobals(self, fields): try: globalsDict = fields.globalsDict except AttributeError: globalsDict = dict() if globalsDict is None: globalsDict = dict() return FieldContainer(globalsDict=globalsDict)
def initIRISDriver(self, robotSystem): from director import irisdriver irisDriver = irisdriver.IRISDriver( robotSystem.robotStateJointController, robotSystem.footstepsDriver.params) return FieldContainer(irisDriver=irisDriver)
def _setupLocalServer(self): initArgs = FieldContainer( urdfFile=self.ikPlanner.robotModel.getProperty('Filename'), packagePaths=roboturdf.getPackagePaths()) self.ikServer = PyDrakeIkServer() self.ikServer.initInstance(initArgs)
def initViewOptions(self, fields): viewOptions = vis.ViewOptionsItem(fields.view) fields.objectModel.addToObjectModel( viewOptions, parentObj=fields.objectModel.findObjectByName('scene')) viewOptions.setProperty('Background color', [0.3, 0.3, 0.35]) viewOptions.setProperty('Background color 2', [0.95, 0.95, 1]) return FieldContainer(viewOptions=viewOptions)
def getDefaultOptions(self): options = dict() for name in self.componentGraph.getComponentNames(): options['use' + name] = True options = FieldContainer(**options) self.initDefaultOptions(options) return options
def initMeasurementPanel(self, fields): from director import measurementpanel measurementPanel = measurementpanel.MeasurementPanel(fields.app, fields.view) measurementDock = fields.app.addWidgetToDock(measurementPanel.widget, QtCore.Qt.RightDockWidgetArea, visible=False) return FieldContainer( measurementPanel=measurementPanel, measurementDock=measurementDock )
def initScreenGrabberPanel(self, fields): from director.screengrabberpanel import ScreenGrabberPanel screenGrabberPanel = ScreenGrabberPanel(fields.view) screenGrabberDock = fields.app.addWidgetToDock(screenGrabberPanel.widget, QtCore.Qt.RightDockWidgetArea, visible=False) return FieldContainer( screenGrabberPanel=screenGrabberPanel, screenGrabberDock=screenGrabberDock )
def initCameraControlPanel(self, fields): from director import cameracontrolpanel cameraControlPanel = cameracontrolpanel.CameraControlPanel(fields.view) cameraControlDock = fields.app.addWidgetToDock(cameraControlPanel.widget, QtCore.Qt.RightDockWidgetArea, visible=False) return FieldContainer( cameraControlPanel=cameraControlPanel, cameraControlDock=cameraControlDock )
def initDrakeVisualizer(self, fields): from director import drakevisualizer drakeVisualizer = drakevisualizer.DrakeVisualizer(fields.view) applogic.MenuActionToggleHelper('Tools', drakeVisualizer.name, drakeVisualizer.isEnabled, drakeVisualizer.setEnabled) return FieldContainer(drakeVisualizer=drakeVisualizer)
def initAffordances(self, robotSystem): from director import affordancemanager from director import affordanceitems affordanceManager = affordancemanager.AffordanceObjectModelManager( robotSystem.view) affordanceitems.MeshAffordanceItem.getMeshManager() return FieldContainer(affordanceManager=affordanceManager, )
def initTreeViewer(self, fields): from director import treeviewer treeViewer = treeviewer.TreeViewer(fields.view) applogic.MenuActionToggleHelper('Tools', treeViewer.name, treeViewer.isEnabled, treeViewer.setEnabled) return FieldContainer(treeViewer=treeViewer)
def initOpenDataHandler(self, fields): from director import opendatahandler openDataHandler = opendatahandler.OpenDataHandler(fields.app) def loadData(): for filename in drcargs.args().data_files: openDataHandler.openGeometry(filename) fields.app.registerStartupCallback(loadData) return FieldContainer(openDataHandler=openDataHandler)
def initPerceptionDrivers(self, robotSystem): from director import perception perceptionSources = perception.init( robotSystem.view, robotSystem.robotStateJointController) # Expand dict to keyword args, robotSystem object will have objects accessible via keys set in config return FieldContainer(sources=list(perceptionSources.values()), **perceptionSources)
def construct(self, options=None, **kwargs): options = options or self.getDefaultOptions() if isinstance(options, dict): options = self.setDependentOptions(self.getDefaultOptions(), **options) self._verifyOptions(options) componentGraph = self.componentGraph for name in componentGraph.getComponentNames(): if 'use'+name not in options.__dict__: raise Exception('Missing use option: ' + 'use'+name) if 'init'+name not in dir(self): raise Exception('Missing init function: ' + 'init'+name) initOrder = toposort_flatten(componentGraph._graph) componentFields = OrderedDict() defaultFields = FieldContainer(options=options, **kwargs) for name in initOrder: initFunction = getattr(self, 'init'+name) isEnabled = getattr(options, 'use'+name) if isEnabled: dependencies = componentGraph.getComponentDependencies(name) inputFields = self._joinFields([defaultFields] + [componentFields[dep] for dep in dependencies]) newFields = initFunction(inputFields) if not newFields: newFields = FieldContainer() componentFields[name] = newFields # for componentName, componentFields in app.iteritems(): # print componentName, 'exports fields:' # for name in componentFields._fields: # print ' ', name fields = self._joinFields([defaultFields] + componentFields.values()) return fields
def initCameraBookmarksPanel(self, fields): from director import camerabookmarks cameraBookmarksPanel = camerabookmarks.CameraBookmarkWidget(fields.view) cameraBookmarksDock = fields.app.addWidgetToDock(cameraBookmarksPanel.widget, QtCore.Qt.RightDockWidgetArea, visible=False) return FieldContainer( cameraBookmarksPanel=cameraBookmarksPanel, cameraBookmarksDock=cameraBookmarksDock )
def initLCMGLRenderer(self, fields): from director import lcmgl if lcmgl.LCMGL_AVAILABLE: lcmglManager = lcmgl.LCMGLManager(fields.view) applogic.MenuActionToggleHelper('Tools', 'LCMGL Renderer', lcmglManager.isEnabled, lcmglManager.setEnabled) else: lcmglManager = None return FieldContainer(lcmglManager=lcmglManager)
def __init__(self, *strs): kwargs = {s:s for s in strs} assert len(kwargs) == len(strs) FieldContainer.__init__(self, **kwargs)
class ComponentFactory(object): def __init__(self): self.componentGraph = ComponentGraph() self.initFunctions = {} self.componentFields = {} self.defaultOptions = FieldContainer() def register(self, factoryClass): fact = factoryClass() components, disabledComponents = fact.getComponents() for name in sorted(components.keys()): if name in self.componentGraph.getComponentNames(): raise Exception('Component %s from %s has already been registered.' % (name, factoryClass.__name__)) if not hasattr(fact, 'init'+name): raise Exception('Missing init function for component %s' % name) for name in disabledComponents: if name not in components.keys(): raise Exception('Unknown component %s found in list of disabled components.' % name) options = dict() for name, deps in components.iteritems(): self.componentGraph.addComponent(name, deps) self.initFunctions[name] = getattr(fact, 'init'+name) isEnabled = name not in disabledComponents options['use'+name] = isEnabled self.defaultOptions._add_fields(**options) def getDisabledOptions(self, **kwargs): options = self.getDefaultOptions() for opt in options._fields: options[opt] = False self.setDependentOptions(options, **kwargs) return options def getDefaultOptions(self, **kwargs): options = FieldContainer(**dict(self.defaultOptions)) self.setDependentOptions(options, **kwargs) return options def _toComponentName(self, optionName): assert optionName[:3] == 'use' return optionName[3:] def _toOptionName(self, componentName): return 'use' + componentName def _joinFields(self, fieldsList): f = FieldContainer() for fields in fieldsList: f._add_fields(**dict(fields)) return f def setDependentOptions(self, options, **kwargs): # verify the given args exist in the options fields for name in kwargs.keys(): if name not in options._fields: raise Exception('unknown option given: ' + name) for name, enabled in kwargs.iteritems(): setattr(options, name, enabled) # if option is being enabled, also enable its dependencies if enabled: name = self._toComponentName(name) dependencies = self.componentGraph.getComponentDependencies(name) for dep in dependencies: setattr(options, self._toOptionName(dep), True) return options def _verifyOptions(self, options): for name, enabled in options: if enabled: name = self._toComponentName(name) dependencies = self.componentGraph.getComponentDependencies(name) for dep in dependencies: if not getattr(options, self._toOptionName(dep)): raise Exception('Component %s depends on component %s, but %s is disabled.' % (name, dep, dep)) def construct(self, options=None, **kwargs): options = options or self.getDefaultOptions() if isinstance(options, dict): options = self.setDependentOptions(self.getDefaultOptions(), **options) self._verifyOptions(options) defaultFields = FieldContainer(options=options, **kwargs) initOrder = toposort_flatten(self.componentGraph.getComponentGraph()) for name in initOrder: isEnabled = getattr(options, 'use'+name) if isEnabled: self.initComponent(name, defaultFields) fields = self._joinFields([defaultFields] + self.componentFields.values()) return fields def printComponentFields(self): for k, v in sorted(self.componentFields.items()): print '%s:' % k for name in v._fields: print ' ', name def initComponent(self, name, defaultFields): initFunction = self.initFunctions[name] dependencies = self.componentGraph.getComponentDependencies(name) inputFields = self._joinFields([defaultFields] + [self.componentFields[dep] for dep in dependencies]) newFields = initFunction(inputFields) if not newFields: newFields = FieldContainer() self.componentFields[name] = newFields
def _joinFields(self, fieldsList): f = FieldContainer() for fields in fieldsList: f._add_fields(**dict(fields)) return f
def __init__(self): self.componentGraph = ComponentGraph() self.initFunctions = {} self.componentFields = {} self.defaultOptions = FieldContainer()
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 = drcargs.getDirectorConfig() neckPitchJoint = 'neck_ay' colorMode = 'URDF Colors' if 'colorMode' in directorConfig: assert directorConfig['colorMode'] in ['URDF Colors', 'Solid Color', 'Textures'] colorMode = directorConfig['colorMode'] 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, colorMode=colorMode) 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.getRobotOrangeColor(), visible=False, colorMode=colorMode) teleopRobotModel, teleopJointController = roboturdf.loadRobotModel('teleop model', view, urdfFile=directorConfig['urdfConfig']['teleop'], parent='planning', color=roboturdf.getRobotBlueColor(), visible=False, colorMode=colorMode) 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() handFactory = roboturdf.HandFactory(robotStateModel) handModels = [] for side in ['left', 'right']: if side in handFactory.defaultHandTypes: handModels.append(handFactory.getLoader(side)) ikPlanner = ikplanner.IKPlanner(ikServer, ikRobotModel, ikJointController, handModels) manipPlanner = robotplanlistener.ManipulationPlanDriver(ikPlanner) affordanceManager = affordancemanager.AffordanceObjectModelManager(view) affordanceitems.MeshAffordanceItem.getMeshManager().initLCM() affordanceitems.MeshAffordanceItem.getMeshManager().collection.sendEchoRequest() affordanceManager.collection.sendEchoRequest() segmentation.affordanceManager = affordanceManager plannerPub = plannerPublisher.PlannerPublisher(ikPlanner,affordanceManager,ikRobotModel) 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) viewBehaviors = None robotSystemArgs = dict(locals()) for arg in ['globalsDict', 'self']: del robotSystemArgs[arg] if globalsDict is not None: globalsDict.update(robotSystemArgs) robotSystem = FieldContainer(**robotSystemArgs) robotSystem.viewBehaviors = robotviewbehaviors.RobotViewBehaviors(view, robotSystem) return robotSystem