def __init__(self):
if burst.options.trace_proxies:
print "INFO: Proxy tracing is on"
# avoid logging None objects - like when getSpeechProxy returns None
callWrapper = lambda name, obj: (obj and LogCalls(name, obj) or obj)
else:
callWrapper = lambda name, obj: obj
self._memory = callWrapper("ALMemory", burst.getMemoryProxy(deferred=True))
self._motion = callWrapper("ALMotion", burst.getMotionProxy(deferred=True))
self._sentinel = callWrapper("ALSentinel", burst.getSentinelProxy(deferred=True))
self._speech = callWrapper("ALSpeech", burst.getSpeechProxy(deferred=True))
self._video = callWrapper("ALVideoDevice", burst.getALVideoDeviceProxy(deferred=True))
self._leds = callWrapper("ALLeds", burst.getLedsProxy(deferred=True))
self._imops = callWrapper("imops", burst.getImopsProxy(deferred=True))
if burst.options.run_sonar:
self._sonar = callWrapper("ALSonar", burst.getSonarProxy(deferred=True))
self._events = set()
self._deferreds = []
self.burst_deferred_maker = BurstDeferredMaker()
# This makes sure stuff actually works if nothing is being updated on the nao.
self._default_proxied_variable_value = 0.0
# We do memory.getListData once per self.update, in one go.
# later when this is done with shared memory, it will be changed here.
# initialize these before initializing objects that use them (Ball etc.)
default_vars = self.getDefaultVars()
self._vars_to_get_set = set()
self._vars_to_get_list = list()
self.vars = {} # no leading underscore - meant as a public interface (just don't write here, it will be overwritten every update)
self.addMemoryVars(default_vars)
self._shm = None
self.time = 0.0
self.start_time = time() # construction time
# Variables for body joint angles from dcm
self._getAnglesMap = dict([(joint,
'Device/SubDeviceList/%s/Position/Sensor/Value' % joint)
for joint in self.jointnames])
if self.connected_to_webots and is_120:
print "World: Using ALMotion for body angles"
self._updateBodyAngles = self._updateBodyAngles_from_ALMotion
else:
print "World: Using DCM for body angles"
self._body_angles_vars = self._getAnglesMap.values()
self._updateBodyAngles = self._updateBodyAngles_from_DCM
self.addMemoryVars(self._body_angles_vars)
self.body_angles = [0.0] * 26
# Variables for Inclination angles
self._inclination_vars = ['Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value',
'Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value']
self.addMemoryVars(self._inclination_vars)
self._recorded_vars = self._getRecorderVariableNames()
print "world will record (if asked) %s vars" % len(self._recorded_vars)
self._recorded_header = self._recorded_vars
self._record_basename = World.running_on_nao and '/media/userdata' or '/tmp'
# Stuff that we prefer the users use directly doesn't get a leading
# underscore
# * Vision recognized objects
self.ball = Ball(self)
# Goals Notes: We start at UNCONFIGURED State, we record all the variables
# but only start using them once we configure all the goal according to our team and
# goal color.
our_lp_xy = field.our_goal.top_post.xy # left is from pov of goalie looking at opponent goal.
our_rp_xy = field.our_goal.bottom_post.xy
opposing_lp_xy = field.opposing_goal.bottom_post.xy
opposing_rp_xy = field.opposing_goal.top_post.xy
self.our_goal = Goal(name='OurGoal', which_team=OUR_TEAM,
world=self, left_name='OurLeftPost', right_name='OurRightPost',
left_world=our_lp_xy, right_world=our_rp_xy)
self.our_lp = self.our_goal.left
self.our_rp = self.our_goal.right
self.opposing_goal = Goal(name='OpposingGoal', which_team=OPPOSING_TEAM,
world=self, left_name='OpposingLeftPost', right_name='OpposingRightPost',
left_world=opposing_lp_xy, right_world=opposing_rp_xy)
self.opposing_lp = self.opposing_goal.left
self.opposing_rp = self.opposing_goal.right
self.all_posts = set([self.our_lp, self.our_rp, self.opposing_lp, self.opposing_rp])
self.all_posts_hack = [self.our_lp, self.our_rp, self.opposing_lp, self.opposing_rp]
for name in VISION_POSTS_NAMES:
self.addMemoryVars(GoalPost.getVarsForName(name))
# TODO - other robots
# Buttons, Leds (TODO: sonar,
self.robot = Robot(self)
# TODO - is computed used? should be renamed for legibility
self.computed = Computed(self)
# Self orientation and Location of self and other objects in field.
# (passes some stuff into other objects)
self.pose = Pose(self)
self.localization = Localization(self)
self.odometry = Odometry(self)
# The Game-Status, Game-Controller and RobotData Trifecta # TODO: This is messy.
self.playerSettings = PlayerSettings(self) # Start with the default settings. You will be configured later to the right ones by the referees.
self.gameStatus = GameStatus(self, self.playerSettings)
self._gameController = GameController(self.gameStatus)
self._movecoordinator = MoveCoordinator(self)
# All objects that we delegate the event computation and naoqi
# interaction to. TODO: we have the exact state of B-HUMAN, so we
# could use exactly their solution, and hence this todo. We have
# multiple objects that calculate their events based on ground truths
# (naoqi proxies) and on calculated truths. We need to rerun them
# every time something is updated, *in the correct order*. So right
# now I'm hardcoding this by using an ordered list of lists, but
# later this should be calculated by storing a "needed" and
# "provided" list, just like B-HUMAN, and doing the sorting once (and
# that value can be cached to avoid recomputing on each run).
self._objects = [
# All basic objects that rely on just naoproxies should be in the
# first list
[self._movecoordinator,
self.ball, self.our_goal, self.opposing_goal,
self.robot, self._gameController],
[self.gameStatus],
# anything that relies on basics but nothing else should go next
[self],
# self.computed should always be last
[self.localization],
[self.computed],
]
# Another useful collection
self.vision_objects = [self.ball, self.our_lp, self.our_rp, self.opposing_lp, self.opposing_rp]
# logging variables
self._logged_objects = [[obj, None] for obj in [self.ball]]
self._object_to_filename = {self.ball: 'ball'}
self._do_log_positions = burst.options.log_positions
if self._do_log_positions:
self._openPositionLogs()
if burst.options.no_memory_updates:
print "world: NO MEMORY UPDATES.. N-O M-E-M-O-R-Y U-P-D-A-T-E-S"
self._updateMemoryVariables = self._updateMemoryVariables_noop
else:
# Try using shared memory to access variables
if World.running_on_nao:
#self.updateMmapVariablesFile() # // TODO -remove the file! it is EVIL
SharedMemoryReader.tryToInitMMap()
if SharedMemoryReader.isMMapAvailable():
print "world: using SharedMemoryReader"
if US_DISTANCES_VARNAME in self._vars_to_get_list:
self._vars_to_get_list.remove(US_DISTANCES_VARNAME)
self._shm = SharedMemoryReader(self._vars_to_get_list)
self._updateMemoryVariables = self._updateMemoryVariables_noop #(temp)
self._shm.openDeferred.addCallback(self._switchToSharedMemory).addErrback(log.err)
if self._shm is None:
print "world: using ALMemory"
self.checkManModule()
class World(object):
"""
Main access to any information about the world around the robot,
including other robots. Generally access is done through attributes
that are themselves objects, such as ball, opposing_lp, (TODO: opponent_goal,
our_goal, teammate1, teammate2, opponent{1,2,3})
"""
# Some variable we are sure to export if Man module (man) is
# actually running on the robot / webots.
MAN_ALMEMORY_EXISTANCE_TEST_VARIABLES = ['/BURST/Vision/Ball/BearingDeg']
# TODO - move this to __init__?
connected_to_nao = burst.connecting_to_nao()
connected_to_webots = burst.connecting_to_webots()
running_on_nao = burst.running_on_nao()
hostname = gethostname()
# TODO -use callbacks, and don't use this hardcoded list (or do?)
jointnames = ['HeadYaw', 'HeadPitch', 'LShoulderPitch', 'LShoulderRoll',
'LElbowYaw', 'LElbowRoll', 'LWristYaw', 'LHand', 'LHipYawPitch',
'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll',
'RHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch',
'RAnkleRoll', 'RShoulderPitch', 'RShoulderRoll', 'RElbowYaw',
'RElbowRoll', 'RWristYaw', 'RHand']
chainnames = ['Head', 'LArm', 'RArm', 'LLeg', 'RLeg']
def __init__(self):
if burst.options.trace_proxies:
print "INFO: Proxy tracing is on"
# avoid logging None objects - like when getSpeechProxy returns None
callWrapper = lambda name, obj: (obj and LogCalls(name, obj) or obj)
else:
callWrapper = lambda name, obj: obj
self._memory = callWrapper("ALMemory", burst.getMemoryProxy(deferred=True))
self._motion = callWrapper("ALMotion", burst.getMotionProxy(deferred=True))
self._sentinel = callWrapper("ALSentinel", burst.getSentinelProxy(deferred=True))
self._speech = callWrapper("ALSpeech", burst.getSpeechProxy(deferred=True))
self._video = callWrapper("ALVideoDevice", burst.getALVideoDeviceProxy(deferred=True))
self._leds = callWrapper("ALLeds", burst.getLedsProxy(deferred=True))
self._imops = callWrapper("imops", burst.getImopsProxy(deferred=True))
if burst.options.run_sonar:
self._sonar = callWrapper("ALSonar", burst.getSonarProxy(deferred=True))
self._events = set()
self._deferreds = []
self.burst_deferred_maker = BurstDeferredMaker()
# This makes sure stuff actually works if nothing is being updated on the nao.
self._default_proxied_variable_value = 0.0
# We do memory.getListData once per self.update, in one go.
# later when this is done with shared memory, it will be changed here.
# initialize these before initializing objects that use them (Ball etc.)
default_vars = self.getDefaultVars()
self._vars_to_get_set = set()
self._vars_to_get_list = list()
self.vars = {} # no leading underscore - meant as a public interface (just don't write here, it will be overwritten every update)
self.addMemoryVars(default_vars)
self._shm = None
self.time = 0.0
self.start_time = time() # construction time
# Variables for body joint angles from dcm
self._getAnglesMap = dict([(joint,
'Device/SubDeviceList/%s/Position/Sensor/Value' % joint)
for joint in self.jointnames])
if self.connected_to_webots and is_120:
print "World: Using ALMotion for body angles"
self._updateBodyAngles = self._updateBodyAngles_from_ALMotion
else:
print "World: Using DCM for body angles"
self._body_angles_vars = self._getAnglesMap.values()
self._updateBodyAngles = self._updateBodyAngles_from_DCM
self.addMemoryVars(self._body_angles_vars)
self.body_angles = [0.0] * 26
# Variables for Inclination angles
self._inclination_vars = ['Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value',
'Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value']
self.addMemoryVars(self._inclination_vars)
self._recorded_vars = self._getRecorderVariableNames()
print "world will record (if asked) %s vars" % len(self._recorded_vars)
self._recorded_header = self._recorded_vars
self._record_basename = World.running_on_nao and '/media/userdata' or '/tmp'
# Stuff that we prefer the users use directly doesn't get a leading
# underscore
# * Vision recognized objects
self.ball = Ball(self)
# Goals Notes: We start at UNCONFIGURED State, we record all the variables
# but only start using them once we configure all the goal according to our team and
# goal color.
our_lp_xy = field.our_goal.top_post.xy # left is from pov of goalie looking at opponent goal.
our_rp_xy = field.our_goal.bottom_post.xy
opposing_lp_xy = field.opposing_goal.bottom_post.xy
opposing_rp_xy = field.opposing_goal.top_post.xy
self.our_goal = Goal(name='OurGoal', which_team=OUR_TEAM,
world=self, left_name='OurLeftPost', right_name='OurRightPost',
left_world=our_lp_xy, right_world=our_rp_xy)
self.our_lp = self.our_goal.left
self.our_rp = self.our_goal.right
self.opposing_goal = Goal(name='OpposingGoal', which_team=OPPOSING_TEAM,
world=self, left_name='OpposingLeftPost', right_name='OpposingRightPost',
left_world=opposing_lp_xy, right_world=opposing_rp_xy)
self.opposing_lp = self.opposing_goal.left
self.opposing_rp = self.opposing_goal.right
self.all_posts = set([self.our_lp, self.our_rp, self.opposing_lp, self.opposing_rp])
self.all_posts_hack = [self.our_lp, self.our_rp, self.opposing_lp, self.opposing_rp]
for name in VISION_POSTS_NAMES:
self.addMemoryVars(GoalPost.getVarsForName(name))
# TODO - other robots
# Buttons, Leds (TODO: sonar,
self.robot = Robot(self)
# TODO - is computed used? should be renamed for legibility
self.computed = Computed(self)
# Self orientation and Location of self and other objects in field.
# (passes some stuff into other objects)
self.pose = Pose(self)
self.localization = Localization(self)
self.odometry = Odometry(self)
# The Game-Status, Game-Controller and RobotData Trifecta # TODO: This is messy.
self.playerSettings = PlayerSettings(self) # Start with the default settings. You will be configured later to the right ones by the referees.
self.gameStatus = GameStatus(self, self.playerSettings)
self._gameController = GameController(self.gameStatus)
self._movecoordinator = MoveCoordinator(self)
# All objects that we delegate the event computation and naoqi
# interaction to. TODO: we have the exact state of B-HUMAN, so we
# could use exactly their solution, and hence this todo. We have
# multiple objects that calculate their events based on ground truths
# (naoqi proxies) and on calculated truths. We need to rerun them
# every time something is updated, *in the correct order*. So right
# now I'm hardcoding this by using an ordered list of lists, but
# later this should be calculated by storing a "needed" and
# "provided" list, just like B-HUMAN, and doing the sorting once (and
# that value can be cached to avoid recomputing on each run).
self._objects = [
# All basic objects that rely on just naoproxies should be in the
# first list
[self._movecoordinator,
self.ball, self.our_goal, self.opposing_goal,
self.robot, self._gameController],
[self.gameStatus],
# anything that relies on basics but nothing else should go next
[self],
# self.computed should always be last
[self.localization],
[self.computed],
]
# Another useful collection
self.vision_objects = [self.ball, self.our_lp, self.our_rp, self.opposing_lp, self.opposing_rp]
# logging variables
self._logged_objects = [[obj, None] for obj in [self.ball]]
self._object_to_filename = {self.ball: 'ball'}
self._do_log_positions = burst.options.log_positions
if self._do_log_positions:
self._openPositionLogs()
if burst.options.no_memory_updates:
print "world: NO MEMORY UPDATES.. N-O M-E-M-O-R-Y U-P-D-A-T-E-S"
self._updateMemoryVariables = self._updateMemoryVariables_noop
else:
# Try using shared memory to access variables
if World.running_on_nao:
#self.updateMmapVariablesFile() # // TODO -remove the file! it is EVIL
SharedMemoryReader.tryToInitMMap()
if SharedMemoryReader.isMMapAvailable():
print "world: using SharedMemoryReader"
if US_DISTANCES_VARNAME in self._vars_to_get_list:
self._vars_to_get_list.remove(US_DISTANCES_VARNAME)
self._shm = SharedMemoryReader(self._vars_to_get_list)
self._updateMemoryVariables = self._updateMemoryVariables_noop #(temp)
self._shm.openDeferred.addCallback(self._switchToSharedMemory).addErrback(log.err)
if self._shm is None:
print "world: using ALMemory"
self.checkManModule()
def _setActions(self, actions):
self._actions = actions
def _switchToSharedMemory(self, _):
if set(self.vars.keys()) != set(self._shm.vars.keys()):
num_world, num_shared = len(self.vars), len(self._shm.vars)
print "Notice: world requested %s vars, shared memory provides %s vars. This is ok if %s > %s" % (
num_world, num_shared, num_shared, num_world)
self.vars = self._shm.vars
self._updateMemoryVariables = self._updateMemoryVariablesFromSharedMem
# ########################################
# Actions API
# ########################################
def _onCameraChange(self, which):
# we need to update pitch to all objects we have seen.
pitch_change = CAMERA_PITCH_ANGLE if which is CAMERA_WHICH_TOP_CAMERA else -CAMERA_PITCH_ANGLE
info("Updating all vision objects pitch by %3.2f" % pitch_change)
for object in self.vision_objects:
if object.centered_self.head_pitch is None: continue
object.centered_self.head_pitch += pitch_change
# ########################################
# EventManager API
# ########################################
def collectNewUpdates(self, cur_time):
if self.time == cur_time - self.start_time:
print "TIME ERROR: World.collectNewUpdates called with the same time twice. Ignoring"
return
self.time = cur_time - self.start_time
# BodyAngles Note: if using ALMotion, this completed using a deferred - meaning with twisted not
# during this callback, so doRecord will record previous time frame values
self._updateBodyAngles()
self._updateMemoryVariables() # must be first in update
self._doRecord()
# TODO: automatic calculation of event dependencies (see constructor)
for objlist in self._objects:
for obj in objlist:
obj.calc_events(self._events, self._deferreds)
if self._do_log_positions:
self._logPositions()
# some debugging tests TODO - put under some flag
if burst.options.verbose_goals: # verbose == debug in my book. ok, in this instance of life.
g = self.opposing_goal
if abs(g.left.bearing - g.right.bearing) < 1e-4 and self.opposing_lp.bearing != 0.0:
import pdb; pdb.set_trace()
def getEventsAndDeferreds(self):
events, deferreds = self._events, self._deferreds
self._events = set()
self._deferreds = []
return events, deferreds
def cleanup(self):
if self._do_log_positions:
self._closePositionLogs()
self.odometry.cleanup()
# ########################################
# Player API
# ########################################
def configure(self, our_color):
""" called when entering CONFIGURED state """
self.our_goal.configure(color=our_color)
self.opposing_goal.configure(color=burst_consts.opposite_color(our_color))
# ########################################
# Player/Behavior API (in addition to all
# the exported variables)
# ########################################
def singleGoal(self, targets):
return len(targets) == 2 and set(self.opposing_goal.bottom_top) == set(targets) or set(self.our_goal.bottom_top) == set(targets)
def seenObjects(self):
for o in self.vision_objects:
if o.seen:
yield o
def getSeenGoal(self):
""" returns None or the Goal object """
seen = set(self.seenObjects())
if set(self.opposing_goal.bottom_top) <= seen:
return self.opposing_goal
if set(self.our_goal.bottom_top) <= seen:
return self.our_goal
return None
seen_goal = property(getSeenGoal)
# Accessors
def getMemoryProxy(self):
return self._memory
def getMotionProxy(self):
return self._motion
def getSpeechProxy(self):
return self._speech
def getALVideoDeviceProxy(self):
return self._video
def getImopsProxy(self):
return self._imops
def getDefaultVars(self):
""" return list of variables we want anyway, regardless of what
the objects we use want. This currently includes:
Device/SubDeviceList/<jointname>/Position/Sensor/Value
- for getAngle
"""
return ['Device/SubDeviceList/%s/Position/Sensor/Value' % joint
for joint in self.jointnames]
# accessors that wrap the ALMemory - you can also
# use world.vars
def getAngle(self, jointname):
""" almost the same as ALMotion.getAngle as the help tells it -
returns the sensed angle, which we take to be the sensor position,
not the actuated position
"""
return self.vars[self._getAnglesMap[jointname]]
def getBodyAngles(self):
# This works for DCM, but new naoqi doesn't support DCM very well. - no bigee, we have ALMotion.getBodyAngles
# that works fine in simulation too.
return self.body_angles
def getInclinationAngles(self):
# TODO - OPTIMIZE?
return self.getVars(self._inclination_vars)
# accessors that wrap ALMotion
# TODO - cache these
def getRemainingFootstepCount(self):
return self._motion.getRemainingFootStepCount()
def startRecordAll(self, filename):
import csv
import gzip
self.addMemoryVars(self._recorded_vars)
self._record_file_name = '%s/%s.csv.gz' % (self._record_basename, filename)
self._record_file = gzip.open(self._record_file_name, 'a+')
self._record_csv = csv.writer(self._record_file)
self._record_csv.writerow(self._recorded_header)
self._record_line_num = 0
def stopRecord(self):
if self._record_file:
print "file stored in %s, writing to disk (closing file).." % self._record_file_name
sys.stdout.flush()
self._record_file.close()
print "done"
sys.stdout.flush()
self._record_file = None
self._record_csv = None
self.removeMemoryVars(self._recorded_vars)
# ALMemory and Shared memory functions
def addMemoryVars(self, vars):
# slow? but retains the order of the registration
for v in vars:
if v not in self._vars_to_get_set:
self._vars_to_get_list.append(v)
self._vars_to_get_set.add(v)
self.vars[v] = self._default_proxied_variable_value
def removeMemoryVars(self, vars):
""" IMPORTANT NOTICE: you must make sure you don't remove variables that
another object has added before, or you will probably break it
"""
for v in vars:
if v in self._vars_to_get_set:
self._vars_to_get_list.remove(v)
self._vars_to_get_set.remove(v)
del self.vars[v]
def getVars(self, vars, returnNoneOnMissing=True):
""" quick access to multiple vars (like memory.getListData, only doesn't go to the
network or anything). notice the returnNoneOnMissing bit
"""
if returnNoneOnMissing:
return [self.vars.get(k, None) for k in vars]
return [self.vars[k] for k in vars]
def _updateMemoryVariables_noop(self):
pass
def _updateMemoryVariablesFromSharedMem(self):
self._shm.update()
#@timeit('al update time = %s ms')
def _updateMemoryVariablesFromALMemory(self):
# TODO: optmize the heck out of this. Options include:
# * subscribeOnData / subscribeOnDataChange
# * module with alfastmemoryaccess, and shared memory with python
vars = self._vars_to_get_list
self._memory.getListData(vars).addCallback(self._updateMemoryFromALMemory_onResults).addErrback(log.err)
def _updateMemoryFromALMemory_onResults(self, values):
for k, v in zip(self._vars_to_get_list, values):
if v == 'None':
v = self._default_proxied_variable_value
self.vars[k] = v
_updateMemoryVariables = _updateMemoryVariablesFromALMemory
def _updateBodyAngles_from_ALMotion_onResults(self, angles):
#print "World: updated BodyAngles: %s" % nicefloats(angles)
self.body_angles = angles
def _updateBodyAngles_from_ALMotion(self):
self._motion.getBodyAngles().addCallback(self._updateBodyAngles_from_ALMotion_onResults).addErrback(log.err)
def _updateBodyAngles_from_DCM(self):
self.body_angles = self.getVars(self._body_angles_vars)
# Callbacks
def calc_events(self, events, deferreds):
""" World treats itself as a regular object by having an update function,
this is called after the basic objects and before the computed object (it
may set some events / variables needed by the computed object)
"""
# TODO - move these to the Goal objects (which already exist)
if self.our_lp.seen and self.our_rp.seen:
events.add(EVENT_ALL_OUR_GOAL_IN_FRAME)
if self.opposing_lp.seen and self.opposing_rp.seen:
events.add(EVENT_ALL_OPPOSING_GOAL_IN_FRAME)
# Logging Functions
def _openPositionLogs(self):
timestamp = datetime.datetime.now().strftime('%Y%m%d_%H%M%S')
for i, data in enumerate(self._logged_objects):
filename = '%s_%s.csv' % (self._object_to_filename[data[0]], timestamp)
fd = open(filename, 'w+')
writer = csv.writer(fd)
writer.writerow(Locatable.HISTORY_LABELS)
data[1] = (fd, writer)
print "LOGGING: opened %s for logging %s" % (filename, data[0])
def _logPositions(self):
for obj, (fd, writer) in self._logged_objects:
if obj.history[-1] != None:
writer.writerow(obj.history[-1])
else:
writer.writerow([self.time] + [0.0] * (len(Locatable.HISTORY_LABELS) - 1))
def _closePositionLogs(self):
for obj, (fd, writer) in self._logged_objects:
fd.close()
# record robot state
def _getRecorderVariableNames(self):
joints = self.jointnames
chains = self.chainnames
# Recording of joints / sensors
dcm_one_time_vars = ['DCM/HeatLogPath', 'DCM/I2Cpath', 'DCM/RealtimePriority']
self._record_file = self._record_csv = None
# Center of mass (computed)
com = ['Motion/Spaces/World/Com/%s/%s' % args for args in cross(
['Sensor', 'Command'], 'XYZ')] + ['Motion/BodyCommandAngles']
# various dcm stuff
dcm = ['DCM/Realtime', 'DCM/Time', 'DCM/TargetCycleTime',
'DCM/CycleTime', 'DCM/Simulation', 'DCM/hardnessMode',
'DCM/CycleTimeWarning']
# Joint positions
jsense = ['Device/SubDeviceList/%s/Position/Sensor/Value' % j for j in
joints]
# Actuator commanded position
actsense = ['Device/SubDeviceList/%s/%s/Value' % args for args in cross(
joints, ['ElectricCurrent/Sensor',
'Hardness/Actuator', 'Position/Actuator'])]
# inertial sensors
inert = ['Device/SubDeviceList/InertialSensor/%s/Sensor/Value' % sense
for sense in [
'AccX', 'AccY', 'AccZ', 'AngleX', 'AngleY',
'GyrRef', 'GyrX', 'GyrY']]
# Force SensoR
force = ['Device/SubDeviceList/%s/FSR/%s/Sensor/Value' % args for args in
cross(['RFoot', 'LFoot'],
['FrontLeft', 'FrontRight', 'RearLeft', 'RearRight'])]
# position of chains and __?
poschains = ['Motion/Spaces/Body/%s/Sensor/Position/%s' % args
for args in cross(chains, ['WX', 'WY', 'WZ', 'X', 'Y', 'Z'])]
transform = ['Motion/Spaces/World/Transform/%s' % coord for coord in
['WX', 'WY', 'WZ', 'X', 'Y', 'Z']]
# Various other stuff
various = ['Motion/SupportMode', 'Motion/Synchro', 'Motion/Walk/Active',
'MotorAngles', 'WalkIsActive', 'extractors/alinertial/position']
return (com + dcm + jsense + actsense + inert + force +
poschains + transform + various)
def _doRecord(self):
if not self._record_csv: return
# actuators and sensors for all dcm values
self._record_csv.writerow(self.getVars(self._recorded_vars))
self._record_line_num += 1
if self._record_line_num % 10 == 0:
print "(%3.3f) written csv line %s" % (self.time - self.start_time, self._record_line_num)
# Utility
def checkManModule(self):
""" report to user if the Man module isn't loaded. Since recently Man stopped being
logged as a module, we look for some of the variables we export.
"""
# note - blocking
print "Checking for Man module by getting a vision variable: %s" % self.MAN_ALMEMORY_EXISTANCE_TEST_VARIABLES
self._memory.getListData(self.MAN_ALMEMORY_EXISTANCE_TEST_VARIABLES).addCallback(self.onCheckManModuleResults).addErrback(log.err)
def onCheckManModuleResults(self, result):
#print "onCheckManModuleResults: %r" % (result,)
if result[0] == 'None':
print "WARNING " + "*"*60
print "WARNING"
print "WARNING >>>>>>> Man Isn't Running - naoload && naoqi restart <<<<<<<"
print "WARNING"
print "WARNING " + "*"*60
else:
print "Man found"
