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