joy.plans

1 import types, sys 2 3 # For NBRPlan 4 from os import O_NONBLOCK 5 from fcntl import F_SETFL, F_GETFL, fcntl 6 from errno import EAGAIN 7 8 from pygix import EventType 9 from decl import * 10 11 from math import pi,exp,floor 12 from warnings import warn 13 14 # Logging interface 15 from loggit import progress, debugMsg, dbgId 16 17 from misc import curry,printExc 18 19 from events import describeEvt 20 DEBUG = []

21 22 -class Plan( object ):

23 """ 24 Abstract superclass of scheduled behaviors 25 26 Each Plan implements a "co-routine" consisting of a behavior 27 that executes in parallel with other plans. It has its own event 28 queue and event handler, and its own bindings to outputs. 29 30 Class Plan is use by subclassing it and overriding the behavior() 31 method with a generator -- i.e. a method that has a bunch of yield 32 statements in it. The yield statements MUST all be "yield None", 33 which can be abbreviated to "yield" with no parameters. 34 The yields indicate the locations at which the Plan returns control 35 to the owner JoyApp. 36 37 Plans continue executing from where they stopped whenever the event 38 handler returns True. Otherwise it should return False. Other return 39 values will generate an exception. 40 41 Plans may be nested; simply yield the plan that is to take over control. 42 NOTE: Make sure that the nested Plan was start()-ed 43 44 Some methods are designed to be used with yield, specifically: 45 untilTime, forDuration 46 47 WARNING: JoyApp is single-threaded. If a behavior spends too long 48 between yield statements the entire JoyApp waits for it to complete. 49 50 Typical usage: 51 >>> class MyPlan( Plan ): 52 ... def __init__(self,app): 53 ... Plan.__init__(self,app) 54 ... 55 ... def onEvent( self, evt ): 56 ... if evt.type == MOUSEMOTION: 57 ... self.x = evt.pos[0] 58 ... return True 59 ... return False 60 ... 61 ... def behavior(self): 62 ... print "Move mouse to the left" 63 ... while self.x > self.app.cfg.windowSize[0]*0.25: 64 ... yield 65 ... print "Move mouse to the right" 66 ... while self.x < self.app.cfg.windowSize[0]*0.75: 67 ... yield 68 ... print "Great job!! here are 3 tail wags for you" 69 ... for wag in xrange(3): 70 ... self.tailWagTo(100) 71 ... yield self.forDuration( 500 ) 72 ... self.tailWagTo(-100) 73 ... yield self.forDuration( 500 ) 74 75 BINDINGS: To allow plans to be easily adapted and re-used 76 without sub-classing, the Plan constructor accepts a list 77 of keyword argument "bindings" of the form <name>=<setter>, 78 where <setter> is either a callable or a string naming a 79 robot property to be resolved using app.robot.setterOf(). 80 This allows self.<name> to be used in the Plan code for 81 setting values that will only be chosen at the time the 82 plan subclass instance is constructed. In the example above, 83 we could have used MyPlan( tailWagTo='Nx55/@set_pos' ) to create 84 an instance whose self.tailWagTo sets the position in robot 85 node 0x55. Similarly, tailWagTo='>tail wag' would bind the 86 output to the Scratch sensor named 'tail wag' 87 88 """

89 - def __init__( self, app, **binding ):

90 """ 91 Initialize a Plan within some JoyApp application. 92 """ 93 self.DEBUG = app.DEBUG 94 self.app = app 95 self.__evts = [] 96 self.__stack = [] 97 if binding and not hasattr(self,'_binding'): 98 self._initBindings(app,binding)

99

100 - def _initBindings( self, app, binding, allowOverride = False ):

101 """(protected) initialize bindings, resolving any string valued bindings""" 102 for key,func in binding.iteritems(): 103 if hasattr(self,key) and not allowOverride: 104 raise KeyError("Plan already has an attribute '%s' -- bind another name" % key ) 105 if type(func) is str: 106 func = app.setterOf( func ) 107 if not callable(func): 108 warn( "Binding non-callable %s to '%s'; are you sure?" % (repr(func),key)) 109 setattr(self,key,func) 110 self._binding = binding

111

112 - def push( self, *evts ):

113 """ 114 Push events into this Plan's event queue 115 116 Events are only queued while the Plan isRunning(); otherwise 117 they are silently dropped. 118 """ 119 if not self.isRunning(): 120 return 121 for evt in evts: 122 assert type(evt) is EventType 123 self.__evts.append(evt)

124

125 - def onEvent( self, evt ):

126 """(default) 127 Event handler. Override this method to handle all events that 128 were push()-ed to this Plan. 129 130 The sequential behavior() in this plan only runs if onEvent 131 returned True. 132 133 All Plan-s recieve a copy of all TIMEREVENT events. If your 134 sequential code does not need any other events, and just wants 135 to execute in parallel with other Plan-s, it is safe to leave 136 the default onEvent method, which always returns True 137 138 NOTE: 139 onEvent MUST return True or False. Other values raise an 140 exception at runtime. 141 """ 142 return True

143

144 - def behavior( self ):

145 """ 146 (default) 147 148 Override this method to implement the sequential behavior of a Plan 149 150 This method MUST be a python iterator, i.e. must contain a "yield" statement 151 """ 152 progress("Goodbye, cruel world!") 153 yield

154

155 - def untilTime(self,time):

156 """ 157 A sub-plan that sleeps until a specified time. 158 159 Use this from .behavior() with a statement of the form: 160 yield self.untilTime( wakeup ) 161 to sleep until the specified wakeup time. 162 """ 163 while self.app.now<time: 164 yield

165

166 - def forDuration(self,duration):

167 """ 168 A sub-plan that sleeps for a specified duration. 169 170 Use this from .behavior() with a statement of the form: 171 yield self.forDuration( naplength ) 172 to sleep for the specified duration 173 """ 174 end = self.app.now + duration 175 while self.app.now<end: 176 yield

177

178 - def isRunning( self ):

179 """ 180 Returns True if and only if this Plan is currently "running", 181 i.e. its .onEvent handler receives events and its .behavior 182 has not terminated. 183 """ 184 return bool(self.__stack)

185

186 - def start( self ):

187 """ 188 Start execution of a Plan's behavior. 189 190 If Plan is already running -- issues a warning 191 """ 192 if self.isRunning(): 193 warn("Plan is already running") 194 return 195 # Flush event queue 196 self.__evts = [] 197 self.__stack = [self.behavior()] 198 self.__sub = None 199 self.app._startPlan(self) 200 self.onStart()

201

202 - def onStart( self ):

203 """(default) 204 205 Override this method to perform operations when Plan starts but before the 206 first events are processed. 207 """ 208 pass

209

210 - def stop( self, force=False ):

211 """Stop (abnormally) the execution of a Plan's behavior 212 213 If not running, this call is ignored. 214 215 INPUT: 216 force -- boolean -- stop without raising termination exceptions in co-routines 217 """ 218 if not self.isRunning(): 219 return 220 # Stop sub-Plans first 221 if self.__sub is not None: 222 self.__sub.stop(force) 223 # If forced --> drop reference to the stack 224 if force: 225 self.__stack=[] 226 else: 227 # Regular termination --> close() all co-routines 228 while self.__stack: 229 ctx = self.__stack.pop() 230 try: 231 ctx.close() 232 except StandardError: 233 printExc() 234 self.onStop()

235

236 - def onStop( self ):

237 """(default) 238 239 Override this method to perform operations when Plan terminates. 240 """ 241 pass

242

243 - def _unwind( self, exinfo ):

244 """(private) 245 246 Unwind Plan co-routines by throwing exceptions up the stack 247 """ 248 S = self.__stack 249 top = None 250 sub = None 251 # While stack is not empty 252 while S: 253 try: 254 # Pop the top-most co-routine 255 top = S.pop() 256 # throw the exception in it 257 # if caught --> return a yielded value in sub 258 # else --> go to except clause 259 sub = top.throw( *exinfo ) 260 # If reached exception was handled --> keep remaining stack 261 break 262 except StandardError: 263 # exception was not caught, killing top 264 # tracebacks are now concatenated, so get new head of traceback 265 exinfo = sys.exc_info() 266 # (only reached if exception handled) 267 printExc(exinfo) 268 # Put top back on stack, it isn't dead 269 if top is not None: 270 S.append(top) 271 return sub

272

273 - def _stepDoEvents( self ):

274 """(private) 275 276 Run onEvent() method on all incoming events 277 278 OUTPUT: 279 True if plan can go idle (no relevant events) or False otherwise 280 """ 281 idle = True 282 while self.__evts: 283 evt = self.__evts.pop(0) 284 try: 285 rc = self.onEvent(evt) 286 except StandardError, se: 287 printExc() 288 rc = False 289 if rc is True: 290 idle = False 291 elif rc is not False: 292 raise TypeError("A Plan's onEvent() method MUST return True or False") 293 return idle

294

295 - def _oneStep( self ):

296 """(private) 297 298 Execute one step from the iterator at the top of the stack 299 300 OUTPUT: 301 co-routine to execute as sub-Plan, or None 302 """ 303 S = self.__stack 304 top = S.pop() 305 try: 306 if 'p' in self.DEBUG: debugMsg(self,"stepping from "+repr(top)) 307 sub = top.next() 308 if 'p' in self.DEBUG: debugMsg(self," -->"+dbgId(sub)) 309 # if reached iteration did not terminate normally or by exception 310 # --> put top back on stack 311 S.append(top) 312 except StopIteration: 313 # Top of stack terminated -- step is done 314 sub = None 315 if 'p' in self.DEBUG: debugMsg(self," <<terminated>> "+dbgId(sub)+repr(S)) 316 except StandardError: 317 # An exception occurred -- unwind the stack to expose its origin 318 sub = self._unwind( sys.exc_info() ) 319 return sub

320

321 - def step( self ):

322 """ 323 Execute one time-step of Plan behavior. 324 325 This method is called every time the .onEvent handler returns True for 326 one or more events in a time-slice. 327 """ 328 if not self.isRunning(): 329 raise RuntimeError("Cannot step() %s -- not isRunning()" % dbgId(self)) 330 if 'p' in self.DEBUG: debugMsg(self,"begin step "+repr(self.__stack)) 331 # 332 # If handling events leaves us idle --> done 333 # 334 if self._stepDoEvents(): return 335 if 'p' in self.DEBUG: debugMsg(self,"event detected") 336 # 337 # If an active sub-Plan is running --> done 338 # 339 if self.__sub is not None: 340 if self.__sub.isRunning(): return 341 # Sub-plan finished execution; remove it 342 self.__sub = None 343 # 344 # If execution step yielded something --> process new sub-plan 345 # 346 sub = self._oneStep() 347 if sub is not None: 348 self._stepNewSub( sub ) 349 # 350 # If terminated --> call onStop 351 if not self.isRunning(): 352 self.onStop()

353

354 - def _stepNewSub( self, sub ):

355 """(private) 356 357 Plan yielded a sub-plan to execute -- process it 358 """ 359 # If another Plan 360 if isinstance(sub,Plan): 361 if sub.isRunning(): 362 raise RuntimeError("Sub-plan is already running") 363 self.__sub = sub 364 sub.start() 365 if 's' in self.DEBUG: 366 debugMsg(self,"new sub-Plan "+dbgId(sub)+" isR "+repr(sub.isRunning())) 367 debugMsg(self," app plans %s" % dbgId(self.app.plans) ) 368 # Push Plan behavior() generator on stack 369 # If a generator --> top is None,execution 370 elif type(sub) is types.GeneratorType: 371 self.__stack.append(sub) 372 if 's' in self.DEBUG: debugMsg(self,"new sub-generator "+dbgId(sub)) 373 else: # --> yield returned object of the wrong type 374 raise TypeError("Plan-s may only yield None, Plan-s or generators")

375

376 -class SheetPlan( Plan ):

377 """ 378 concrete class SheetPlan implements a Plan that is read from 379 a "sheet" -- a rectangular list-of-lists containing a spreadsheet 380 specifying the planned settings as a function of time. 381 382 Sheets can be read from a file using loadCSV(), or from an 383 inline multiline string using inlineCSV() 384 385 The first row of the sheet consists of string headings, starting 386 with the heading "t" (for time). Remaining headings specify 387 bindings to properties the Plan should set. 388 389 By default, the column headings will get automatically converted into 390 a binding using the format <<heading>>/@set_pos which means that 391 columns are assumed to be names of modules whose position is to be 392 set. WARNING: this default behavior is disabled if ANY bindings are 393 passed to the constructor. 394 395 The first column of the sheet (2nd row an on) MUST consist of 396 numbers in increasing order specifying the times at which the 397 settings in that row should be applied. 398 399 Empty elements in the sheet (None-s) are skipped. 400 401 If no column bindings are provided, column headings are assumed 402 to be module names of servo modules and each of these is bound to 403 <<module>>/@set_pos of the corresponding module. For example, the sheet: 404 >>> sheet = [ 405 ... ["t", "Nx15"], 406 ... [0, 1000], 407 ... [0.5, -1000] ] 408 will generate a +/- 10 degree step into the position of node 0x15 409 """

410 - def __init__(self,app,sheet,*arg,**kw):

411 if not kw: 412 kw = self._autoBinding(sheet) 413 Plan.__init__(self,app,*arg,**kw) 414 self.setters,self.headings,self.sheet = self._parseSheet(sheet) 415 self.rate = 1.0

416

417 - def update( self, sheet, **kw ):

418 """ 419 Update the sheet specifying the Plan. 420 421 NOTE: if the new sheet has different bindings form the old one, Weird Things Will Happen! 422 """ 423 if self.isRunning(): 424 raise TypeError,"Cannot update sheet while plan is running" 425 if not kw: 426 kw = self._autoBinding(sheet) 427 self._initBindings( self.app, kw, allowOverride = True ) 428 self.setters,self.headings,self.sheet = self._parseSheet(sheet)

429

430 - def _autoBinding( self, sheet, fmt="%s/@set_pos" ):

431 """(private) 432 433 Given a sheet, automatically treat the column heads as 434 servo module names for which positions must be set 435 """ 436 heads = sheet[0][1:] 437 return dict([ (nm,fmt % nm ) for nm in heads])

438

439 - def setRate( self, rate ):

440 """ 441 Set rate at which sheet should be executed, e.g. 442 rate=2 means execute twice as fast 443 444 Only positive rates are allowed. Rate cannot be changed 445 while the Plan isRunning() 446 """ 447 rate = float(rate) 448 if rate<=0: 449 raise ValueError("Rate %g should have been >0" % rate) 450 self.rate = rate

451

452 - def getRate(self): return self.rate

453

454 - def _doRowSets( self, row ):

455 """(private) 456 457 Perform the set operations associated with a row 458 """ 459 try: 460 for func,val in zip(self.setters,row): 461 if val is not None: 462 func(val) 463 if 'G' in self.DEBUG: 464 debugMsg(self,"%s(%s)" % (dbgId(func),repr(val))) 465 except StandardError: 466 printExc()

467

468 - def _parseSheet( self, sheet ):

469 """(private) 470 471 Make sure that the sheet is syntactically correct 472 473 OUTPUT: setters, headings, sheet 474 """ 475 # First column heading is 't' for time 476 if sheet[0][0] != "t": 477 raise KeyError('First column of sheet must have heading "t" not %s' % repr(sheet[0][0])) 478 # Split off headings and contents 479 headings = sheet[0][1:] 480 sheet = sheet[1:] 481 # Ensure rows are equal length 482 w = len(headings)+1 483 lb = sheet[0][0]-1 484 for li in xrange(len(sheet)): 485 l = sheet[li] 486 if len(l)!=w: 487 raise ValueError( 488 'Sheet row %d has %d entries instead of %d'%(li+1,len(sheet[li]),w) ) 489 if type(l[0]) not in [int,float]: 490 raise TypeError( 491 'Sheet row %d has "%s" instead of a time' % (li+1,l[0])) 492 if l[0]<=lb: 493 raise IndexError( 494 'Sheet row %d has non-increasing time %d' % (li+1,l[0])) 495 lb = l[0] 496 # Ensure that headings make sense as properties 497 try: 498 res = [] 499 for name in headings: 500 res.append(getattr(self,name)) 501 except AttributeError: 502 raise AttributeError('Sheet addresses property "%s" which has no binding. Hint: use <property>=<setter-function> in constructor' % name) 503 return res,headings,sheet

504

505 - def behavior(self):

506 """(final) 507 508 The behavior of a SheetPlan is defined relative to the Plan's start time 509 and rate. The plan goes down the sheet row by row in the specified rate, 510 sleeping until it is ready to apply the settings in that row. 511 512 Once the settings of the final row are applied, the Plan terminates. 513 514 The 't' debug topic can be used to display SheetPlan timesteps 515 """ 516 t0 = self.app.now 517 rate = self.rate 518 if 't' in self.DEBUG: debugMsg(self,"started at t0=%g" % t0) 519 for row in self.sheet: 520 yield self.untilTime( row[0] / rate + t0 ) 521 if 't' in self.DEBUG: debugMsg(self,"step at t=%g" % (self.app.now-t0)) 522 self._doRowSets( row[1:] ) 523 if 't' in self.DEBUG: debugMsg(self,"ended at t=%g" % (self.app.now-t0))

524

525 -class CyclePlan( Plan ):

526 """ 527 A CyclePlan implements a circular list of action callbacks, indexed by 528 'phase' -- a real number between 0 and 1. At any given time the CyclePlan 529 has a position (stored in the private variable ._pos) on this cycle. 530 531 The position can change by using .moveToPhase or by the passage of time, 532 through setting a period or frequency for cycling. This frequency or period 533 may also be negative, causing the CyclePlan to cycle in reverse order. 534 535 In abstract, it is ambiguous in which direction around the cycle a given 536 .moveToPhase should proceed. This ambiguity is resolved by allowing the goal 537 phase to be outside the range 0 to 1. Goals larger than current phase cycle 538 forward and goals smaller than current phase cycle back. 539 540 Whenever phase changes, all actions in the circular arc of phases between the 541 previous and new phase are executed in order. If the arc consists of more 542 than one cycle, phase changes to 0, the .onCycles method is called with the 543 integer number of positive or negative cycles, and the remaining partial 544 cycle to the goal is called. 545 """

546 - def __init__(self,app,actions,maxFreq=10.0,*arg,**kw):

547 """ 548 Instantiate a CyclePlan 549 550 INPUT: 551 app -- JoyApp -- application containing this Plan instance 552 actions -- dict -- mapping floating point phases in the range 0 to 1 to 553 python callable()-s that take no parameters. 554 maxFreq -- float -- maximal cycling frequency allowed for this Plan. 555 """ 556 Plan.__init__(self,app,*arg,**kw) 557 # Maximal frequency at which to cycle completely thorugh gait 558 self.maxFreq = maxFreq 559 # Validate actions dictionary and construct lookup table 560 steps = self.__initSteps(actions) 561 self._lookup = ( [-1]+steps+[2] ) 562 self.actions = actions 563 self.phase = 0 564 self._pos = 1 565 self.period = 1.0

566

567 - def __initSteps( self, actions ):

568 """(private) 569 570 Scan actions table, construct lookup index and resolve bindings 571 """ 572 steps = [] 573 for phi,act in actions.iteritems(): 574 if phi<0 or phi>1: 575 raise KeyError("Phase %g is outside valid range [0,1]"%phi) 576 if type(act)==str: 577 try: 578 act = getattr(self,act) 579 except AttributeError: 580 raise KeyError("String action at phi=%g was not in bindings" % phi) 581 if not callable(act): 582 raise TypeError("Action at phi=%g is not callable in binding" % phi) 583 steps.append(phi) 584 steps.sort() 585 return steps

586 587 @staticmethod

588 - def bsearch( k, lst ):

589 """ 590 Binary search for k through the sorted sequence lst 591 592 Returns the position of the first element larger or equal to k 593 """ 594 l = 0 595 u = len(lst) 596 while u>l: 597 m = (u+l)/2 598 if k>lst[m]: 599 l = m+1 600 else: 601 u =m 602 return l

603

604 - def setPeriod( self, period ):

605 """ 606 Set period for cycles, 0 to stop in place 607 """ 608 if abs(period)>1.0/self.maxFreq: 609 self.period = float(period) 610 elif period==0: 611 self.period = 0 612 elif period>0: 613 self.period = 1.0/self.maxFreq 614 elif period<0: 615 self.period = -1.0/self.maxFreq

616

617 - def getPeriod( self ):

618 """Return the cycling period""" 619 return self.period

620

621 - def setFrequency(self, freq):

622 """ 623 Set frequency for cycles, 0 to stop in place 624 """ 625 if freq==0: 626 self.period = 0 627 elif freq>self.maxFreq: 628 self.period = (1.0/self.maxFreq) 629 elif freq<-self.maxFreq: 630 self.period = (-1.0/self.maxFreq) 631 else: 632 self.period = (1.0/freq)

633

634 - def getFrequency(self):

635 """ 636 Return the cycling frequency 637 """ 638 if self.period: return 1.0/self.period 639 return 0

640

641 - def onCycles(self,cyc):

642 """(default) 643 644 Move forward or back several complete cycles, starting and 645 ending at phase 0 646 647 INPUT: 648 cyc -- number of cycles; nonzero 649 650 The default implementation is to ignore complete cycles. 651 Override in subclasses if you need an accurate count of 652 winding numbers. 653 """ 654 if 'c' in self.DEBUG: 655 debugMsg(self,' onCycles %d' % cyc)

656

657 - def moveToPhase( self, phi ):

658 """ 659 Move from the current phase (self.phase) to a new phase phi. 660 661 phi values larger than 1.0 will always cause forward cycling through 0.0 662 phi values smaller than 0.0 will cause reverse cycling though 1.0 663 Values in the range [0,1] will move via the shortest sequence 664 of steps from current phase to new phase. 665 """ 666 cyc = floor(phi) 667 if phi > self.phase: 668 if phi<1: 669 npos = self.bsearch(phi, self._lookup) 670 if self._pos < len(self._lookup)-1: 671 self._doActions( slice(self._pos, npos,1) ) 672 else: 673 npos = self.bsearch(phi-cyc, self._lookup) 674 if 'c' in self.DEBUG: 675 debugMsg(self,'movePhase up %d --> -2, 1 --> %d' % (self._pos,npos)) 676 if self._pos < len(self._lookup)-1: 677 self._doActions( slice(self._pos, -2,1) ) 678 if phi>=2: 679 self.onCycles(cyc-1) 680 self._doActions( slice(1, npos,1) ) 681 else: 682 assert phi <= self.phase 683 if phi > 0: 684 npos = self.bsearch(phi, self._lookup) 685 if self._pos>1: 686 self._doActions( slice(self._pos-1, npos-1, -1) ) 687 else: 688 npos = self.bsearch(phi-cyc, self._lookup) 689 if 'c' in self.DEBUG: 690 debugMsg(self,'movePhase down %d --> 1, -2 --> %d' % (self._pos-1,npos-1)) 691 if self._pos>1: 692 self._doActions( slice(self._pos-1, 0, -1 ) ) 693 if phi<-1: 694 self.onCycles(cyc+1) 695 self._doActions( slice(-2, npos-1, -1) ) 696 # final state is wrapped phase with new position 697 self.phase = phi-cyc 698 self._pos = npos

699

700 - def _doActions( self, slc ):

701 """(private) 702 703 Do the actions associated with action table entries in the specified slice 704 """ 705 if 'C' in self.DEBUG: 706 debugMsg(self,'doActions %s ~%5.2g ~%5.2g ' % (repr(slc), 707 self._lookup[slc.start],self._lookup[slc.stop - slc.step])) 708 self.seq = slc.indices(len(self._lookup)) 709 for self._pos in xrange(*self.seq): 710 self.phase = self._lookup[self._pos] 711 if 'C' in self.DEBUG: 712 debugMsg(self,' -- do %d ~%5.2g' % (self._pos,self.phase)) 713 try: 714 self.actions[self.phase](self) 715 except StandardError: 716 printExc()

717

718 - def resetPhase(self, phi0=0):

719 """ 720 Reset phase to the specified value at the current time, without 721 taking any actions 722 """ 723 self.phase = float(phi0) % 1.0

724

725 - def behavior(self):

726 """(final) 727 728 CyclePlan behavior is to cycle through the actions at the 729 period controlled by self.period 730 731 If period is set to 0, actions will only be called as consequence 732 of calling .moveToPhase 733 """ 734 last = self.app.now 735 while True: 736 if self.period: 737 phi = self.phase + (last-self.app.now) / self.period 738 self.moveToPhase(phi) 739 last = self.app.now 740 yield

741

742 -class FunctionCyclePlan( CyclePlan ):

743 """Concrete class FunctionCyclePlan implements a plan that cycles 744 'phase' at an adjustable frequency through the interval [0..1], 745 calling a user specified function every time the phase crosses 746 a knot point. 747 748 Additional features: 749 (1) An upper limit on allowed frequency 750 (2) Knots points may be specified at construction or equally spaced 751 (3) Function calls may be decimated so that a minimal time interval 752 elapses between calls. 753 754 Typical usage is to write a function mapping phase to some kinematic 755 state of a robot gait. In that case, one typically sets knots to 756 be either uniform, or preferentially represent the high-curvature 757 parts of the gait data. The interval limit is set to a realistic 758 closed loop delay value, ensuring that commands to the motors are 759 not sent faster than they can be processed. 760 """

761 - def __init__(self, app, atPhiFun, N=None, knots=None, maxFreq=10.0, interval=0, *arg, **kw):

762 """ 763 INPUTS: 764 app -- JoyApp -- owner 765 atPhiFun -- callable -- function that will be called with a 766 phase in the range [0..1]. 767 N -- integer (optional) -- number of knot points, for regularly 768 spaced knot points. 769 knots -- sequence -- sorted sequence of values in the range 770 [0..1], designating phases at which atPhiFun is called 771 maxFreq -- float -- maximal frequency for cycling (Hz) 772 interval -- float -- minimal time elapsed between calls 773 """ 774 if knots is None: 775 if N is None or N != int(N) or N<1: 776 raise ValueError('Must specify either positive integer N or knots') 777 knots = [ float(x)/N for x in xrange(N) ] 778 if not callable(atPhiFun): 779 raise TypeError('atPhiFun must be callable') 780 self._func = atPhiFun 781 self._time = app.now 782 self._interval = interval 783 act = {} 784 for k in knots: 785 act[k] = self._action 786 787 CyclePlan.__init__(self,app,act,maxFreq)

788

789 - def _action( self, arg ):

790 # If call rate is limited 791 if self._interval: 792 # If it isn't time to call yet --> return 793 if self.app.now<self._time: 794 return 795 # Set time limit for next call 796 self._time = self.app.now + self._interval 797 assert arg is self 798 self._func( self.phase )

799

800 -class GaitCyclePlan( CyclePlan, SheetPlan ):

801 """ 802 GaitCyclePlan-s combine the benefits of a SheetPlan with those of a 803 CyclePlan. They represent "cyclical spreadsheets" consisting of settings 804 that should be applied via the Plan bindings at various phases in the 805 cycle. 806 807 GaitCyclePlan usage example: 808 >>> gcp = GaitCyclePlan( app, 809 ... sheet = [['t','x'],[0,1],[0.25,0],[0.5,-1],[0.75,0]], 810 ... x = '>pos' ) 811 >>> gcp.start() 812 813 would make a 4-step triangle wave be emitted to Scratch variable 'pos' 814 """

815 - def __init__(self, app, sheet, maxFreq=10.0, *arg, **binding ):

816 """ 817 Initialize a GaitCyclePlan instance 818 819 INPUTS: 820 app -- JoyApp -- application containing this Plan 821 sheet -- list of lists -- gait table (see SheetPlan for details) 822 maxFreq -- float -- maximal gait frequency; default is 10 Hz 823 824 Additional keyword arguments provide output bindings for the 825 gait table columns. sheet[0] is a list of column headings, with 826 sheet[0][0]=='t' the time (phase). Unlike other SheetPlans, the 827 time column in a GaitCyclePlan MUST range 0.0 to 1.0 828 829 If no column bindings are provided, column headings are assumed 830 to be module names of servo modules and each of these is bound to 831 <<module>>/@set_pos of the corresponding module. For example, the sheet: 832 >>> sheet = [ 833 ... ["t", "Nx15"], 834 ... [0, 1000], 835 ... [0.5, -1000] ] 836 will generate a +/- 10 degree square wave into the position of node 0x15 837 """ 838 self.DEBUG = app.DEBUG 839 if not hasattr(self,'_binding'): 840 if not binding: 841 binding = self._autoBinding(sheet) 842 self._initBindings( app, binding ) 843 self.setters,self.headings,self.sheet = self._parseSheet(sheet) 844 act = {} 845 for row in self.sheet: 846 act[row[0]] = self._action 847 CyclePlan.__init__(self, app, act, maxFreq=maxFreq, *arg)

848

849 - def _action( self, arg ):

850 assert arg is self 851 if 'g' in self.DEBUG: 852 debugMsg(self,"phase %4.2f pos %d" % (self.phase, self._pos) ) 853 self._doRowSets( self.sheet[self._pos-1][1:] )

854

855 -class StickFilter( Plan ):

856 """ 857 StickFilter Plans are event processors designed to simplify the problem of 858 processing joystick and midi readings into continuous-time values. 859 860 The problem arises because pygame joystick events appear only when joystick 861 values change -- even if they are far from 'zero'. This means that triggering 862 actions directly from joystick events can lead to rather erratic responses. 863 864 StickFilter solves this problem by simulating the behavior of a regularly 865 sampled time-series for each incoming event type. All these time series 866 have the same sample interval (self.dt), but a different linear transfer 867 function can be associated with the values in each channel. 868 869 In particular, StickFilter provides methods to lowpass filter joystick 870 readings or to integrate them. The former is used to limit the "jerkiness" 871 of controller requests, whereas the latter is used to control velocity 872 with the joystick instead of position. 873 874 Channel Names 875 ------------- 876 877 Currently, StickFilter supports the channel name schemata listed below. 878 Any given channel will only be instantiated if a filter is set for it 879 and the related events are .push()-ed into the StickFilter plan. 880 881 The schemata: 882 joy<joystick-number>ball<ball-number> 883 joy<joystick-number>hat<hat-number> 884 joy<joystick-number>axis<axis-number>, e.g. joy0axis1, for joystick events 885 Nx<node-id-2-HEX-digits>, e.g. Nx3C, for CKBOTPOSITION events 886 midi<dev-number>sc<scene><kind><index-number> MIDI input device 887 """

888 - def __init__(self,app,t0=None,dt=0.1):

889 """ 890 Initialize a StickFilter 891 """ 892 Plan.__init__(self,app) 893 # Mapping name --> filter_A, filter_B, x, y, t 894 self.flt = {} 895 self.dt = dt 896 self.t = None

897

898 - def setLowpass( self, evt, tau, t0=None, func=float ):

899 """ 900 Process evt events with a first-order lowpass with time constant tau. 901 902 Uses an approximation to the Butterworth construction 903 """ 904 if tau<2: 905 raise ValueError('Only tau>=2 supported, got tau=%g' % tau) 906 a = exp(-pi / tau) 907 b = (1-a)/2.0 908 return self.setFilter( evt, t0, [1,-a], [b,b], func=func )

909

910 - def setIntegrator( self, evt, gain=1, t0=None, lower=-1e9, upper=1e-9, func=float):

911 """ 912 Process evt events with a (leaky) integrator 913 first-order lowpass with time constant tau. 914 915 By default, integrator is not leaky. 916 """ 917 return self.setFilter( evt, t0, [1,-1], [gain],lower=lower,upper=upper, func=func )

918

919 - def setFilter( self, evt, t0=None, A=[1.0], B=[1.0], x0=None, y0=None,lower=-1e9, upper=1e9, func=float ):

920 """ 921 Specify a filter for an event channel. 922 923 The filter equation is given by: 924 A[0]*y[n] = B[0]*x[n] + B[1]*B[n-1] + ... + B[nb]*x[n-nb] 925 - A[1]*y[n-1] - ... - A[na]*y[n-na] 926 927 or, in terms of transfer functions: 928 nb 929 B[0] + B[1] z + ... + B[nb] z 930 Y = ------------------------------------ X 931 na 932 A[0] + A[1] z + ... + A[na] z 933 934 INPUTS: 935 evt -- event object from channel, or its name as string 936 t0 -- float -- initial time to start channel, or None to use self.app.now 937 A,B -- sequences of floats -- transfer function 938 x0 -- sequence of len(B) floats -- initial state of x values (default 0-s) 939 y0 -- sequence of len(A) floats -- initial state of y values (default 0-s) 940 lower -- number -- lower limit on filter values (saturation value) 941 upper -- number -- upper limit on filter values (saturation value) 942 func -- callable -- input mapping applied to channel values before filtering 943 """ 944 # Identify the event class 945 if isinstance(evt,EventType): 946 evt,_ = self.nameValFor(evt) 947 if type(evt) != str: 948 raise TypeError( 949 "Event '%s' must be string / pygix.Event" % repr(evt)) 950 # Make sure parameters are lists of floats with matching lengths 951 if x0 is None: x0 = [0.0]*len(B) 952 else: x0 = [float(x0i) for x0i in x0] 953 if y0 is None: y0 = [0.0]*len(A) 954 else: y0 = [float(y0i) for y0i in y0] 955 A = [ float(a) for a in A ] 956 B = [ float(b) for b in B ] 957 if len(x0) != len(B): 958 raise ValueError('len(x0)=%d; must equal len(A)=%d' % (len(x0),len(A))) 959 if len(y0) != len(A): 960 raise ValueError('len(y0)=%d; must equal len(B)=%d' % (len(y0),len(B))) 961 # Test func 962 if not callable(func): 963 raise TypeError("'func' must be a callable") 964 # Store 965 if t0 is None: t0 = self.app.now 966 self.flt[evt] = (tuple(A),tuple(B),x0,y0,[],[t0],(lower,upper),func)

967

968 - def feed(self, evt ):

969 """ 970 Feed an event into the StickFilter. 971 Retrieves value and puts it on input queue for filter 972 973 INPUT: 974 evt -- EventType object 975 """ 976 # Search for key name of this event 977 key,val = self.nameValFor(evt) 978 if (key is None) or (not self.flt.has_key(key)): 979 return 980 # Retrieve filter state 981 flt = self.flt[key] 982 # Overwrite queue to contain this value 983 flt[4][:]=[flt[-1](val)]

984

985 - def setToZero(self, evt):

986 """ 987 Set a specific filter to zero 988 Sets all previous state variables (X, Y) to zero 989 990 INPUT: 991 evt -- EventType object or string name of event channel 992 """ 993 # Search for key name of this event 994 key,val = self.nameValFor(evt) 995 if (key is None) or (not self.flt.has_key(key)): 996 raise KeyError("No filter for %s" % repr(evt)) 997 # Retrieve filter state 998 flt = self.flt[key] 999 # Overwrite queue to contain this value 1000 A,B,X,Y,Q,(last,),(lb,ub),func = flt 1001 X[:]=[0]*len(X) 1002 Y[:]=[0]*len(Y) 1003 self.flt[key] = ( A,B,X,Y,Q,(last,),(lb,ub),func )

1004

1005 - def _runFilterTo(self, flt, t):

1006 """(private) 1007 1008 run filter off input in its queue until time t 1009 INPUT: 1010 flt -- a filter state 1011 t -- float -- time 1012 """ 1013 A,B,X,Y,Q,(last,),(lb,ub),func = flt 1014 # Interpolating up to time t, feed samples into filter 1015 # System equation is: 1016 # a[0]*y[n] = b[0]*x[n] + b[1]*x[n-1] + ... + b[nb]*x[n-nb] 1017 # - a[1]*y[n-1] - ... - a[na]*y[n-na] 1018 ts = last 1019 t -= self.dt 1020 while ts<t: 1021 ts += self.dt 1022 # Repeat previous sample / take from queue Q 1023 if Q: X.insert(0,Q.pop(0)) 1024 else: X.insert(0,X[0]) 1025 X.pop() 1026 ## Initialize filter 1027 z0 = sum( ( b*x for b,x in zip(B,X) ) ) 1028 z1 = sum( ( a*y for a,y in zip(A[1:],Y[:-1]) ) ) 1029 ## Store new result 1030 y = (z0-z1)/A[0] 1031 y = max(min(y,ub),lb) 1032 Y.insert(0,y) 1033 Y.pop() 1034 #DEBUG# progress("A %s B %s X %s Y %s" % tuple(map(repr,(A,B,X,Y)))) 1035 #DEBUG# progress("-FLT- %9g %g %g %g %g" % (ts,X[0],Y[0],z0,z1)) 1036 # Update filter timestamp 1037 flt[-3][0] = ts 1038 return ts

1039

1040 - def getValue( self, evt ):

1041 """ 1042 Obtain the filtered value for an event channel by using an event 1043 object or the string name of the event. 1044 """ 1045 # Identify the event class 1046 if isinstance(evt,EventType): 1047 evt,_ = self.nameValFor(evt) 1048 if type(evt) != str: 1049 raise TypeError( 1050 "Event '%s' must be string / pygix.Event" % repr(evt)) 1051 # Retrieve last value from filter 1052 flt = self.flt[evt] 1053 return flt[3][0]

1054

1055 - def getterOf( self, evt ):

1056 """ 1057 Obtain a getter function for an event channel 1058 """ 1059 return curry( self.getValue, evt )

1060 1061 @classmethod

1062 - def nameValFor( cls, evt ):

1063 ''' 1064 Generate channel name (a string) from an event object 1065 ''' 1066 if evt.type==JOYAXISMOTION: 1067 return 'joy%daxis%d' % (evt.joy,evt.axis), evt.value 1068 elif evt.type==JOYBALLMOTION: 1069 return 'joy%dball%d' % (evt.joy,evt.ball), evt.rel 1070 elif evt.type==JOYHATMOTION: 1071 return 'joy%dhat%d' % (evt.joy,evt.hat), evt.value 1072 elif evt.type==CKBOTPOSITION: 1073 return 'Nx%02X' % evt.module, evt.pos 1074 elif evt.type==MIDIEVENT: 1075 return 'midi%dsc%d%s%d' % (evt.dev,evt.sc,evt.kind,evt.index), evt.value 1076 return None, None

1077

1078 - def onEvent( self, evt ):

1079 """(final) 1080 1081 Handle incoming events. Timer events allow the filters to update state; 1082 other events are pushed into the filters with .feed() and do not require 1083 the behavior() to run (hence return False). 1084 """ 1085 if evt.type==TIMEREVENT: 1086 return True 1087 self.feed(evt) 1088 return False

1089

1090 - def behavior( self ):

1091 """(final) 1092 1093 Runs periodically every dt time units. 1094 1095 Evolves states of all filters up to the current time. 1096 """ 1097 while True: 1098 yield self.forDuration(self.dt) 1099 for flt in self.flt.itervalues(): 1100 if self.t is None: t = self.app.now 1101 else: t = self.t 1102 self._runFilterTo( flt, t )

1103

1104 -class MultiClick( Plan ):

1105 """ 1106 The MultiClick Plan class is an event processing aid for using events that 1107 come in <something>UP and <something>DOWN pairs. <something> can currently 1108 be KEY, MOUSEBUTTON or JOYBUTTON. 1109 1110 Because UP and DOWN events happen for individual keys, it is cumbersome to 1111 write interfaces that use key combinations, or use multi-button game 1112 controller combinations as commands. MultiClick exists to address this 1113 difficultly. 1114 1115 MultiClick takes in UP and DOWN events and processes them into two kinds 1116 of events: Click and MultiClick. Click events occur when an UP event is 1117 received that quickly followed the corresponding DOWN event, indicating a 1118 short "click". If the DOWN event is quickly followed by more DOWN events, 1119 these are combined until no additional events are received for a while. 1120 At that point, a "MultiClick" event is generated. Similarly, UP events are 1121 combined if they are received close to each other. If a "Click" occurs while 1122 another "MultiClick" is happening, the MultiClick is re-generated after the 1123 "Click". 1124 1125 Example 1126 ------- 1127 1128 To illustrate these ideas, assume three buttons 1 2 and 3 and take a '-' 1129 to indicate a long delay. The following timeline demonstrates these ideas: 1130 DOWN 1 1131 UP 1 --> click 1 1132 DOWN 1 1133 DOWN 2 1134 - --> MultiClick [1,2] 1135 UP 1 1136 - --> MultiClick [2] 1137 DOWN 3 1138 UP 3 --> Click 3, MultiClick [2] 1139 - 1140 UP 2 1141 - --> (optional) MultiClick [] 1142 1143 Usage 1144 ----- 1145 For convenience, the default behavior for MultiClick is to call event 1146 handler methods of the JoyApp that owns it. This means that for typical use 1147 cases the MultiClick Plan can be used as is, without subclassing. 1148 1149 MultiClick will call self.app.onClick( self,evt ) for click events, where evt 1150 is the UP event that generated the click. 1151 1152 MultiClick will call self.app.onMultiClick( self, evts ) for multi-click 1153 events, where evts is a dictionary whose values are the DOWN events that 1154 combined into this multi-click combination. 1155 1156 The MultiClick object is passed to these event handlers to allow events 1157 from multiple MultiClick Plans to be distinguished by the event handler. 1158 1159 Alternatively, MultiClick can be subclassed. Subclasses may override the 1160 .onClick(evt) and .onMultiClick(evts) methods to process the events. 1161 1162 NOTE: the MultiClick .onEvent returns False unless the onClick or 1163 onMultiClick handlers return boolean True. Thus the .behavior never gets to 1164 run -- but a subclass may override the .behavior and use a True return 1165 value from the JoyApp to control its execution. 1166 """

1167 - def __init__(self,app,allowEmpty=False,delay=0.2):

1168 """ 1169 Initialize a MultiClick Plan 1170 INPUTS: 1171 app -- JoyApp -- application running this Plan 1172 allowEmpty -- boolean -- should MultiClick events indicating no keys are 1173 currently pressed (empty set payload) be emitted. Default is False 1174 delay -- float -- delay used for merging multiple events 1175 1176 NOTE: 1177 MultiClick can only combine the events you .push() to it. For example, 1178 a MultiClick that only gets JOYBUTTONDOWN and JOYBUTTONUP from one 1179 joystick will only generate Click and MultiClick events from that 1180 joystick's buttons -- no keyboard, no mouse, no other joysticks. 1181 """ 1182 Plan.__init__(self,app) 1183 self._when = None 1184 self.delay = delay 1185 self._acts = {} 1186 self.allowEmptyEvent = allowEmpty

1187 1188 @classmethod

1189 - def nameFor( cls, evt ):

1190 ''' 1191 Generate a hashable name for each event we handle 1192 ''' 1193 if evt.type in [KEYDOWN,KEYUP]: 1194 return 'key%08x' % (evt.key | (evt.mod<<10)) 1195 elif evt.type in [MOUSEBUTTONUP,MOUSEBUTTONDOWN]: 1196 return 'mouse%d' % evt.button 1197 elif evt.type in [JOYBUTTONDOWN,JOYBUTTONUP]: 1198 return 'joy%dbtn%d' % (evt.joy,evt.button) 1199 return None

1200

1201 - def onEvent( self, evt ):

1202 """(final) 1203 1204 Process incoming UP or DOWN events. Use TIMEREVENT to test whether a 1205 key combination has persisted long enough for emitting a MultiClick. 1206 """ 1207 # If a TIMEREVENT and key combination has been stable for a while 1208 if (evt.type==TIMEREVENT 1209 and self._when is not None 1210 and self.app.now > self._when): 1211 self._when = None 1212 if self._acts or self.allowEmptyEvent: 1213 return (self.onMultiClick( self._acts.copy() ) is True) 1214 return False 1215 # Find the lookup key for this event 1216 nm = self.nameFor(evt) 1217 if nm is None: 1218 return False 1219 # 1220 A = self._acts 1221 res = False 1222 if evt.type in [KEYDOWN,MOUSEBUTTONDOWN,JOYBUTTONDOWN]: 1223 A[nm] = (evt,self.app.now) 1224 self._when = self.app.now + self.delay 1225 elif evt.type in [KEYUP,MOUSEBUTTONUP,JOYBUTTONUP]: 1226 if A.has_key(nm): 1227 if self.app.now<self._when: 1228 res = self.onClick(evt) 1229 elif len(A)==1: 1230 self._when =self.app.now + self.delay 1231 del A[nm] 1232 return (res is True)

1233

1234 - def behavior( self ):

1235 """(default) 1236 1237 Override in subclass if you plan to have onEvent return True 1238 """ 1239 while True: 1240 yield 1241 debugMsg(self,"onEvent returned True")

1242

1243 - def onClick(self, evt):

1244 """(default) 1245 Punts to self.app.OnClick(self,evt) 1246 1247 Override in subclass to process "Click" events -- i.e. events 1248 that represent short (less than self.delay) keypresses / clicks 1249 1250 INPUT: 1251 evt -- pygame event 1252 """ 1253 return self.app.onClick(self,evt)

1254

1255 - def onMultiClick(self, evts):

1256 """(default) 1257 Punts to self.app.onMultiClick(self,evts) 1258 1259 Override in subclass to process "MultiClick" events -- i.e. 1260 events that represent one or more keys held together for long 1261 (i.e. more than self.delay). 1262 1263 A key can be clicked while other keys are MultiClicked. This 1264 will generate a new multi-click after the short click is over. 1265 1266 When all keys are up, a multi-click can be generated with an 1267 empty evts dictionary. This feature is controlled by 1268 self.allowEmptyEvents, which is set by the allowEmpty 1269 parameter to the contstructor. 1270 1271 INPUT: 1272 evts -- dictionary -- event category --> pygame event 1273 """ 1274 return self.app.onMultiClick(self,evts)

1275

1276 -class NBRPlan( Plan ):

1277 """ 1278 NBRPlan concrete class for non-blocking file reader 1279 1280 Reads file (typically a serial port) until no data available 1281 by using non-blocking IO. 1282 1283 The contents are then available in self.ln (a list) and the timestamp 1284 of the read operation is in self.ts 1285 1286 ATTRIBUTES: 1287 .ts -- timestamps of lines read (from self.app.now) 1288 .ln -- list of lines read 1289 .iterLimit -- maximum number of lines that will be read at once 1290 """

1291 - def __init__(self, app, fn="/dev/ttyACM0"):

1292 """ 1293 INPUT: 1294 fn -- filename of file to read 1295 """ 1296 Plan.__init__(self,app) 1297 self.fn = fn 1298 self.f = None 1299 self.iterLimit = 1e200 1300 self.clear() 1301 self.__reopen()

1302

1303 - def clear(self):

1304 """ 1305 Clear recorded data 1306 """ 1307 self.ts = [] 1308 self.ln = []

1309 1310

1311 - def __reopen(self):

1312 """ 1313 (PRIVATE) reopen the file in non-blocking mode 1314 """ 1315 assert self.f is None 1316 f = open(self.fn,"r") 1317 fd = f.fileno() 1318 flag = fcntl(fd, F_GETFL) 1319 fcntl(fd, F_SETFL, flag | O_NONBLOCK) 1320 assert fcntl(fd, F_GETFL) & O_NONBLOCK, "success in setting nonblocking mode" 1321 self.f = f

1322

1323 - def behavior(self):

1324 while True: 1325 assert len(self.ts) == len(self.ln),"Did you remember to pop timestamps from .ts to match the lines in .ln?" 1326 # Note: the workloop here will run forever if the file being read 1327 # produces data faster than we can read. This will break JoyApp 1328 # If you want to be sure you're safe, set self.iterLimit 1329 if self.f is None: 1330 self.__reopen() 1331 try: 1332 while len(self.ln)<self.iterLimit: 1333 self.ln.append(self.f.readline()) 1334 self.ts.append(self.app.now) 1335 except IOError,ioe: 1336 if ioe.errno == EAGAIN: 1337 # We are done reading 1338 yield 1339 else: 1340 raise

1341

1342 -class AnimatorPlan(Plan):

1343 """ 1344 Concrete class AnimatorPlan 1345 1346 Wrapper for making easy animations using matplotlib and JoyApp. 1347 1348 USAGE: 1349 The AnimatorPlan constructor is given a function that takes a matplotlib 1350 figure object and updates the figure each iteration. 1351 """

1352 - def __init__(self,app,fun=None,fps=20):

1353 Plan.__init__(self,app) 1354 self.set_fps(fps) 1355 self.set_generator(fun)

1356

1357 - def set_fps(self,fps):

1358 """Set FPS for animation""" 1359 self.delay = 1.0 / fps

1360

1361 - def set_generator(self,fun):

1362 """ 1363 Set the frame generator function 1364 INPUT: 1365 fun -- function -- takes matplotlib.figure and returns a generator 1366 that updates the figure every .next() call 1367 """ 1368 assert callable(fun) 1369 self.fun = fun

1370

1371 - def behavior(self):

1372 for fr in self.fun(self.app.fig): 1373 self.app.animate() 1374 yield self.forDuration(self.delay)

1375

Source Code for Module joy.plans