Beispiel #1
0
    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)
Beispiel #2
0
    def initConvexHullModel(self, robotSystem):

        from director import roboturdf

        directorConfig = robotSystem.directorConfig
        chullRobotModel, chullJointController = roboturdf.loadRobotModel(
            'convex hull model',
            view,
            urdfFile=directorConfig['urdfConfig']['chull'],
            parent='planning',
            color=roboturdf.getRobotOrangeColor(),
            colorMode=directorConfig['colorMode'],
            visible=False)

        robotSystem.playbackJointController.models.append(chullRobotModel)

        return FieldContainer(chullRobotModel=chullRobotModel,
                              chullJointController=chullJointController)
Beispiel #3
0
    def __init__(self, jointController):
        self.jointController = jointController
        self.lastFootstepPlan = None
        self.lastFootstepRequest = None
        self.goalSteps = None
        self.lastWalkingPlan = None
        self.walkingPlanCallback = None
        self.default_step_params = DEFAULT_STEP_PARAMS
        self.contact_slices = DEFAULT_CONTACT_SLICES
        self.show_contact_slices = False
        self.toolbarWidget = None

        ### Stuff pertaining to rendering BDI-frame steps
        self.pose_bdi = None
        self.bdi_plan = None
        self.bdi_plan_adjusted = None

        view = app.getDRCView()
        self.bdiRobotModel, self.bdiJointController = roboturdf.loadRobotModel('bdi model', view, parent='bdi model', color=roboturdf.getRobotOrangeColor(), visible=False)
        self.bdiRobotModel.setProperty('Visible', False)
        self.showBDIPlan = False # hide the BDI plans when created
        self.bdiChannel = "POSE_BDI"
        self.bdiSubcribe = None
        #enable this to used the bdi model to render a different state
        #self.bdiJointController.addLCMUpdater("EST_ROBOT_STATE_ALT")

        self._setupSubscriptions()
        self._setupProperties()

        self.showToolbarWidget()
        # If we're a consoleapp and have no main window execButton won't exist
        if hasattr(self, 'execButton'):
            self.execButton.setEnabled(False)

        self.committedPlans = []
Beispiel #4
0
    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
            )
Beispiel #5
0
    def initConvexHullModel(self, robotSystem):

        from director import roboturdf

        directorConfig = robotSystem.directorConfig
        chullRobotModel, chullJointController = roboturdf.loadRobotModel('convex hull model', view, urdfFile=directorConfig['urdfConfig']['chull'], parent='planning', color=roboturdf.getRobotOrangeColor(), colorMode=directorConfig['colorMode'], visible=False)

        robotSystem.playbackJointController.models.append(chullRobotModel)

        return FieldContainer(
            chullRobotModel=chullRobotModel,
            chullJointController=chullJointController
            )
Beispiel #6
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 = 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
Beispiel #7
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 = 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
# load robot state model
robotStateModel, robotStateJointController = roboturdf.loadRobotModel(
    'robot model', view, colorMode='Textures')

# listen for robot state updates
robotStateJointController.addLCMUpdater('EST_ROBOT_STATE')

# reload urdf from model publisher lcm
roboturdf.startModelPublisherListener([(robotStateModel,
                                        robotStateJointController)])

# load  playback model
playbackRobotModel, playbackJointController = roboturdf.loadRobotModel(
    'playback robot model',
    view,
    color=roboturdf.getRobotOrangeColor(),
    visible=False)

# initialize the playback panel
planPlayback = planplayback.PlanPlayback()
manipPlanner = robotplanlistener.ManipulationPlanDriver(ikPlanner=None)
playbackPanel = playbackpanel.PlaybackPanel(planPlayback, playbackRobotModel,
                                            playbackJointController,
                                            robotStateModel,
                                            robotStateJointController,
                                            manipPlanner)
manipPlanner.connectPlanReceived(playbackPanel.setPlan)

# initialize pointcloud lcm
pointcloudlcm.init(view)
# create the application
app = ConsoleApp()
view = app.createView()

# load robot state model
robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot model', view, colorMode='Textures')

# listen for robot state updates
robotStateJointController.addLCMUpdater('EST_ROBOT_STATE')

# reload urdf from model publisher lcm
roboturdf.startModelPublisherListener([(robotStateModel, robotStateJointController)])

# load  playback model
playbackRobotModel, playbackJointController = roboturdf.loadRobotModel('playback robot model', view, color=roboturdf.getRobotOrangeColor(), visible=False)

# initialize the playback panel
planPlayback = planplayback.PlanPlayback()
manipPlanner = robotplanlistener.ManipulationPlanDriver(ikPlanner=None)
playbackPanel = playbackpanel.PlaybackPanel(planPlayback, playbackRobotModel, playbackJointController, robotStateModel, robotStateJointController, manipPlanner)
manipPlanner.connectPlanReceived(playbackPanel.setPlan)

# initialize pointcloud lcm
pointcloudlcm.init(view)

# initialize camera view
cameraChannel = 'MULTISENSE_CAMERA_LEFT'
imageManager = cameraview.ImageManager()
imageManager.queue.addCameraStream(cameraChannel)
imageManager.addImage(cameraChannel)