<s> import os <EOL> import math <EOL> import functools <EOL> import numpy as np <EOL> from director import transformUtils <EOL> from director import objectmodel as om <EOL> from director import visualization as vis <EOL> from director import applogic as app <EOL> from director import ik <EOL> from director . ikparameters import IkParameters <EOL> from director import lcmUtils <EOL> from director import robotstate <EOL> from director import segmentation <EOL> from director . tasks . taskuserpanel import TaskUserPanel <EOL> from director . tasks . taskuserpanel import ImageBasedAffordanceFit <EOL> from director . uuidutil import newUUID <EOL> from director import ioUtils <EOL> from director import ikplanner <EOL> from director . debugVis import DebugData <EOL> import director . tasks . robottasks as rt <EOL> from PythonQt import QtCore <EOL> class ValvePlannerDemo ( object ) : <EOL> def __init__ ( self , robotModel , footstepPlanner , footstepsPanel , manipPlanner , ikPlanner , <EOL> lhandDriver , rhandDriver , sensorJointController ) : <EOL> self . robotModel = robotModel <EOL> self . footstepPlanner = footstepPlanner <EOL> self . footstepsPanel = footstepsPanel <EOL> self . manipPlanner = manipPlanner <EOL> self . ikPlanner = ikPlanner <EOL> self . lhandDriver = lhandDriver <EOL> self . rhandDriver = rhandDriver <EOL> self . sensorJointController = sensorJointController <EOL> self . graspingObject = '<STR_LIT>' <EOL> self . turningMode = <NUM_LIT:0> <EOL> self . setGraspingHand ( '<STR_LIT:left>' ) <EOL> self . graspFrame = None <EOL> self . stanceFrame = None <EOL> self . planFromCurrentRobotState = False <EOL> self . plans = [ ] <EOL> self . speedLow = <NUM_LIT:10> <EOL> self . speedHigh = <NUM_LIT> <EOL> self . speedTurn = <NUM_LIT:100> <EOL> self . maxHandTranslationSpeed = <NUM_LIT> <EOL> self . graspFrameXYZ = [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , - <NUM_LIT:0.1> ] <EOL> self . graspFrameRPY = [ <NUM_LIT> , <NUM_LIT:0> , <NUM_LIT> ] <EOL> self . nominalPelvisXYZ = None <EOL> self . useLargeValveDefaults ( ) <EOL> self . coaxialTol = <NUM_LIT> <EOL> self . coaxialGazeTol = <NUM_LIT:2> <EOL> self . shxMaxTorque = <NUM_LIT> <EOL> self . elxMaxTorque = <NUM_LIT:10> <EOL> self . elxLowerBoundDegrees = <NUM_LIT:30> <EOL> self . reachPoseName = None <EOL> self . touchPose = None <EOL> self . quasiStaticShrinkFactor = <NUM_LIT:0.5> <EOL> self . lockBack = True <EOL> self . lockBase = True <EOL> self . nominalPoseName = '<STR_LIT>' <EOL> self . startPoseName = '<STR_LIT>' <EOL> def setGraspingHand ( self , side ) : <EOL> self . graspingHand = side <EOL> self . setupStance ( ) <EOL> def useLargeValveDefaults ( self ) : <EOL> self . reachHeight = <NUM_LIT:0.0> <EOL> self . reachDepth = - <NUM_LIT:0.1> <EOL> self . retractDepth = - <NUM_LIT> <EOL> self . touchDepth = <NUM_LIT> <EOL> self . openAmount = <NUM_LIT:20> <EOL> self . closedAmount = <NUM_LIT:20> <EOL> self . smallValve = False <EOL> def useSmallValveDefaults ( self ) : <EOL> self . reachHeight = <NUM_LIT:0.0> <EOL> self . reachDepth = - <NUM_LIT> <EOL> self . retractDepth = - <NUM_LIT> <EOL> self . touchDepth = <NUM_LIT> <EOL> self . openAmount = <NUM_LIT:0> <EOL> self . closedAmount = <NUM_LIT:50> <EOL> self . smallValve = True <EOL> def setupStance ( self ) : <EOL> if ( self . turningMode == <NUM_LIT:0> ) : <EOL> self . relativeStanceXYZInitial = [ - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT:0.0> ] <EOL> self . relativeStanceRPYInitial = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] <EOL> else : <EOL> self . relativeStanceXYZInitial = [ - <NUM_LIT:0.5> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] <EOL> self . relativeStanceRPYInitial = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] <EOL> self . relativeStanceXYZ = self . relativeStanceXYZInitial <EOL> self . relativeStanceRPY = self . relativeStanceRPYInitial <EOL> if self . graspingHand == '<STR_LIT:right>' : <EOL> self . relativeStanceXYZ [ <NUM_LIT:1> ] = - self . relativeStanceXYZ [ <NUM_LIT:1> ] <EOL> self . relativeStanceRPY [ <NUM_LIT:2> ] = - self . relativeStanceRPY [ <NUM_LIT:2> ] <EOL> def addPlan ( self , plan ) : <EOL> self . plans . append ( plan ) <EOL> def computeGroundFrame ( self , robotModel ) : <EOL> '''<STR_LIT>''' <EOL> t1 = robotModel . getLinkFrame ( self . ikPlanner . leftFootLink ) <EOL> t2 = robotModel . getLinkFrame ( self . ikPlanner . rightFootLink ) <EOL> pelvisT = robotModel . getLinkFrame ( self . ikPlanner . pelvisLink ) <EOL> xaxis = [ <NUM_LIT:1.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] <EOL> pelvisT . TransformVector ( xaxis , xaxis ) <EOL> xaxis = np . array ( xaxis ) <EOL> zaxis = np . array ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> ] ) <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> stancePosition = ( np . array ( t2 . GetPosition ( ) ) + np . array ( t1 . GetPosition ( ) ) ) / <NUM_LIT> <EOL> footHeight = <NUM_LIT> <EOL> t = transformUtils . getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( stancePosition ) <EOL> t . Translate ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , - footHeight ] ) <EOL> return t <EOL> def computeRobotStanceFrame ( self , objectTransform , relativeStanceTransform ) : <EOL> '''<STR_LIT>''' <EOL> groundFrame = self . footstepPlanner . getFeetMidPoint ( self . robotModel ) <EOL> groundHeight = groundFrame . GetPosition ( ) [ <NUM_LIT:2> ] <EOL> graspPosition = np . array ( objectTransform . GetPosition ( ) ) <EOL> graspYAxis = [ <NUM_LIT:0.0> , <NUM_LIT:1.0> , <NUM_LIT:0.0> ] <EOL> graspZAxis = [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> ] <EOL> objectTransform . TransformVector ( graspYAxis , graspYAxis ) <EOL> objectTransform . TransformVector ( graspZAxis , graspZAxis ) <EOL> xaxis = graspYAxis <EOL> zaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> graspGroundTransform = transformUtils . getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> graspGroundTransform . PostMultiply ( ) <EOL> graspGroundTransform . Translate ( graspPosition [ <NUM_LIT:0> ] , graspPosition [ <NUM_LIT:1> ] , groundHeight ) <EOL> robotStance = transformUtils . copyFrame ( relativeStanceTransform ) <EOL> robotStance . Concatenate ( graspGroundTransform ) <EOL> return robotStance <EOL> def updatePointcloudSnapshot ( self ) : <EOL> if ( self . useLidar is True ) : <EOL> return vis . updatePolyData ( segmentation . getCurrentRevolutionData ( ) , <EOL> '<STR_LIT>' , parent = '<STR_LIT>' ) <EOL> else : <EOL> return vis . updatePolyData ( segmentation . getDisparityPointCloud ( <NUM_LIT:4> ) , <EOL> '<STR_LIT>' , parent = '<STR_LIT>' ) <EOL> def onImageViewDoubleClick ( self , displayPoint , modifiers , imageView ) : <EOL> if modifiers != QtCore . Qt . ControlModifier : <EOL> return <EOL> imagePixel = imageView . getImagePixel ( displayPoint ) <EOL> cameraPos , ray = imageView . getWorldPositionAndRay ( imagePixel ) <EOL> polyData = self . updatePointcloudSnapshot ( ) . polyData <EOL> pickPoint = segmentation . extractPointsAlongClickRay ( cameraPos , ray , <EOL> polyData ) <EOL> om . removeFromObjectModel ( om . findObjectByName ( '<STR_LIT>' ) ) <EOL> segmentation . segmentValveByBoundingBox ( polyData , pickPoint ) <EOL> self . findAffordance ( ) <EOL> def getValveAffordance ( self ) : <EOL> return om . findObjectByName ( '<STR_LIT>' ) <EOL> def computeStanceFrame ( self , useIkTraj = False ) : <EOL> objectTransform = transformUtils . copyFrame ( self . computeGraspFrame ( ) . transform ) <EOL> if useIkTraj : <EOL> startPose = self . getNominalPose ( ) <EOL> plan = self . planInsertTraj ( self . speedLow , lockFeet = False , lockBase = False , <EOL> resetPoses = True , startPose = startPose ) <EOL> stancePose = robotstate . convertStateMessageToDrakePose ( plan . plan [ <NUM_LIT:0> ] ) <EOL> stanceRobotModel = self . ikPlanner . getRobotModelAtPose ( stancePose ) <EOL> self . nominalPelvisXYZ = stancePose [ : <NUM_LIT:3> ] <EOL> robotStance = self . footstepPlanner . getFeetMidPoint ( stanceRobotModel ) <EOL> else : <EOL> robotStance = self . computeRobotStanceFrame ( objectTransform , <EOL> self . computeRelativeStanceTransform ( ) ) <EOL> stanceFrame = vis . updateFrame ( robotStance , '<STR_LIT>' , <EOL> parent = self . getValveAffordance ( ) , visible = False , scale = <NUM_LIT> ) <EOL> stanceFrame . addToView ( app . getDRCView ( ) ) <EOL> return stanceFrame <EOL> def computeRelativeStanceTransform ( self ) : <EOL> return transformUtils . copyFrame ( <EOL> transformUtils . frameFromPositionAndRPY ( self . relativeStanceXYZ , self . relativeStanceRPY ) ) <EOL> def computeRelativeGraspTransform ( self ) : <EOL> t = transformUtils . copyFrame ( transformUtils . frameFromPositionAndRPY ( self . graspFrameXYZ , <EOL> self . graspFrameRPY ) ) <EOL> t . PostMultiply ( ) <EOL> t . RotateX ( <NUM_LIT> ) <EOL> t . RotateY ( - <NUM_LIT> ) <EOL> return t <EOL> def computeGraspFrame ( self ) : <EOL> t = self . computeRelativeGraspTransform ( ) <EOL> t . Concatenate ( self . getValveAffordance ( ) . getChildFrame ( ) . transform ) <EOL> graspFrame = vis . updateFrame ( t , '<STR_LIT>' , <EOL> parent = self . getValveAffordance ( ) , <EOL> visible = False , scale = <NUM_LIT> ) <EOL> graspFrame . addToView ( app . getDRCView ( ) ) <EOL> return graspFrame <EOL> def spawnValveAffordance ( self ) : <EOL> radius = <NUM_LIT> <EOL> tubeRadius = <NUM_LIT> <EOL> position = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT> ] <EOL> rpy = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] <EOL> t_feet_mid = self . footstepPlanner . getFeetMidPoint ( self . robotModel ) <EOL> t = transformUtils . frameFromPositionAndRPY ( position , rpy ) <EOL> t_grasp = self . computeRelativeGraspTransform ( ) <EOL> t_grasp . Concatenate ( t ) <EOL> t_stance = self . computeRobotStanceFrame ( t_grasp , self . computeRelativeStanceTransform ( ) ) <EOL> t_valve = t_stance . GetInverse ( ) <EOL> t_valve . GetMatrix ( ) <EOL> t_valve . Concatenate ( t ) <EOL> t_valve . Concatenate ( t_feet_mid ) <EOL> pose = transformUtils . poseFromTransform ( t_valve ) <EOL> desc = dict ( classname = '<STR_LIT>' , Name = '<STR_LIT>' , uuid = newUUID ( ) , pose = pose , <EOL> Color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , Radius = float ( radius ) , Segments = <NUM_LIT:20> ) <EOL> desc [ '<STR_LIT>' ] = tubeRadius <EOL> obj = segmentation . affordanceManager . newAffordanceFromDescription ( desc ) <EOL> obj . params = dict ( radius = radius ) <EOL> def planFootstepsToStance ( self , ** kwargs ) : <EOL> f = transformUtils . copyFrame ( self . computeStanceFrame ( ** kwargs ) . transform ) <EOL> self . footstepsPanel . onNewWalkingGoal ( f ) <EOL> def planPreGrasp ( self ) : <EOL> startPose = self . getPlanningStartPose ( ) <EOL> endPose = self . ikPlanner . getMergedPostureFromDatabase ( <EOL> startPose , '<STR_LIT>' , '<STR_LIT>' , side = self . graspingHand ) <EOL> newPlan = self . ikPlanner . computePostureGoal ( startPose , endPose ) <EOL> self . addPlan ( newPlan ) <EOL> def planNominal ( self ) : <EOL> startPose = self . getPlanningStartPose ( ) <EOL> endPose , info = self . ikPlanner . computeStandPose ( startPose ) <EOL> endPose = self . ikPlanner . getMergedPostureFromDatabase ( endPose , '<STR_LIT>' , '<STR_LIT>' ) <EOL> newPlan = self . ikPlanner . computePostureGoal ( startPose , endPose ) <EOL> self . addPlan ( newPlan ) <EOL> def createStaticTorqueConstraint ( self ) : <EOL> if self . graspingHand == '<STR_LIT:left>' : <EOL> elxJoint = '<STR_LIT>' <EOL> shxJoint = '<STR_LIT>' <EOL> else : <EOL> elxJoint = '<STR_LIT>' <EOL> shxJoint = '<STR_LIT>' <EOL> constraint = ik . GravityCompensationTorqueConstraint ( ) <EOL> constraint . joints = [ shxJoint , elxJoint ] <EOL> constraint . torquesLowerBound = - np . array ( [ self . shxMaxTorque , self . elxMaxTorque ] ) <EOL> constraint . torquesUpperBound = np . array ( [ self . shxMaxTorque , self . elxMaxTorque ] ) <EOL> return constraint <EOL> def createElbowPostureConstraint ( self ) : <EOL> if self . graspingHand == '<STR_LIT:left>' : <EOL> elxJoint = '<STR_LIT>' <EOL> elxLowerBound = np . radians ( self . elxLowerBoundDegrees ) <EOL> elxUpperBound = <NUM_LIT> <EOL> else : <EOL> elxJoint = '<STR_LIT>' <EOL> elxLowerBound = - <NUM_LIT> <EOL> elxUpperBound = np . radians ( - self . elxLowerBoundDegrees ) <EOL> constraint = ik . PostureConstraint ( ) <EOL> constraint . joints = [ elxJoint ] <EOL> constraint . jointsLowerBound = [ elxLowerBound ] <EOL> constraint . jointsUpperBound = [ elxUpperBound ] <EOL> return constraint <EOL> def createWristAngleConstraint ( self , wristAngleCW , planFromCurrentRobotState ) : <EOL> if self . graspingHand == '<STR_LIT:left>' : <EOL> wristJoint = [ '<STR_LIT>' ] <EOL> wristJointLowerBound = [ - np . radians ( <NUM_LIT> ) + wristAngleCW ] <EOL> wristJointUpperBound = [ - np . radians ( <NUM_LIT> ) + wristAngleCW ] <EOL> else : <EOL> wristJoint = [ '<STR_LIT>' ] <EOL> wristJointLowerBound = [ np . radians ( <NUM_LIT> ) - wristAngleCW ] <EOL> wristJointUpperBound = [ np . radians ( <NUM_LIT> ) - wristAngleCW ] <EOL> constraint = ik . PostureConstraint ( ) <EOL> constraint . joints = wristJoint <EOL> constraint . jointsLowerBound = wristJointLowerBound <EOL> constraint . jointsUpperBound = wristJointUpperBound <EOL> if planFromCurrentRobotState : <EOL> constraint . tspan = [ <NUM_LIT:1.0> , <NUM_LIT:1.0> ] <EOL> return constraint <EOL> def createHandGazeConstraint ( self ) : <EOL> constraint = self . ikPlanner . createGazeGraspConstraint ( <EOL> self . graspingHand , self . computeGraspFrame ( ) , coneThresholdDegrees = self . coaxialGazeTol ) <EOL> constraint . tspan = [ <NUM_LIT:0.0> , <NUM_LIT:1.0> ] <EOL> return constraint <EOL> def createHandFixedOrientConstraint ( self ) : <EOL> if self . graspingHand == '<STR_LIT:left>' : <EOL> handLink = '<STR_LIT>' <EOL> else : <EOL> handLink = '<STR_LIT>' <EOL> return ik . WorldFixedOrientConstraint ( linkName = handLink ) <EOL> def createBackPostureConstraint ( self ) : <EOL> if self . lockBack : <EOL> return self . ikPlanner . createLockedBackPostureConstraint ( self . startPoseName ) <EOL> else : <EOL> return self . ikPlanner . createMovingBackLimitedPostureConstraint ( ) <EOL> def createFootConstraints ( self , lockFeet ) : <EOL> if lockFeet : <EOL> return self . ikPlanner . createFixedFootConstraints ( self . startPoseName ) <EOL> else : <EOL> constraints = [ ] <EOL> constraints . extend ( self . ikPlanner . createSlidingFootConstraints ( self . startPoseName ) ) <EOL> constraints . append ( ik . WorldFixedBodyPoseConstraint ( linkName = '<STR_LIT>' ) ) <EOL> constraints . append ( ik . WorldFixedBodyPoseConstraint ( linkName = '<STR_LIT>' ) ) <EOL> p = ik . RelativePositionConstraint ( ) <EOL> p . bodyNameA = self . ikPlanner . leftFootLink <EOL> p . bodyNameB = self . ikPlanner . rightFootLink <EOL> p . positionTarget = np . array ( [ <NUM_LIT:0> , <NUM_LIT> , <NUM_LIT:0> ] ) <EOL> p . lowerBound = np . array ( [ <NUM_LIT:0> , <NUM_LIT:0> , - np . inf ] ) <EOL> p . upperBound = np . array ( [ <NUM_LIT:0> , <NUM_LIT:0> , np . inf ] ) <EOL> constraints . append ( p ) <EOL> p = ik . RelativePositionConstraint ( ) <EOL> p . bodyNameA = self . ikPlanner . rightFootLink <EOL> p . bodyNameB = self . ikPlanner . leftFootLink <EOL> p . lowerBound = np . array ( [ <NUM_LIT:0> , - np . inf , - np . inf ] ) <EOL> p . upperBound = np . array ( [ <NUM_LIT:0> , np . inf , np . inf ] ) <EOL> constraints . append ( p ) <EOL> return constraints <EOL> def createHeadGazeConstraint ( self ) : <EOL> valveCenter = np . array ( self . computeGraspFrame ( ) . transform . GetPosition ( ) ) <EOL> return ik . WorldGazeTargetConstraint ( linkName = '<STR_LIT>' , bodyPoint = np . zeros ( <NUM_LIT:3> ) , <EOL> worldPoint = valveCenter , coneThreshold = np . radians ( <NUM_LIT:20> ) ) <EOL> def createBaseConstraints ( self , resetBase , lockBase , lockFeet , yawDesired ) : <EOL> constraints = [ ] <EOL> if lockBase is None : <EOL> lockBase = self . lockBase <EOL> if resetBase : <EOL> poseName = self . nominalPoseName <EOL> else : <EOL> poseName = self . startPoseName <EOL> if lockFeet : <EOL> if lockBase : <EOL> constraints . append ( <EOL> self . ikPlanner . createLockedBasePostureConstraint ( poseName , lockLegs = False ) ) <EOL> else : <EOL> constraints . append ( <EOL> self . ikPlanner . createXYZMovingBasePostureConstraint ( poseName ) ) <EOL> constraints . append ( ik . WorldFixedBodyPoseConstraint ( linkName = '<STR_LIT>' ) ) <EOL> else : <EOL> constraints . append ( self . ikPlanner . createXYZYawMovingBasePostureConstraint ( poseName ) ) <EOL> constraints . append ( ik . WorldFixedBodyPoseConstraint ( linkName = '<STR_LIT>' ) ) <EOL> constraints . append ( self . createHeadGazeConstraint ( ) ) <EOL> p = ik . PostureConstraint ( ) <EOL> p . joints = [ '<STR_LIT>' ] <EOL> p . jointsLowerBound = [ yawDesired - np . radians ( <NUM_LIT:20> ) ] <EOL> p . jointsUpperBound = [ yawDesired + np . radians ( <NUM_LIT:20> ) ] <EOL> constraints . append ( p ) <EOL> return constraints <EOL> def getStartPoseName ( self , planFromCurrentRobotState , retract , usePoses ) : <EOL> if planFromCurrentRobotState : <EOL> poseName = self . startPoseName <EOL> else : <EOL> if not usePoses or self . reachPoseName is None : <EOL> poseName = self . nominalPoseName <EOL> else : <EOL> if retract : <EOL> poseName = self . touchPoseName <EOL> else : <EOL> poseName = self . reachPoseName <EOL> return poseName <EOL> def getEndPoseName ( self , retract , usePoses ) : <EOL> if not usePoses or self . touchPose is None : <EOL> return self . nominalPoseName <EOL> else : <EOL> if retract : <EOL> return self . reachPoseName <EOL> else : <EOL> return self . touchPoseName <EOL> def createHandPositionConstraint ( self , radialTol , axialLowerBound , axialUpperBound , tspan ) : <EOL> linkOffsetFrame = self . ikPlanner . getPalmToHandLink ( self . graspingHand ) <EOL> constraint = ik . PositionConstraint ( ) <EOL> constraint . linkName = self . ikPlanner . getHandLink ( self . graspingHand ) <EOL> constraint . pointInLink = np . array ( linkOffsetFrame . GetPosition ( ) ) <EOL> constraint . referenceFrame = self . computeGraspFrame ( ) . transform <EOL> constraint . lowerBound = np . array ( [ - radialTol , axialLowerBound , - radialTol ] ) <EOL> constraint . upperBound = np . array ( [ radialTol , axialUpperBound , radialTol ] ) <EOL> constraint . tspan = tspan <EOL> return constraint <EOL> def createAllHandPositionConstraints ( self , radialTol , retract ) : <EOL> constraints = [ ] <EOL> constraints . append ( self . createHandPositionConstraint ( radialTol , self . reachDepth , <EOL> self . touchDepth , [ <NUM_LIT:0.0> , <NUM_LIT:1.0> ] ) ) <EOL> if retract : <EOL> initialDepth = self . touchDepth <EOL> finalDepth = self . reachDepth <EOL> else : <EOL> initialDepth = self . reachDepth <EOL> finalDepth = self . touchDepth <EOL> constraints . append ( self . createHandPositionConstraint ( np . inf , initialDepth , initialDepth , <EOL> [ <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) ) <EOL> constraints . append ( self . createHandPositionConstraint ( np . inf , finalDepth , finalDepth , <EOL> [ <NUM_LIT:1.0> , <NUM_LIT:1.0> ] ) ) <EOL> return constraints <EOL> def setReachAndTouchPoses ( self , plan = None ) : <EOL> if plan is None : <EOL> self . reachPoseName = None <EOL> self . touchPoseName = None <EOL> self . reachPose = None <EOL> self . touchPose = None <EOL> else : <EOL> self . reachPoseName = '<STR_LIT>' <EOL> self . touchPoseName = '<STR_LIT>' <EOL> self . reachPose = robotstate . convertStateMessageToDrakePose ( plan . plan [ <NUM_LIT:0> ] ) <EOL> self . touchPose = robotstate . convertStateMessageToDrakePose ( plan . plan [ - <NUM_LIT:1> ] ) <EOL> self . ikPlanner . addPose ( self . reachPose , self . reachPoseName ) <EOL> self . ikPlanner . addPose ( self . touchPose , self . touchPoseName ) <EOL> def planInsertTraj ( self , speed , lockFeet = True , lockBase = None , resetBase = False , wristAngleCW = <NUM_LIT:0> , <EOL> startPose = None , verticalOffset = <NUM_LIT> , usePoses = False , resetPoses = True , <EOL> planFromCurrentRobotState = False , retract = False ) : <EOL> ikParameters = IkParameters ( usePointwise = False , maxDegreesPerSecond = speed , <EOL> numberOfAddedKnots = <NUM_LIT:1> , <EOL> quasiStaticShrinkFactor = self . quasiStaticShrinkFactor , <EOL> fixInitialState = planFromCurrentRobotState ) <EOL> _ , yaxis , _ = transformUtils . getAxesFromTransform ( self . computeGraspFrame ( ) . transform ) <EOL> yawDesired = np . arctan2 ( yaxis [ <NUM_LIT:1> ] , yaxis [ <NUM_LIT:0> ] ) <EOL> if startPose is None : <EOL> startPose = self . getPlanningStartPose ( ) <EOL> nominalPose = self . getNominalPose ( ) <EOL> self . ikPlanner . addPose ( nominalPose , self . nominalPoseName ) <EOL> self . ikPlanner . addPose ( startPose , self . startPoseName ) <EOL> self . ikPlanner . reachingSide = self . graspingHand <EOL> constraints = [ ] <EOL> constraints . extend ( self . createBaseConstraints ( resetBase , lockBase , lockFeet , yawDesired ) ) <EOL> constraints . append ( self . createBackPostureConstraint ( ) ) <EOL> constraints . append ( self . ikPlanner . createQuasiStaticConstraint ( ) ) <EOL> constraints . extend ( self . createFootConstraints ( lockFeet ) ) <EOL> constraints . append ( self . ikPlanner . createLockedArmPostureConstraint ( self . startPoseName ) ) <EOL> constraints . append ( self . ikPlanner . createKneePostureConstraint ( [ <NUM_LIT> , <NUM_LIT> ] ) ) <EOL> constraints . append ( self . createElbowPostureConstraint ( ) ) <EOL> constraints . append ( self . createStaticTorqueConstraint ( ) ) <EOL> constraints . append ( self . createHandGazeConstraint ( ) ) <EOL> constraints . append ( self . createHandFixedOrientConstraint ( ) ) <EOL> constraints . append ( self . createWristAngleConstraint ( wristAngleCW , <EOL> planFromCurrentRobotState ) ) <EOL> constraints . extend ( self . createAllHandPositionConstraints ( self . coaxialTol , retract ) ) <EOL> if retract : <EOL> startPoseName = self . getStartPoseName ( planFromCurrentRobotState , True , usePoses ) <EOL> endPoseName = self . getEndPoseName ( True , usePoses ) <EOL> endPose = self . ikPlanner . jointController . getPose ( endPoseName ) <EOL> endPose = self . ikPlanner . mergePostures ( endPose , robotstate . matchJoints ( '<STR_LIT>' ) , startPose ) <EOL> endPoseName = '<STR_LIT>' <EOL> self . ikPlanner . addPose ( endPose , endPoseName ) <EOL> else : <EOL> startPoseName = self . getStartPoseName ( planFromCurrentRobotState , retract , usePoses ) <EOL> endPoseName = self . getEndPoseName ( retract , usePoses ) <EOL> plan = self . ikPlanner . runIkTraj ( constraints , startPoseName , endPoseName , self . nominalPoseName , ikParameters = ikParameters ) <EOL> if resetPoses and not retract and max ( plan . plan_info ) <= <NUM_LIT:10> and min ( plan . plan_info ) >= <NUM_LIT:0> : <EOL> self . setReachAndTouchPoses ( plan ) <EOL> return plan <EOL> def planReach ( self , verticalOffset = None , ** kwargs ) : <EOL> startPose = self . getPlanningStartPose ( ) <EOL> insert_plan = self . planInsertTraj ( self . speedHigh , lockFeet = True , usePoses = True , <EOL> resetPoses = True , ** kwargs ) <EOL> info = max ( insert_plan . plan_info ) <EOL> reachPose = robotstate . convertStateMessageToDrakePose ( insert_plan . plan [ <NUM_LIT:0> ] ) <EOL> ikParameters = IkParameters ( maxDegreesPerSecond = <NUM_LIT:2> * self . speedTurn , <EOL> rescaleBodyNames = [ self . ikPlanner . getHandLink ( side = self . graspingHand ) ] , <EOL> rescaleBodyPts = list ( self . ikPlanner . getPalmPoint ( side = self . graspingHand ) ) , <EOL> maxBodyTranslationSpeed = self . maxHandTranslationSpeed ) <EOL> plan = self . ikPlanner . computePostureGoal ( startPose , reachPose , ikParameters = ikParameters ) <EOL> plan . plan_info = [ info ] * len ( plan . plan_info ) <EOL> lcmUtils . publish ( '<STR_LIT>' , plan ) <EOL> self . addPlan ( plan ) <EOL> def planTouch ( self , ** kwargs ) : <EOL> plan = self . planInsertTraj ( self . speedLow , lockBase = True , lockFeet = True , usePoses = True , <EOL> resetPoses = False , planFromCurrentRobotState = True , ** kwargs ) <EOL> self . addPlan ( plan ) <EOL> def planTurn ( self , wristAngleCW = np . radians ( <NUM_LIT> ) ) : <EOL> ikParameters = IkParameters ( maxDegreesPerSecond = self . speedTurn ) <EOL> startPose = self . getPlanningStartPose ( ) <EOL> wristAngleCW = min ( np . radians ( <NUM_LIT> ) - <NUM_LIT> , max ( - np . radians ( <NUM_LIT> ) + <NUM_LIT> , wristAngleCW ) ) <EOL> if self . graspingHand == '<STR_LIT:left>' : <EOL> postureJoints = { '<STR_LIT>' : - np . radians ( <NUM_LIT> ) + wristAngleCW } <EOL> else : <EOL> postureJoints = { '<STR_LIT>' : np . radians ( <NUM_LIT> ) - wristAngleCW } <EOL> endPose = self . ikPlanner . mergePostures ( startPose , postureJoints ) <EOL> plan = self . ikPlanner . computePostureGoal ( startPose , endPose , ikParameters = ikParameters ) <EOL> app . displaySnoptInfo ( <NUM_LIT:1> ) <EOL> self . addPlan ( plan ) <EOL> def planRetract ( self , ** kwargs ) : <EOL> startPose = self . getPlanningStartPose ( ) <EOL> if self . graspingHand == '<STR_LIT:left>' : <EOL> jointId = robotstate . getDrakePoseJointNames ( ) . index ( '<STR_LIT>' ) <EOL> wristAngleCW = np . radians ( <NUM_LIT> ) + startPose [ jointId ] <EOL> else : <EOL> jointId = robotstate . getDrakePoseJointNames ( ) . index ( '<STR_LIT>' ) <EOL> wristAngleCW = np . radians ( <NUM_LIT> ) - startPose [ jointId ] <EOL> plan = self . planInsertTraj ( self . speedLow , retract = True , lockBase = True , lockFeet = True , <EOL> usePoses = True , planFromCurrentRobotState = True , resetPoses = False , <EOL> wristAngleCW = wristAngleCW , ** kwargs ) <EOL> self . addPlan ( plan ) <EOL> def getNominalPose ( self ) : <EOL> axes = transformUtils . getAxesFromTransform ( self . computeGraspFrame ( ) . transform ) <EOL> yaxis = axes [ <NUM_LIT:1> ] <EOL> yawDesired = np . arctan2 ( yaxis [ <NUM_LIT:1> ] , yaxis [ <NUM_LIT:0> ] ) <EOL> seedDistance = <NUM_LIT:1> <EOL> nominalPose = self . ikPlanner . jointController . getPose ( '<STR_LIT>' ) <EOL> nominalPose [ <NUM_LIT:0> ] = ( self . computeGraspFrame ( ) . transform . GetPosition ( ) [ <NUM_LIT:0> ] - <EOL> seedDistance * yaxis [ <NUM_LIT:0> ] ) <EOL> nominalPose [ <NUM_LIT:1> ] = ( self . computeGraspFrame ( ) . transform . GetPosition ( ) [ <NUM_LIT:1> ] - <EOL> seedDistance * yaxis [ <NUM_LIT:1> ] ) <EOL> nominalPose [ <NUM_LIT:5> ] = yawDesired <EOL> if self . scribeDirection == <NUM_LIT:1> : <EOL> nominalPose = self . ikPlanner . getMergedPostureFromDatabase ( nominalPose , '<STR_LIT>' , '<STR_LIT>' , side = self . graspingHand ) <EOL> else : <EOL> nominalPose = self . ikPlanner . getMergedPostureFromDatabase ( nominalPose , '<STR_LIT>' , '<STR_LIT>' , side = self . graspingHand ) <EOL> return nominalPose <EOL> def computeHumanTouchFrame ( self , touchValve = True , reachAngle = - <NUM_LIT> ) : <EOL> if touchValve : <EOL> faceDepth = <NUM_LIT:0.0> <EOL> else : <EOL> faceDepth = self . reachDepth <EOL> tDepth = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0> , faceDepth , <NUM_LIT:0> ] , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> scribeRadius = om . findObjectByName ( '<STR_LIT>' ) . params [ '<STR_LIT>' ] <EOL> position = [ <NUM_LIT:0> , scribeRadius * math . sin ( math . radians ( reachAngle ) ) , scribeRadius * math . cos ( math . radians ( reachAngle ) ) ] <EOL> if ( self . graspingObject is '<STR_LIT>' ) : <EOL> rpy = [ ( - self . palmInAngle - <NUM_LIT> ) , reachAngle , ( <NUM_LIT> ) ] <EOL> else : <EOL> if ( self . graspingHand is '<STR_LIT:left>' ) : <EOL> rpy = [ - <NUM_LIT> , ( <NUM_LIT> + reachAngle ) , <NUM_LIT> ] <EOL> else : <EOL> rpy = [ - <NUM_LIT> , reachAngle , <NUM_LIT> ] <EOL> t = om . findObjectByName ( '<STR_LIT>' ) . getChildFrame ( ) . transform <EOL> t2 = transformUtils . frameFromPositionAndRPY ( position , rpy ) <EOL> t3 = transformUtils . copyFrame ( t ) <EOL> t3 . PreMultiply ( ) <EOL> t3 . Concatenate ( t2 ) <EOL> t3 . Concatenate ( tDepth ) <EOL> faceFrameDesired = vis . showFrame ( t3 , '<STR_LIT>' , parent = om . findObjectByName ( '<STR_LIT>' ) , visible = False , scale = <NUM_LIT> ) <EOL> return faceFrameDesired <EOL> def planHumanReach ( self ) : <EOL> faceFrameDesired = self . computeHumanTouchFrame ( False , reachAngle = self . reachAngle ) <EOL> self . computeHumanTouchPlan ( faceFrameDesired ) <EOL> def planHumanTouch ( self ) : <EOL> faceFrameDesired = self . computeHumanTouchFrame ( True , reachAngle = self . reachAngle ) <EOL> self . computeHumanTouchPlan ( faceFrameDesired ) <EOL> def planHumanRetract ( self ) : <EOL> faceFrameDesired = self . computeHumanTouchFrame ( False , reachAngle = ( self . reachAngle + self . turnAngle ) ) <EOL> self . computeHumanTouchPlan ( faceFrameDesired ) <EOL> def computeHumanTouchPlan ( self , faceFrameDesired ) : <EOL> startPose = self . getPlanningStartPose ( ) <EOL> self . constraintSet = self . ikPlanner . planEndEffectorGoal ( startPose , self . graspingHand , faceFrameDesired , lockBase = self . lockBase , lockBack = self . lockBack ) <EOL> endPose , info = self . constraintSet . runIk ( ) <EOL> self . ikPlanner . ikServer . maxDegreesPerSecond = self . speedLow <EOL> self . planTrajectory ( ) <EOL> self . ikPlanner . ikServer . maxDegreesPerSecond = self . speedHigh <EOL> def planTrajectory ( self ) : <EOL> self . ikPlanner . ikServer . usePointwise = False <EOL> plan = self . constraintSet . runIkTraj ( ) <EOL> self . addPlan ( plan ) <EOL> def resetTurnPath ( self ) : <EOL> for obj in om . getObjects ( ) : <EOL> if obj . getProperty ( '<STR_LIT:Name>' ) == '<STR_LIT>' : <EOL> om . removeFromObjectModel ( obj ) <EOL> for obj in om . getObjects ( ) : <EOL> if obj . getProperty ( '<STR_LIT:Name>' ) == '<STR_LIT>' : <EOL> om . removeFromObjectModel ( obj ) <EOL> def initConstraintSet ( self , goalFrame ) : <EOL> startPose = self . getPlanningStartPose ( ) <EOL> startPoseName = '<STR_LIT>' <EOL> endPoseName = '<STR_LIT>' <EOL> self . ikPlanner . addPose ( startPose , startPoseName ) <EOL> self . ikPlanner . addPose ( startPose , endPoseName ) <EOL> self . constraintSet = ikplanner . ConstraintSet ( self . ikPlanner , [ ] , startPoseName , endPoseName ) <EOL> self . constraintSet . endPose = startPose <EOL> bodyConstraints = self . ikPlanner . createMovingBodyConstraints ( startPoseName , lockBase = self . lockBase , lockBack = self . lockBack , lockLeftArm = self . graspingHand == '<STR_LIT:right>' , lockRightArm = self . graspingHand == '<STR_LIT:left>' ) <EOL> self . constraintSet . constraints . extend ( bodyConstraints ) <EOL> self . graspToHandLinkFrame = self . ikPlanner . newGraspToHandFrame ( self . graspingHand ) <EOL> gazeConstraint = self . ikPlanner . createGazeGraspConstraint ( self . graspingHand , goalFrame , self . graspToHandLinkFrame , coneThresholdDegrees = <NUM_LIT> ) <EOL> self . constraintSet . constraints . insert ( <NUM_LIT:0> , gazeConstraint ) <EOL> def appendPositionOrientationConstraintForTargetFrame ( self , goalFrame , t ) : <EOL> positionConstraint , orientationConstraint = self . ikPlanner . createPositionOrientationGraspConstraints ( self . graspingHand , goalFrame , self . graspToHandLinkFrame ) <EOL> positionConstraint . tspan = [ t , t ] <EOL> orientationConstraint . tspan = [ t , t ] <EOL> self . constraintSet . constraints . append ( positionConstraint ) <EOL> self . constraintSet . constraints . append ( orientationConstraint ) <EOL> def planHumanValveTurn ( self ) : <EOL> initialReachAngle = self . reachAngle <EOL> numberOfSamples = abs ( int ( round ( self . turnAngle / <NUM_LIT> ) ) ) <EOL> degreeStep = float ( self . turnAngle ) / numberOfSamples <EOL> facePath = [ ] <EOL> self . resetTurnPath ( ) <EOL> reachAngle = initialReachAngle <EOL> faceFrameDesired = self . computeHumanTouchFrame ( touchValve = True , reachAngle = reachAngle ) <EOL> self . initConstraintSet ( faceFrameDesired ) <EOL> facePath . append ( faceFrameDesired ) <EOL> for i in xrange ( numberOfSamples ) : <EOL> reachAngle += degreeStep <EOL> faceFrameDesired = self . computeHumanTouchFrame ( touchValve = True , reachAngle = reachAngle ) <EOL> self . appendPositionOrientationConstraintForTargetFrame ( faceFrameDesired , i + <NUM_LIT:1> ) <EOL> facePath . append ( faceFrameDesired ) <EOL> self . facePath = facePath <EOL> self . drawFacePath ( facePath ) <EOL> self . planTrajectory ( ) <EOL> def drawFacePath ( self , facePath ) : <EOL> valveTransform = om . findObjectByName ( '<STR_LIT>' ) . getChildFrame ( ) . transform <EOL> path = DebugData ( ) <EOL> for i in range ( <NUM_LIT:1> , len ( facePath ) ) : <EOL> end0 = transformUtils . copyFrame ( facePath [ i - <NUM_LIT:1> ] . transform ) <EOL> end0 . Concatenate ( valveTransform . GetLinearInverse ( ) ) <EOL> end1 = transformUtils . copyFrame ( facePath [ i ] . transform ) <EOL> end1 . Concatenate ( valveTransform . GetLinearInverse ( ) ) <EOL> path . addLine ( np . array ( end0 . GetPosition ( ) ) , np . array ( end1 . GetPosition ( ) ) , radius = <NUM_LIT> ) <EOL> pathMesh = path . getPolyData ( ) <EOL> pointerTipLinePath = vis . showPolyData ( pathMesh , '<STR_LIT>' , color = [ <NUM_LIT:0.0> , <NUM_LIT> , <NUM_LIT:1.0> ] , parent = om . findObjectByName ( '<STR_LIT>' ) , alpha = <NUM_LIT:1.0> ) <EOL> pointerTipLinePath . actor . SetUserTransform ( om . findObjectByName ( '<STR_LIT>' ) . getChildFrame ( ) . transform ) <EOL> def moveRobotToGraspStanceFrame ( self ) : <EOL> self . teleportRobotToStanceFrame ( om . findObjectByName ( '<STR_LIT>' ) . transform ) <EOL> def teleportRobotToStanceFrame ( self , frame ) : <EOL> self . sensorJointController . setPose ( '<STR_LIT>' ) <EOL> stancePosition = frame . GetPosition ( ) <EOL> stanceOrientation = frame . GetOrientation ( ) <EOL> q = self . sensorJointController . q . copy ( ) <EOL> q [ : <NUM_LIT:2> ] = [ stancePosition [ <NUM_LIT:0> ] , stancePosition [ <NUM_LIT:1> ] ] <EOL> q [ <NUM_LIT:5> ] = math . radians ( stanceOrientation [ <NUM_LIT:2> ] ) <EOL> self . sensorJointController . setPose ( '<STR_LIT>' , q ) <EOL> def getHandDriver ( self , side ) : <EOL> assert side in ( '<STR_LIT:left>' , '<STR_LIT:right>' ) <EOL> return self . lhandDriver if side == '<STR_LIT:left>' else self . rhandDriver <EOL> def openHand ( self , side ) : <EOL> self . getHandDriver ( side ) . sendCustom ( <NUM_LIT:0.0> , <NUM_LIT> , <NUM_LIT> , <NUM_LIT:0> ) <EOL> def openPinch ( self , side ) : <EOL> self . getHandDriver ( side ) . sendCustom ( <NUM_LIT> , <NUM_LIT> , <NUM_LIT> , <NUM_LIT:1> ) <EOL> def closeHand ( self , side ) : <EOL> self . getHandDriver ( side ) . sendCustom ( <NUM_LIT> , <NUM_LIT> , <NUM_LIT> , <NUM_LIT:0> ) <EOL> def sendNeckPitchLookDown ( self ) : <EOL> self . multisenseDriver . setNeckPitch ( <NUM_LIT> ) <EOL> def sendNeckPitchLookForward ( self ) : <EOL> self . multisenseDriver . setNeckPitch ( <NUM_LIT:15> ) <EOL> def waitForAtlasBehaviorAsync ( self , behaviorName ) : <EOL> assert behaviorName in self . atlasDriver . getBehaviorMap ( ) . values ( ) <EOL> while self . atlasDriver . getCurrentBehaviorName ( ) != behaviorName : <EOL> yield <EOL> def printAsync ( self , s ) : <EOL> yield <EOL> print s <EOL> def optionalUserPrompt ( self , message ) : <EOL> if not self . optionalUserPromptEnabled : <EOL> return <EOL> yield <EOL> result = raw_input ( message ) <EOL> if result != '<STR_LIT:y>' : <EOL> raise Exception ( '<STR_LIT>' ) <EOL> def requiredUserPrompt ( self , message ) : <EOL> if not self . requiredUserPromptEnabled : <EOL> return <EOL> yield <EOL> result = raw_input ( message ) <EOL> if result != '<STR_LIT:y>' : <EOL> raise Exception ( '<STR_LIT>' ) <EOL> def delay ( self , delayTimeInSeconds ) : <EOL> yield <EOL> t = SimpleTimer ( ) <EOL> while t . elapsed ( ) < delayTimeInSeconds : <EOL> yield <EOL> def waitForCleanLidarSweepAsync ( self ) : <EOL> currentRevolution = self . multisenseDriver . displayedRevolution <EOL> desiredRevolution = currentRevolution + <NUM_LIT:2> <EOL> while self . multisenseDriver . displayedRevolution < desiredRevolution : <EOL> yield <EOL> def getEstimatedRobotStatePose ( self ) : <EOL> return self . sensorJointController . getPose ( '<STR_LIT>' ) <EOL> def getPlanningStartPose ( self ) : <EOL> if self . planFromCurrentRobotState : <EOL> return self . sensorJointController . getPose ( '<STR_LIT>' ) <EOL> else : <EOL> if self . plans : <EOL> return robotstate . convertStateMessageToDrakePose ( <EOL> self . plans [ - <NUM_LIT:1> ] . plan [ - <NUM_LIT:1> ] ) <EOL> else : <EOL> return self . getEstimatedRobotStatePose ( ) <EOL> def commitManipPlan ( self ) : <EOL> self . manipPlanner . commitManipPlan ( self . plans [ - <NUM_LIT:1> ] ) <EOL> def prepFromFile ( self , moveRobot = True ) : <EOL> filename = os . path . expanduser ( '<STR_LIT>' ) <EOL> polyData = ioUtils . readPolyData ( filename ) <EOL> vis . showPolyData ( polyData , '<STR_LIT>' ) <EOL> segmentation . segmentValveWallAuto ( <NUM_LIT> , mode = '<STR_LIT>' , removeGroundMethod = segmentation . removeGround ) <EOL> self . computeStanceFrame ( ) <EOL> if ( moveRobot ) : <EOL> self . moveRobotToGraspStanceFrame ( ) <EOL> class ValveImageFitter ( ImageBasedAffordanceFit ) : <EOL> def __init__ ( self , valveDemo ) : <EOL> ImageBasedAffordanceFit . __init__ ( self , numberOfPoints = <NUM_LIT:2> ) <EOL> self . valveDemo = valveDemo <EOL> def onImageViewDoubleClick ( self , displayPoint , modifiers , imageView ) : <EOL> self . valveDemo . onImageViewDoubleClick ( displayPoint , modifiers , imageView ) <EOL> def fit ( self , polyData , points ) : <EOL> om . removeFromObjectModel ( om . findObjectByName ( '<STR_LIT>' ) ) <EOL> segmentation . segmentValveByRim ( polyData , points [ <NUM_LIT:0> ] , points [ <NUM_LIT:1> ] ) <EOL> class ValveTaskPanel ( TaskUserPanel ) : <EOL> def __init__ ( self , valveDemo ) : <EOL> TaskUserPanel . __init__ ( self , windowTitle = '<STR_LIT>' ) <EOL> self . valveDemo = valveDemo <EOL> self . fitter = ValveImageFitter ( self . valveDemo ) <EOL> self . initImageView ( self . fitter . imageView ) <EOL> self . addDefaultProperties ( ) <EOL> self . addButtons ( ) <EOL> self . addTasks ( ) <EOL> def addButtons ( self ) : <EOL> self . addManualButton ( '<STR_LIT>' , self . onSpawnValveClicked ) <EOL> self . addManualSpacer ( ) <EOL> self . addManualButton ( '<STR_LIT>' , self . valveDemo . planFootstepsToStance ) <EOL> self . addManualButton ( '<STR_LIT>' , <EOL> functools . partial ( self . valveDemo . planFootstepsToStance , <EOL> useIkTraj = True ) ) <EOL> self . addManualSpacer ( ) <EOL> self . addManualButton ( '<STR_LIT>' , self . valveDemo . planPreGrasp ) <EOL> self . addManualButton ( '<STR_LIT>' , self . setFingers ) <EOL> self . addManualSpacer ( ) <EOL> self . addManualButton ( '<STR_LIT>' , self . reach ) <EOL> self . addManualButton ( '<STR_LIT>' , self . touch ) <EOL> self . addManualButton ( '<STR_LIT>' , self . turnValve ) <EOL> self . addManualButton ( '<STR_LIT>' , self . retract ) <EOL> self . addManualSpacer ( ) <EOL> self . addManualButton ( '<STR_LIT>' , self . valveDemo . planNominal ) <EOL> self . addManualSpacer ( ) <EOL> self . addManualButton ( '<STR_LIT>' , self . valveDemo . commitManipPlan ) <EOL> def onSpawnValveClicked ( self ) : <EOL> self . valveDemo . spawnValveAffordance ( ) <EOL> def setFingers ( self ) : <EOL> driver = self . valveDemo . getHandDriver ( self . valveDemo . graspingHand ) <EOL> driver . sendClose ( self . valveDemo . openAmount ) <EOL> def reach ( self ) : <EOL> self . valveDemo . setReachAndTouchPoses ( ) <EOL> self . valveDemo . planReach ( wristAngleCW = self . initialWristAngleCW ) <EOL> def touch ( self ) : <EOL> self . valveDemo . planTouch ( wristAngleCW = self . initialWristAngleCW ) <EOL> def turnValve ( self ) : <EOL> self . valveDemo . planTurn ( wristAngleCW = self . finalWristAngleCW ) <EOL> def retract ( self ) : <EOL> self . valveDemo . planRetract ( ) <EOL> def addDefaultProperties ( self ) : <EOL> self . params . addProperty ( '<STR_LIT>' , [ '<STR_LIT:left>' , '<STR_LIT:right>' ] . index ( self . valveDemo . graspingHand ) , attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT>' , <EOL> '<STR_LIT>' ] ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , <NUM_LIT:1> , <EOL> attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT>' , <EOL> '<STR_LIT>' ] ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , <NUM_LIT:0> , <EOL> attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT>' , '<STR_LIT>' ] ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , <NUM_LIT:0> , <EOL> attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT>' , '<STR_LIT>' ] ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , <NUM_LIT:1> , <EOL> attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT>' , '<STR_LIT>' ] ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , self . valveDemo . turningMode , <EOL> attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT>' , '<STR_LIT>' ] ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , - <NUM_LIT> , <EOL> attributes = om . PropertyAttributes ( singleStep = <NUM_LIT:5> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , - <NUM_LIT> , <EOL> attributes = om . PropertyAttributes ( singleStep = <NUM_LIT:5> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , <NUM_LIT:30> , <EOL> attributes = om . PropertyAttributes ( singleStep = <NUM_LIT:5> ) ) <EOL> self . _syncProperties ( ) <EOL> def onPropertyChanged ( self , propertySet , propertyName ) : <EOL> self . _syncProperties ( ) <EOL> self . taskTree . removeAllTasks ( ) <EOL> self . addTasks ( ) <EOL> def _syncProperties ( self ) : <EOL> self . valveDemo . planFromCurrentRobotState = True <EOL> self . valveDemo . turningMode = self . params . getProperty ( '<STR_LIT>' ) <EOL> self . valveDemo . setGraspingHand ( self . params . getPropertyEnumValue ( '<STR_LIT>' ) . lower ( ) ) <EOL> if self . params . getPropertyEnumValue ( '<STR_LIT>' ) == '<STR_LIT>' : <EOL> self . valveDemo . scribeDirection = <NUM_LIT:1> <EOL> self . initialWristAngleCW = <NUM_LIT:0> <EOL> self . finalWristAngleCW = np . radians ( <NUM_LIT> ) <EOL> else : <EOL> self . valveDemo . scribeDirection = - <NUM_LIT:1> <EOL> self . initialWristAngleCW = np . radians ( <NUM_LIT> ) <EOL> self . finalWristAngleCW = <NUM_LIT:0> <EOL> if self . params . getPropertyEnumValue ( '<STR_LIT>' ) == '<STR_LIT>' : <EOL> self . valveDemo . useLargeValveDefaults ( ) <EOL> else : <EOL> self . valveDemo . useSmallValveDefaults ( ) <EOL> if self . params . getPropertyEnumValue ( '<STR_LIT>' ) == '<STR_LIT>' : <EOL> self . valveDemo . lockBase = True <EOL> else : <EOL> self . valveDemo . lockBase = False <EOL> if self . params . getPropertyEnumValue ( '<STR_LIT>' ) == '<STR_LIT>' : <EOL> self . valveDemo . lockBack = True <EOL> else : <EOL> self . valveDemo . lockBack = False <EOL> self . valveDemo . reachAngle = self . params . getProperty ( '<STR_LIT>' ) <EOL> self . valveDemo . turnAngle = self . params . getProperty ( '<STR_LIT>' ) <EOL> self . valveDemo . palmInAngle = self . params . getProperty ( '<STR_LIT>' ) <EOL> def addTasks ( self ) : <EOL> self . taskTree . removeAllTasks ( ) <EOL> if self . valveDemo . turningMode == <NUM_LIT:0> : <EOL> self . addSimpleTurnTasks ( ) <EOL> elif self . valveDemo . turningMode == <NUM_LIT:1> : <EOL> self . addHumanTurnTasks ( ) <EOL> else : <EOL> return <EOL> def addSimpleTurnTasks ( self ) : <EOL> def addTask ( task , parent = None ) : <EOL> self . taskTree . onAddTask ( task , copy = False , parent = parent ) <EOL> def addFunc ( func , name , parent = None ) : <EOL> addTask ( rt . CallbackTask ( callback = func , name = name ) , parent = parent ) <EOL> def addManipulation ( func , name , parent = None ) : <EOL> group = self . taskTree . addGroup ( name , parent = parent ) <EOL> addFunc ( func , name = '<STR_LIT>' , parent = group ) <EOL> addTask ( rt . CheckPlanInfo ( name = '<STR_LIT>' ) , parent = group ) <EOL> addFunc ( v . commitManipPlan , name = '<STR_LIT>' , parent = group ) <EOL> addTask ( rt . WaitForManipulationPlanExecution ( name = '<STR_LIT>' ) , <EOL> parent = group ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , message = '<STR_LIT>' ) , <EOL> parent = group ) <EOL> def addLargeValveTurn ( parent = None ) : <EOL> group = self . taskTree . addGroup ( '<STR_LIT>' , parent = parent ) <EOL> addManipulation ( functools . partial ( v . planReach , wristAngleCW = self . initialWristAngleCW ) , <EOL> name = '<STR_LIT>' , parent = group ) <EOL> addManipulation ( functools . partial ( v . planTouch , wristAngleCW = self . initialWristAngleCW ) , <EOL> name = '<STR_LIT>' , parent = group ) <EOL> addManipulation ( functools . partial ( v . planTurn , wristAngleCW = self . finalWristAngleCW ) , <EOL> name = '<STR_LIT>' , parent = group ) <EOL> addManipulation ( v . planRetract , name = '<STR_LIT>' , parent = group ) <EOL> def addSmallValveTurn ( parent = None ) : <EOL> group = self . taskTree . addGroup ( '<STR_LIT>' , parent = parent ) <EOL> side = '<STR_LIT>' if v . graspingHand == '<STR_LIT:right>' else '<STR_LIT>' <EOL> addManipulation ( functools . partial ( v . planReach , wristAngleCW = self . initialWristAngleCW ) , <EOL> name = '<STR_LIT>' , parent = group ) <EOL> addManipulation ( functools . partial ( v . planTouch , wristAngleCW = self . initialWristAngleCW ) , <EOL> name = '<STR_LIT>' , parent = group ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = side , mode = '<STR_LIT>' , <EOL> amount = self . valveDemo . closedAmount ) , <EOL> parent = group ) <EOL> addManipulation ( functools . partial ( v . planTurn , wristAngleCW = self . finalWristAngleCW ) , <EOL> name = '<STR_LIT>' , parent = group ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = side , mode = '<STR_LIT>' , <EOL> amount = self . valveDemo . openAmount ) , <EOL> parent = group ) <EOL> addManipulation ( v . planRetract , name = '<STR_LIT>' , parent = group ) <EOL> v = self . valveDemo <EOL> side = self . params . getPropertyEnumValue ( '<STR_LIT>' ) <EOL> prep = self . taskTree . addGroup ( '<STR_LIT>' ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = '<STR_LIT>' ) , parent = prep ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = '<STR_LIT>' ) , parent = prep ) <EOL> fit = self . taskTree . addGroup ( '<STR_LIT>' ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , <EOL> message = '<STR_LIT>' ) , parent = fit ) <EOL> addTask ( rt . FindAffordance ( name = '<STR_LIT>' , affordanceName = '<STR_LIT>' ) , <EOL> parent = fit ) <EOL> walk = self . taskTree . addGroup ( '<STR_LIT>' ) <EOL> addFunc ( v . planFootstepsToStance , '<STR_LIT>' , parent = walk ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , <EOL> message = '<STR_LIT>' ) , parent = walk ) <EOL> addTask ( rt . CommitFootstepPlan ( name = '<STR_LIT>' , <EOL> planName = '<STR_LIT>' ) , parent = walk ) <EOL> addTask ( rt . SetNeckPitch ( name = '<STR_LIT>' , angle = <NUM_LIT> ) , parent = walk ) <EOL> addTask ( rt . WaitForWalkExecution ( name = '<STR_LIT>' ) , parent = walk ) <EOL> refit = self . taskTree . addGroup ( '<STR_LIT>' ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , <EOL> message = '<STR_LIT>' ) , <EOL> parent = refit ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = side , mode = '<STR_LIT>' , <EOL> amount = self . valveDemo . openAmount ) , parent = refit ) <EOL> if v . smallValve : <EOL> for i in range ( <NUM_LIT:0> , <NUM_LIT:2> ) : <EOL> addSmallValveTurn ( ) <EOL> else : <EOL> for i in range ( <NUM_LIT:0> , <NUM_LIT:2> ) : <EOL> addLargeValveTurn ( ) <EOL> prep = self . taskTree . addGroup ( '<STR_LIT>' ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = '<STR_LIT>' ) , parent = prep ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = '<STR_LIT>' ) , parent = prep ) <EOL> addTask ( rt . PlanPostureGoal ( name = '<STR_LIT>' , postureGroup = '<STR_LIT>' , <EOL> postureName = '<STR_LIT>' , side = '<STR_LIT>' ) , parent = prep ) <EOL> addTask ( rt . CommitManipulationPlan ( name = '<STR_LIT>' , <EOL> planName = '<STR_LIT>' ) , parent = prep ) <EOL> addTask ( rt . WaitForManipulationPlanExecution ( name = '<STR_LIT>' ) , parent = prep ) <EOL> def addHumanTurnTasks ( self ) : <EOL> def addTask ( task , parent = None ) : <EOL> self . taskTree . onAddTask ( task , copy = False , parent = parent ) <EOL> def addFunc ( func , name , parent = None ) : <EOL> addTask ( rt . CallbackTask ( callback = func , name = name ) , parent = parent ) <EOL> def addManipulation ( func , name , parent = None ) : <EOL> group = self . taskTree . addGroup ( name , parent = parent ) <EOL> addFunc ( func , name = '<STR_LIT>' , parent = group ) <EOL> addTask ( rt . CheckPlanInfo ( name = '<STR_LIT>' ) , parent = group ) <EOL> addFunc ( v . commitManipPlan , name = '<STR_LIT>' , parent = group ) <EOL> addTask ( rt . WaitForManipulationPlanExecution ( name = '<STR_LIT>' ) , <EOL> parent = group ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , message = '<STR_LIT>' ) , <EOL> parent = group ) <EOL> v = self . valveDemo <EOL> side = self . params . getPropertyEnumValue ( '<STR_LIT>' ) <EOL> prep = self . taskTree . addGroup ( '<STR_LIT>' ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = '<STR_LIT>' ) , parent = prep ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = '<STR_LIT>' ) , parent = prep ) <EOL> addFunc ( v . prepFromFile , '<STR_LIT>' , parent = prep ) <EOL> fit = self . taskTree . addGroup ( '<STR_LIT>' ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , <EOL> message = '<STR_LIT>' ) , parent = fit ) <EOL> addTask ( rt . FindAffordance ( name = '<STR_LIT>' , affordanceName = '<STR_LIT>' ) , <EOL> parent = fit ) <EOL> walk = self . taskTree . addGroup ( '<STR_LIT>' ) <EOL> addFunc ( v . planFootstepsToStance , '<STR_LIT>' , parent = walk ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , <EOL> message = '<STR_LIT>' ) , parent = walk ) <EOL> addTask ( rt . CommitFootstepPlan ( name = '<STR_LIT>' , <EOL> planName = '<STR_LIT>' ) , parent = walk ) <EOL> addTask ( rt . SetNeckPitch ( name = '<STR_LIT>' , angle = <NUM_LIT> ) , parent = walk ) <EOL> addTask ( rt . WaitForWalkExecution ( name = '<STR_LIT>' ) , parent = walk ) <EOL> refit = self . taskTree . addGroup ( '<STR_LIT>' ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , <EOL> message = '<STR_LIT>' ) , <EOL> parent = refit ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = side , mode = '<STR_LIT>' , <EOL> amount = self . valveDemo . openAmount ) , parent = refit ) <EOL> turning = self . taskTree . addGroup ( '<STR_LIT>' ) <EOL> preturn = self . taskTree . addGroup ( '<STR_LIT>' , parent = turning ) <EOL> addTask ( rt . PlanPostureGoal ( name = '<STR_LIT>' , postureGroup = '<STR_LIT>' , <EOL> postureName = '<STR_LIT>' , side = side ) , parent = preturn ) <EOL> addTask ( rt . CommitManipulationPlan ( name = '<STR_LIT>' , <EOL> planName = '<STR_LIT>' ) , parent = preturn ) <EOL> addTask ( rt . WaitForManipulationPlanExecution ( name = '<STR_LIT>' ) , parent = preturn ) <EOL> addManipulation ( functools . partial ( v . planHumanReach ) , <EOL> name = '<STR_LIT>' , parent = turning ) <EOL> addManipulation ( functools . partial ( v . planHumanTouch ) , <EOL> name = '<STR_LIT>' , parent = turning ) <EOL> addManipulation ( functools . partial ( v . planHumanValveTurn ) , <EOL> name = '<STR_LIT>' , parent = turning ) <EOL> addManipulation ( functools . partial ( v . planHumanRetract ) , <EOL> name = '<STR_LIT>' , parent = turning ) <EOL> prep = self . taskTree . addGroup ( '<STR_LIT>' ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = '<STR_LIT>' ) , parent = prep ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = '<STR_LIT>' ) , parent = prep ) <EOL> addTask ( rt . PlanPostureGoal ( name = '<STR_LIT>' , postureGroup = '<STR_LIT>' , <EOL> postureName = '<STR_LIT>' , side = '<STR_LIT>' ) , parent = prep ) <EOL> addTask ( rt . CommitManipulationPlan ( name = '<STR_LIT>' , <EOL> planName = '<STR_LIT>' ) , parent = prep ) <EOL> addTask ( rt . WaitForManipulationPlanExecution ( name = '<STR_LIT>' ) , parent = prep ) </s>
