def _complete_init(self, _):
# TODO - this is slow but only done on init
map_d = chainDeferreds([lambda result, i=i, varname=varname: self._shm_proxy.addMappedVariable(i, varname)
for i, varname in enumerate(self._var_names)])
map_d.addCallback(lambda _: self._shm_proxy.getNumberOfVariables().addCallback(
self._reportNumberOfVariablesInBurstmem))
map_d.addCallback(lambda _:
self._shm_proxy.isMemoryMapRunning().addCallback(self._completeOpen))
map_d.addErrback(log.err)
def onSearchCallback(self):
self.log('Search done')
self.log(' | '.join(map(str, self.targets)))
for t in self.targets:
if t.centered_self.sighted_centered:
print "%s sighted centered" % t.name
else:
print "%s NOT sighted centered" % t.name
if len([x for x in self.targets if x.centered_self.sighted]) == 0:
print "Nothing found, not looking at targets"
return self._start()
self.log("Going towards all targets")
chainDeferreds([
lambda _, t=t: self.lookAt(t).onDone(
lambda: self._eventmanager.callLaterBD(2.0)).getDeferred()
for t in self.targets]).addCallback(self._start)
def sitPoseAndRelax_returnDeferred(self): # TODO: This appears to be a blocking function!
dgens = []
def removeStiffness(_):
if burst.options.debug:
print "sitPoseAndRelax: removing body stiffness"
return self._motion.setBodyStiffness(0)
dgens.append(lambda _: self._clearFootsteps_returnDeferred())
#dgens.append(lambda _: self.executeMove(poses.STAND).getDeferred())
dgens.append(lambda _: self.executeMove(poses.SIT_POS).getDeferred())
dgens.append(removeStiffness)
return chainDeferreds(dgens)
def turn(self, deltaTheta, walk=walks.TURN_WALK):
print walk
self.setWalkConfig(walk.walkParameters)
dgens = []
dgens.append(lambda _: self._motion.setSupportMode(SUPPORT_MODE_DOUBLE_LEFT))
print "ADD TURN (deltaTheta): %f" % (deltaTheta)
dgens.append(lambda _: self._motion.addTurn(deltaTheta, walk.defaultSpeed))
duration = 1.0 # TODO - compute duration correctly
d = chainDeferreds(dgens)
self._current_motion_bd = self._movecoordinator.walk(d, duration=duration,
description=('turn', deltaTheta, walk))
return self._current_motion_bd
def changeLocationArc(self, delta_x, delta_y, walk=walks.STRAIGHT_WALK):
#handle divide by zero
if delta_y==0:
return self.changeLocationRelativeSideways(delta_x, delta_y, walk)
#calculate radius
#r=((y{-,+}r)**2 + x**2)**0.5
#0= y**2 + r**2 {+/-}y*r*2 + x**2 - r**2
#r=abs( (y**2 + x**2) / (2*y) )
r= abs( delta_y/2 + (delta_x**2)/(2*delta_y) )
#sin angle = y/r
angle = asin(delta_x/r)
if delta_y<0:
angle=-angle
#print "Calculated radius: %f, calculated angle %f" % (r, angle)
# TODO: Handle addWalkArc limitations (min/max radius)
# TODO: Move to journey.py (???)
dgens = [] # deferred generators. This is the DESIGN PATTERN to collect a bunch of
# stuff that would have been synchronous and turn it asynchronous
# All lambda's should have one parameter, the result of the last deferred.
dgens.append(lambda _: self._motion.setSupportMode(SUPPORT_MODE_DOUBLE_LEFT))
dgens.append(lambda _: self.setWalkConfig(walk.walkParameters))
defaultSpeed = walk.defaultSpeed
# give radius in meters!!!
dgens.append(lambda _: self._motion.addWalkArc( angle, r / 100. , defaultSpeed ))
# TODO: Calculate arc length to get possible duration (by arc_length/speed)
duration = 10
d = chainDeferreds(dgens)
self._current_motion_bd = self._movecoordinator.walk(d, duration=duration,
description=('arc', delta_x, delta_y, walk.name))
return self._current_motion_bd
def _executeAllCommands(self):
if self._printQueueBeforeExecution and len(self._cmds) > 4: # 3 - walkconfig, support, single leg (slow+regular walk).
print "Executing Journey Queue: %s" % ('; '.join(desc for desc, f in self._cmds))
d = chainDeferreds([f for desc, f in self._cmds])
self._cmds = []
return d
def changeLocationRelativeSideways(self, delta_x, delta_y = 0.0, walk=walks.SIDESTEP_WALK):
"""
Add an optional addWalkSideways and StraightWalk to ALMotion's queue.
Will fire EVENT_CHANGE_LOCATION_DONE once finished.
Coordinate frame for robot is same as world: x forward, y left (z up)
X & Y are in radians!
What kind of walk is this: for simplicity we do sidewalking then walking to target
(we assume no orientation change is required)
"""
print "changeLocationRelativeSideways (delta_x: %3.3f delta_y: %3.3f)" % (delta_x, delta_y)
distance, distanceSideways = delta_x / CM_TO_METER, delta_y / CM_TO_METER
did_sideways = None
dgens = [] # deferred generators. This is the DESIGN PATTERN to collect a bunch of
# stuff that would have been synchronous and turn it asynchronous
# All lambda's should have one parameter, the result of the last deferred.
dgens.append(lambda _: self._motion.setSupportMode(SUPPORT_MODE_DOUBLE_LEFT))
# if abs(distanceSideways) >= MINIMAL_CHANGELOCATION_SIDEWAYS:
# walk = walks.SIDESTEP_WALK
dgens.append(lambda _: self.setWalkConfig(walk.walkParameters))
defaultSpeed = walk.defaultSpeed
stepLength = walk[WalkParameters.StepLength]
if distance >= MINIMAL_CHANGELOCATION_X:
print "WALKING STRAIGHT (stepLength: %3.3f distance: %3.3f defaultSpeed: %3.3f)" % (stepLength, distance, defaultSpeed)
#dgens.append(lambda _: self._motion.addWalkStraight( distance, defaultSpeed ))
# Vova trick - start with slower walk, then do the faster walk.
slow_walk_distance = min(distance, stepLength*2)
if walks.FIRST_TWO_SLOW_STEPS and World.connected_to_nao:
dgens.append(lambda _: self._motion.addWalkStraight( slow_walk_distance, DEFAULT_SLOW_WALK_STEPS ))
dgens.append(lambda _: self._motion.addWalkStraight( distance - slow_walk_distance, defaultSpeed ))
else:
print "ADD WALK STRAIGHT: %f, %f" % (distance, defaultSpeed)
dgens.append(lambda _: self._motion.addWalkStraight( distance, defaultSpeed ))
# When asked to do side-stepping for a very short distance, do a minimal one
if abs(distanceSideways) <= MINIMAL_CHANGELOCATION_SIDEWAYS:
print "MINOR SIDEWAYS MOVEMENT ENLARGED! (%3.3f)" % distanceSideways
if distanceSideways < 0:
distanceSideways = -MINIMAL_CHANGELOCATION_SIDEWAYS
else:
distanceSideways = MINIMAL_CHANGELOCATION_SIDEWAYS
print "WALKING SIDEWAYS (%3.3f)" % distanceSideways
did_sideways = distanceSideways
dgens.append(lambda _: self._motion.addWalkSideways(distanceSideways, defaultSpeed))
duration = (defaultSpeed * distance / stepLength +
(did_sideways and defaultSpeed or self._eventmanager.dt) ) * 0.02 # 20ms steps
print "Estimated duration: %3.3f" % (duration)
d = chainDeferreds(dgens)
self._current_motion_bd = self._movecoordinator.walk(d, duration=duration,
description=('sideway', delta_x, delta_y, walk.name))
return self._current_motion_bd
