Ejemplo n.º 1
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
Ejemplo n.º 2
0
from ddapp import segmentation
from ddapp import segmentationroutines
from ddapp import applogic
from ddapp import visualization as vis

from ddapp import roboturdf


app = ConsoleApp()

# create a view
view = app.createView()
segmentation._defaultSegmentationView = view
segmentation.initAffordanceManager(view)

robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot state model', view, parent='sensors', color=roboturdf.getRobotGrayColor(), visible=True)
segmentationroutines.SegmentationContext.initWithRobot(robotStateModel)

# load poly data
dataDir = app.getTestingDataDirectory()
polyData = ioUtils.readPolyData(os.path.join(dataDir, 'valve/valve-sparse-stereo.pcd'))
vis.showPolyData(polyData, 'pointcloud snapshot original', colorByName='rgb_colors')
polyData = segmentationroutines.sparsifyStereoCloud( polyData )
vis.showPolyData(polyData, 'pointcloud snapshot')

# fit valve and lever
segmentation.segmentValveWallAuto(.2, mode='valve', removeGroundMethod=segmentation.removeGroundSimple )

if app.getTestingInteractiveEnabled():

    v = applogic.getCurrentView()
Ejemplo n.º 3
0
from ddapp import applogic
from ddapp import visualization as vis

from ddapp import roboturdf

app = ConsoleApp()

# create a view
view = app.createView()
segmentation._defaultSegmentationView = view

robotStateModel, robotStateJointController = roboturdf.loadRobotModel(
    'robot state model',
    view,
    parent='sensors',
    color=roboturdf.getRobotGrayColor(),
    visible=True)
segmentationroutines.SegmentationContext.initWithRobot(robotStateModel)

# load poly data
dataDir = app.getTestingDataDirectory()
polyData = ioUtils.readPolyData(
    os.path.join(dataDir, 'valve/valve-sparse-stereo.pcd'))
vis.showPolyData(polyData,
                 'pointcloud snapshot original',
                 colorByName='rgb_colors')
polyData = segmentationroutines.sparsifyStereoCloud(polyData)
vis.showPolyData(polyData, 'pointcloud snapshot')

# fit valve and lever
segmentation.segmentValveWallAuto(
Ejemplo n.º 4
0
        self.linkName = linkName
        self.pickedPoint = pickedPoint


    def onMousePress(self, displayPoint, modifiers=None):
        print self.linkName, self.pickedPoint


###########################

app = ConsoleApp()
app.setupGlobals(globals())

view = app.createView()
view.show()
view.resize(1080, 768)

robotModel, jointController = roboturdf.loadRobotModel('robot model', view, parent='scene', color=roboturdf.getRobotGrayColor(), visible=True)
robotModel.addToView(view)

widget = LinkWidget(view, robotModel)
widget.start()

app.viewOptions.setProperty('Gradient background', False)
app.viewOptions.setProperty('Background color', [1,1,1])
app.viewOptions.setProperty('Orientation widget', False)
app.gridObj.setProperty('Color', [0,0,0])
app.gridObj.setProperty('Surface Mode', 'Surface with edges')

app.start()
Ejemplo n.º 5
0
from ddapp import ioUtils
from ddapp import segmentation
from ddapp import segmentationroutines
from ddapp import applogic
from ddapp import visualization as vis

from ddapp import roboturdf

app = ConsoleApp()

# create a view
view = app.createView()
segmentation._defaultSegmentationView = view
segmentation.initAffordanceManager(view)

robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot state model', view, parent='sensors', color=roboturdf.getRobotGrayColor(), visible=True)
segmentationroutines.SegmentationContext.initWithRobot(robotStateModel)

# Move robot to near to valve wall:
# 0degrees
#robotStateJointController.q[5] = math.radians(120)
#robotStateJointController.q[0] = 0
#robotStateJointController.q[1] = 0
# 30,60,90
robotStateJointController.q [5] = math.radians(-120)
robotStateJointController.q [0] = 1
robotStateJointController.q [1] = 1

robotStateJointController.q[2] = 0.85
robotStateJointController.push()
Ejemplo n.º 6
0
from ddapp.consoleapp import ConsoleApp
from ddapp import visualization as vis
from ddapp import roboturdf

app = ConsoleApp()
view = app.createView()

robotStateModel, robotStateJointController = roboturdf.loadRobotModel(
    "robot state model", view, parent="sensors", color=roboturdf.getRobotGrayColor(), visible=True
)

robotStateModel.addToView(view)

if app.getTestingInteractiveEnabled():
    view.show()
    app.start()
Ejemplo n.º 7
0
        self.linkName = linkName
        self.pickedPoint = pickedPoint


    def onMousePress(self, displayPoint, modifiers=None):
        print self.linkName, self.pickedPoint


###########################

app = ConsoleApp()
app.setupGlobals(globals())

view = app.createView()
view.show()
view.resize(1080, 768)

robotModel, jointController = roboturdf.loadRobotModel('robot model', view, parent='scene', color=roboturdf.getRobotGrayColor(), visible=True)
robotModel.addToView(view)

widget = LinkWidget(view, robotModel)
widget.start()

app.viewOptions.setProperty('Gradient background', False)
app.viewOptions.setProperty('Background color', [1,1,1])
app.viewOptions.setProperty('Orientation widget', False)
app.gridObj.setProperty('Color', [0,0,0])
app.gridObj.setProperty('Surface Mode', 'Surface with edges')

app.start()
Ejemplo n.º 8
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