<s> import os <EOL> import sys <EOL> import vtkAll as vtk <EOL> import math <EOL> import time <EOL> import types <EOL> import functools <EOL> import numpy as np <EOL> from director import transformUtils <EOL> from director import lcmUtils <EOL> from director . timercallback import TimerCallback <EOL> from director . asynctaskqueue import AsyncTaskQueue <EOL> from director import objectmodel as om <EOL> from director import visualization as vis <EOL> from director import applogic as app <EOL> from director . debugVis import DebugData <EOL> from director import ikplanner <EOL> from director . ikparameters import IkParameters <EOL> from director import ioUtils <EOL> from director . simpletimer import SimpleTimer <EOL> from director . utime import getUtime <EOL> from director import affordanceitems <EOL> from director import robotstate <EOL> from director import robotplanlistener <EOL> from director import segmentation <EOL> from director import planplayback <EOL> from director import affordanceupdater <EOL> from director import segmentationpanel <EOL> from director import vtkNumpy as vnp <EOL> from director import switchplanner <EOL> from director . tasks . taskuserpanel import TaskUserPanel <EOL> from director . tasks . taskuserpanel import ImageBasedAffordanceFit <EOL> import director . tasks . robottasks as rt <EOL> import director . tasks . taskmanagerwidget as tmw <EOL> import drc as lcmdrc <EOL> import copy <EOL> from PythonQt import QtCore , QtGui <EOL> class SurpriseTaskPlanner ( object ) : <EOL> def __init__ ( self , robotSystem ) : <EOL> self . robotSystem = robotSystem <EOL> self . robotModel = robotSystem . robotStateModel <EOL> self . ikPlanner = robotSystem . ikPlanner <EOL> self . lockBackForManip = False <EOL> self . lockBaseForManip = True <EOL> self . side = '<STR_LIT:right>' <EOL> self . toolTipToHandFrame = robotSystem . ikPlanner . newPalmOffsetGraspToHandFrame ( self . side , <NUM_LIT:0.1> ) <EOL> class ImageFitter ( ImageBasedAffordanceFit ) : <EOL> def __init__ ( self , switchPlanner ) : <EOL> ImageBasedAffordanceFit . __init__ ( self , numberOfPoints = <NUM_LIT:1> ) <EOL> self . switchPlanner = switchPlanner <EOL> self . fitFunc = None <EOL> self . pickLineRadius = <NUM_LIT> <EOL> self . pickNearestToCamera = False <EOL> self . useLocalPlaneFit = True <EOL> self . useVoxelGrid = True <EOL> def fit ( self , polyData , points ) : <EOL> if self . fitFunc : <EOL> self . fitFunc ( polyData , points ) <EOL> def fitSwitchBox ( self , polyData , points ) : <EOL> boxPosition = points [ <NUM_LIT:0> ] <EOL> wallPoint = points [ <NUM_LIT:1> ] <EOL> searchRadius = <NUM_LIT> <EOL> planePoints , normal = segmentation . applyLocalPlaneFit ( polyData , points [ <NUM_LIT:0> ] , searchRadius = np . linalg . norm ( points [ <NUM_LIT:1> ] - points [ <NUM_LIT:0> ] ) , searchRadiusEnd = <NUM_LIT:1.0> ) <EOL> obj = vis . updatePolyData ( planePoints , '<STR_LIT>' , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , visible = False ) <EOL> obj . setProperty ( '<STR_LIT>' , <NUM_LIT:7> ) <EOL> viewDirection = segmentation . SegmentationContext . getGlobalInstance ( ) . getViewDirection ( ) <EOL> if np . dot ( normal , viewDirection ) < <NUM_LIT:0> : <EOL> normal = - normal <EOL> origin = segmentation . computeCentroid ( planePoints ) <EOL> zaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> xaxis = normal <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> zaxis = np . cross ( xaxis , yaxis ) <EOL> zaxis /= np . linalg . norm ( zaxis ) <EOL> t = transformUtils . getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( boxPosition ) <EOL> boxFrame = transformUtils . copyFrame ( t ) <EOL> self . switchPlanner . spawnBoxAffordanceAtFrame ( boxFrame ) <EOL> class SurpriseTaskPanel ( TaskUserPanel ) : <EOL> def __init__ ( self , robotSystem ) : <EOL> TaskUserPanel . __init__ ( self , windowTitle = '<STR_LIT>' ) <EOL> self . planner = SurpriseTaskPlanner ( robotSystem ) <EOL> self . switchPlanner = switchplanner . SwitchPlanner ( robotSystem ) <EOL> self . fitter = ImageFitter ( self . switchPlanner ) <EOL> self . initImageView ( self . fitter . imageView ) <EOL> self . addDefaultProperties ( ) <EOL> self . addButtons ( ) <EOL> self . addSwitchTasks ( ) <EOL> def test ( self ) : <EOL> print '<STR_LIT:test>' <EOL> def addButtons ( self ) : <EOL> self . addManualSpacer ( ) <EOL> self . addManualButton ( '<STR_LIT>' , self . switchPlanner . planArmsPrep1 ) <EOL> self . addManualButton ( '<STR_LIT>' , self . switchPlanner . planArmsPrep2 ) <EOL> self . addManualButton ( '<STR_LIT>' , self . fitSwitchBox ) <EOL> self . addManualButton ( '<STR_LIT>' , self . switchPlanner . spawnBoxAffordance ) <EOL> self . addManualButton ( '<STR_LIT>' , self . switchPlanner . spawnFootstepFrame ) <EOL> self . addManualButton ( '<STR_LIT>' , self . switchPlanner . updateReachFrame ) <EOL> self . addManualButton ( '<STR_LIT>' , self . onPlanPinchReach ) <EOL> self . addManualButton ( '<STR_LIT>' , self . switchPlanner . commitManipPlan ) <EOL> def onPlanPinchReach ( self ) : <EOL> self . switchPlanner . planPinchReach ( maxDegreesPerSecond = self . maxDegreesPerSecond ) <EOL> def getSide ( self ) : <EOL> return self . params . getPropertyEnumValue ( '<STR_LIT>' ) . lower ( ) <EOL> def addDefaultProperties ( self ) : <EOL> self . params . addProperty ( '<STR_LIT>' , <NUM_LIT:10> , attributes = om . PropertyAttributes ( singleStep = <NUM_LIT:1> , decimals = <NUM_LIT:2> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , <NUM_LIT:0> , attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT>' , '<STR_LIT>' ] ) ) <EOL> self . params . setProperty ( '<STR_LIT>' , self . planner . side . capitalize ( ) ) <EOL> def onPropertyChanged ( self , propertySet , propertyName ) : <EOL> if propertyName == '<STR_LIT>' : <EOL> self . planner . side = self . getSide ( ) <EOL> self . syncProperties ( ) <EOL> def syncProperties ( self ) : <EOL> self . maxDegreesPerSecond = self . params . getProperty ( '<STR_LIT>' ) <EOL> def setParamsPreTeleop ( self ) : <EOL> self . params . setProperty ( '<STR_LIT>' , <NUM_LIT:30> ) <EOL> def setParamsTeleop ( self ) : <EOL> self . params . setProperty ( '<STR_LIT>' , <NUM_LIT:10> ) <EOL> def addTasks ( self ) : <EOL> self . folder = None <EOL> def addTask ( task , parent = None ) : <EOL> parent = parent or self . folder <EOL> self . taskTree . onAddTask ( task , copy = False , parent = parent ) <EOL> def addFunc ( name , func , parent = None ) : <EOL> addTask ( rt . CallbackTask ( callback = func , name = name ) , parent = parent ) <EOL> def addFolder ( name , parent = None ) : <EOL> self . folder = self . taskTree . addGroup ( name , parent = parent ) <EOL> return self . folder <EOL> def addManipTask ( name , planFunc , userPrompt = False ) : <EOL> prevFolder = self . folder <EOL> addFolder ( name , prevFolder ) <EOL> addFunc ( '<STR_LIT>' , planFunc ) <EOL> if not userPrompt : <EOL> addTask ( rt . CheckPlanInfo ( name = '<STR_LIT>' ) ) <EOL> else : <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , message = '<STR_LIT>' ) ) <EOL> addFunc ( '<STR_LIT>' , self . drillDemo . commitManipPlan ) <EOL> addTask ( rt . WaitForManipulationPlanExecution ( name = '<STR_LIT>' ) ) <EOL> self . folder = prevFolder <EOL> self . taskTree . removeAllTasks ( ) <EOL> side = self . getSide ( ) <EOL> self . addSwitchTasks ( ) <EOL> def addSwitchTasks ( self ) : <EOL> self . folder = None <EOL> def addTask ( task , parent = None ) : <EOL> parent = parent or self . folder <EOL> self . taskTree . onAddTask ( task , copy = False , parent = parent ) <EOL> def addFunc ( name , func , parent = None ) : <EOL> addTask ( rt . CallbackTask ( callback = func , name = name ) , parent = parent ) <EOL> def addFolder ( name , parent = None ) : <EOL> self . folder = self . taskTree . addGroup ( name , parent = parent ) <EOL> return self . folder <EOL> def addManipTask ( name , planFunc , userPrompt = False ) : <EOL> prevFolder = self . folder <EOL> addFolder ( name , prevFolder ) <EOL> addFunc ( '<STR_LIT>' , planFunc ) <EOL> if not userPrompt : <EOL> addTask ( rt . CheckPlanInfo ( name = '<STR_LIT>' ) ) <EOL> else : <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , message = '<STR_LIT>' ) ) <EOL> addFunc ( '<STR_LIT>' , self . switchPlanner . commitManipPlan ) <EOL> addTask ( rt . WaitForManipulationPlanExecution ( name = '<STR_LIT>' ) ) <EOL> self . folder = prevFolder <EOL> self . taskTree . removeAllTasks ( ) <EOL> side = self . getSide ( ) <EOL> addFolder ( '<STR_LIT>' ) <EOL> addFunc ( '<STR_LIT>' , self . fitSwitchBox ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , message = '<STR_LIT>' ) ) <EOL> addFolder ( '<STR_LIT>' ) <EOL> addFunc ( '<STR_LIT>' , self . switchPlanner . spawnFootstepFrame ) <EOL> addTask ( rt . RequestFootstepPlan ( name = '<STR_LIT>' , stanceFrameName = '<STR_LIT>' ) ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , message = '<STR_LIT>' ) ) <EOL> addTask ( rt . CommitFootstepPlan ( name = '<STR_LIT>' , planName = '<STR_LIT>' ) ) <EOL> addTask ( rt . WaitForWalkExecution ( name = '<STR_LIT>' ) ) <EOL> armsUp = addFolder ( '<STR_LIT>' ) <EOL> addManipTask ( '<STR_LIT>' , self . switchPlanner . planArmsPrep1 , userPrompt = True ) <EOL> self . folder = armsUp <EOL> addManipTask ( '<STR_LIT>' , self . switchPlanner . planArmsPrep2 , userPrompt = True ) <EOL> addTask ( rt . CloseHand ( side = '<STR_LIT>' , mode = '<STR_LIT>' , name = '<STR_LIT>' ) ) <EOL> reach = addFolder ( '<STR_LIT>' ) <EOL> addFunc ( '<STR_LIT>' , self . setParamsPreTeleop ) <EOL> addFunc ( '<STR_LIT>' , self . switchPlanner . updateReachFrame ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , message = '<STR_LIT>' ) ) <EOL> addManipTask ( '<STR_LIT>' , self . onPlanPinchReach , userPrompt = True ) <EOL> teleop = addFolder ( '<STR_LIT>' ) <EOL> addFunc ( '<STR_LIT>' , self . setParamsTeleop ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , message = '<STR_LIT>' ) ) <EOL> armsDown = addFolder ( '<STR_LIT>' ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , message = '<STR_LIT>' ) ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = '<STR_LIT>' ) ) <EOL> addManipTask ( '<STR_LIT>' , self . switchPlanner . planArmsPrep2 , userPrompt = True ) <EOL> self . folder = armsDown <EOL> self . folder = armsDown <EOL> addManipTask ( '<STR_LIT>' , self . switchPlanner . planArmsPrep1 , userPrompt = True ) <EOL> self . folder = armsDown <EOL> addManipTask ( '<STR_LIT>' , self . switchPlanner . planNominal , userPrompt = True ) <EOL> def fitSwitchBox ( self ) : <EOL> print '<STR_LIT>' <EOL> self . fitter . imagePicker . numberOfPoints = <NUM_LIT:2> <EOL> self . fitter . pointCloudSource = '<STR_LIT>' <EOL> self . fitter . fitFunc = self . fitter . fitSwitchBox </s>
