<s> import PythonQt <EOL> from PythonQt import QtCore , QtGui , QtUiTools <EOL> from director import applogic as app <EOL> from director import objectmodel as om <EOL> from director import propertyset <EOL> from director import visualization as vis <EOL> from director . debugVis import DebugData <EOL> from director . pointpicker import PlacerWidget <EOL> from director import segmentation <EOL> from director import filterUtils <EOL> from director import vtkNumpy as vnp <EOL> import numpy as np <EOL> def _makeButton ( text , func ) : <EOL> b = QtGui . QPushButton ( text ) <EOL> b . connect ( '<STR_LIT>' , func ) <EOL> return b <EOL> def addWidgetsToDict ( widgets , d ) : <EOL> for widget in widgets : <EOL> if widget . objectName : <EOL> d [ str ( widget . objectName ) ] = widget <EOL> addWidgetsToDict ( widget . children ( ) , d ) <EOL> class WidgetDict ( object ) : <EOL> def __init__ ( self , widgets ) : <EOL> addWidgetsToDict ( widgets , self . __dict__ ) <EOL> class FootstepsPanel ( object ) : <EOL> def __init__ ( self , driver , robotModel , jointController , irisDriver ) : <EOL> self . driver = driver <EOL> self . robotModel = robotModel <EOL> self . jointController = jointController <EOL> loader = QtUiTools . QUiLoader ( ) <EOL> uifile = QtCore . QFile ( '<STR_LIT>' ) <EOL> assert uifile . open ( uifile . ReadOnly ) <EOL> self . widget = loader . load ( uifile ) <EOL> self . placer = None <EOL> self . irisDriver = irisDriver <EOL> self . region_seed_frames = [ ] <EOL> self . ui = WidgetDict ( self . widget . children ( ) ) <EOL> self . ui . walkingGoalButton . connect ( "<STR_LIT>" , self . onNewWalkingGoal ) <EOL> self . ui . walkingPlanButton . connect ( "<STR_LIT>" , self . onShowWalkingPlan ) <EOL> self . ui . stopButton . connect ( "<STR_LIT>" , self . onStop ) <EOL> self . ui . simulateDrakeButton . connect ( "<STR_LIT>" , self . onSimulateDrake ) <EOL> self . ui . haltSimulationDrakeButton . connect ( "<STR_LIT>" , self . onHaltSimulationDrake ) <EOL> self . ui . restoreDefaultsButton . connect ( "<STR_LIT>" , lambda : self . applyDefaults ( ) ) <EOL> self . ui . showWalkingVolumesCheck . connect ( "<STR_LIT>" , self . onShowWalkingVolumes ) <EOL> self . ui . hideBDIButton . setVisible ( False ) <EOL> self . ui . showBDIButton . setVisible ( False ) <EOL> self . ui . hideBDIButton . connect ( "<STR_LIT>" , self . onHideBDIButton ) <EOL> self . ui . showBDIButton . connect ( "<STR_LIT>" , self . onShowBDIButton ) <EOL> self . ui . newRegionSeedButton . connect ( "<STR_LIT>" , self . onNewRegionSeed ) <EOL> self . ui . autoIRISSegmentationButton . connect ( "<STR_LIT>" , self . onAutoIRISSegmentation ) <EOL> self . _setupPropertiesPanel ( ) <EOL> def onShowWalkingVolumes ( self ) : <EOL> self . driver . show_contact_slices = self . ui . showWalkingVolumesCheck . checked <EOL> folder = om . findObjectByName ( '<STR_LIT>' ) <EOL> for obj in folder . children ( ) : <EOL> obj . setProperty ( '<STR_LIT>' , self . driver . show_contact_slices ) <EOL> def onNewRegionSeed ( self ) : <EOL> t = self . newWalkingGoalFrame ( self . robotModel , distanceForward = <NUM_LIT:0.5> ) <EOL> self . irisDriver . newTerrainItem ( t ) <EOL> def onAutoIRISSegmentation ( self ) : <EOL> self . irisDriver . autoIRISSegmentation ( ) <EOL> def _setupPropertiesPanel ( self ) : <EOL> propertyNames = self . driver . params . propertyNames ( ) <EOL> l = QtGui . QGridLayout ( self . ui . paramsOneContainer ) <EOL> l . setMargin ( <NUM_LIT:0> ) <EOL> self . propertiesPanel = PythonQt . dd . ddPropertiesPanel ( ) <EOL> self . propertiesPanel . setBrowserModeToWidget ( ) <EOL> l . addWidget ( self . propertiesPanel ) <EOL> self . panelConnector = propertyset . PropertyPanelConnector ( self . driver . params . properties , self . propertiesPanel , propertyNames [ <NUM_LIT:0> : <NUM_LIT:15> ] ) <EOL> l = QtGui . QGridLayout ( self . ui . paramsTwoContainer ) <EOL> l . setMargin ( <NUM_LIT:0> ) <EOL> self . propertiesPanel = PythonQt . dd . ddPropertiesPanel ( ) <EOL> self . propertiesPanel . setBrowserModeToWidget ( ) <EOL> l . addWidget ( self . propertiesPanel ) <EOL> self . panelConnector = propertyset . PropertyPanelConnector ( self . driver . params . properties , self . propertiesPanel , propertyNames [ <NUM_LIT:15> : ] ) <EOL> self . driver . params . properties . connectPropertyChanged ( self . onPropertyChanged ) <EOL> def onPropertyChanged ( self , propertySet , propertyName ) : <EOL> self . driver . updateRequest ( ) <EOL> if propertyName == '<STR_LIT>' : <EOL> self . applyDefaults ( ) <EOL> def applyDefaults ( self ) : <EOL> set_name = self . driver . defaults_map [ self . driver . params . properties . defaults ] <EOL> for k , v in self . driver . default_step_params [ set_name ] . iteritems ( ) : <EOL> self . driver . params . setProperty ( k , v ) <EOL> def newWalkingGoalFrame ( self , robotModel , distanceForward = <NUM_LIT:1.0> ) : <EOL> t = self . driver . getFeetMidPoint ( robotModel ) <EOL> t . PreMultiply ( ) <EOL> t . Translate ( distanceForward , <NUM_LIT:0.0> , <NUM_LIT:0.0> ) <EOL> t . PostMultiply ( ) <EOL> return t <EOL> def onNewWalkingGoal ( self , walkingGoal = None ) : <EOL> walkingGoal = walkingGoal or self . newWalkingGoalFrame ( self . robotModel ) <EOL> frameObj = vis . updateFrame ( walkingGoal , '<STR_LIT>' , parent = '<STR_LIT>' , scale = <NUM_LIT> ) <EOL> frameObj . setProperty ( '<STR_LIT>' , True ) <EOL> rep = frameObj . widget . GetRepresentation ( ) <EOL> rep . SetTranslateAxisEnabled ( <NUM_LIT:2> , False ) <EOL> rep . SetRotateAxisEnabled ( <NUM_LIT:0> , False ) <EOL> rep . SetRotateAxisEnabled ( <NUM_LIT:1> , False ) <EOL> frameObj . widget . HandleRotationEnabledOff ( ) <EOL> if self . placer : <EOL> self . placer . stop ( ) <EOL> terrain = om . findObjectByName ( '<STR_LIT>' ) <EOL> if terrain : <EOL> pos = np . array ( frameObj . transform . GetPosition ( ) ) <EOL> polyData = filterUtils . removeNonFinitePoints ( terrain . polyData ) <EOL> if polyData . GetNumberOfPoints ( ) : <EOL> polyData = segmentation . labelDistanceToLine ( polyData , pos , pos + [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] ) <EOL> polyData = segmentation . thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT:0.0> , <NUM_LIT:0.1> ] ) <EOL> if polyData . GetNumberOfPoints ( ) : <EOL> pos [ <NUM_LIT:2> ] = np . nanmax ( vnp . getNumpyFromVtk ( polyData , '<STR_LIT>' ) [ : , <NUM_LIT:2> ] ) <EOL> frameObj . transform . Translate ( pos - np . array ( frameObj . transform . GetPosition ( ) ) ) <EOL> d = DebugData ( ) <EOL> d . addSphere ( ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ) , radius = <NUM_LIT> ) <EOL> handle = vis . showPolyData ( d . getPolyData ( ) , '<STR_LIT>' , parent = frameObj , visible = True , color = [ <NUM_LIT:1> , <NUM_LIT:1> , <NUM_LIT:0> ] ) <EOL> handle . actor . SetUserTransform ( frameObj . transform ) <EOL> self . placer = PlacerWidget ( app . getCurrentRenderView ( ) , handle , terrain ) <EOL> def onFramePropertyModified ( propertySet , propertyName ) : <EOL> if propertyName == '<STR_LIT>' : <EOL> if propertySet . getProperty ( propertyName ) : <EOL> self . placer . start ( ) <EOL> else : <EOL> self . placer . stop ( ) <EOL> frameObj . properties . connectPropertyChanged ( onFramePropertyModified ) <EOL> onFramePropertyModified ( frameObj , '<STR_LIT>' ) <EOL> frameObj . connectFrameModified ( self . onWalkingGoalModified ) <EOL> self . onWalkingGoalModified ( frameObj ) <EOL> def onWalkingGoalModified ( self , frame ) : <EOL> om . removeFromObjectModel ( om . findObjectByName ( '<STR_LIT>' ) ) <EOL> request = self . driver . constructFootstepPlanRequest ( self . jointController . q , frame . transform ) <EOL> self . driver . sendFootstepPlanRequest ( request ) <EOL> def onExecute ( self ) : <EOL> self . driver . commitFootstepPlan ( self . driver . lastFootstepPlan ) <EOL> def onShowWalkingPlan ( self ) : <EOL> startPose = self . jointController . getPose ( '<STR_LIT>' ) <EOL> self . driver . sendWalkingPlanRequest ( self . driver . lastFootstepPlan , startPose , waitForResponse = True ) <EOL> def onStop ( self ) : <EOL> self . driver . sendStopWalking ( ) <EOL> def onSimulateDrake ( self ) : <EOL> startPose = self . jointController . getPose ( '<STR_LIT>' ) <EOL> self . robotModel . setProperty ( '<STR_LIT>' , False ) ; <EOL> self . driver . sendWalkingPlanRequest ( self . driver . lastFootstepPlan , startPose , waitForResponse = False , req_type = '<STR_LIT>' ) <EOL> def onHaltSimulationDrake ( self ) : <EOL> om . findObjectByName ( "<STR_LIT>" ) . findChild ( "<STR_LIT>" ) . setProperty ( "<STR_LIT>" , False ) <EOL> self . robotModel . setProperty ( '<STR_LIT>' , True ) ; <EOL> self . driver . sendHaltSimulationDrakeRequest ( ) <EOL> def onHideBDIButton ( self ) : <EOL> self . driver . showBDIPlan = False <EOL> self . driver . bdiRobotModel . setProperty ( '<STR_LIT>' , False ) <EOL> folder = om . getOrCreateContainer ( "<STR_LIT>" ) <EOL> om . removeFromObjectModel ( folder ) <EOL> folder = om . getOrCreateContainer ( "<STR_LIT>" ) <EOL> om . removeFromObjectModel ( folder ) <EOL> def onShowBDIButton ( self ) : <EOL> self . driver . showBDIPlan = True <EOL> self . driver . bdiRobotModel . setProperty ( '<STR_LIT>' , True ) <EOL> self . driver . drawBDIFootstepPlan ( ) <EOL> self . driver . drawBDIFootstepPlanAdjusted ( ) <EOL> def _getAction ( ) : <EOL> return app . getToolBarActions ( ) [ '<STR_LIT>' ] <EOL> def init ( * args , ** kwargs ) : <EOL> global panel <EOL> global dock <EOL> panel = FootstepsPanel ( * args , ** kwargs ) <EOL> dock = app . addWidgetToDock ( panel . widget , action = _getAction ( ) ) <EOL> dock . hide ( ) <EOL> return panel </s>
