<s> from __future__ import division <EOL> import os <EOL> import sys <EOL> import math <EOL> import time <EOL> import types <EOL> import functools <EOL> import random <EOL> import numpy as np <EOL> from director import ik <EOL> from director import ikconstraints <EOL> from director import ikconstraintencoder <EOL> import drc as lcmdrc <EOL> import json <EOL> from director . utime import getUtime <EOL> from director import lcmUtils <EOL> from director import robotstate <EOL> class PlannerPublisher ( object ) : <EOL> def __init__ ( self , ikPlanner , affordanceMan , robotModel ) : <EOL> self . ikPlanner = ikPlanner <EOL> self . affordanceManager = affordanceMan <EOL> self . poses = { } <EOL> self . robotModel = robotModel <EOL> self . jointLimits = { } <EOL> for jointName in robotstate . getDrakePoseJointNames ( ) : <EOL> self . jointLimits [ jointName ] = [ self . robotModel . model . getJointLimits ( jointName ) [ <NUM_LIT:0> ] , self . robotModel . model . getJointLimits ( jointName ) [ <NUM_LIT:1> ] ] <EOL> def updateJointLimits ( self , limitData ) : <EOL> for jointName , epsilon , jointPosition in limitData : <EOL> if epsilon < <NUM_LIT:0> : <EOL> self . jointLimits [ jointName ] [ <NUM_LIT:0> ] = jointPosition <EOL> else : <EOL> self . jointLimits [ jointName ] [ <NUM_LIT:1> ] = jointPosition <EOL> print limitData <EOL> print self . jointLimits <EOL> def setupMessage ( self , constraints , endPoseName = "<STR_LIT>" , nominalPoseName = "<STR_LIT>" , seedPoseName = "<STR_LIT>" , additionalTimeSamples = None ) : <EOL> poses = ikconstraintencoder . getPlanPoses ( constraints , self . ikPlanner ) <EOL> poses . update ( self . poses ) <EOL> msg = lcmdrc . exotica_planner_request_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . poses = json . dumps ( poses ) <EOL> msg . constraints = ikconstraintencoder . encodeConstraints ( constraints ) <EOL> msg . seed_pose = seedPoseName <EOL> msg . nominal_pose = nominalPoseName <EOL> msg . end_pose = endPoseName <EOL> msg . joint_names = json . dumps ( list ( self . ikPlanner . jointController . jointNames ) ) <EOL> msg . affordances = self . processAffordances ( ) <EOL> opt = ikplanner . getIkOptions ( ) . _properties <EOL> if additionalTimeSamples : <EOL> opt . update ( { '<STR_LIT>' : additionalTimeSamples } ) <EOL> opt [ '<STR_LIT>' ] = self . jointLimits <EOL> msg . options = json . dumps ( opt ) <EOL> return msg <EOL> def processIK ( self , constraints , endPoseName = "<STR_LIT>" , nominalPoseName = "<STR_LIT>" , seedPoseName = "<STR_LIT>" , additionalTimeSamples = None ) : <EOL> listener = self . ikPlanner . getManipIKListener ( ) <EOL> msg = self . setupMessage ( constraints , endPoseName , nominalPoseName , seedPoseName , additionalTimeSamples ) <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> ikplan = listener . waitForResponse ( timeout = <NUM_LIT> ) <EOL> listener . finish ( ) <EOL> endPose = [ <NUM_LIT:0> ] * self . ikPlanner . jointController . numberOfJoints <EOL> if ikplan . num_states > <NUM_LIT:0> : <EOL> endPose [ len ( endPose ) - len ( ikplan . plan [ ikplan . num_states - <NUM_LIT:1> ] . joint_position ) : ] = ikplan . plan [ ikplan . num_states - <NUM_LIT:1> ] . joint_position <EOL> info = ikplan . plan_info [ ikplan . num_states - <NUM_LIT:1> ] <EOL> else : <EOL> info = - <NUM_LIT:1> <EOL> self . ikPlanner . ikServer . infoFunc ( info ) <EOL> return endPose , info <EOL> def processTraj ( self , constraints , endPoseName = "<STR_LIT>" , nominalPoseName = "<STR_LIT>" , seedPoseName = "<STR_LIT>" , additionalTimeSamples = None ) : <EOL> largestTspan = [ <NUM_LIT:0> , <NUM_LIT:0> ] <EOL> for constraintIndex , _ in enumerate ( constraints ) : <EOL> if np . isfinite ( constraints [ constraintIndex ] . tspan [ <NUM_LIT:0> ] ) and np . isfinite ( constraints [ constraintIndex ] . tspan [ <NUM_LIT:1> ] ) : <EOL> largestTspan [ <NUM_LIT:0> ] = constraints [ constraintIndex ] . tspan [ <NUM_LIT:0> ] if ( constraints [ constraintIndex ] . tspan [ <NUM_LIT:0> ] < largestTspan [ <NUM_LIT:0> ] ) else largestTspan [ <NUM_LIT:0> ] <EOL> largestTspan [ <NUM_LIT:1> ] = constraints [ constraintIndex ] . tspan [ <NUM_LIT:1> ] if ( constraints [ constraintIndex ] . tspan [ <NUM_LIT:1> ] > largestTspan [ <NUM_LIT:1> ] ) else largestTspan [ <NUM_LIT:1> ] <EOL> for constraintIndex , _ in enumerate ( constraints ) : <EOL> if np . isfinite ( constraints [ constraintIndex ] . tspan [ <NUM_LIT:0> ] ) and np . isfinite ( constraints [ constraintIndex ] . tspan [ <NUM_LIT:1> ] ) : <EOL> if largestTspan [ <NUM_LIT:1> ] != <NUM_LIT:0> : <EOL> constraints [ constraintIndex ] . tspan [ <NUM_LIT:0> ] = constraints [ constraintIndex ] . tspan [ <NUM_LIT:0> ] / largestTspan [ <NUM_LIT:1> ] <EOL> constraints [ constraintIndex ] . tspan [ <NUM_LIT:1> ] = constraints [ constraintIndex ] . tspan [ <NUM_LIT:1> ] / largestTspan [ <NUM_LIT:1> ] <EOL> listener = self . ikPlanner . getManipPlanListener ( ) <EOL> msg = self . setupMessage ( constraints , endPoseName , nominalPoseName , seedPoseName , additionalTimeSamples ) <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> lastManipPlan = listener . waitForResponse ( timeout = <NUM_LIT> ) <EOL> listener . finish ( ) <EOL> self . ikPlanner . ikServer . infoFunc ( lastManipPlan . plan_info [ <NUM_LIT:0> ] ) <EOL> return lastManipPlan , lastManipPlan . plan_info [ <NUM_LIT:0> ] <EOL> def processAddPose ( self , pose , poseName ) : <EOL> self . poses [ poseName ] = list ( pose ) ; <EOL> def processAffordances ( self ) : <EOL> affs = self . affordanceManager . getCollisionAffordances ( ) <EOL> s = '<STR_LIT:[>' <EOL> first = True <EOL> for aff in affs : <EOL> des = aff . getDescription ( ) <EOL> classname = des [ '<STR_LIT>' ] ; <EOL> if first : <EOL> s += '<STR_LIT:{>' <EOL> else : <EOL> s += '<STR_LIT>' <EOL> first = False <EOL> s += '<STR_LIT>' + classname + '<STR_LIT:">' <EOL> s += '<STR_LIT>' + des [ '<STR_LIT:Name>' ] + '<STR_LIT:">' <EOL> s += '<STR_LIT>' + des [ '<STR_LIT>' ] + '<STR_LIT:">' <EOL> s += '<STR_LIT>' + repr ( des [ '<STR_LIT>' ] [ <NUM_LIT:0> ] . tolist ( ) ) + '<STR_LIT>' + repr ( des [ '<STR_LIT>' ] [ <NUM_LIT:1> ] . tolist ( ) ) + '<STR_LIT>' <EOL> if self . affordanceManager . affordanceUpdater is not None : <EOL> if des [ '<STR_LIT:Name>' ] in self . affordanceManager . affordanceUpdater . attachedAffordances : <EOL> s += '<STR_LIT>' + self . affordanceManager . affordanceUpdater . attachedAffordances [ des [ '<STR_LIT:Name>' ] ] + '<STR_LIT:">' <EOL> else : <EOL> s += '<STR_LIT>' <EOL> else : <EOL> s += '<STR_LIT>' <EOL> if classname == '<STR_LIT>' : <EOL> s += '<STR_LIT>' + aff . getMeshManager ( ) . getFilesystemFilename ( des [ '<STR_LIT>' ] ) + '<STR_LIT:">' <EOL> if classname == '<STR_LIT>' : <EOL> s += '<STR_LIT>' + repr ( des [ '<STR_LIT>' ] ) <EOL> if classname == '<STR_LIT>' or classname == '<STR_LIT>' : <EOL> s += '<STR_LIT>' + repr ( des [ '<STR_LIT>' ] ) <EOL> s += '<STR_LIT>' + repr ( des [ '<STR_LIT>' ] ) <EOL> if classname == '<STR_LIT>' : <EOL> s += '<STR_LIT>' + repr ( des [ '<STR_LIT>' ] ) <EOL> if classname == '<STR_LIT>' : <EOL> s += '<STR_LIT>' + repr ( des [ '<STR_LIT>' ] ) <EOL> s += '<STR_LIT>' + repr ( des [ '<STR_LIT>' ] ) <EOL> s += '<STR_LIT>' + repr ( des [ '<STR_LIT>' ] ) <EOL> s += '<STR_LIT:}>' <EOL> s = s + '<STR_LIT:]>' <EOL> return s <EOL> import ikplanner </s>
