def killAllTasks():
# Deprecated.
motionProxy = burst.getMotionProxy()
motionProxy.killAll()
# motionProxy.setBalanceMode(1)
# motionProxy.setSupportMode(0)
return None
def setStiffness(stiffness):
motionProxy = burst.getMotionProxy().post
pid = motionProxy.setBodyStiffness(stiffness)
# Dirty hack:
if stiffness == 0.0:
stopWalking()
return pid
def slowStraightWalk(distance):
motionProxy = burst.getMotionProxy().post
motionProxy.setWalkArmsConfig(
100.0 * burst.motion.TO_RAD, 10.0 * burst.motion.TO_RAD, 30.0 * burst.motion.TO_RAD, 10.0 * burst.motion.TO_RAD
)
motionProxy.setWalkArmsEnable(True)
motionProxy.setWalkExtraConfig(4.5, -4.5, 0.22, 2.0)
motionProxy.setWalkConfig(0.05, 0.04, 0.04, 0.4, 0.01, 0.00)
motionProxy.addWalkStraight(distance, 80)
return motionProxy.walk()
def initPosition():
motionProxy = burst.getMotionProxy().post
joints = [
"RShoulderPitch",
"RShoulderRoll",
"RElbowYaw",
"RElbowRoll",
"LShoulderPitch",
"LShoulderRoll",
"LElbowYaw",
"LElbowRoll",
"LHipYawPitch",
"RHipPitch",
"RHipRoll",
"LHipPitch",
"LHipRoll",
"RKneePitch",
"LKneePitch",
"RAnklePitch",
"RAnkleRoll",
"LAnklePitch",
"LAnkleRoll",
]
times = len(joints) * [[3]]
angles = [
[50],
[0],
[10],
[35],
[50],
[0],
[0],
[-35],
[-10],
[-40],
[0],
[-40],
[0],
[125],
[125],
[-70],
[0],
[-70],
[0],
]
for i in range(0, len(angles)):
for j in range(0, len(angles[i])):
angles[i][j] *= burst.motion.TO_RAD
return motionProxy.doMove(joints, angles, times, 1)
def fastStraightWalk(distance):
motionProxy = burst.getMotionProxy().post
motionProxy.setWalkExtraConfig(3.5, -3.5, 0.23, 3.0)
motionProxy.setWalkConfig(0.04, 0.02, 0.02, 0.3, 0.015, 0.018)
motionProxy.addWalkStraight(distance, 25)
return motionProxy.walk()
def addWalkStraight(distance, samples):
motionProxy = burst.getMotionProxy().post
return motionProxy.addWalkStraight(distance, samples)
def moveHead(x, y):
motionProxy = burst.getMotionProxy()
headStiffnessOn()
motionProxy.gotoChainAngles("Head", [float(x), float(y)], 1, 1)
def closeRightHand():
motionProxy = burst.getMotionProxy().post
return motionProxy.closeHand("RHand")
def addTurn(degrees):
motionProxy = burst.getMotionProxy().post
motionProxy.addTurn(float(degrees), 60)
def headStiffnessOff():
motionProxy = burst.getMotionProxy().post
motionProxy.setChainStiffness("Head", 0.0, 0)
# TODO: This entire file looks like shit.
import time
import burst
burst.init()
motion = burst.motion
def do():
broker = burst.getBroker()
motionProxy = burst.getMotionProxy()
# Define The Initial Position
kneeAngle = 60.0 * burst.motion.TO_RAD
torsoAngle = 0.0 * burst.motion.TO_RAD
wideAngle = -3.0 * burst.motion.TO_RAD
#Get the Number of Joints
NumJoints = len(motionProxy.getBodyJointNames())
# Define The Initial Position
if (NumJoints == 22) :
InitialPosition = [0.0 * motion.TO_RAD, 0.0 * motion.TO_RAD,
120.0 * motion.TO_RAD, 15.0 * motion.TO_RAD, -80.0 * motion.TO_RAD, -80.0 * motion.TO_RAD,
0.0, wideAngle, -kneeAngle/2-torsoAngle, kneeAngle, -kneeAngle/2, -wideAngle,
0.0, -wideAngle, -kneeAngle/2-torsoAngle, kneeAngle, -kneeAngle/2, wideAngle,
120.0 * motion.TO_RAD, -15.0 * motion.TO_RAD, 80.0 * motion.TO_RAD, 80.0 * motion.TO_RAD]
elif (NumJoints == 26) :
InitialPosition = [0.0 * motion.TO_RAD, 0.0 * motion.TO_RAD,
120.0 * motion.TO_RAD, 15.0 * motion.TO_RAD, -80.0 * motion.TO_RAD, -80.0 * motion.TO_RAD, 0.0 * motion.TO_RAD,0.0,
def unflexRightArm():
motionProxy = burst.getMotionProxy().post
x = motionProxy.getChainAngles("RArm")
x[3] = -math.pi
return motionProxy.post.gotoChainAngles("RArm", x, 3.0, 1)
def zeroPosition():
motionProxy = burst.getMotionProxy().post
return motionProxy.gotoBodyAnglesWithSpeed(26 * [0.0], 15, burst.motion.INTERPOLATION_SMOOTH)
def openRightHand():
motionProxy = burst.getMotionProxy().post
return motionProxy.openHand("RHand")
def stopWalking():
motionProxy = burst.getMotionProxy().post
motionProxy.clearFootsteps()
return None
def openLeftHand():
motionProxy = burst.getMotionProxy().post
return motionProxy.openHand("LHand")
def closeLeftHand():
motionProxy = burst.getMotionProxy().post
return motionProxy.closeHand("LHand")
def do(): motion = burst.motion ALMotion_Proxy = burst.getMotionProxy() TimeInterpolation = 0.05 ALMotion_Proxy.setBodyStiffness(1.0,TimeInterpolation) time.sleep(TimeInterpolation) joints = ["HeadPitch","HeadYaw","LAnklePitch","LAnkleRoll","LElbowRoll","LElbowYaw","LHipPitch","LHipRoll","LHipYawPitch","LKneePitch","LShoulderPitch","LShoulderRoll","RAnklePitch","RAnkleRoll","RElbowRoll","RElbowYaw","RHipPitch","RHipRoll","RKneePitch","RShoulderPitch","RShoulderRoll"]; angles = [[0,0], [3.37175e-007,3.37175e-007], [0.523599,0.523599], [0,0], [0,0], [0,0], [0,0], [0,0], [0,0], [0,0], [0,0], [1.5708,1.5708], [0.523599,0.523599], [0,0], [0,0], [0,0], [0,0], [0,0], [0,0], [0,0], [-1.5708,-1.5708], ]; times = [[1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], [1.9,2.9], ]; try: moveId = ALMotion_Proxy.doMove(joints, angles, times, 1); except Exception,e: print "get up from back test Failed" exit(1)
def turn(degrees):
motionProxy = burst.getMotionProxy().post
motionProxy.addTurn(float(degrees), 60)
return motionProxy.walk()
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 wait(pid):
motionProxy = burst.getMotionProxy()
motionProxy.wait(pid, 0)