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 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 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 __init__(self): self.timer = TimerCallback(targetFps=10) #self.timer.disableScheduledTimer() self.app = ConsoleApp() self.robotModel, self.jointController = roboturdf.loadRobotModel( 'robot model') self.fpsCounter = simpletimer.FPSCounter() self.drakePoseJointNames = robotstate.getDrakePoseJointNames() self.fpsCounter.printToConsole = True self.timer.callback = self._tick self._initialized = False self.publishChannel = 'JOINT_POSITION_GOAL' # self.lastCommandMessage = newAtlasCommandMessageAtZero() self._numPositions = len(robotstate.getDrakePoseJointNames()) self._previousElapsedTime = 100 self._baseFlag = 0 self.jointLimitsMin = np.array([ self.robotModel.model.getJointLimits(jointName)[0] for jointName in robotstate.getDrakePoseJointNames() ]) self.jointLimitsMax = np.array([ self.robotModel.model.getJointLimits(jointName)[1] for jointName in robotstate.getDrakePoseJointNames() ]) self.useControllerFlag = False self.drivingGainsFlag = False self.applyDefaults()
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 test(): import os import pydrake from director import roboturdf filename = os.path.join(pydrake.getDrakePath(), 'examples/PR2/pr2.urdf') assert os.path.isfile(filename) robotModel, jointController = roboturdf.loadRobotModel(urdfFile=filename, view=view, useConfigFile=False) conf = { 'pr2LeftGripper': [0.07], 'pr2RightArm': [-0.91698, 0.042600, -1.5, -2.01531, -1.57888, -1.65300, -2.04511], 'pr2Base': [0.0, 0.0, 0.0], 'pr2Torso': [0.3], 'pr2RightGripper': [0.07], 'pr2Head': [0.0, 0.0], 'pr2LeftArm': [2.1, 1.29, 0.0, -0.15, 0.0, -0.1, 0.0] } t = BHPNTranslator(jointController) t.setRobotConf(conf)
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 = []
def __init__(self): self.app = ConsoleApp() self.view = self.app.createView() self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model', self.view) self.jointController.setZeroPose() self.view.show() self.sub = lcmUtils.addSubscriber('ATLAS_COMMAND', lcmbotcore.atlas_command_t, self.onAtlasCommand) self.sub.setSpeedLimit(60)
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 )
def __init__(self): self.sub = lcmUtils.addSubscriber('JOINT_POSITION_GOAL', lcmbotcore.robot_state_t, self.onJointPositionGoal) self.sub = lcmUtils.addSubscriber('SINGLE_JOINT_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal) lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t, self.onPause) self.debug = False if self.debug: self.app = ConsoleApp() self.view = self.app.createView() self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model', self.view) self.jointController.setPose('ATLAS_COMMAND', commandStream._currentCommandedPose) self.view.show() self.timer = TimerCallback(targetFps=30) self.timer.callback = self.onDebug
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 initRobotState(self, robotSystem): from director import roboturdf robotStateModel, robotStateJointController = roboturdf.loadRobotModel( 'robot state model', robotSystem.view, urdfFile=robotSystem.directorConfig['urdfConfig']['robotState'], color=roboturdf.getRobotGrayColor(), colorMode=robotSystem.directorConfig['colorMode'], parent='sensors', visible=True) robotStateJointController.setPose('EST_ROBOT_STATE', robotStateJointController.getPose('q_nom')) roboturdf.startModelPublisherListener([(robotStateModel, robotStateJointController)]) robotStateJointController.addLCMUpdater('EST_ROBOT_STATE') return FieldContainer(robotStateModel=robotStateModel, robotStateJointController=robotStateJointController)
def initRobotState(self, robotSystem): from director import roboturdf from director import objectmodel as om robotStateModel, robotStateJointController = roboturdf.loadRobotModel( "robot state model", robotSystem.view, color=roboturdf.getRobotGrayColor(), colorMode=robotSystem.directorConfig["colorMode"], parent=om.getOrCreateContainer( "sensors", om.getOrCreateContainer(robotSystem.robotName)), visible=True, robotName=robotSystem.robotName, ) # robotStateJointController.setPose('EST_ROBOT_STATE', robotStateJointController.getPose('q_nom')) # roboturdf.startModelPublisherListener([(robotStateModel, robotStateJointController)]) return FieldContainer( robotStateModel=robotStateModel, robotStateJointController=robotStateJointController, )
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()
type=str, default=None, help='recursively search this directory and load every urdf found') args, unknown = parser.parse_known_args() return args app = mainwindowapp.construct() view = app.view args = getArgs() if args.urdf: robotModel, jointController = roboturdf.loadRobotModel(urdfFile=args.urdf, view=view, useConfigFile=False) jointNames = robotModel.model.getJointNames() jointController = jointcontrol.JointController([robotModel], jointNames=jointNames) elif args.glob_dir: urdfFiles = [] for root, dirnames, filenames in os.walk(args.glob_dir): for filename in fnmatch.filter(filenames, '*.urdf'): urdfFiles.append(os.path.join(root, filename)) failedFiles = [] for urdfFile in urdfFiles:
from director import playbackpanel from director import robotplanlistener from director import robotviewbehaviors from director import pointcloudlcm from director import cameraview from director import lcmUtils from PythonQt import QtCore, QtGui import drc as lcmdrc import bot_core as lcmbotcore # 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)
from director import segmentation from director import segmentationroutines from director import applogic from director import visualization as vis from director 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) # Move robot to near to table: robotStateJointController.q[5] = math.radians(120) robotStateJointController.q[0] = -1.5 robotStateJointController.q[1] = 2 robotStateJointController.q[2] = 0.95 robotStateJointController.push() # load poly data dataDir = app.getTestingDataDirectory() polyData = ioUtils.readPolyData( os.path.join(dataDir, 'tabletop/table-and-bin-scene.vtp'))
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
return args app = ConsoleApp() view = app.createView() args = getArgs() if args.urdf: robotModel = roboturdf.openUrdf(args.urdf, view) jointNames = robotModel.model.getJointNames() jointController = jointcontrol.JointController([robotModel], jointNames=jointNames) else: robotModel, jointController = roboturdf.loadRobotModel('robot model', view) print 'urdf file:', robotModel.getProperty('Filename') for joint in robotModel.model.getJointNames(): print 'joint:', joint for link in robotModel.model.getLinkNames(): print 'link:', link robotModel.getLinkFrame(link) if app.getTestingInteractiveEnabled(): view.show() app.start()