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 initAffordances(self, robotSystem): from director import affordancemanager from director import affordanceitems affordanceManager = affordancemanager.AffordanceObjectModelManager( robotSystem.view) affordanceitems.MeshAffordanceItem.getMeshManager() if affordancemanager.lcmobjectcollection.USE_LCM: affordanceitems.MeshAffordanceItem.getMeshManager( ).collection.sendEchoRequest() affordanceManager.collection.sendEchoRequest() return FieldContainer(affordanceManager=affordanceManager, )
actions = [ (None, None), ('Select parent...', onSelectAffordanceParent), ] return actions viewbehaviors.registerContextMenuActions(getAffordanceContextMenuActions) app = mainwindowapp.construct() view = app.view affordanceManager = affordancemanager.AffordanceObjectModelManager(view) if affordancemanager.lcmobjectcollection.USE_LCM: affordanceitems.MeshAffordanceItem.getMeshManager().collection.sendEchoRequest() affordanceManager.collection.sendEchoRequest() objectPicker = pointpicker.ObjectPicker(view=view, callback=onAffordancePick, getObjectsFunction=affordanceManager.getAffordances) panel = affordancepanel.AffordancePanel(view, affordanceManager) dock = app.app.addWidgetToDock(panel.widget, QtCore.Qt.RightDockWidgetArea) printButton = QtGui.QPushButton('Print URDF') printButton.connect('clicked()', printAffordanceUrdf) panel.ui.spawnTab.layout().addWidget(printButton)
# parse args first parser = drcargs.getGlobalArgParser().getParser() parser.add_argument('--logFolder', type=str, dest='logFolder', help='location of top level folder for this log, relative to CorlDev/data') args = parser.parse_args() print 'log folder:', args.logFolder # construct the app fields = mainwindowapp.construct() imageManager = initImageManager() openniDepthPointCloud = initDepthPointCloud(imageManager, fields.view) cameraView = newCameraView(imageManager) affordanceManager = affordancemanager.AffordanceObjectModelManager(fields.view) myObjects = dict() myObjects['imageManager'] = imageManager myObjects['openniDepthPointCloud'] = openniDepthPointCloud myObjects['cameraView'] = cameraView myObjects['affordanceManager'] = affordanceManager myObjects['CorlUtils'] = corl.utils # these lines are used to update the globals for the interactive python console fields.globalsDict.update(**dict(fields)) globals().update(**fields.globalsDict) globals().update(**myObjects) # # add custom code here corl.setup.setupCorlDirector(affordanceManager,
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