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
from ddapp import atlasdriver from ddapp import consoleapp from PythonQt import QtCore, QtGui from collections import namedtuple atlasDriver = atlasdriver.init() w = QtGui.QWidget() l = QtGui.QVBoxLayout(w) Button = namedtuple('Button', ['name', 'callback', 'color']); buttons = [ Button('Reactive Recovery', atlasDriver.sendRecoveryTriggerOn, None) ] for button in buttons: qb = QtGui.QPushButton(button.name) qb.connect('clicked()', button.callback) qb.setSizePolicy(QtGui.QSizePolicy.Expanding, QtGui.QSizePolicy.Expanding) s = qb.styleSheet s += "font: 36pt;" if button.color: s += "background-color: {:s}; color: white;".format(button.color) qb.setStyleSheet(s) l.addWidget(qb) w.setWindowTitle('Atlas Recovery Button') w.show() w.resize(500,600)
def main(): atlasdriver.init() panel = atlasdriverpanel.AtlasDriverPanel(atlasdriver.driver) panel.widget.show() ConsoleApp.start()
from ddapp.consoleapp import ConsoleApp from ddapp import atlasdriver from ddapp import consoleapp from ddapp.timercallback import TimerCallback from ddapp import robotsystem from PythonQt import QtCore, QtGui from collections import namedtuple import time, math FPS = 4 LABEL_DEFAULT_STYLE_SHEET = "font: 36pt; border-style: outset; border-width: 2px; border-color: black;" TITLE_DEFAULT_STYLE_SHEET = "font: 24pt" atlasDriver = atlasdriver.init() app = ConsoleApp() app.setupGlobals(globals()) view = app.createView() robotsystem.create(view, globals()) w = QtGui.QWidget() l = QtGui.QHBoxLayout(w) def wrapInVTitledItem(name, items): box = QtGui.QGroupBox(name) box.setAlignment(QtCore.Qt.AlignCenter) boxLayout = QtGui.QVBoxLayout(box) for item in items: boxLayout.addWidget(item) return box
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