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.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 __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', lcmdrc.atlas_command_t, self.onAtlasCommand) self.sub.setSpeedLimit(60)
def __init__(self): self.sub = lcmUtils.addSubscriber('JOINT_POSITION_GOAL', lcmdrc.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 __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 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 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 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
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()
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()
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()
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