<s> from segmentation import getFootFramesFromReferenceFrame <EOL> import math <EOL> from director . debugVis import DebugData <EOL> def getBoardCorners ( params ) : <EOL> axes = [ np . array ( params [ axis ] ) for axis in [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] ] <EOL> widths = [ np . array ( params [ axis ] ) / <NUM_LIT> for axis in [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] ] <EOL> edges = [ axes [ i ] * widths [ i ] for i in xrange ( <NUM_LIT:3> ) ] <EOL> origin = np . array ( params [ '<STR_LIT>' ] ) <EOL> return [ <EOL> origin + edges [ <NUM_LIT:0> ] + edges [ <NUM_LIT:1> ] + edges [ <NUM_LIT:2> ] , <EOL> origin - edges [ <NUM_LIT:0> ] + edges [ <NUM_LIT:1> ] + edges [ <NUM_LIT:2> ] , <EOL> origin - edges [ <NUM_LIT:0> ] - edges [ <NUM_LIT:1> ] + edges [ <NUM_LIT:2> ] , <EOL> origin + edges [ <NUM_LIT:0> ] - edges [ <NUM_LIT:1> ] + edges [ <NUM_LIT:2> ] , <EOL> origin + edges [ <NUM_LIT:0> ] + edges [ <NUM_LIT:1> ] - edges [ <NUM_LIT:2> ] , <EOL> origin - edges [ <NUM_LIT:0> ] + edges [ <NUM_LIT:1> ] - edges [ <NUM_LIT:2> ] , <EOL> origin - edges [ <NUM_LIT:0> ] - edges [ <NUM_LIT:1> ] - edges [ <NUM_LIT:2> ] , <EOL> origin + edges [ <NUM_LIT:0> ] - edges [ <NUM_LIT:1> ] - edges [ <NUM_LIT:2> ] , <EOL> ] <EOL> def getPointDistances ( target , points ) : <EOL> return np . array ( [ np . linalg . norm ( target - p ) for p in points ] ) <EOL> def computeClosestCorner ( aff , referenceFrame ) : <EOL> corners = getBoardCorners ( aff . params ) <EOL> dists = getPointDistances ( np . array ( referenceFrame . GetPosition ( ) ) , corners ) <EOL> return corners [ dists . argmin ( ) ] <EOL> def computeGroundFrame ( aff , referenceFrame ) : <EOL> refAxis = np . array ( [ <NUM_LIT:0> , - <NUM_LIT:1> , <NUM_LIT:0> ] ) <EOL> referenceFrame . TransformVector ( refAxis , refAxis ) <EOL> axes = [ np . array ( aff . params [ axis ] ) for axis in [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] ] <EOL> axisProjections = np . array ( [ np . abs ( np . dot ( axis , refAxis ) ) for axis in axes ] ) <EOL> boardAxis = axes [ axisProjections . argmax ( ) ] <EOL> if np . dot ( boardAxis , refAxis ) < <NUM_LIT:0> : <EOL> boardAxis = - boardAxis <EOL> xaxis = boardAxis <EOL> zaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> closestCorner = computeClosestCorner ( aff , referenceFrame ) <EOL> groundFrame = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> groundFrame . PostMultiply ( ) <EOL> groundFrame . Translate ( closestCorner [ <NUM_LIT:0> ] , closestCorner [ <NUM_LIT:1> ] , <NUM_LIT:0.0> ) <EOL> return groundFrame <EOL> def showBoardDebug ( affs = None ) : <EOL> referenceFrame = vtk . vtkTransform ( ) <EOL> referenceFrame . Translate ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT> ) <EOL> affs = affs or om . getObjects ( ) <EOL> for obj in affs : <EOL> if isinstance ( obj , BlockAffordanceItem ) : <EOL> d = DebugData ( ) <EOL> d . addSphere ( computeClosestCorner ( obj , referenceFrame ) , radius = <NUM_LIT> ) <EOL> showPolyData ( d . getPolyData ( ) , '<STR_LIT>' , parent = '<STR_LIT>' , visible = True ) <EOL> showFrame ( computeGroundFrame ( obj , referenceFrame ) , '<STR_LIT>' , parent = '<STR_LIT>' , visible = True ) <EOL> def loadFeet ( ) : <EOL> meshDir = os . path . join ( app . getDRCBase ( ) , '<STR_LIT>' ) <EOL> meshes = [ ] <EOL> for foot in [ '<STR_LIT:l>' , '<STR_LIT:r>' ] : <EOL> d = DebugData ( ) <EOL> d . addPolyData ( io . readPolyData ( os . path . join ( meshDir , '<STR_LIT>' % foot ) ) ) <EOL> d . addPolyData ( io . readPolyData ( os . path . join ( meshDir , '<STR_LIT>' % foot ) ) ) <EOL> meshes . append ( d . getPolyData ( ) ) <EOL> return meshes <EOL> import vs as lcmvs <EOL> from director import lcmUtils <EOL> triadCount = <NUM_LIT:0> <EOL> def publishTriad ( transform , collectionId = <NUM_LIT> ) : <EOL> global triadCount <EOL> triadCount += <NUM_LIT:1> <EOL> o = lcmvs . obj_t ( ) <EOL> xyz = transform . GetPosition ( ) <EOL> rpy = transformUtils . rollPitchYawFromTransform ( transform ) <EOL> o . roll , o . pitch , o . yaw = rpy <EOL> o . x , o . y , o . z = xyz <EOL> o . id = triadCount <EOL> m = vs . obj_collection_t ( ) <EOL> m . id = collectionId <EOL> m . name = '<STR_LIT>' <EOL> m . type = lcmvs . obj_collection_t . AXIS3D <EOL> m . nobjs = <NUM_LIT:1> <EOL> m . reset = False <EOL> m . objs = [ o ] <EOL> lcmUtils . publish ( '<STR_LIT>' , m ) <EOL> def generateFeetForDebris ( affs = None ) : <EOL> affs = affs or om . getObjects ( ) <EOL> affs = [ aff for aff in affs if isinstance ( aff , BlockAffordanceItem ) ] <EOL> stanceWidth = <NUM_LIT> <EOL> nameMap = { <EOL> '<STR_LIT>' : '<STR_LIT>' , <EOL> '<STR_LIT>' : '<STR_LIT>' , <EOL> '<STR_LIT>' : '<STR_LIT>' , <EOL> '<STR_LIT>' : '<STR_LIT>' , <EOL> '<STR_LIT>' : '<STR_LIT>' , <EOL> '<STR_LIT>' : '<STR_LIT>' , <EOL> '<STR_LIT>' : '<STR_LIT>' , <EOL> } <EOL> stances = { <EOL> '<STR_LIT>' : ( - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ) , <EOL> '<STR_LIT>' : ( - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ) , <EOL> '<STR_LIT>' : ( - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ) , <EOL> '<STR_LIT>' : ( - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ) , <EOL> '<STR_LIT>' : ( - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ) , <EOL> '<STR_LIT>' : ( - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ) , <EOL> '<STR_LIT>' : ( - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ) , <EOL> '<STR_LIT>' : ( - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ) , <EOL> } <EOL> referenceFrame = vtk . vtkTransform ( ) <EOL> referenceFrame . Translate ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT> ) <EOL> stanceRotation = <NUM_LIT:0.0> <EOL> stanceOffset = [ - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT:0> ] <EOL> for aff in affs : <EOL> name = aff . getProperty ( '<STR_LIT:Name>' ) <EOL> if name not in nameMap : <EOL> print '<STR_LIT>' , name <EOL> continue <EOL> affGroundFrame = computeGroundFrame ( aff , referenceFrame ) <EOL> stanceOffsetX , stanceOffsetY , pelvisHeight , stanceRotation = stances [ nameMap [ name ] ] <EOL> stanceFrame , lfootFrame , rfootFrame = getFootFramesFromReferenceFrame ( affGroundFrame , stanceWidth , math . degrees ( stanceRotation ) , [ stanceOffsetX , stanceOffsetY , <NUM_LIT:0.0> ] ) <EOL> publishTriad ( stanceFrame ) <EOL> showFrame ( affGroundFrame , name + '<STR_LIT>' , parent = '<STR_LIT>' , scale = <NUM_LIT> , visible = True ) <EOL> lfootFrame = showFrame ( lfootFrame , name + '<STR_LIT>' , parent = '<STR_LIT>' , scale = <NUM_LIT> ) <EOL> rfootFrame = showFrame ( rfootFrame , name + '<STR_LIT>' , parent = '<STR_LIT>' , scale = <NUM_LIT> ) <EOL> lfoot , rfoot = loadFeet ( ) <EOL> lfoot = showPolyData ( lfoot , name + '<STR_LIT>' , parent = '<STR_LIT>' ) <EOL> lfoot . actor . SetUserTransform ( lfootFrame . transform ) <EOL> rfoot = showPolyData ( rfoot , name + '<STR_LIT>' , parent = '<STR_LIT>' ) <EOL> rfoot . actor . SetUserTransform ( rfootFrame . transform ) <EOL> generateFeetForDebris ( ) </s>
