<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 . fieldcontainer import FieldContainer <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 ik <EOL> from director . ikparameters import IkParameters <EOL> from director import ikplanner <EOL> from director import ioUtils <EOL> from director import affordanceitems <EOL> from director . simpletimer import SimpleTimer <EOL> from director . utime import getUtime <EOL> from director import robotstate <EOL> from director import robotplanlistener <EOL> from director import segmentation <EOL> from director import planplayback <EOL> from director . footstepsdriver import FootstepRequestGenerator <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> from PythonQt import QtCore , QtGui <EOL> class DoorDemo ( object ) : <EOL> def __init__ ( self , robotModel , footstepPlanner , manipPlanner , ikPlanner , lhandDriver , rhandDriver , atlasDriver , multisenseDriver , affordanceFitFunction , sensorJointController , planPlaybackFunction , showPoseFunction ) : <EOL> self . robotModel = robotModel <EOL> self . footstepPlanner = footstepPlanner <EOL> self . manipPlanner = manipPlanner <EOL> self . ikPlanner = ikPlanner <EOL> self . lhandDriver = lhandDriver <EOL> self . rhandDriver = rhandDriver <EOL> self . atlasDriver = atlasDriver <EOL> self . multisenseDriver = multisenseDriver <EOL> self . affordanceFitFunction = affordanceFitFunction <EOL> self . sensorJointController = sensorJointController <EOL> self . planPlaybackFunction = planPlaybackFunction <EOL> self . showPoseFunction = showPoseFunction <EOL> self . graspingHand = '<STR_LIT:left>' <EOL> self . endPose = None <EOL> self . planFromCurrentRobotState = True <EOL> self . visOnly = False <EOL> self . useFootstepPlanner = False <EOL> self . userPromptEnabled = True <EOL> self . constraintSet = None <EOL> self . plans = [ ] <EOL> self . usePinchGrasp = False <EOL> self . pinchDistance = <NUM_LIT:0.1> <EOL> self . doorHandleFrame = None <EOL> self . doorHandleGraspFrame = None <EOL> self . doorHingeFrame = None <EOL> self . handleTouchHeight = <NUM_LIT:0.0> <EOL> self . handleTouchDepth = - <NUM_LIT> <EOL> self . handleTouchWidth = <NUM_LIT> <EOL> self . handleReachAngle = <NUM_LIT:20> <EOL> self . handleTurnHeight = - <NUM_LIT> <EOL> self . handleTurnWidth = <NUM_LIT> <EOL> self . handleTurnAngle = <NUM_LIT> <EOL> self . handleLiftHeight = <NUM_LIT> <EOL> self . handlePushDepth = <NUM_LIT:0.0> <EOL> self . handlePushAngle = <NUM_LIT:2> <EOL> self . handleOpenDepth = <NUM_LIT:0.1> <EOL> self . handleOpenWidth = <NUM_LIT> <EOL> self . speedHigh = <NUM_LIT> <EOL> self . speedLow = <NUM_LIT:15> <EOL> self . setFootstepThroughDoorParameters ( ) <EOL> self . setChopParametersToDefaults ( ) <EOL> def setChopParametersToDefaults ( self ) : <EOL> self . preChopDepth = - <NUM_LIT> <EOL> self . preChopWidth = - <NUM_LIT> <EOL> self . preChopHeight = <NUM_LIT> <EOL> self . chopDistance = - <NUM_LIT> <EOL> self . chopSidewaysDistance = <NUM_LIT> <EOL> def addPlan ( self , plan ) : <EOL> self . plans . append ( plan ) <EOL> def computeGraspOrientation ( self ) : <EOL> return [ <NUM_LIT> + self . handleReachAngle , <NUM_LIT:0> , <NUM_LIT> ] <EOL> def computeGroundFrame ( self , robotModel ) : <EOL> '''<STR_LIT>''' <EOL> t1 = robotModel . getLinkFrame ( '<STR_LIT>' ) <EOL> t2 = robotModel . getLinkFrame ( '<STR_LIT>' ) <EOL> pelvisT = robotModel . getLinkFrame ( '<STR_LIT>' ) <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 computeDoorHandleGraspFrame ( self ) : <EOL> doorSide = <NUM_LIT:1> if self . graspingHand == '<STR_LIT:left>' else - <NUM_LIT:1> <EOL> graspOrientation = self . computeGraspOrientation ( ) <EOL> self . doorHandleAxisFrame = self . computeDoorHandleAxisFrame ( ) <EOL> def makeFrame ( name , offset , turnAngle = <NUM_LIT:0> ) : <EOL> t = transformUtils . frameFromPositionAndRPY ( offset , graspOrientation ) <EOL> t . PostMultiply ( ) <EOL> t . Concatenate ( transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] , [ - turnAngle , <NUM_LIT:0> , <NUM_LIT:0> ] ) ) <EOL> t . Concatenate ( transformUtils . copyFrame ( self . doorHandleAxisFrame . transform ) ) <EOL> return vis . updateFrame ( t , name , parent = self . doorHandleAffordance , visible = False , scale = <NUM_LIT> ) <EOL> def makeFrameNew ( name , transforms ) : <EOL> t = transformUtils . concatenateTransforms ( transforms ) <EOL> return vis . updateFrame ( t , name , parent = self . doorHandleAffordance , visible = False , scale = <NUM_LIT> ) <EOL> graspToAxisTransform = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] , <EOL> graspOrientation ) <EOL> self . doorHandleGraspFrame = makeFrameNew ( '<STR_LIT>' , <EOL> [ graspToAxisTransform , <EOL> self . doorHandleAxisFrame . transform ] ) <EOL> if self . usePinchGrasp : <EOL> reachToGraspTransform = transformUtils . frameFromPositionAndRPY ( [ - doorSide * self . handleTouchWidth , <EOL> self . handleTouchDepth , <EOL> - self . handleTouchHeight ] , <EOL> [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> self . doorHandleReachFrame = makeFrameNew ( '<STR_LIT>' , [ reachToGraspTransform , <EOL> self . doorHandleGraspFrame . transform ] ) <EOL> handleTurnTransform = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] , [ - self . handleTurnAngle , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> self . doorHandleTurnFrame = makeFrameNew ( '<STR_LIT>' , [ reachToGraspTransform , <EOL> graspToAxisTransform , <EOL> handleTurnTransform , <EOL> self . doorHandleAxisFrame . transform ] ) <EOL> handlePushTransform = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] , <EOL> [ <NUM_LIT:0> , <NUM_LIT:0> , self . handlePushAngle ] ) <EOL> self . doorHandlePushFrame = makeFrameNew ( '<STR_LIT>' , <EOL> [ self . doorHandleTurnFrame . transform , <EOL> self . doorHingeFrame . transform . GetInverse ( ) , <EOL> handlePushTransform , <EOL> self . doorHingeFrame . transform ] ) <EOL> self . doorHandlePushLiftFrame = makeFrameNew ( '<STR_LIT>' , <EOL> [ self . doorHandleReachFrame . transform , <EOL> self . doorHingeFrame . transform . GetInverse ( ) , <EOL> handlePushTransform , <EOL> self . doorHingeFrame . transform ] ) <EOL> self . doorHandlePushLiftAxisFrame = makeFrameNew ( '<STR_LIT>' , <EOL> [ self . doorHandleAxisFrame . transform , <EOL> self . doorHingeFrame . transform . GetInverse ( ) , <EOL> handlePushTransform , <EOL> self . doorHingeFrame . transform ] ) <EOL> self . doorHandlePushOpenFrame = makeFrame ( '<STR_LIT>' , [ self . handleOpenDepth , self . handleOpenWidth , self . handleLiftHeight ] ) <EOL> t = vtk . vtkTransform ( ) <EOL> t . PostMultiply ( ) <EOL> t . RotateX ( <NUM_LIT> ) <EOL> t . Concatenate ( self . doorHandlePushOpenFrame . transform ) <EOL> self . doorHandlePushOpenFrame . copyFrame ( t ) <EOL> else : <EOL> reachToAxisTransform = transformUtils . frameFromPositionAndRPY ( [ self . preChopDepth , <EOL> doorSide * self . preChopWidth , <EOL> self . preChopHeight ] , <EOL> [ <NUM_LIT:0> , <NUM_LIT> , - <NUM_LIT> ] ) <EOL> obj = om . findObjectByName ( '<STR_LIT>' ) <EOL> self . doorHandleReachFrame = makeFrameNew ( '<STR_LIT>' , <EOL> [ reachToAxisTransform , self . doorHandleAxisFrame . transform ] ) <EOL> if not obj : <EOL> obj = self . doorHandleReachFrame <EOL> obj . setProperty ( '<STR_LIT>' , True ) <EOL> obj . setProperty ( '<STR_LIT>' , True ) <EOL> rep = obj . widget . GetRepresentation ( ) <EOL> rep . SetRotateAxisEnabled ( <NUM_LIT:0> , False ) <EOL> rep . SetRotateAxisEnabled ( <NUM_LIT:1> , False ) <EOL> rep . SetRotateAxisEnabled ( <NUM_LIT:2> , False ) <EOL> obj . widget . HandleRotationEnabledOff ( ) <EOL> obj . setProperty ( '<STR_LIT>' , False ) <EOL> preChopToReachTransform = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0.0> , <EOL> - <NUM_LIT> , <EOL> <NUM_LIT:0.0> ] , <EOL> [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> self . doorHandlePreChopFrame = makeFrameNew ( '<STR_LIT>' , <EOL> [ preChopToReachTransform , self . doorHandleReachFrame . transform ] ) <EOL> self . doorHandleFrame . frameSync = vis . FrameSync ( ) <EOL> self . doorHandleFrame . frameSync . addFrame ( self . doorHandleFrame ) <EOL> self . doorHandleFrame . frameSync . addFrame ( self . doorHandleAxisFrame ) <EOL> self . doorHandleFrame . frameSync . addFrame ( self . doorHandleGraspFrame , ignoreIncoming = True ) <EOL> self . doorHandleFrame . frameSync . addFrame ( self . doorHandleReachFrame , ignoreIncoming = True ) <EOL> if self . usePinchGrasp : <EOL> self . doorHandleFrame . frameSync . addFrame ( self . doorHandleTurnFrame , ignoreIncoming = True ) <EOL> self . doorHandleFrame . frameSync . addFrame ( self . doorHandlePushFrame , ignoreIncoming = True ) <EOL> self . doorHandleFrame . frameSync . addFrame ( self . doorHandlePushLiftFrame , ignoreIncoming = True ) <EOL> self . doorHandleFrame . frameSync . addFrame ( self . doorHandlePushLiftAxisFrame , ignoreIncoming = True ) <EOL> self . doorHandleFrame . frameSync . addFrame ( self . doorHandlePushOpenFrame , ignoreIncoming = True ) <EOL> else : <EOL> self . doorHandleFrame . frameSync . addFrame ( self . doorHandlePreChopFrame , ignoreIncoming = True ) <EOL> def computeDoorHandleAxisFrame ( self ) : <EOL> handleLength = self . doorHandleAffordance . getProperty ( '<STR_LIT>' ) [ <NUM_LIT:1> ] <EOL> doorSide = <NUM_LIT:1> if self . graspingHand == '<STR_LIT:left>' else - <NUM_LIT:1> <EOL> t = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0.0> , doorSide * handleLength / <NUM_LIT> , <NUM_LIT:0.0> ] , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> t . PostMultiply ( ) <EOL> t . Concatenate ( transformUtils . copyFrame ( self . doorHandleFrame . transform ) ) <EOL> return vis . updateFrame ( t , '<STR_LIT>' , parent = self . doorHandleAffordance , <EOL> visible = False , scale = <NUM_LIT> ) <EOL> def computeDoorHingeFrame ( self ) : <EOL> doorSide = <NUM_LIT:1> if self . graspingHand == '<STR_LIT:left>' else - <NUM_LIT:1> <EOL> doorAffordance = om . findObjectByName ( '<STR_LIT>' ) <EOL> doorDimensions = doorAffordance . getProperty ( '<STR_LIT>' ) <EOL> doorDepth = doorDimensions [ <NUM_LIT:0> ] <EOL> doorWidth = doorDimensions [ <NUM_LIT:1> ] <EOL> t = transformUtils . frameFromPositionAndRPY ( [ doorDepth / <NUM_LIT:2> , - doorSide * doorWidth / <NUM_LIT> , <NUM_LIT:0.0> ] , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> t . PostMultiply ( ) <EOL> t . Concatenate ( transformUtils . copyFrame ( doorAffordance . getChildFrame ( ) . transform ) ) <EOL> self . doorHingeFrame = vis . updateFrame ( t , '<STR_LIT>' , parent = doorAffordance , <EOL> visible = False , scale = <NUM_LIT> ) <EOL> return self . doorHingeFrame <EOL> def computeDoorHandleStanceFrame ( self ) : <EOL> graspFrame = self . doorHandleFrame . transform <EOL> groundFrame = self . computeGroundFrame ( self . robotModel ) <EOL> groundHeight = groundFrame . GetPosition ( ) [ <NUM_LIT:2> ] <EOL> graspPosition = np . array ( graspFrame . GetPosition ( ) ) <EOL> xaxis = [ <NUM_LIT:1.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] <EOL> yaxis = [ <NUM_LIT:0.0> , <NUM_LIT:1.0> , <NUM_LIT:0.0> ] <EOL> zaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> graspFrame . TransformVector ( xaxis , xaxis ) <EOL> graspFrame . TransformVector ( yaxis , yaxis ) <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> graspGroundFrame = transformUtils . getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> graspGroundFrame . PostMultiply ( ) <EOL> graspGroundFrame . Translate ( graspPosition [ <NUM_LIT:0> ] , graspPosition [ <NUM_LIT:1> ] , groundHeight ) <EOL> position = [ - <NUM_LIT> , <NUM_LIT> , <NUM_LIT:0.0> ] <EOL> rpy = [ <NUM_LIT:0> , <NUM_LIT:0> , - <NUM_LIT:20> ] <EOL> t = transformUtils . frameFromPositionAndRPY ( position , rpy ) <EOL> t . Concatenate ( graspGroundFrame ) <EOL> self . doorHandleStanceFrame = vis . updateFrame ( t , '<STR_LIT>' , parent = self . doorHandleAffordance , visible = True , scale = <NUM_LIT> ) <EOL> def moveRobotToStanceFrame ( self ) : <EOL> frame = self . doorHandleStanceFrame . transform <EOL> self . sensorJointController . setPose ( '<STR_LIT>' ) <EOL> stancePosition = frame . GetPosition ( ) <EOL> stanceOrientation = frame . GetOrientation ( ) <EOL> self . sensorJointController . q [ : <NUM_LIT:2> ] = [ stancePosition [ <NUM_LIT:0> ] , stancePosition [ <NUM_LIT:1> ] ] <EOL> self . sensorJointController . q [ <NUM_LIT:5> ] = math . radians ( stanceOrientation [ <NUM_LIT:2> ] ) <EOL> self . sensorJointController . push ( ) <EOL> def planNominal ( self ) : <EOL> startPose = self . getPlanningStartPose ( ) <EOL> endPose = self . ikPlanner . getMergedPostureFromDatabase ( startPose , '<STR_LIT>' , '<STR_LIT>' ) <EOL> endPose , info = self . ikPlanner . computeStandPose ( endPose ) <EOL> newPlan = self . ikPlanner . computePostureGoal ( startPose , endPose ) <EOL> self . addPlan ( newPlan ) <EOL> def planPreReach ( self ) : <EOL> ikParameters = IkParameters ( usePointwise = False , maxDegreesPerSecond = self . speedHigh ) <EOL> nonGraspingHand = '<STR_LIT:right>' if self . graspingHand == '<STR_LIT:left>' else '<STR_LIT:left>' <EOL> startPose = self . getPlanningStartPose ( ) <EOL> if self . usePinchGrasp : <EOL> endPose = self . ikPlanner . getMergedPostureFromDatabase ( startPose , '<STR_LIT>' , <EOL> '<STR_LIT>' , <EOL> side = self . graspingHand ) <EOL> else : <EOL> endPose = self . ikPlanner . getMergedPostureFromDatabase ( startPose , '<STR_LIT>' , <EOL> '<STR_LIT>' , <EOL> side = self . graspingHand ) <EOL> endPose = self . ikPlanner . getMergedPostureFromDatabase ( endPose , '<STR_LIT>' , <EOL> '<STR_LIT>' , <EOL> side = nonGraspingHand ) <EOL> endPose , info = self . ikPlanner . computeStandPose ( endPose , ikParameters = ikParameters ) <EOL> newPlan = self . ikPlanner . computePostureGoal ( startPose , endPose , ikParameters = ikParameters ) <EOL> self . addPlan ( newPlan ) <EOL> def planUnReach ( self ) : <EOL> ikParameters = IkParameters ( usePointwise = False , maxDegreesPerSecond = self . speedLow ) <EOL> startPose = self . getPlanningStartPose ( ) <EOL> endPose = self . ikPlanner . getMergedPostureFromDatabase ( startPose , '<STR_LIT>' , <EOL> '<STR_LIT>' , <EOL> side = self . graspingHand ) <EOL> endPose , info = self . ikPlanner . computeStandPose ( endPose , ikParameters = ikParameters ) <EOL> newPlan = self . ikPlanner . computePostureGoal ( startPose , endPose , ikParameters = ikParameters ) <EOL> self . addPlan ( newPlan ) <EOL> def planTuckArms ( self ) : <EOL> ikParameters = IkParameters ( usePointwise = False , maxDegreesPerSecond = self . speedHigh ) <EOL> otherSide = '<STR_LIT:left>' if self . graspingHand == '<STR_LIT:right>' else '<STR_LIT:right>' <EOL> startPose = self . getPlanningStartPose ( ) <EOL> standPose , info = self . ikPlanner . computeStandPose ( startPose , ikParameters = ikParameters ) <EOL> q2 = self . ikPlanner . getMergedPostureFromDatabase ( standPose , '<STR_LIT>' , '<STR_LIT>' , side = otherSide ) <EOL> a = <NUM_LIT> <EOL> q2 = ( <NUM_LIT:1.0> - a ) * np . array ( standPose ) + a * q2 <EOL> q2 = self . ikPlanner . getMergedPostureFromDatabase ( q2 , '<STR_LIT>' , '<STR_LIT>' , side = self . graspingHand ) <EOL> a = <NUM_LIT> <EOL> q2 = ( <NUM_LIT:1.0> - a ) * np . array ( standPose ) + a * q2 <EOL> endPose = self . ikPlanner . getMergedPostureFromDatabase ( standPose , '<STR_LIT>' , '<STR_LIT>' , side = self . graspingHand ) <EOL> endPose = self . ikPlanner . getMergedPostureFromDatabase ( endPose , '<STR_LIT>' , '<STR_LIT>' , side = otherSide ) <EOL> newPlan = self . ikPlanner . computeMultiPostureGoal ( [ startPose , q2 , endPose ] , ikParameters = ikParameters ) <EOL> self . addPlan ( newPlan ) <EOL> def planTuckArmsPrePush ( self ) : <EOL> ikParameters = IkParameters ( usePointwise = False , maxDegreesPerSecond = self . speedHigh ) <EOL> otherSide = '<STR_LIT:left>' if self . graspingHand == '<STR_LIT:right>' else '<STR_LIT:right>' <EOL> startPose = self . getPlanningStartPose ( ) <EOL> standPose , info = self . ikPlanner . computeStandPose ( startPose , ikParameters = ikParameters ) <EOL> q2 = self . ikPlanner . getMergedPostureFromDatabase ( standPose , '<STR_LIT>' , '<STR_LIT>' , side = self . graspingHand ) <EOL> a = <NUM_LIT> <EOL> q2 = ( <NUM_LIT:1.0> - a ) * np . array ( standPose ) + a * q2 <EOL> q2 = self . ikPlanner . getMergedPostureFromDatabase ( q2 , '<STR_LIT>' , '<STR_LIT>' , side = otherSide ) <EOL> a = <NUM_LIT> <EOL> q2 = ( <NUM_LIT:1.0> - a ) * np . array ( standPose ) + a * q2 <EOL> endPose = self . ikPlanner . getMergedPostureFromDatabase ( standPose , '<STR_LIT>' , '<STR_LIT>' , side = self . graspingHand ) <EOL> endPose = self . ikPlanner . getMergedPostureFromDatabase ( endPose , '<STR_LIT>' , '<STR_LIT>' , side = otherSide ) <EOL> newPlan = self . ikPlanner . computeMultiPostureGoal ( [ startPose , q2 , endPose ] , ikParameters = ikParameters ) <EOL> self . addPlan ( newPlan ) <EOL> def planChop ( self , deltaZ = None , deltaY = None , deltaX = None ) : <EOL> startPose = self . getPlanningStartPose ( ) <EOL> if deltaZ is None : <EOL> deltaZ = self . chopDistance <EOL> if deltaY is None : <EOL> deltaY = self . chopSidewaysDistance <EOL> if deltaX is None : <EOL> deltaX = <NUM_LIT:0.0> <EOL> linkOffsetFrame = self . ikPlanner . getPalmToHandLink ( self . graspingHand ) <EOL> handLinkName = self . ikPlanner . getHandLink ( self . graspingHand ) <EOL> startFrame = self . ikPlanner . getLinkFrameAtPose ( handLinkName , startPose ) <EOL> endToStartTransform = transformUtils . frameFromPositionAndRPY ( [ deltaZ , - deltaX , - deltaY ] , <EOL> [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> endFrame = transformUtils . concatenateTransforms ( [ endToStartTransform , startFrame ] ) ; <EOL> vis . updateFrame ( endFrame , '<STR_LIT>' , parent = self . doorHandleAffordance , visible = False , scale = <NUM_LIT> ) <EOL> palmToWorld1 = transformUtils . concatenateTransforms ( [ linkOffsetFrame , startFrame ] ) <EOL> palmToWorld2 = transformUtils . concatenateTransforms ( [ linkOffsetFrame , endFrame ] ) <EOL> constraintSet = self . ikPlanner . planEndEffectorGoal ( startPose , self . graspingHand , palmToWorld2 ) <EOL> constraintSet . nominalPoseName = '<STR_LIT>' <EOL> constraintSet . ikParameters = IkParameters ( usePointwise = False , <EOL> maxDegreesPerSecond = self . speedLow , <EOL> numberOfAddedKnots = <NUM_LIT:2> ) <EOL> endPose , info = constraintSet . runIk ( ) <EOL> motionVector = np . array ( palmToWorld2 . GetPosition ( ) ) - np . array ( palmToWorld1 . GetPosition ( ) ) <EOL> motionTargetFrame = transformUtils . getTransformFromOriginAndNormal ( np . array ( palmToWorld2 . GetPosition ( ) ) , motionVector ) <EOL> p = self . ikPlanner . createLinePositionConstraint ( handLinkName , linkOffsetFrame , motionTargetFrame , lineAxis = <NUM_LIT:2> , bounds = [ - np . linalg . norm ( motionVector ) , <NUM_LIT:0.0> ] , positionTolerance = <NUM_LIT> ) <EOL> constraintSet . constraints . append ( p ) <EOL> plan = constraintSet . runIkTraj ( ) <EOL> self . addPlan ( plan ) <EOL> def stopPushing ( self ) : <EOL> startPose = self . getPlanningStartPose <EOL> plan = self . robotSystem . ikPlanner . computePostureGoal ( startPose , startPose ) <EOL> self . addPlan ( plan ) <EOL> self . commitManipPlan ( ) <EOL> def planReach ( self , reachTargetFrame = None , jointSpeedLimit = None ) : <EOL> if reachTargetFrame is None : <EOL> reachTargetFrame = self . doorHandleReachFrame <EOL> if jointSpeedLimit is None : <EOL> jointSpeedLimit = self . speedLow <EOL> startPose = self . getPlanningStartPose ( ) <EOL> constraintSet = self . ikPlanner . planEndEffectorGoal ( startPose , self . graspingHand , reachTargetFrame ) <EOL> constraintSet . nominalPoseName = '<STR_LIT>' <EOL> constraintSet . ikParameters = IkParameters ( usePointwise = False , <EOL> maxDegreesPerSecond = self . speedLow , <EOL> numberOfAddedKnots = <NUM_LIT:2> ) <EOL> endPose , info = constraintSet . runIk ( ) <EOL> linkOffsetFrame = self . ikPlanner . getPalmToHandLink ( self . graspingHand ) <EOL> handLinkName = self . ikPlanner . getHandLink ( self . graspingHand ) <EOL> handToWorld1 = self . ikPlanner . getLinkFrameAtPose ( handLinkName , startPose ) <EOL> handToWorld2 = self . ikPlanner . getLinkFrameAtPose ( handLinkName , endPose ) <EOL> palmToWorld1 = transformUtils . concatenateTransforms ( [ linkOffsetFrame , handToWorld1 ] ) <EOL> palmToWorld2 = transformUtils . concatenateTransforms ( [ linkOffsetFrame , handToWorld2 ] ) <EOL> motionVector = np . array ( palmToWorld2 . GetPosition ( ) ) - np . array ( palmToWorld1 . GetPosition ( ) ) <EOL> motionTargetFrame = transformUtils . getTransformFromOriginAndNormal ( np . array ( palmToWorld2 . GetPosition ( ) ) , motionVector ) <EOL> p = self . ikPlanner . createLinePositionConstraint ( handLinkName , linkOffsetFrame , motionTargetFrame , lineAxis = <NUM_LIT:2> , bounds = [ - np . linalg . norm ( motionVector ) , <NUM_LIT:0.0> ] , positionTolerance = <NUM_LIT> ) <EOL> constraintSet . constraints . append ( p ) <EOL> plan = constraintSet . runIkTraj ( ) <EOL> self . addPlan ( plan ) <EOL> def planPreChop ( self ) : <EOL> self . planReach ( self . doorHandlePreChopFrame , self . speedHigh ) <EOL> def createHingeConstraint ( self , referenceFrame , axis , linkName , startPose , tspan = [ <NUM_LIT:0> , <NUM_LIT:1> ] ) : <EOL> constraints = [ ] <EOL> linkFrame = self . ikPlanner . getLinkFrameAtPose ( linkName , startPose ) <EOL> def addPivotPoint ( constraints , pivotPoint ) : <EOL> constraints . append ( ik . PositionConstraint ( ) ) <EOL> constraints [ - <NUM_LIT:1> ] . linkName = linkName <EOL> constraints [ - <NUM_LIT:1> ] . referenceFrame = referenceFrame . transform <EOL> constraints [ - <NUM_LIT:1> ] . lowerBound = np . array ( pivotPoint ) <EOL> constraints [ - <NUM_LIT:1> ] . upperBound = np . array ( pivotPoint ) <EOL> pivotPointInWorld = referenceFrame . transform . TransformDoublePoint ( pivotPoint ) <EOL> constraints [ - <NUM_LIT:1> ] . pointInLink = linkFrame . GetInverse ( ) . TransformDoublePoint ( pivotPointInWorld ) <EOL> constraints [ - <NUM_LIT:1> ] . tspan = tspan <EOL> addPivotPoint ( constraints , [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> addPivotPoint ( constraints , axis ) <EOL> return constraints <EOL> def planHandleTurn ( self , turnAngle = None ) : <EOL> doorSide = <NUM_LIT:1> if self . graspingHand == '<STR_LIT:left>' else - <NUM_LIT:1> <EOL> if turnAngle is None : <EOL> turnAngle = self . handleTurnAngle <EOL> startPose = self . getPlanningStartPose ( ) <EOL> linkFrame = self . ikPlanner . getLinkFrameAtPose ( self . ikPlanner . getHandLink ( ) , startPose ) <EOL> finalGraspToReferenceTransfrom = transformUtils . concatenateTransforms ( <EOL> [ self . ikPlanner . getPalmToHandLink ( self . graspingHand ) , linkFrame , <EOL> self . doorHandleAxisFrame . transform . GetInverse ( ) ] ) <EOL> handleTurnTransform = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] , <EOL> [ doorSide * turnAngle , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> doorHandleTurnFrame = transformUtils . concatenateTransforms ( [ finalGraspToReferenceTransfrom , <EOL> handleTurnTransform , <EOL> self . doorHandleAxisFrame . transform ] ) <EOL> vis . updateFrame ( doorHandleTurnFrame , '<STR_LIT>' , parent = self . doorHandleAffordance , visible = False , scale = <NUM_LIT> ) <EOL> constraintSet = self . ikPlanner . planEndEffectorGoal ( startPose , self . graspingHand , <EOL> doorHandleTurnFrame ) <EOL> constraintSet . ikParameters = IkParameters ( usePointwise = False , <EOL> maxDegreesPerSecond = self . speedLow , <EOL> numberOfAddedKnots = <NUM_LIT:2> ) <EOL> constraintSet . nominalPoseName = '<STR_LIT>' <EOL> endPose , info = constraintSet . runIk ( ) <EOL> constraints = constraintSet . constraints <EOL> constraints . extend ( self . createHingeConstraint ( self . doorHandleAxisFrame , [ <NUM_LIT:1.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] , <EOL> self . ikPlanner . getHandLink ( ) , <EOL> constraintSet . startPoseName ) ) <EOL> constraints . append ( self . ikPlanner . createLockedBasePostureConstraint ( constraintSet . startPoseName ) ) <EOL> constraints . append ( self . ikPlanner . createLockedBackPostureConstraint ( constraintSet . startPoseName ) ) <EOL> constraints . extend ( self . ikPlanner . createFixedFootConstraints ( constraintSet . startPoseName ) ) <EOL> constraints . append ( self . ikPlanner . createLockedArmPostureConstraint ( constraintSet . startPoseName ) ) <EOL> plan = constraintSet . runIkTraj ( ) <EOL> self . addPlan ( plan ) <EOL> def planDoorPushOpen ( self ) : <EOL> ikParameters = IkParameters ( usePointwise = False , maxDegreesPerSecond = <NUM_LIT:15> ) <EOL> nonGraspingHand = '<STR_LIT:right>' if self . graspingHand == '<STR_LIT:left>' else '<STR_LIT:left>' <EOL> startPose = self . getPlanningStartPose ( ) <EOL> endPose = self . ikPlanner . getMergedPostureFromDatabase ( startPose , '<STR_LIT>' , '<STR_LIT>' , side = nonGraspingHand ) <EOL> newPlan = self . ikPlanner . computePostureGoal ( startPose , endPose , ikParameters = ikParameters ) <EOL> self . addPlan ( newPlan ) <EOL> def planDoorPushOpenTwist ( self ) : <EOL> ikParameters = IkParameters ( usePointwise = False , maxDegreesPerSecond = <NUM_LIT> ) <EOL> nonGraspingHand = '<STR_LIT:right>' if self . graspingHand == '<STR_LIT:left>' else '<STR_LIT:left>' <EOL> startPose = self . getPlanningStartPose ( ) <EOL> endPose = self . ikPlanner . getMergedPostureFromDatabase ( startPose , '<STR_LIT>' , '<STR_LIT>' , side = nonGraspingHand ) <EOL> newPlan = self . ikPlanner . computePostureGoal ( startPose , endPose , ikParameters = ikParameters ) <EOL> self . addPlan ( newPlan ) <EOL> def planHandlePush ( self ) : <EOL> doorSide = <NUM_LIT:1> if self . graspingHand == '<STR_LIT:left>' else - <NUM_LIT:1> <EOL> startPose = self . getPlanningStartPose ( ) <EOL> linkFrame = self . ikPlanner . getLinkFrameAtPose ( self . ikPlanner . getHandLink ( ) , startPose ) <EOL> finalGraspToReferenceTransfrom = transformUtils . concatenateTransforms ( <EOL> [ self . ikPlanner . getPalmToHandLink ( self . graspingHand ) , linkFrame , <EOL> self . doorHingeFrame . transform . GetInverse ( ) ] ) <EOL> handlePushTransform = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] , <EOL> [ <NUM_LIT:0> , <NUM_LIT:0> , - doorSide * self . handlePushAngle ] ) <EOL> doorHandlePushFrame = transformUtils . concatenateTransforms ( [ finalGraspToReferenceTransfrom , <EOL> handlePushTransform , <EOL> self . doorHingeFrame . transform ] ) <EOL> vis . updateFrame ( doorHandlePushFrame , '<STR_LIT>' , parent = self . doorHandleAffordance , visible = False , scale = <NUM_LIT> ) <EOL> constraintSet = self . ikPlanner . planEndEffectorGoal ( startPose , self . graspingHand , <EOL> doorHandlePushFrame ) <EOL> constraintSet . ikParameters = IkParameters ( usePointwise = False , maxDegreesPerSecond = self . speedLow ) <EOL> constraintSet . nominalPoseName = '<STR_LIT>' <EOL> endPose , info = constraintSet . runIk ( ) <EOL> constraints = constraintSet . constraints <EOL> constraints . extend ( self . createHingeConstraint ( self . doorHingeFrame , [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> ] , <EOL> self . ikPlanner . getHandLink ( side = self . graspingHand ) , <EOL> constraintSet . startPoseName ) ) <EOL> constraints . append ( self . ikPlanner . createLockedBasePostureConstraint ( constraintSet . startPoseName ) ) <EOL> constraints . extend ( self . ikPlanner . createFixedFootConstraints ( constraintSet . startPoseName ) ) <EOL> constraints . append ( self . ikPlanner . createLockedArmPostureConstraint ( constraintSet . startPoseName ) ) <EOL> plan = constraintSet . runIkTraj ( ) <EOL> self . addPlan ( plan ) <EOL> def planHandlePushLift ( self ) : <EOL> self . planHandleTurn ( - self . handleTurnAngle ) <EOL> def planDoorTouch ( self ) : <EOL> ikParameters = IkParameters ( usePointwise = False , maxDegreesPerSecond = self . speedHigh ) <EOL> nonGraspingHand = '<STR_LIT:right>' if self . graspingHand == '<STR_LIT:left>' else '<STR_LIT:left>' <EOL> startPose = self . getPlanningStartPose ( ) <EOL> endPose = self . ikPlanner . getMergedPostureFromDatabase ( startPose , '<STR_LIT>' , '<STR_LIT>' , side = nonGraspingHand ) <EOL> newPlan = self . ikPlanner . computePostureGoal ( startPose , endPose , ikParameters = ikParameters ) <EOL> self . addPlan ( newPlan ) <EOL> def planHandlePushOpen ( self ) : <EOL> startPose = self . getPlanningStartPose ( ) <EOL> constraintSet = self . ikPlanner . planEndEffectorGoal ( startPose , self . graspingHand , self . doorHandlePushOpenFrame ) <EOL> constraintSet . ikParameters = IkParameters ( usePointwise = False , <EOL> maxDegreesPerSecond = self . speedLow , <EOL> numberOfAddedKnots = <NUM_LIT:2> ) <EOL> endPose , info = constraintSet . runIk ( ) <EOL> plan = constraintSet . runIkTraj ( ) <EOL> self . addPlan ( plan ) <EOL> def planFootstepsToDoor ( self ) : <EOL> startPose = self . getPlanningStartPose ( ) <EOL> goalFrame = self . doorHandleStanceFrame . transform <EOL> request = self . footstepPlanner . constructFootstepPlanRequest ( startPose , goalFrame ) <EOL> self . footstepPlan = self . footstepPlanner . sendFootstepPlanRequest ( request , waitForResponse = True ) <EOL> def planFootstepsThroughDoor ( self ) : <EOL> startPose = self . getPlanningStartPose ( ) <EOL> goalFrame = self . doorWalkFrame . transform <EOL> request = self . footstepPlanner . constructFootstepPlanRequest ( startPose , goalFrame ) <EOL> request . params . nom_step_width = <NUM_LIT> <EOL> self . footstepPlan = self . footstepPlanner . sendFootstepPlanRequest ( request , waitForResponse = True ) <EOL> rt . _addPlanItem ( self . footstepPlan , '<STR_LIT>' , rt . FootstepPlanItem ) <EOL> def setFootstepThroughDoorParameters ( self ) : <EOL> bias = - <NUM_LIT> <EOL> self . doorFootstepParams = FieldContainer ( <EOL> leadingFoot = '<STR_LIT:right>' , <EOL> preEntryFootWidth = - <NUM_LIT> + bias , <EOL> preEntryFootDistance = - <NUM_LIT> , <EOL> entryFootWidth = <NUM_LIT> + bias , <EOL> entryFootDistance = - <NUM_LIT> , <EOL> exitFootWidth = - <NUM_LIT> + bias , <EOL> exitFootDistance = <NUM_LIT> , <EOL> exitStepDistance = <NUM_LIT> , <EOL> endStanceWidth = <NUM_LIT> , <EOL> numberOfExitSteps = <NUM_LIT:1> , <EOL> centerStepDistance = <NUM_LIT> , <EOL> centerStanceWidth = <NUM_LIT> , <EOL> centerLeadingFoot = '<STR_LIT:right>' <EOL> ) <EOL> def setTestingFootstepThroughDoorParameters ( self ) : <EOL> self . doorFootstepParams = FieldContainer ( <EOL> endStanceWidth = <NUM_LIT> , <EOL> entryFootDistance = - <NUM_LIT> , <EOL> entryFootWidth = <NUM_LIT> , <EOL> exitFootDistance = <NUM_LIT> , <EOL> exitFootWidth = - <NUM_LIT> , <EOL> exitStepDistance = <NUM_LIT> , <EOL> leadingFoot = '<STR_LIT:right>' , <EOL> numberOfExitSteps = <NUM_LIT:1> , <EOL> preEntryFootDistance = - <NUM_LIT> , <EOL> preEntryFootWidth = - <NUM_LIT> <EOL> ) <EOL> def getRelativeFootstepsThroughDoorWithSway ( self ) : <EOL> p = self . doorFootstepParams <EOL> stepFrames = [ <EOL> [ p . preEntryFootDistance , p . preEntryFootWidth , <NUM_LIT:0.0> ] , <EOL> [ p . entryFootDistance , p . entryFootWidth , <NUM_LIT:0.0> ] , <EOL> [ p . exitFootDistance , p . exitFootWidth , <NUM_LIT:0.0> ] , <EOL> ] <EOL> for i in xrange ( p . numberOfExitSteps ) : <EOL> sign = - <NUM_LIT:1> if ( i % <NUM_LIT:2> ) else <NUM_LIT:1> <EOL> stepFrames . append ( [ p . exitFootDistance + ( i + <NUM_LIT:1> ) * p . exitStepDistance , sign * p . endStanceWidth / <NUM_LIT> , <NUM_LIT:0.0> ] ) <EOL> lastStep = list ( stepFrames [ - <NUM_LIT:1> ] ) <EOL> lastStep [ <NUM_LIT:1> ] *= - <NUM_LIT:1> <EOL> stepFrames . append ( lastStep ) <EOL> return FootstepRequestGenerator . makeStepFrames ( stepFrames , relativeFrame = self . doorGroundFrame . transform , showFrames = False ) , p . leadingFoot <EOL> def getRelativeFootstepsThroughDoorCentered ( self ) : <EOL> p = self . doorFootstepParams <EOL> stepDistance = p . centerStepDistance <EOL> stanceWidth = p . centerStanceWidth <EOL> leadingFoot = p . centerLeadingFoot <EOL> stepFrames = [ ] <EOL> for i in xrange ( <NUM_LIT:30> ) : <EOL> sign = - <NUM_LIT:1> if leadingFoot is '<STR_LIT:right>' else <NUM_LIT:1> <EOL> if i % <NUM_LIT:2> : <EOL> sign = - sign <EOL> stepX = ( i + <NUM_LIT:1> ) * stepDistance <EOL> if stepX > <NUM_LIT> : <EOL> stepX = <NUM_LIT> <EOL> stepFrames . append ( [ stepX , sign * stanceWidth / <NUM_LIT> , <NUM_LIT:0.0> ] ) <EOL> if stepX == <NUM_LIT> : <EOL> break <EOL> lastStep = list ( stepFrames [ - <NUM_LIT:1> ] ) <EOL> lastStep [ <NUM_LIT:1> ] *= - <NUM_LIT:1> <EOL> stepFrames . append ( lastStep ) <EOL> stepFrames [ - <NUM_LIT:1> ] [ <NUM_LIT:1> ] = np . sign ( stepFrames [ - <NUM_LIT:1> ] [ <NUM_LIT:1> ] ) * ( p . endStanceWidth / <NUM_LIT> ) <EOL> stepFrames [ - <NUM_LIT:2> ] [ <NUM_LIT:1> ] = np . sign ( stepFrames [ - <NUM_LIT:2> ] [ <NUM_LIT:1> ] ) * ( p . endStanceWidth / <NUM_LIT> ) <EOL> return FootstepRequestGenerator . makeStepFrames ( stepFrames , relativeFrame = self . doorHandleStanceFrame . transform , showFrames = False ) , leadingFoot <EOL> def planManualFootstepsTest ( self , stepDistance = <NUM_LIT> , stanceWidth = <NUM_LIT> , numberOfSteps = <NUM_LIT:4> , leadingFoot = '<STR_LIT:right>' ) : <EOL> stepFrames = [ ] <EOL> for i in xrange ( numberOfSteps ) : <EOL> sign = - <NUM_LIT:1> if leadingFoot is '<STR_LIT:right>' else <NUM_LIT:1> <EOL> if i % <NUM_LIT:2> : <EOL> sign = - sign <EOL> stepFrames . append ( [ ( i + <NUM_LIT:1> ) * stepDistance , sign * stanceWidth / <NUM_LIT> , <NUM_LIT:0.0> ] ) <EOL> lastStep = list ( stepFrames [ - <NUM_LIT:1> ] ) <EOL> lastStep [ <NUM_LIT:1> ] *= - <NUM_LIT:1> <EOL> stepFrames . append ( lastStep ) <EOL> stanceFrame = FootstepRequestGenerator . getRobotStanceFrame ( self . robotModel ) <EOL> stepFrames = FootstepRequestGenerator . makeStepFrames ( stepFrames , relativeFrame = stanceFrame ) <EOL> startPose = self . getPlanningStartPose ( ) <EOL> helper = FootstepRequestGenerator ( self . footstepPlanner ) <EOL> request = helper . makeFootstepRequest ( startPose , stepFrames , leadingFoot ) <EOL> self . footstepPlanner . sendFootstepPlanRequest ( request , waitForResponse = True ) <EOL> def planFootstepsThroughDoorManual ( self ) : <EOL> startPose = self . getPlanningStartPose ( ) <EOL> stepFrames , leadingFoot = self . getRelativeFootstepsThroughDoorCentered ( ) <EOL> helper = FootstepRequestGenerator ( self . footstepPlanner ) <EOL> request = helper . makeFootstepRequest ( startPose , stepFrames , leadingFoot , numberOfFillSteps = <NUM_LIT:2> ) <EOL> self . footstepPlan = self . footstepPlanner . sendFootstepPlanRequest ( request , waitForResponse = True ) <EOL> rt . _addPlanItem ( self . footstepPlan , '<STR_LIT>' , rt . FootstepPlanItem ) <EOL> def computeWalkingPlan ( self ) : <EOL> startPose = self . getPlanningStartPose ( ) <EOL> self . walkingPlan = self . footstepPlanner . sendWalkingPlanRequest ( self . footstepPlan , startPose , waitForResponse = True ) <EOL> self . addPlan ( self . walkingPlan ) <EOL> def commitManipPlan ( self ) : <EOL> self . manipPlanner . commitManipPlan ( self . plans [ - <NUM_LIT:1> ] ) <EOL> def fitDoor ( self , doorGroundFrame ) : <EOL> om . removeFromObjectModel ( om . findObjectByName ( '<STR_LIT>' ) ) <EOL> self . spawnDoorAffordance ( ) <EOL> affordanceFrame = om . findObjectByName ( '<STR_LIT>' ) <EOL> assert affordanceFrame is not None <EOL> affordanceFrame . copyFrame ( doorGroundFrame ) <EOL> om . findObjectByName ( '<STR_LIT>' ) . setProperty ( '<STR_LIT>' , False ) <EOL> def showDoorHandlePoints ( self , polyData ) : <EOL> doorHandle = om . findObjectByName ( '<STR_LIT>' ) <EOL> door = om . findObjectByName ( '<STR_LIT>' ) <EOL> doorWidth = door . getProperty ( '<STR_LIT>' ) [ <NUM_LIT:1> ] <EOL> doorAxes = transformUtils . getAxesFromTransform ( door . getChildFrame ( ) . transform ) <EOL> doorOrigin = np . array ( door . getChildFrame ( ) . transform . GetPosition ( ) ) <EOL> handleAxes = transformUtils . getAxesFromTransform ( doorHandle . getChildFrame ( ) . transform ) <EOL> handleOrigin = np . array ( doorHandle . getChildFrame ( ) . transform . GetPosition ( ) ) <EOL> doorSide = <NUM_LIT:1> if self . graspingHand == '<STR_LIT:left>' else - <NUM_LIT:1> <EOL> polyData = segmentation . cropToLineSegment ( polyData , doorOrigin - doorAxes [ <NUM_LIT:0> ] * <NUM_LIT> , doorOrigin - doorAxes [ <NUM_LIT:0> ] * <NUM_LIT:0.1> ) <EOL> polyData = segmentation . cropToLineSegment ( polyData , doorOrigin , doorOrigin + doorAxes [ <NUM_LIT:1> ] * ( doorWidth * <NUM_LIT:0.5> - <NUM_LIT> ) * doorSide ) <EOL> polyData = segmentation . cropToLineSegment ( polyData , handleOrigin - handleAxes [ <NUM_LIT:2> ] * <NUM_LIT:0.1> , handleOrigin + handleAxes [ <NUM_LIT:2> ] * <NUM_LIT:0.1> ) <EOL> pointsName = '<STR_LIT>' <EOL> existed = om . findObjectByName ( pointsName ) is not None <EOL> obj = vis . updatePolyData ( polyData , pointsName , parent = doorHandle , color = [ <NUM_LIT:1> , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> if not existed : <EOL> obj . setProperty ( '<STR_LIT>' , <NUM_LIT:10> ) <EOL> def spawnDoorAffordance ( self ) : <EOL> groundFrame = self . computeGroundFrame ( self . robotModel ) <EOL> doorOffsetX = <NUM_LIT> <EOL> doorOffsetY = <NUM_LIT:0.0> <EOL> doorGroundFrame = transformUtils . frameFromPositionAndRPY ( [ doorOffsetX , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] , [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> doorGroundFrame . PostMultiply ( ) <EOL> doorGroundFrame . Concatenate ( groundFrame ) <EOL> stanceFrame = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] , [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> stanceFrame . PostMultiply ( ) <EOL> stanceFrame . Concatenate ( groundFrame ) <EOL> doorWalkFrame = transformUtils . frameFromPositionAndRPY ( [ doorOffsetX + <NUM_LIT> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] , [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> doorWalkFrame . PostMultiply ( ) <EOL> doorWalkFrame . Concatenate ( groundFrame ) <EOL> doorWidth = <NUM_LIT> * <NUM_LIT> <EOL> doorHeight = <NUM_LIT> * <NUM_LIT> <EOL> doorDepth = <NUM_LIT:0.5> * <NUM_LIT> <EOL> doorSide = <NUM_LIT:1> if self . graspingHand == '<STR_LIT:left>' else - <NUM_LIT:1> <EOL> handleHeightFromGround = <NUM_LIT> * <NUM_LIT> <EOL> handleDistanceFromEdge = <NUM_LIT> * <NUM_LIT> <EOL> handleDistanceFromDoor = <NUM_LIT> * <NUM_LIT> <EOL> handleLength = <NUM_LIT> * <NUM_LIT> <EOL> handleDepth = <NUM_LIT> * <NUM_LIT> <EOL> doorJamWidth = <NUM_LIT:0.5> <EOL> doorJamDepth = <NUM_LIT> * <NUM_LIT> <EOL> handleFrame = transformUtils . frameFromPositionAndRPY ( [ - handleDistanceFromDoor - doorDepth / <NUM_LIT> - handleDepth / <NUM_LIT:2> , doorSide * ( doorWidth / <NUM_LIT> - handleDistanceFromEdge - handleLength / <NUM_LIT> ) , handleHeightFromGround ] , [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> handleFrame . PostMultiply ( ) <EOL> handleFrame . Concatenate ( doorGroundFrame ) <EOL> doorFrame = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , doorHeight / <NUM_LIT> ] , [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> doorFrame . PostMultiply ( ) <EOL> doorFrame . Concatenate ( doorGroundFrame ) <EOL> leftDoorJamFrame = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0.0> , ( doorWidth / <NUM_LIT> + doorJamWidth / <NUM_LIT> ) , doorHeight / <NUM_LIT> ] , [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> leftDoorJamFrame . PostMultiply ( ) <EOL> leftDoorJamFrame . Concatenate ( doorGroundFrame ) <EOL> rightDoorJamFrame = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0.0> , - ( doorWidth / <NUM_LIT> + doorJamWidth / <NUM_LIT> ) , doorHeight / <NUM_LIT> ] , [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> rightDoorJamFrame . PostMultiply ( ) <EOL> rightDoorJamFrame . Concatenate ( doorGroundFrame ) <EOL> desc = dict ( classname = '<STR_LIT>' , Name = '<STR_LIT>' , <EOL> pose = transformUtils . poseFromTransform ( handleFrame ) , Dimensions = [ handleDepth , handleLength , <NUM_LIT> ] , Color = [ <NUM_LIT:0.0> , <NUM_LIT:1.0> , <NUM_LIT:0.0> ] ) <EOL> handleAffordance = segmentation . affordanceManager . newAffordanceFromDescription ( desc ) <EOL> desc = dict ( classname = '<STR_LIT>' , Name = '<STR_LIT>' , <EOL> pose = transformUtils . poseFromTransform ( doorFrame ) , Dimensions = [ doorDepth , doorWidth , doorHeight ] , Color = [ <NUM_LIT:0.5> , <NUM_LIT:0.5> , <NUM_LIT:0.5> ] ) <EOL> doorAffordance = segmentation . affordanceManager . newAffordanceFromDescription ( desc ) <EOL> desc = dict ( classname = '<STR_LIT>' , Name = '<STR_LIT>' , <EOL> pose = transformUtils . poseFromTransform ( leftDoorJamFrame ) , Dimensions = [ doorJamDepth , doorJamWidth , doorHeight ] , Color = [ <NUM_LIT> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> leftDoorJamAffordance = segmentation . affordanceManager . newAffordanceFromDescription ( desc ) <EOL> desc = dict ( classname = '<STR_LIT>' , Name = '<STR_LIT>' , <EOL> pose = transformUtils . poseFromTransform ( rightDoorJamFrame ) , Dimensions = [ doorJamDepth , doorJamWidth , doorHeight ] , Color = [ <NUM_LIT> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> rightDoorJamAffordance = segmentation . affordanceManager . newAffordanceFromDescription ( desc ) <EOL> doorGroundFrame = vis . showFrame ( doorGroundFrame , '<STR_LIT>' , parent = doorAffordance ) <EOL> stanceFrame = vis . showFrame ( stanceFrame , '<STR_LIT>' , parent = doorAffordance ) <EOL> doorWalkFrame = vis . showFrame ( doorWalkFrame , '<STR_LIT>' , visible = False , parent = doorAffordance ) <EOL> doorFrame = doorAffordance . getChildFrame ( ) <EOL> handleFrame = handleAffordance . getChildFrame ( ) <EOL> leftDoorJamFrame = leftDoorJamAffordance . getChildFrame ( ) <EOL> rightDoorJamFrame = rightDoorJamAffordance . getChildFrame ( ) <EOL> self . doorFrameSync = vis . FrameSync ( ) <EOL> self . doorFrameSync . addFrame ( doorGroundFrame ) <EOL> self . doorFrameSync . addFrame ( stanceFrame , ignoreIncoming = True ) <EOL> self . doorFrameSync . addFrame ( doorWalkFrame , ignoreIncoming = True ) <EOL> self . doorFrameSync . addFrame ( doorFrame , ignoreIncoming = True ) <EOL> self . doorFrameSync . addFrame ( leftDoorJamFrame , ignoreIncoming = True ) <EOL> self . doorFrameSync . addFrame ( rightDoorJamFrame , ignoreIncoming = True ) <EOL> self . doorHandleFrameSync = vis . FrameSync ( ) <EOL> self . doorHandleFrameSync . addFrame ( doorFrame ) <EOL> self . doorHandleFrameSync . addFrame ( handleFrame , ignoreIncoming = True ) <EOL> self . findDoorHandleAffordance ( ) <EOL> self . doorGroundFrame = doorGroundFrame <EOL> self . doorHandleStanceFrame = stanceFrame <EOL> self . doorWalkFrame = doorWalkFrame <EOL> def findDoorHandleAffordance ( self ) : <EOL> self . doorHandleAffordance = om . findObjectByName ( '<STR_LIT>' ) <EOL> self . doorHandleFrame = self . doorHandleAffordance . getChildFrame ( ) <EOL> self . computeDoorHingeFrame ( ) <EOL> self . computeDoorHandleGraspFrame ( ) <EOL> def getEstimatedRobotStatePose ( self ) : <EOL> return np . array ( self . sensorJointController . getPose ( '<STR_LIT>' ) ) <EOL> def getPlanningStartPose ( self ) : <EOL> if self . planFromCurrentRobotState : <EOL> return self . getEstimatedRobotStatePose ( ) <EOL> else : <EOL> if self . plans : <EOL> return robotstate . convertStateMessageToDrakePose ( self . plans [ - <NUM_LIT:1> ] . plan [ - <NUM_LIT:1> ] ) <EOL> else : <EOL> return self . getEstimatedRobotStatePose ( ) <EOL> class DoorImageFitter ( ImageBasedAffordanceFit ) : <EOL> def __init__ ( self , doorDemo ) : <EOL> ImageBasedAffordanceFit . __init__ ( self , numberOfPoints = <NUM_LIT:1> ) <EOL> self . doorDemo = doorDemo <EOL> def fit ( self , polyData , points ) : <EOL> stanceFrame = FootstepRequestGenerator . getRobotStanceFrame ( self . doorDemo . robotModel ) <EOL> doorGroundFrame = segmentation . segmentDoorPlane ( polyData , points [ <NUM_LIT:0> ] , stanceFrame ) <EOL> self . doorDemo . fitDoor ( doorGroundFrame ) <EOL> self . doorDemo . showDoorHandlePoints ( polyData ) <EOL> class DoorTaskPanel ( TaskUserPanel ) : <EOL> def __init__ ( self , doorDemo ) : <EOL> TaskUserPanel . __init__ ( self , windowTitle = '<STR_LIT>' ) <EOL> self . doorDemo = doorDemo <EOL> self . fitter = DoorImageFitter ( self . doorDemo ) <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 . doorDemo . spawnDoorAffordance ) <EOL> self . addManualSpacer ( ) <EOL> self . addManualButton ( '<STR_LIT>' , self . doorDemo . planFootstepsToDoor ) <EOL> self . addManualButton ( '<STR_LIT>' , self . doorDemo . planFootstepsThroughDoor ) <EOL> self . addManualSpacer ( ) <EOL> self . addManualButton ( '<STR_LIT>' , self . doorDemo . planPreReach ) <EOL> self . addManualButton ( '<STR_LIT>' , self . doorDemo . planTuckArmsPrePush ) <EOL> self . addManualButton ( '<STR_LIT>' , self . doorDemo . planTuckArms ) <EOL> self . addManualSpacer ( ) <EOL> self . addManualButton ( '<STR_LIT>' , self . openPinch ) <EOL> self . addManualButton ( '<STR_LIT>' , self . closePinch ) <EOL> self . addManualSpacer ( ) <EOL> self . addManualButton ( '<STR_LIT>' , self . doorDemo . planReach ) <EOL> self . addManualButton ( '<STR_LIT>' , self . doorDemo . planUnReach ) <EOL> self . addManualSpacer ( ) <EOL> self . addManualButton ( '<STR_LIT>' , self . doorDemo . planPreChop ) <EOL> self . addManualButton ( '<STR_LIT>' , self . doorDemo . planChop ) <EOL> self . addManualButton ( '<STR_LIT>' , functools . partial ( self . doorDemo . planChop , deltaX = - <NUM_LIT:0.1> , deltaY = - <NUM_LIT:0.1> , deltaZ = <NUM_LIT:0.1> ) ) <EOL> self . addManualSpacer ( ) <EOL> self . addManualButton ( '<STR_LIT>' , functools . partial ( self . doorDemo . planHandleTurn , <NUM_LIT:10> ) ) <EOL> self . addManualButton ( '<STR_LIT>' , functools . partial ( self . doorDemo . planHandleTurn , - <NUM_LIT:10> ) ) <EOL> self . addManualButton ( '<STR_LIT>' , self . doorDemo . planDoorPushOpenTwist ) <EOL> self . addManualSpacer ( ) <EOL> self . addManualButton ( '<STR_LIT>' , self . doorDemo . commitManipPlan ) <EOL> self . addManualButton ( '<STR_LIT>' , self . doorDemo . stopPushing ) <EOL> def getSide ( self ) : <EOL> return self . params . getPropertyEnumValue ( '<STR_LIT>' ) . lower ( ) <EOL> def openPinch ( self ) : <EOL> rt . OpenHand ( side = self . getSide ( ) . capitalize ( ) , mode = '<STR_LIT>' ) . run ( ) <EOL> def closePinch ( self ) : <EOL> rt . CloseHand ( side = self . getSide ( ) . capitalize ( ) , mode = '<STR_LIT>' ) . run ( ) <EOL> def addDefaultProperties ( self ) : <EOL> self . params . addProperty ( '<STR_LIT>' , <NUM_LIT:0> , attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT>' , '<STR_LIT>' ] ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , self . doorDemo . preChopWidth , attributes = om . PropertyAttributes ( singleStep = <NUM_LIT> , decimals = <NUM_LIT:3> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , self . doorDemo . preChopDepth , attributes = om . PropertyAttributes ( singleStep = <NUM_LIT> , decimals = <NUM_LIT:3> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , self . doorDemo . preChopHeight , attributes = om . PropertyAttributes ( singleStep = <NUM_LIT> , decimals = <NUM_LIT:3> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , self . doorDemo . chopDistance , attributes = om . PropertyAttributes ( singleStep = <NUM_LIT> , decimals = <NUM_LIT:3> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , self . doorDemo . chopSidewaysDistance , attributes = om . PropertyAttributes ( singleStep = <NUM_LIT> , decimals = <NUM_LIT:3> ) ) <EOL> self . _syncProperties ( ) <EOL> def onPropertyChanged ( self , propertySet , propertyName ) : <EOL> if propertyName == '<STR_LIT>' : <EOL> self . taskTree . removeAllTasks ( ) <EOL> self . addTasks ( ) <EOL> self . doorDemo . findDoorHandleAffordance ( ) <EOL> self . _syncProperties ( ) <EOL> def _syncProperties ( self ) : <EOL> self . doorDemo . graspingHand = self . params . getPropertyEnumValue ( '<STR_LIT>' ) . lower ( ) <EOL> self . doorDemo . ikPlanner . reachingSide = self . doorDemo . graspingHand <EOL> if hasattr ( self . doorDemo , '<STR_LIT>' ) : <EOL> self . doorDemo . computeDoorHandleGraspFrame ( ) <EOL> self . doorDemo . chopDistance = self . params . getProperty ( '<STR_LIT>' ) <EOL> self . doorDemo . chopSidewaysDistance = self . params . getProperty ( '<STR_LIT>' ) <EOL> self . doorDemo . preChopWidth = self . params . getProperty ( '<STR_LIT>' ) <EOL> self . doorDemo . preChopDepth = self . params . getProperty ( '<STR_LIT>' ) <EOL> self . doorDemo . preChopHeight = self . params . getProperty ( '<STR_LIT>' ) <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 ( func , name , 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> d = self . doorDemo <EOL> self . taskTree . removeAllTasks ( ) <EOL> side = self . params . getPropertyEnumValue ( '<STR_LIT>' ) <EOL> folder = addFolder ( '<STR_LIT>' ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = '<STR_LIT>' ) ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = '<STR_LIT>' ) ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , message = '<STR_LIT>' ) ) <EOL> addTask ( rt . FindAffordance ( name = '<STR_LIT>' , affordanceName = '<STR_LIT>' ) ) <EOL> addTask ( rt . SetNeckPitch ( name = '<STR_LIT>' , angle = <NUM_LIT> ) ) <EOL> folder = addFolder ( '<STR_LIT>' ) <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> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , message = '<STR_LIT>' ) ) <EOL> addTask ( rt . OpenHand ( name = '<STR_LIT>' , side = side , mode = '<STR_LIT>' ) ) <EOL> def addManipTask ( name , planFunc , userPrompt = False ) : <EOL> folder = addFolder ( name ) <EOL> addFunc ( planFunc , name = '<STR_LIT>' ) <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 ( d . commitManipPlan , name = '<STR_LIT>' ) <EOL> addTask ( rt . WaitForManipulationPlanExecution ( name = '<STR_LIT>' ) ) <EOL> addManipTask ( '<STR_LIT>' , d . planPreReach , userPrompt = False ) <EOL> addManipTask ( '<STR_LIT>' , d . planDoorTouch , userPrompt = False ) <EOL> if d . usePinchGrasp : <EOL> addManipTask ( '<STR_LIT>' , d . planReach , userPrompt = True ) <EOL> addFunc ( self . closePinch , name = '<STR_LIT>' ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , <EOL> message = '<STR_LIT>' ) ) <EOL> addManipTask ( '<STR_LIT>' , d . planHandleTurn , userPrompt = False ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , <EOL> message = '<STR_LIT>' ) ) <EOL> else : <EOL> addFunc ( self . doorDemo . setChopParametersToDefaults , name = '<STR_LIT>' ) <EOL> addFunc ( self . closePinch , name = '<STR_LIT>' ) <EOL> addManipTask ( '<STR_LIT>' , d . planReach , userPrompt = True ) <EOL> addManipTask ( '<STR_LIT>' , d . planChop , userPrompt = True ) <EOL> addManipTask ( '<STR_LIT>' , d . planHandlePush , userPrompt = False ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , <EOL> message = '<STR_LIT>' ) ) <EOL> addManipTask ( '<STR_LIT>' , d . planHandlePush , userPrompt = False ) <EOL> if d . usePinchGrasp : <EOL> addManipTask ( '<STR_LIT>' , d . planHandlePushLift , userPrompt = False ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = side , mode = '<STR_LIT>' , amount = <NUM_LIT:0> ) ) <EOL> addManipTask ( '<STR_LIT>' , d . planDoorPushOpen , userPrompt = False ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , <EOL> message = '<STR_LIT>' ) ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = side ) ) <EOL> addManipTask ( '<STR_LIT>' , d . planTuckArms , userPrompt = False ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = side ) ) <EOL> folder = addFolder ( '<STR_LIT>' ) <EOL> addFunc ( d . planFootstepsThroughDoorManual , name = '<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> folder = addFolder ( '<STR_LIT>' ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = '<STR_LIT>' ) ) <EOL> addTask ( rt . CloseHand ( name = '<STR_LIT>' , side = '<STR_LIT>' ) ) <EOL> addTask ( rt . PlanPostureGoal ( name = '<STR_LIT>' , postureGroup = '<STR_LIT>' , postureName = '<STR_LIT>' , side = '<STR_LIT>' ) ) <EOL> addTask ( rt . UserPromptTask ( name = '<STR_LIT>' , message = '<STR_LIT>' ) ) <EOL> addTask ( rt . CommitManipulationPlan ( name = '<STR_LIT>' , planName = '<STR_LIT>' ) ) <EOL> addTask ( rt . WaitForManipulationPlanExecution ( name = '<STR_LIT>' ) ) </s>
