<s> from director import lcmUtils <EOL> from director import objectmodel as om <EOL> from director import visualization as vis <EOL> from director . utime import getUtime <EOL> from director import transformUtils <EOL> from director . debugVis import DebugData <EOL> from director import ioUtils <EOL> from director import robotstate <EOL> from director import applogic as app <EOL> from director import vtkAll as vtk <EOL> from director . lcmframe import frameFromPositionMessage , positionMessageFromFrame <EOL> from director . simpletimer import SimpleTimer <EOL> from director . shallowCopy import shallowCopy <EOL> from director import roboturdf <EOL> from director import filterUtils <EOL> import director . vtkNumpy as vnp <EOL> import os <EOL> import math <EOL> import numpy as np <EOL> from director import drcargs <EOL> import drc as lcmdrc <EOL> from bot_core . pose_t import pose_t <EOL> from bot_core . robot_state_t import robot_state_t <EOL> import functools <EOL> import json <EOL> from PythonQt import QtGui , QtCore <EOL> _footMeshes = None <EOL> _footMeshFiles = [ ] <EOL> _robotType = <NUM_LIT:0> <EOL> _pelvisLink = '<STR_LIT>' <EOL> _leftFootLink = '<STR_LIT>' <EOL> _rightFootLink = '<STR_LIT>' <EOL> with open ( drcargs . args ( ) . directorConfigFile ) as directorConfigFile : <EOL> directorConfig = json . load ( directorConfigFile ) <EOL> if ( directorConfigFile . name . find ( "<STR_LIT>" ) > - <NUM_LIT:1> ) : <EOL> _robotType = <NUM_LIT:1> <EOL> if ( directorConfigFile . name . find ( "<STR_LIT>" ) > - <NUM_LIT:1> ) : <EOL> _robotType = <NUM_LIT:2> <EOL> directorConfigDirectory = os . path . dirname ( os . path . abspath ( directorConfigFile . name ) ) <EOL> if '<STR_LIT>' in directorConfig : <EOL> _footMeshFiles . append ( directorConfig [ '<STR_LIT>' ] ) <EOL> _footMeshFiles . append ( directorConfig [ '<STR_LIT>' ] ) <EOL> for j in range ( <NUM_LIT:0> , <NUM_LIT:2> ) : <EOL> for i in range ( len ( _footMeshFiles [ j ] ) ) : <EOL> _footMeshFiles [ j ] [ i ] = os . path . join ( directorConfigDirectory , _footMeshFiles [ j ] [ i ] ) <EOL> if '<STR_LIT>' in directorConfig : <EOL> _pelvisLink = directorConfig [ '<STR_LIT>' ] <EOL> if '<STR_LIT>' in directorConfig : <EOL> _leftFootLink = directorConfig [ '<STR_LIT>' ] <EOL> _rightFootLink = directorConfig [ '<STR_LIT>' ] <EOL> DEFAULT_PARAM_SET = '<STR_LIT>' <EOL> DEFAULT_STEP_PARAMS = { '<STR_LIT>' : { '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT:12> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> } , <EOL> '<STR_LIT>' : { '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT:16> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT:2> , <EOL> '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT:1.0> , <EOL> '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> } , <EOL> '<STR_LIT>' : { '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT:16> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT:2> , <EOL> '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT:0> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> } } <EOL> DEFAULT_STEP_PARAMS [ '<STR_LIT>' ] = DEFAULT_STEP_PARAMS [ '<STR_LIT>' ] . copy ( ) <EOL> DEFAULT_STEP_PARAMS [ '<STR_LIT>' ] . update ( { '<STR_LIT>' : <NUM_LIT:1.0> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT:6> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT:1> } ) <EOL> DEFAULT_STEP_PARAMS [ '<STR_LIT>' ] = DEFAULT_STEP_PARAMS [ '<STR_LIT>' ] . copy ( ) <EOL> DEFAULT_STEP_PARAMS [ '<STR_LIT>' ] . update ( { '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT:8> , <EOL> '<STR_LIT>' : <NUM_LIT:8> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : lcmdrc . footstep_params_t . SUPPORT_GROUPS_MIDFOOT_TOE , <EOL> '<STR_LIT>' : <NUM_LIT:2> } ) <EOL> DEFAULT_STEP_PARAMS [ '<STR_LIT>' ] = DEFAULT_STEP_PARAMS [ '<STR_LIT>' ] . copy ( ) <EOL> DEFAULT_STEP_PARAMS [ '<STR_LIT>' ] . update ( { '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT:1> , <EOL> '<STR_LIT>' : <NUM_LIT> , <EOL> '<STR_LIT>' : <NUM_LIT:1> } ) <EOL> DEFAULT_CONTACT_SLICES = { ( <NUM_LIT> , <NUM_LIT> ) : np . array ( [ [ - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] , <EOL> [ <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] ] ) , <EOL> ( <NUM_LIT> , <NUM_LIT> ) : np . array ( [ [ - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] , <EOL> [ <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] ] ) , <EOL> ( <NUM_LIT> , <NUM_LIT> ) : np . array ( [ [ - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] , <EOL> [ <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] ] ) , <EOL> ( <NUM_LIT> , <NUM_LIT> ) : np . array ( [ [ - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] , <EOL> [ <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] ] ) <EOL> } <EOL> def loadFootMeshes ( ) : <EOL> meshes = [ ] <EOL> for i in range ( <NUM_LIT:0> , <NUM_LIT:2> ) : <EOL> d = DebugData ( ) <EOL> for footMeshFile in _footMeshFiles [ i ] : <EOL> d . addPolyData ( ioUtils . readPolyData ( footMeshFile , computeNormals = True ) ) <EOL> t = vtk . vtkTransform ( ) <EOL> t . Scale ( <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ) <EOL> pd = filterUtils . transformPolyData ( d . getPolyData ( ) , t ) <EOL> meshes . append ( pd ) <EOL> return meshes <EOL> def getLeftFootMesh ( ) : <EOL> return shallowCopy ( getFootMeshes ( ) [ <NUM_LIT:0> ] ) <EOL> def getRightFootMesh ( ) : <EOL> return shallowCopy ( getFootMeshes ( ) [ <NUM_LIT:1> ] ) <EOL> def getLeftFootColor ( ) : <EOL> return [ <NUM_LIT:1.0> , <NUM_LIT:1.0> , <NUM_LIT:0.0> ] <EOL> def getRightFootColor ( ) : <EOL> return [ <NUM_LIT> , <NUM_LIT:1.0> , <NUM_LIT:0.0> ] <EOL> def getFootMeshes ( ) : <EOL> global _footMeshes <EOL> if not _footMeshes : <EOL> _footMeshes = loadFootMeshes ( ) <EOL> return _footMeshes <EOL> def getFootstepsFolder ( ) : <EOL> obj = om . findObjectByName ( '<STR_LIT>' ) <EOL> if obj is None : <EOL> obj = om . getOrCreateContainer ( '<STR_LIT>' , parentObj = om . getOrCreateContainer ( '<STR_LIT>' ) ) <EOL> obj . setIcon ( om . Icons . Feet ) <EOL> om . collapse ( obj ) <EOL> return obj <EOL> def getWalkingVolumesFolder ( ) : <EOL> obj = om . findObjectByName ( '<STR_LIT>' ) <EOL> if obj is None : <EOL> obj = om . getOrCreateContainer ( '<STR_LIT>' , parentObj = getFootstepsFolder ( ) ) <EOL> om . collapse ( obj ) <EOL> return obj <EOL> def getTerrainSlicesFolder ( ) : <EOL> obj = om . findObjectByName ( '<STR_LIT>' ) <EOL> if obj is None : <EOL> obj = om . getOrCreateContainer ( '<STR_LIT>' , parentObj = getFootstepsFolder ( ) ) <EOL> obj . setProperty ( '<STR_LIT>' , False ) <EOL> om . collapse ( obj ) <EOL> return obj <EOL> def getBDIAdjustedFootstepsFolder ( ) : <EOL> obj = om . findObjectByName ( '<STR_LIT>' ) <EOL> if obj is None : <EOL> obj = om . getOrCreateContainer ( '<STR_LIT>' ) <EOL> obj . setIcon ( om . Icons . Feet ) <EOL> om . collapse ( obj ) <EOL> return obj <EOL> class FootstepPlanItem ( om . ObjectModelItem ) : <EOL> pass <EOL> class WalkingPlanItem ( om . ObjectModelItem ) : <EOL> pass <EOL> class FootstepsDriver ( object ) : <EOL> def __init__ ( self , jointController ) : <EOL> self . jointController = jointController <EOL> self . lastFootstepPlan = None <EOL> self . lastFootstepRequest = None <EOL> self . goalSteps = None <EOL> self . lastWalkingPlan = None <EOL> self . walkingPlanCallback = None <EOL> self . default_step_params = DEFAULT_STEP_PARAMS <EOL> self . contact_slices = DEFAULT_CONTACT_SLICES <EOL> self . show_contact_slices = False <EOL> self . toolbarWidget = None <EOL> self . pose_bdi = None <EOL> self . bdi_plan = None <EOL> self . bdi_plan_adjusted = None <EOL> view = app . getDRCView ( ) <EOL> self . bdiRobotModel , self . bdiJointController = roboturdf . loadRobotModel ( '<STR_LIT>' , view , parent = '<STR_LIT>' , color = roboturdf . getRobotOrangeColor ( ) , visible = False ) <EOL> self . bdiRobotModel . setProperty ( '<STR_LIT>' , False ) <EOL> self . showBDIPlan = False <EOL> self . bdiChannel = "<STR_LIT>" <EOL> self . bdiSubcribe = None <EOL> self . _setupSubscriptions ( ) <EOL> self . _setupProperties ( ) <EOL> self . showToolbarWidget ( ) <EOL> if hasattr ( self , '<STR_LIT>' ) : <EOL> self . execButton . setEnabled ( False ) <EOL> self . committedPlans = [ ] <EOL> def _setupProperties ( self ) : <EOL> self . params = om . ObjectModelItem ( '<STR_LIT>' ) <EOL> self . defaults_map = [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] <EOL> self . params . addProperty ( '<STR_LIT>' , <NUM_LIT:0> , attributes = om . PropertyAttributes ( enumNames = self . defaults_map ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , <NUM_LIT:0> , attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , <NUM_LIT:1> , attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] ) ) <EOL> self . leading_foot_map = [ lcmdrc . footstep_plan_params_t . LEAD_AUTO , <EOL> lcmdrc . footstep_plan_params_t . LEAD_LEFT , <EOL> lcmdrc . footstep_plan_params_t . LEAD_RIGHT ] <EOL> self . params . addProperty ( '<STR_LIT>' , <NUM_LIT:0> , attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] ) ) <EOL> self . map_mode_map = [ <EOL> lcmdrc . footstep_plan_params_t . FOOT_PLANE , <EOL> lcmdrc . footstep_plan_params_t . TERRAIN_HEIGHTS_AND_NORMALS , <EOL> lcmdrc . footstep_plan_params_t . TERRAIN_HEIGHTS_Z_NORMALS , <EOL> lcmdrc . footstep_plan_params_t . HORIZONTAL_PLANE <EOL> ] <EOL> self . params . addProperty ( '<STR_LIT>' , None , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:0> , minimum = <NUM_LIT:0> , maximum = <NUM_LIT:30> , singleStep = <NUM_LIT:1> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , None , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:0> , minimum = <NUM_LIT:1> , maximum = <NUM_LIT:30> , singleStep = <NUM_LIT:1> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , None , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:2> , minimum = <NUM_LIT:0.1> , maximum = <NUM_LIT> , singleStep = <NUM_LIT> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , None , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:2> , minimum = <NUM_LIT> , maximum = <NUM_LIT> , singleStep = <NUM_LIT> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , None , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:2> , minimum = <NUM_LIT> , maximum = <NUM_LIT:0.5> , singleStep = <NUM_LIT> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , None , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:2> , minimum = <NUM_LIT:0> , maximum = <NUM_LIT:0.5> , singleStep = <NUM_LIT> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , None , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:2> , minimum = <NUM_LIT:0> , maximum = <NUM_LIT:0.5> , singleStep = <NUM_LIT> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , None , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:2> , minimum = <NUM_LIT:0> , maximum = <NUM_LIT:0.5> , singleStep = <NUM_LIT> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , None , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:2> , minimum = <NUM_LIT:0> , maximum = <NUM_LIT:0.5> , singleStep = <NUM_LIT> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , None , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:2> , minimum = <NUM_LIT:0> , maximum = <NUM_LIT:0.5> , singleStep = <NUM_LIT> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , None , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:2> , minimum = <NUM_LIT> , maximum = <NUM_LIT> , singleStep = <NUM_LIT> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , None , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:2> , minimum = <NUM_LIT:0> , maximum = <NUM_LIT> , singleStep = <NUM_LIT> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , None , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:4> , minimum = - <NUM_LIT> , maximum = <NUM_LIT> , singleStep = <NUM_LIT> ) ) <EOL> self . behavior_lcm_map = { <EOL> <NUM_LIT:0> : lcmdrc . footstep_plan_params_t . BEHAVIOR_BDI_STEPPING , <EOL> <NUM_LIT:1> : lcmdrc . footstep_plan_params_t . BEHAVIOR_BDI_WALKING , <EOL> <NUM_LIT:2> : lcmdrc . footstep_plan_params_t . BEHAVIOR_WALKING } <EOL> self . params . addProperty ( '<STR_LIT>' , <NUM_LIT:0> , attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT>' , '<STR_LIT>' ] ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , <NUM_LIT:0> , attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , <NUM_LIT:0> , attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT:False>' , '<STR_LIT:True>' ] ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , <NUM_LIT:0> , attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT:False>' , '<STR_LIT:True>' ] ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , None , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:2> , minimum = <NUM_LIT> , maximum = <NUM_LIT> , singleStep = <NUM_LIT> ) ) <EOL> self . params . addProperty ( '<STR_LIT>' , None , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:2> , minimum = <NUM_LIT> , maximum = <NUM_LIT> , singleStep = <NUM_LIT> ) ) <EOL> self . applyDefaults ( DEFAULT_PARAM_SET ) <EOL> def applyDefaults ( self , set_name ) : <EOL> defaults = self . default_step_params [ set_name ] <EOL> for k , v in defaults . iteritems ( ) : <EOL> self . params . setProperty ( k , v ) <EOL> def _setupSubscriptions ( self ) : <EOL> useHistoricalLoader = False <EOL> historicalLoader = lcmUtils . HistoricalLCMLoader ( '<STR_LIT>' , '<STR_LIT>' , os . getenv ( '<STR_LIT>' ) ) if useHistoricalLoader else None <EOL> lcmUtils . addSubscriber ( '<STR_LIT>' , lcmdrc . footstep_plan_t , self . onFootstepPlan , historicalLoader ) <EOL> lcmUtils . addSubscriber ( '<STR_LIT>' , lcmdrc . robot_plan_t , self . onWalkingPlan ) <EOL> lcmUtils . addSubscriber ( '<STR_LIT>' , lcmdrc . robot_plan_t , self . onWalkingPlan ) <EOL> self . bdiSubcribe = lcmUtils . addSubscriber ( self . bdiChannel , pose_t , self . onPoseBDI ) <EOL> self . bdiSubcribe . setSpeedLimit ( <NUM_LIT> ) <EOL> sub2 = lcmUtils . addSubscriber ( '<STR_LIT>' , lcmdrc . footstep_plan_t , self . onBDIAdjustedFootstepPlan ) <EOL> sub2 . setSpeedLimit ( <NUM_LIT:1> ) <EOL> def changeSubscriptionBDI ( self , newBDIChannel = "<STR_LIT>" ) : <EOL> self . bdiChannel = newBDIChannel <EOL> lcmUtils . removeSubscriber ( self . bdiSubcribe ) <EOL> self . bdiSubcribe = lcmUtils . addSubscriber ( self . bdiChannel , pose_t , self . onPoseBDI ) <EOL> self . bdiSubcribe . setSpeedLimit ( <NUM_LIT> ) <EOL> def getDefaultStepParams ( self ) : <EOL> default_step_params = lcmdrc . footstep_params_t ( ) <EOL> default_step_params . step_speed = self . params . properties . drake_swing_speed <EOL> default_step_params . drake_min_hold_time = self . params . properties . drake_min_hold_time <EOL> default_step_params . drake_instep_shift = self . params . properties . drake_instep_shift <EOL> default_step_params . step_height = self . params . properties . swing_height <EOL> default_step_params . constrain_full_foot_pose = True <EOL> default_step_params . bdi_step_duration = <NUM_LIT> <EOL> default_step_params . bdi_sway_duration = <NUM_LIT:0.0> <EOL> default_step_params . bdi_lift_height = <NUM_LIT> <EOL> default_step_params . bdi_toe_off = <NUM_LIT:1> <EOL> default_step_params . bdi_knee_nominal = <NUM_LIT:0.0> <EOL> default_step_params . bdi_max_foot_vel = <NUM_LIT:0.0> <EOL> default_step_params . bdi_sway_end_dist = <NUM_LIT> <EOL> default_step_params . bdi_step_end_dist = <NUM_LIT> <EOL> default_step_params . mu = <NUM_LIT:1.0> <EOL> default_step_params . ihmc_transfer_time = self . params . properties . ihmc_transfer_time <EOL> default_step_params . ihmc_swing_time = self . params . properties . ihmc_swing_time <EOL> default_step_params . support_contact_groups = self . params . properties . support_contact_groups <EOL> default_step_params . prevent_swing_undershoot = self . params . properties . prevent_swing_undershoot <EOL> default_step_params . prevent_swing_overshoot = self . params . properties . prevent_swing_overshoot <EOL> return default_step_params <EOL> def onWalkingPlan ( self , msg ) : <EOL> self . lastWalkingPlan = msg <EOL> if self . walkingPlanCallback : <EOL> self . walkingPlanCallback ( self . lastWalkingPlan ) <EOL> def onBDIAdjustedFootstepPlan ( self , msg ) : <EOL> folder = getBDIAdjustedFootstepsFolder ( ) <EOL> om . removeFromObjectModel ( folder ) <EOL> folder = getBDIAdjustedFootstepsFolder ( ) <EOL> self . drawFootstepPlan ( msg , folder ) <EOL> def onFootstepPlan ( self , msg ) : <EOL> self . lastFootstepPlan = msg <EOL> planFolder = getFootstepsFolder ( ) <EOL> self . drawFootstepPlan ( self . lastFootstepPlan , planFolder ) <EOL> self . transformPlanToBDIFrame ( self . lastFootstepPlan ) <EOL> self . showToolbarWidget ( ) <EOL> def showToolbarWidget ( self ) : <EOL> if app . getMainWindow ( ) is None : <EOL> return <EOL> if self . toolbarWidget : <EOL> self . execButton . setEnabled ( True ) <EOL> return <EOL> w = QtGui . QWidget ( ) <EOL> l = QtGui . QHBoxLayout ( w ) <EOL> label = QtGui . QLabel ( '<STR_LIT>' ) <EOL> execButton = QtGui . QPushButton ( '<STR_LIT>' ) <EOL> execButton . setIcon ( QtGui . QApplication . style ( ) . standardIcon ( QtGui . QStyle . SP_MediaPlay ) ) <EOL> clearButton = QtGui . QPushButton ( '<STR_LIT>' ) <EOL> clearButton . setIcon ( QtGui . QApplication . style ( ) . standardIcon ( QtGui . QStyle . SP_TrashIcon ) ) <EOL> stopButton = QtGui . QPushButton ( '<STR_LIT>' ) <EOL> stopButton . setIcon ( QtGui . QApplication . style ( ) . standardIcon ( QtGui . QStyle . SP_MediaStop ) ) <EOL> l . addWidget ( label ) <EOL> l . addWidget ( execButton ) <EOL> l . addWidget ( stopButton ) <EOL> l . addWidget ( clearButton ) <EOL> l . setContentsMargins ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ) <EOL> execButton . setShortcut ( QtGui . QKeySequence ( '<STR_LIT>' ) ) <EOL> execButton . connect ( '<STR_LIT>' , self . onExecClicked ) <EOL> clearButton . connect ( '<STR_LIT>' , self . onClearClicked ) <EOL> stopButton . connect ( '<STR_LIT>' , self . sendStopWalking ) <EOL> self . execButton = execButton <EOL> self . stopButton = stopButton <EOL> self . toolbarWidget = app . getMainWindow ( ) . toolBar ( ) . addWidget ( w ) <EOL> self . execButton . show ( ) <EOL> def onExecClicked ( self ) : <EOL> self . commitFootstepPlan ( self . lastFootstepPlan ) <EOL> om . removeFromObjectModel ( om . findObjectByName ( '<STR_LIT>' ) ) <EOL> walkGoal = om . findObjectByName ( '<STR_LIT>' ) <EOL> if walkGoal : <EOL> walkGoal . setProperty ( '<STR_LIT>' , False ) <EOL> self . execButton . setEnabled ( False ) <EOL> def onClearClicked ( self ) : <EOL> om . removeFromObjectModel ( om . findObjectByName ( '<STR_LIT>' ) ) <EOL> om . removeFromObjectModel ( om . findObjectByName ( '<STR_LIT>' ) ) <EOL> om . removeFromObjectModel ( om . findObjectByName ( '<STR_LIT>' ) ) <EOL> self . clearFootstepPlan ( ) <EOL> if self . toolbarWidget : <EOL> self . execButton . setEnabled ( False ) <EOL> def clearFootstepPlan ( self ) : <EOL> self . lastFootstepPlan = None <EOL> om . removeFromObjectModel ( getFootstepsFolder ( ) ) <EOL> def drawFootstepPlan ( self , msg , folder , left_color = None , right_color = None , alpha = <NUM_LIT:1.0> ) : <EOL> for step in folder . children ( ) : <EOL> om . removeFromObjectModel ( step ) <EOL> allTransforms = [ ] <EOL> volFolder = getWalkingVolumesFolder ( ) <EOL> map ( om . removeFromObjectModel , volFolder . children ( ) ) <EOL> slicesFolder = getTerrainSlicesFolder ( ) <EOL> map ( om . removeFromObjectModel , slicesFolder . children ( ) ) <EOL> for i , footstep in enumerate ( msg . footsteps ) : <EOL> trans = footstep . pos . translation <EOL> trans = [ trans . x , trans . y , trans . z ] <EOL> quat = footstep . pos . rotation <EOL> quat = [ quat . w , quat . x , quat . y , quat . z ] <EOL> footstepTransform = transformUtils . transformFromPose ( trans , quat ) <EOL> allTransforms . append ( footstepTransform ) <EOL> if i < <NUM_LIT:2> : <EOL> continue <EOL> if footstep . is_right_foot : <EOL> mesh = getRightFootMesh ( ) <EOL> if ( right_color is None ) : <EOL> color = getRightFootColor ( ) <EOL> else : <EOL> color = right_color <EOL> else : <EOL> mesh = getLeftFootMesh ( ) <EOL> if ( left_color is None ) : <EOL> color = getLeftFootColor ( ) <EOL> else : <EOL> color = left_color <EOL> frac = float ( i ) / float ( msg . num_steps - <NUM_LIT:1> ) <EOL> this_color = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] <EOL> this_color [ <NUM_LIT:0> ] = <NUM_LIT> * color [ <NUM_LIT:0> ] + <NUM_LIT> * frac * color [ <NUM_LIT:0> ] <EOL> this_color [ <NUM_LIT:1> ] = <NUM_LIT> * color [ <NUM_LIT:1> ] + <NUM_LIT> * frac * color [ <NUM_LIT:1> ] <EOL> this_color [ <NUM_LIT:2> ] = <NUM_LIT> * color [ <NUM_LIT:2> ] + <NUM_LIT> * frac * color [ <NUM_LIT:2> ] <EOL> if self . show_contact_slices : <EOL> self . drawContactVolumes ( footstepTransform , color ) <EOL> contact_pts_left , contact_pts_right = FootstepsDriver . getContactPts ( ) <EOL> if footstep . is_right_foot : <EOL> sole_offset = np . mean ( contact_pts_right , axis = <NUM_LIT:0> ) <EOL> else : <EOL> sole_offset = np . mean ( contact_pts_left , axis = <NUM_LIT:0> ) <EOL> t_sole_prev = frameFromPositionMessage ( msg . footsteps [ i - <NUM_LIT:2> ] . pos ) <EOL> t_sole_prev . PreMultiply ( ) <EOL> t_sole_prev . Translate ( sole_offset ) <EOL> t_sole = transformUtils . copyFrame ( footstepTransform ) <EOL> t_sole . Translate ( sole_offset ) <EOL> yaw = np . arctan2 ( t_sole . GetPosition ( ) [ <NUM_LIT:1> ] - t_sole_prev . GetPosition ( ) [ <NUM_LIT:1> ] , <EOL> t_sole . GetPosition ( ) [ <NUM_LIT:0> ] - t_sole_prev . GetPosition ( ) [ <NUM_LIT:0> ] ) <EOL> T_terrain_to_world = transformUtils . frameFromPositionAndRPY ( [ t_sole_prev . GetPosition ( ) [ <NUM_LIT:0> ] , t_sole_prev . GetPosition ( ) [ <NUM_LIT:1> ] , <NUM_LIT:0> ] , <EOL> [ <NUM_LIT:0> , <NUM_LIT:0> , math . degrees ( yaw ) ] ) <EOL> path_dist = np . array ( footstep . terrain_path_dist ) <EOL> height = np . array ( footstep . terrain_height ) <EOL> terrain_pts_in_local = np . vstack ( ( path_dist , np . zeros ( len ( footstep . terrain_path_dist ) ) , height ) ) <EOL> d = DebugData ( ) <EOL> for j in range ( terrain_pts_in_local . shape [ <NUM_LIT:1> ] - <NUM_LIT:1> ) : <EOL> d . addLine ( terrain_pts_in_local [ : , j ] , terrain_pts_in_local [ : , j + <NUM_LIT:1> ] , radius = <NUM_LIT> ) <EOL> obj = vis . showPolyData ( d . getPolyData ( ) , '<STR_LIT>' , parent = slicesFolder , visible = slicesFolder . getProperty ( '<STR_LIT>' ) , color = [ <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] ) <EOL> obj . actor . SetUserTransform ( T_terrain_to_world ) <EOL> renderInfeasibility = False <EOL> if renderInfeasibility and footstep . infeasibility > <NUM_LIT> : <EOL> d = DebugData ( ) <EOL> start = allTransforms [ i - <NUM_LIT:1> ] . GetPosition ( ) <EOL> end = footstepTransform . GetPosition ( ) <EOL> d . addArrow ( start , end , <NUM_LIT> , <NUM_LIT> , <EOL> startHead = True , <EOL> endHead = True ) <EOL> vis . showPolyData ( d . getPolyData ( ) , '<STR_LIT>' % ( i - <NUM_LIT:2> , i - <NUM_LIT:1> ) , parent = folder , color = [ <NUM_LIT:1> , <NUM_LIT> , <NUM_LIT> ] ) <EOL> stepName = '<STR_LIT>' % ( i - <NUM_LIT:1> ) <EOL> obj = vis . showPolyData ( mesh , stepName , color = this_color , alpha = alpha , parent = folder ) <EOL> obj . setIcon ( om . Icons . Feet ) <EOL> frameObj = vis . showFrame ( footstepTransform , stepName + '<STR_LIT>' , parent = obj , scale = <NUM_LIT> , visible = False ) <EOL> obj . actor . SetUserTransform ( footstepTransform ) <EOL> obj . addProperty ( '<STR_LIT>' , footstep . params . support_contact_groups , attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] ) ) <EOL> obj . properties . setPropertyIndex ( '<STR_LIT>' , <NUM_LIT:0> ) <EOL> obj . footstep_index = i <EOL> obj . footstep_property_callback = obj . properties . connectPropertyChanged ( functools . partial ( self . onFootstepPropertyChanged , obj ) ) <EOL> self . drawContactPts ( obj , footstep , color = this_color ) <EOL> def drawContactVolumes ( self , footstepTransform , color ) : <EOL> volFolder = getWalkingVolumesFolder ( ) <EOL> for zs , xy in self . contact_slices . iteritems ( ) : <EOL> points0 = np . vstack ( ( xy , zs [ <NUM_LIT:0> ] + np . zeros ( ( <NUM_LIT:1> , xy . shape [ <NUM_LIT:1> ] ) ) ) ) <EOL> points1 = np . vstack ( ( xy , zs [ <NUM_LIT:1> ] + np . zeros ( ( <NUM_LIT:1> , xy . shape [ <NUM_LIT:1> ] ) ) ) ) <EOL> points = np . hstack ( ( points0 , points1 ) ) <EOL> points = points + np . array ( [ [ <NUM_LIT> ] , [ <NUM_LIT:0> ] , [ - <NUM_LIT> ] ] ) <EOL> points = points . T <EOL> polyData = vnp . getVtkPolyDataFromNumpyPoints ( points . copy ( ) ) <EOL> vol_mesh = filterUtils . computeDelaunay3D ( polyData ) <EOL> obj = vis . showPolyData ( vol_mesh , '<STR_LIT>' , parent = volFolder , alpha = <NUM_LIT:0.5> , visible = self . show_contact_slices , color = color ) <EOL> obj . actor . SetUserTransform ( footstepTransform ) <EOL> def onFootstepPropertyChanged ( self , obj , propertySet , propertyName ) : <EOL> if propertyName == "<STR_LIT>" : <EOL> self . lastFootstepPlan . footsteps [ obj . footstep_index ] . params . support_contact_groups = obj . properties . support_contact_groups <EOL> self . sendUpdatePlanRequest ( ) <EOL> def drawContactPts ( self , obj , footstep , ** kwargs ) : <EOL> leftPoints , rightPoints = FootstepsDriver . getContactPts ( footstep . params . support_contact_groups ) <EOL> contact_pts = rightPoints if footstep . is_right_foot else leftPoints <EOL> d = DebugData ( ) <EOL> for pt in contact_pts : <EOL> d . addSphere ( pt , radius = <NUM_LIT> ) <EOL> d_obj = vis . showPolyData ( d . getPolyData ( ) , "<STR_LIT>" , parent = obj , ** kwargs ) <EOL> d_obj . actor . SetUserTransform ( obj . actor . GetUserTransform ( ) ) <EOL> @ staticmethod <EOL> def getContactPts ( support_contact_groups = lcmdrc . footstep_params_t . SUPPORT_GROUPS_HEEL_TOE ) : <EOL> '''<STR_LIT>''' <EOL> contact_pts_left = np . zeros ( ( <NUM_LIT:4> , <NUM_LIT:3> ) ) <EOL> contact_pts_right = np . zeros ( ( <NUM_LIT:4> , <NUM_LIT:3> ) ) <EOL> if ( _robotType == <NUM_LIT:0> ) : <EOL> if support_contact_groups == lcmdrc . footstep_params_t . SUPPORT_GROUPS_HEEL_TOE : <EOL> contact_pts_left [ <NUM_LIT:0> , : ] = [ - <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:1> , : ] = [ - <NUM_LIT> , - <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:2> , : ] = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:3> , : ] = [ <NUM_LIT> , - <NUM_LIT> , - <NUM_LIT> ] <EOL> elif support_contact_groups == lcmdrc . footstep_params_t . SUPPORT_GROUPS_MIDFOOT_TOE : <EOL> contact_pts_left [ <NUM_LIT:0> , : ] = [ - <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:1> , : ] = [ - <NUM_LIT> , - <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:2> , : ] = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:3> , : ] = [ <NUM_LIT> , - <NUM_LIT> , - <NUM_LIT> ] <EOL> elif support_contact_groups == lcmdrc . footstep_params_t . SUPPORT_GROUPS_HEEL_MIDFOOT : <EOL> contact_pts_left [ <NUM_LIT:0> , : ] = [ - <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:1> , : ] = [ - <NUM_LIT> , - <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:2> , : ] = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:3> , : ] = [ <NUM_LIT> , - <NUM_LIT> , - <NUM_LIT> ] <EOL> else : <EOL> raise ValueError ( "<STR_LIT>" . format ( support_contact_groups ) ) <EOL> contact_pts_right = contact_pts_left . copy ( ) <EOL> if ( _robotType == <NUM_LIT:1> ) : <EOL> contact_pts_left [ <NUM_LIT:0> , : ] = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:1> , : ] = [ <NUM_LIT> , - <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:2> , : ] = [ <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:3> , : ] = [ <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> ] <EOL> contact_pts_right [ <NUM_LIT:0> , : ] = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_right [ <NUM_LIT:1> , : ] = [ <NUM_LIT> , - <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_right [ <NUM_LIT:2> , : ] = [ <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> contact_pts_right [ <NUM_LIT:3> , : ] = [ <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> ] <EOL> if ( _robotType == <NUM_LIT:2> ) : <EOL> if support_contact_groups == lcmdrc . footstep_params_t . SUPPORT_GROUPS_HEEL_TOE : <EOL> contact_pts_left [ <NUM_LIT:0> , : ] = [ - <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:1> , : ] = [ - <NUM_LIT> , - <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:2> , : ] = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:3> , : ] = [ <NUM_LIT> , - <NUM_LIT> , - <NUM_LIT> ] <EOL> elif support_contact_groups == lcmdrc . footstep_params_t . SUPPORT_GROUPS_MIDFOOT_TOE : <EOL> contact_pts_left [ <NUM_LIT:0> , : ] = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:1> , : ] = [ <NUM_LIT> , - <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:2> , : ] = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:3> , : ] = [ <NUM_LIT> , - <NUM_LIT> , - <NUM_LIT> ] <EOL> elif support_contact_groups == lcmdrc . footstep_params_t . SUPPORT_GROUPS_HEEL_MIDFOOT : <EOL> contact_pts_left [ <NUM_LIT:0> , : ] = [ - <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:1> , : ] = [ - <NUM_LIT> , - <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:2> , : ] = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> ] <EOL> contact_pts_left [ <NUM_LIT:3> , : ] = [ <NUM_LIT> , - <NUM_LIT> , - <NUM_LIT> ] <EOL> else : <EOL> raise ValueError ( "<STR_LIT>" . format ( support_contact_groups ) ) <EOL> contact_pts_right = contact_pts_left . copy ( ) <EOL> return contact_pts_left , contact_pts_right <EOL> @ staticmethod <EOL> def getFeetMidPoint ( model , useWorldZ = True ) : <EOL> '''<STR_LIT>''' <EOL> contact_pts_left , contact_pts_right = FootstepsDriver . getContactPts ( ) <EOL> contact_pts_mid_left = np . mean ( contact_pts_left , axis = <NUM_LIT:0> ) <EOL> contact_pts_mid_right = np . mean ( contact_pts_right , axis = <NUM_LIT:0> ) <EOL> t_lf_mid = model . getLinkFrame ( _leftFootLink ) <EOL> t_lf_mid . PreMultiply ( ) <EOL> t_lf_mid . Translate ( contact_pts_mid_left ) <EOL> t_rf_mid = model . getLinkFrame ( _rightFootLink ) <EOL> t_rf_mid . PreMultiply ( ) <EOL> t_rf_mid . Translate ( contact_pts_mid_right ) <EOL> if ( _robotType == <NUM_LIT:0> ) : <EOL> t_feet_mid = transformUtils . frameInterpolate ( t_lf_mid , t_rf_mid , <NUM_LIT:0.5> ) <EOL> elif ( _robotType == <NUM_LIT:1> ) : <EOL> l_foot_sole = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] , [ <NUM_LIT> , <NUM_LIT> , <NUM_LIT:0> ] ) <EOL> l_foot_sole . PostMultiply ( ) <EOL> l_foot_sole . Concatenate ( t_lf_mid ) <EOL> r_foot_sole = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] , [ <NUM_LIT:0.0> , - <NUM_LIT> , <NUM_LIT:0> ] ) <EOL> r_foot_sole . PostMultiply ( ) <EOL> r_foot_sole . Concatenate ( t_rf_mid ) <EOL> t_feet_mid = transformUtils . frameInterpolate ( l_foot_sole , r_foot_sole , <NUM_LIT:0.5> ) <EOL> elif ( _robotType == <NUM_LIT:2> ) : <EOL> t_feet_mid = transformUtils . frameInterpolate ( t_lf_mid , t_rf_mid , <NUM_LIT:0.5> ) <EOL> if useWorldZ : <EOL> rpy = [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , np . degrees ( transformUtils . rollPitchYawFromTransform ( t_feet_mid ) [ <NUM_LIT:2> ] ) ] <EOL> return transformUtils . frameFromPositionAndRPY ( t_feet_mid . GetPosition ( ) , rpy ) <EOL> else : <EOL> return t_feet_mid <EOL> @ staticmethod <EOL> def debugDrawFootPoints ( model ) : <EOL> pts_left , pts_right = FootstepsDriver . getContactPts ( ) <EOL> d = DebugData ( ) <EOL> for linkName in [ _leftFootLink , _rightFootLink ] : <EOL> t = model . getLinkFrame ( linkName ) <EOL> d . addFrame ( t , scale = <NUM_LIT> ) <EOL> if ( linkName is _leftFootLink ) : <EOL> pts = pts_left <EOL> else : <EOL> pts = pts_right <EOL> footMidPoint = np . mean ( pts , axis = <NUM_LIT:0> ) <EOL> for p in pts . tolist ( ) + [ footMidPoint . tolist ( ) ] : <EOL> t . TransformPoint ( p , p ) <EOL> d . addSphere ( p , radius = <NUM_LIT> ) <EOL> midpt = FootstepsDriver . getFeetMidPoint ( model ) <EOL> d . addFrame ( midpt , scale = <NUM_LIT> ) <EOL> vis . showPolyData ( d . getPolyData ( ) , '<STR_LIT>' , parent = '<STR_LIT>' , colorByName = '<STR_LIT>' ) <EOL> def createGoalSteps ( self , model , pose ) : <EOL> distanceForward = <NUM_LIT:1.0> <EOL> fr = model . getLinkFrame ( _leftFootLink ) <EOL> fl = model . getLinkFrame ( _rightFootLink ) <EOL> pelvisT = model . getLinkFrame ( _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> xaxis = np . cross ( yaxis , zaxis ) <EOL> numGoalSteps = <NUM_LIT:3> <EOL> is_right_foot = True <EOL> self . goalSteps = [ ] <EOL> for i in range ( numGoalSteps ) : <EOL> t = transformUtils . getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> if is_right_foot : <EOL> t . Translate ( fr . GetPosition ( ) ) <EOL> else : <EOL> t . Translate ( fl . GetPosition ( ) ) <EOL> t . Translate ( xaxis * distanceForward ) <EOL> distanceForward += <NUM_LIT> <EOL> is_right_foot = not is_right_foot <EOL> step = lcmdrc . footstep_t ( ) <EOL> step . pos = positionMessageFromFrame ( t ) <EOL> step . is_right_foot = is_right_foot <EOL> step . params = self . getDefaultStepParams ( ) <EOL> self . goalSteps . append ( step ) <EOL> request = self . constructFootstepPlanRequest ( pose ) <EOL> request . num_goal_steps = len ( self . goalSteps ) <EOL> request . goal_steps = self . goalSteps <EOL> self . sendFootstepPlanRequest ( request ) <EOL> def onStepModified ( self , ndx , frameObj ) : <EOL> self . lastFootstepPlan . footsteps [ ndx + <NUM_LIT:2> ] . pos = positionMessageFromFrame ( frameObj . transform ) <EOL> self . lastFootstepPlan . footsteps [ ndx + <NUM_LIT:2> ] . fixed_x = True <EOL> self . lastFootstepPlan . footsteps [ ndx + <NUM_LIT:2> ] . fixed_y = True <EOL> self . lastFootstepPlan . footsteps [ ndx + <NUM_LIT:2> ] . fixed_yaw = True <EOL> self . sendUpdatePlanRequest ( ) <EOL> def sendUpdatePlanRequest ( self ) : <EOL> msg = lcmdrc . footstep_check_request_t ( ) <EOL> msg . initial_state = self . lastFootstepRequest . initial_state <EOL> msg . footstep_plan = self . lastFootstepPlan <EOL> msg . snap_to_terrain = True <EOL> msg . compute_infeasibility = False <EOL> self . sendFootstepPlanCheckRequest ( msg ) <EOL> def updateRequest ( self ) : <EOL> if self . lastFootstepRequest is not None : <EOL> msg = self . lastFootstepRequest <EOL> msg = self . applyParams ( msg ) <EOL> self . sendFootstepPlanRequest ( msg ) <EOL> def constructFootstepPlanRequest ( self , pose , goalFrame = None ) : <EOL> msg = lcmdrc . footstep_plan_request_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> state_msg = robotstate . drakePoseToRobotState ( pose ) <EOL> msg . initial_state = state_msg <EOL> if goalFrame is None : <EOL> goalFrame = vtk . vtkTransform ( ) <EOL> msg . goal_pos = positionMessageFromFrame ( goalFrame ) <EOL> msg = self . applyParams ( msg ) <EOL> msg = self . applySafeRegions ( msg ) <EOL> return msg <EOL> def applyParams ( self , msg ) : <EOL> msg . params = lcmdrc . footstep_plan_params_t ( ) <EOL> msg . params . max_num_steps = self . params . properties . max_num_steps <EOL> msg . params . min_num_steps = self . params . properties . min_num_steps <EOL> msg . params . min_step_width = self . params . properties . min_step_width <EOL> msg . params . nom_step_width = self . params . properties . nominal_step_width <EOL> msg . params . max_step_width = self . params . properties . max_step_width <EOL> msg . params . nom_forward_step = self . params . properties . nominal_forward_step <EOL> msg . params . max_forward_step = self . params . properties . max_forward_step <EOL> msg . params . nom_upward_step = self . params . properties . max_upward_step <EOL> msg . params . nom_downward_step = self . params . properties . max_downward_step <EOL> msg . params . planning_mode = self . params . properties . planner_mode <EOL> msg . params . behavior = self . behavior_lcm_map [ self . params . properties . behavior ] <EOL> msg . params . map_mode = self . map_mode_map [ self . params . properties . map_mode ] <EOL> msg . params . leading_foot = self . leading_foot_map [ self . params . properties . leading_foot ] <EOL> msg . default_step_params = self . getDefaultStepParams ( ) <EOL> return msg <EOL> def applySafeRegions ( self , msg ) : <EOL> safe_regions_folder = om . findObjectByName ( '<STR_LIT>' ) <EOL> safe_terrain_regions = [ ] <EOL> if safe_regions_folder : <EOL> for obj in safe_regions_folder . children ( ) : <EOL> if obj . getProperty ( '<STR_LIT>' ) : <EOL> safe_terrain_regions . append ( obj . safe_region ) <EOL> msg . num_iris_regions = len ( safe_terrain_regions ) <EOL> for r in safe_terrain_regions : <EOL> msg . iris_regions . append ( r . to_iris_region_t ( ) ) <EOL> return msg <EOL> def sendFootstepPlanCheckRequest ( self , request , waitForResponse = False , waitTimeout = <NUM_LIT> ) : <EOL> assert isinstance ( request , lcmdrc . footstep_check_request_t ) <EOL> requestChannel = '<STR_LIT>' <EOL> responseChannel = '<STR_LIT>' <EOL> if waitForResponse : <EOL> if waitTimeout == <NUM_LIT:0> : <EOL> helper = lcmUtils . MessageResponseHelper ( responseChannel , lcmdrc . footstep_plan_t ) <EOL> lcmUtils . publish ( requestChannel , request ) <EOL> return helper <EOL> return lcmUtils . MessageResponseHelper . publishAndWait ( requestChannel , request , <EOL> responseChannel , lcmdrc . footstep_plan_t , waitTimeout ) <EOL> else : <EOL> lcmUtils . publish ( requestChannel , request ) <EOL> def sendFootstepPlanRequest ( self , request , waitForResponse = False , waitTimeout = <NUM_LIT> ) : <EOL> assert isinstance ( request , lcmdrc . footstep_plan_request_t ) <EOL> self . lastFootstepRequest = request <EOL> requestChannel = '<STR_LIT>' <EOL> responseChannel = '<STR_LIT>' <EOL> if waitForResponse : <EOL> if waitTimeout == <NUM_LIT:0> : <EOL> helper = lcmUtils . MessageResponseHelper ( responseChannel , lcmdrc . footstep_plan_t ) <EOL> lcmUtils . publish ( requestChannel , request ) <EOL> return helper <EOL> return lcmUtils . MessageResponseHelper . publishAndWait ( requestChannel , request , <EOL> responseChannel , lcmdrc . footstep_plan_t , waitTimeout ) <EOL> else : <EOL> lcmUtils . publish ( requestChannel , request ) <EOL> def sendWalkingPlanRequest ( self , footstepPlan , startPose , waitForResponse = False , waitTimeout = <NUM_LIT> , req_type = '<STR_LIT>' ) : <EOL> msg = lcmdrc . walking_plan_request_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> state_msg = robotstate . drakePoseToRobotState ( startPose ) <EOL> msg . initial_state = state_msg <EOL> msg . new_nominal_state = msg . initial_state <EOL> msg . use_new_nominal_state = True <EOL> msg . footstep_plan = footstepPlan <EOL> if req_type == '<STR_LIT>' : <EOL> requestChannel = '<STR_LIT>' <EOL> responseChannel = '<STR_LIT>' <EOL> response_type = lcmdrc . robot_plan_t <EOL> elif req_type == '<STR_LIT>' : <EOL> requestChannel = '<STR_LIT>' <EOL> responseChannel = '<STR_LIT>' <EOL> response_type = lcmdrc . walking_plan_t <EOL> elif req_type == '<STR_LIT>' : <EOL> requestChannel = '<STR_LIT>' <EOL> responseChannel = '<STR_LIT>' <EOL> response_type = lcmdrc . robot_plan_t <EOL> else : <EOL> raise ValueError ( "<STR_LIT>" . format ( req_type ) ) <EOL> if waitForResponse : <EOL> if waitTimeout == <NUM_LIT:0> : <EOL> helper = lcmUtils . MessageResponseHelper ( responseChannel , response_type ) <EOL> lcmUtils . publish ( requestChannel , msg ) <EOL> return helper <EOL> return lcmUtils . MessageResponseHelper . publishAndWait ( requestChannel , msg , <EOL> responseChannel , response_type , waitTimeout ) <EOL> else : <EOL> lcmUtils . publish ( requestChannel , msg ) <EOL> def sendStopWalking ( self ) : <EOL> msg = lcmdrc . plan_control_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . control = lcmdrc . plan_control_t . TERMINATE <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> def commitFootstepPlan ( self , footstepPlan ) : <EOL> for previousPlan in self . committedPlans : <EOL> if previousPlan . utime == footstepPlan . utime : <EOL> raise Exception ( "<STR_LIT>" ) <EOL> self . committedPlans . append ( footstepPlan ) <EOL> self . drawFootstepPlan ( footstepPlan , getFootstepsFolder ( ) , alpha = <NUM_LIT> ) <EOL> if footstepPlan . params . behavior in ( lcmdrc . footstep_plan_params_t . BEHAVIOR_BDI_STEPPING , <EOL> lcmdrc . footstep_plan_params_t . BEHAVIOR_BDI_WALKING ) : <EOL> self . _commitFootstepPlanBDI ( footstepPlan ) <EOL> elif footstepPlan . params . behavior == lcmdrc . footstep_plan_params_t . BEHAVIOR_WALKING : <EOL> self . _commitFootstepPlanDrake ( footstepPlan ) <EOL> def _commitFootstepPlanDrake ( self , footstepPlan ) : <EOL> startPose = self . jointController . getPose ( '<STR_LIT>' ) <EOL> self . sendWalkingPlanRequest ( footstepPlan , startPose , req_type = '<STR_LIT>' ) <EOL> def _commitFootstepPlanBDI ( self , footstepPlan ) : <EOL> footstepPlan . utime = getUtime ( ) <EOL> lcmUtils . publish ( '<STR_LIT>' , footstepPlan ) <EOL> def sendHaltSimulationDrakeRequest ( self ) : <EOL> msg = lcmdrc . utime_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> def onPoseBDI ( self , msg ) : <EOL> self . pose_bdi = msg <EOL> rpy = transformUtils . quaternionToRollPitchYaw ( msg . orientation ) <EOL> pose = self . jointController . q . copy ( ) <EOL> pose [ <NUM_LIT:0> : <NUM_LIT:3> ] = msg . pos <EOL> pose [ <NUM_LIT:3> : <NUM_LIT:6> ] = rpy <EOL> self . bdiJointController . setPose ( "<STR_LIT>" , pose ) <EOL> def onBDIAdjustedFootstepPlan ( self , msg ) : <EOL> self . bdi_plan_adjusted = msg . decode ( msg . encode ( ) ) <EOL> if ( self . showBDIPlan is True ) : <EOL> self . drawBDIFootstepPlanAdjusted ( ) <EOL> def transformPlanToBDIFrame ( self , plan ) : <EOL> if ( self . pose_bdi is None ) : <EOL> return <EOL> t_bodybdi = transformUtils . transformFromPose ( self . pose_bdi . pos , self . pose_bdi . orientation ) <EOL> t_bodybdi . PostMultiply ( ) <EOL> current_pose = self . jointController . q <EOL> t_bodymain = transformUtils . transformFromPose ( current_pose [ <NUM_LIT:0> : <NUM_LIT:3> ] , transformUtils . rollPitchYawToQuaternion ( current_pose [ <NUM_LIT:3> : <NUM_LIT:6> ] ) ) <EOL> t_bodymain . PostMultiply ( ) <EOL> self . bdi_plan = plan . decode ( plan . encode ( ) ) <EOL> for i , footstep in enumerate ( self . bdi_plan . footsteps ) : <EOL> step = footstep . pos <EOL> t_step = frameFromPositionMessage ( step ) <EOL> t_body_to_step = vtk . vtkTransform ( ) <EOL> t_body_to_step . DeepCopy ( t_step ) <EOL> t_body_to_step . PostMultiply ( ) <EOL> t_body_to_step . Concatenate ( t_bodymain . GetLinearInverse ( ) ) <EOL> t_stepbdi = vtk . vtkTransform ( ) <EOL> t_stepbdi . DeepCopy ( t_body_to_step ) <EOL> t_stepbdi . PostMultiply ( ) <EOL> t_stepbdi . Concatenate ( t_bodybdi ) <EOL> footstep . pos = positionMessageFromFrame ( t_stepbdi ) <EOL> if ( self . showBDIPlan is True ) : <EOL> self . drawBDIFootstepPlan ( ) <EOL> def drawBDIFootstepPlan ( self ) : <EOL> if ( self . bdi_plan is None ) : <EOL> return <EOL> folder = om . getOrCreateContainer ( "<STR_LIT>" ) <EOL> om . removeFromObjectModel ( folder ) <EOL> folder = om . getOrCreateContainer ( "<STR_LIT>" ) <EOL> folder . setIcon ( om . Icons . Feet ) <EOL> om . collapse ( folder ) <EOL> self . drawFootstepPlan ( self . bdi_plan , folder , [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> ] , [ <NUM_LIT:1.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> def drawBDIFootstepPlanAdjusted ( self ) : <EOL> if ( self . bdi_plan_adjusted is None ) : <EOL> return <EOL> folder = om . getOrCreateContainer ( '<STR_LIT>' ) <EOL> om . removeFromObjectModel ( folder ) <EOL> folder = om . getOrCreateContainer ( '<STR_LIT>' ) <EOL> folder . setIcon ( om . Icons . Feet ) <EOL> om . collapse ( folder ) <EOL> self . drawFootstepPlan ( self . bdi_plan_adjusted , folder , [ <NUM_LIT:1.0> , <NUM_LIT:1.0> , <NUM_LIT:0.0> ] , [ <NUM_LIT:0.0> , <NUM_LIT:1.0> , <NUM_LIT:1.0> ] ) <EOL> class FootstepRequestGenerator ( object ) : <EOL> def __init__ ( self , footstepsDriver ) : <EOL> self . footstepsDriver = footstepsDriver <EOL> @ staticmethod <EOL> def getRobotStanceFrame ( robotModel ) : <EOL> return FootstepsDriver . getFeetMidPoint ( robotModel ) <EOL> @ staticmethod <EOL> def makeStepFrames ( stepFrames , relativeFrame = None , showFrames = False ) : <EOL> frames = [ ] <EOL> for i , stepFrame in enumerate ( stepFrames ) : <EOL> stepFrame = transformUtils . frameFromPositionAndRPY ( stepFrame , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> stepFrame . PostMultiply ( ) <EOL> if relativeFrame : <EOL> stepFrame . Concatenate ( relativeFrame ) <EOL> if showFrames : <EOL> obj = vis . updateFrame ( stepFrame , '<STR_LIT>' % i , parent = '<STR_LIT>' , scale = <NUM_LIT> ) <EOL> stepFrame = obj . transform <EOL> frames . append ( stepFrame ) <EOL> return frames <EOL> def makeStepMessages ( self , stepFrames , leadingFoot , snapToTerrain = False ) : <EOL> assert leadingFoot in ( '<STR_LIT:left>' , '<STR_LIT:right>' ) <EOL> isRightFootOffset = <NUM_LIT:0> if leadingFoot == '<STR_LIT:left>' else <NUM_LIT:1> <EOL> leftPoints , rightPoints = FootstepsDriver . getContactPts ( ) <EOL> footOriginToSole = - np . mean ( leftPoints , axis = <NUM_LIT:0> ) <EOL> stepMessages = [ ] <EOL> for i , stepFrame in enumerate ( stepFrames ) : <EOL> t = transformUtils . copyFrame ( stepFrame ) <EOL> t . PreMultiply ( ) <EOL> t . Translate ( footOriginToSole ) <EOL> step = lcmdrc . footstep_t ( ) <EOL> step . pos = positionMessageFromFrame ( t ) <EOL> step . is_right_foot = ( i + isRightFootOffset ) % <NUM_LIT:2> <EOL> step . params = self . footstepsDriver . getDefaultStepParams ( ) <EOL> step . fixed_x = True <EOL> step . fixed_y = True <EOL> step . fixed_z = True <EOL> step . fixed_roll = True <EOL> step . fixed_pitch = True <EOL> step . fixed_yaw = True <EOL> if snapToTerrain : <EOL> step . fixed_z = False <EOL> step . fixed_roll = False <EOL> step . fixed_pitch = False <EOL> stepMessages . append ( step ) <EOL> return stepMessages <EOL> def makeFootstepRequest ( self , startPose , stepFrames , leadingFoot , numberOfFillSteps = <NUM_LIT:0> , snapToTerrain = False ) : <EOL> stepMessages = self . makeStepMessages ( stepFrames , leadingFoot , snapToTerrain = snapToTerrain ) <EOL> request = self . footstepsDriver . constructFootstepPlanRequest ( startPose ) <EOL> request . num_goal_steps = len ( stepMessages ) <EOL> request . goal_steps = stepMessages <EOL> request . params . leading_foot = lcmdrc . footstep_plan_params_t . LEAD_LEFT if leadingFoot == '<STR_LIT:left>' else lcmdrc . footstep_plan_params_t . LEAD_RIGHT <EOL> request . params . max_num_steps = len ( stepMessages ) + numberOfFillSteps <EOL> return request </s>
