Source Code for Script script-demo_inchworm_py

  1  from joy import * 
  2   
  3 -class InchwormApp( JoyApp ): 
  4    # Load the gait table for the inchworming gait 
  5    # NOTE: we could equally well generate the gait table here by pasting in the 
  6    #   code from 'demo-makeInchwormCSV.py', but this would not demonstrate the 
  7    #   use of .csv files, which can be edited with a spreadsheet 
  8    SHEET = loadCSV("inchworm.csv") 
  9     
 10 -  def __init__(self,*arg,**kw): 
 11      # We expect a 5 module robot 
 12      JoyApp.__init__(self,robot=dict(count=5),*arg,**kw) 
 13       
 14 -  def onStart( self ): 
 15      # Construct the inchworm gait Plan  
 16      gait = GaitCyclePlan( self, self.SHEET, maxFreq = 1) 
 17      gait.setFrequency(0.2) 
 18      # We don't strictly need the following, but it's nice to be able to see 
 19      #   an explicit message when the GaitCyclePlan starts and stops. For such 
 20      #   simple changes we don't need a sub-class -- we can overwrite the  
 21      #   methods with our own functions at runtime. 
 22      # Using curry(...) allows us to dynamically create a function that consists  
 23      #   of a call to progress(...) with a pre-set string. See joy.curry for 
 24      #   details; this is a staple "trick" from functional programming. 
 25      gait.onStart = curry(progress,">>> START") 
 26      gait.onStop = curry(progress,">>> STOP") 
 27      self.gait = gait 
 28      # 
 29      # We set up integrators for the direct joystick control mode, using the  
 30      #   StickFilter Plan as an interface component     
 31      sf = StickFilter(self) 
 32      sf.setIntegrator( 'joy0axis0',10 ,lower=-9,upper=9) 
 33      sf.setIntegrator( 'joy0axis1',10 ,lower=-9,upper=9) 
 34      sf.setIntegrator( 'joy0axis2', 10 ,lower=-9,upper=9) 
 35      sf.setIntegrator( 'joy0axis3', 10 ,lower=-9,upper=9) 
 36      sf.setIntegrator( 'joy0axis4', 10 ,lower=-9,upper=9) 
 37      sf.start() 
 38      self.sf = sf 
 39      # Start with joystick mode disabled 
 40      self.joyMode = False 
 41      # Create a temporal filter that is true once every 0.5 seconds 
 42      self.oncePer = self.onceEvery(0.5) 
 43     
 44 -  def onStop( self ): 
 45      # Make sure we go_slack on all servo modules when we exit 
 46      self.robot.off() 
 47     
 48 -  def onEvent(self, evt): 
 49      ### Main event handler 
 50      # We have several broad classes of events, which are processed differently 
 51      #  depending on our mode. In most cases, we return after handling the event. 
 52      # 
 53      # Allow operator to quit by closing window or hitting 'q' or <escape> 
 54      #  (actually, the superclass call at the end of this method would do all 
 55      #  of this, but it is good practice to clean our own mess if we can) 
 56      if evt.type==QUIT or (evt.type==KEYDOWN and evt.key in [K_q,K_ESCAPE]): 
 57        self.stop() 
 58      # 
 59      # Joystick buttons 
 60      if evt.type==JOYBUTTONDOWN: 
 61        # Buttons on the "front" of the controller toggle mode 
 62        if evt.button>3: 
 63          self.joyMode = not self.joyMode 
 64          if self.joyMode: 
 65            if self.gait.isRunning(): 
 66              self.gait.stop() 
 67            progress("*"*20 + " joystick mode ON") 
 68          else: 
 69            progress("*"*20 + " joystick mode OFF") 
 70            self.robot.off() 
 71          return 
 72        else: # else buttons on the controller face --> PANIC and go slack 
 73          progress("!"*20 + " RESET: joystick OFF, gait STOPPED, modules SLACK") 
 74          if self.gait.isRunning(): 
 75            self.gait.stop()         
 76          self.joyMode = False 
 77          self.robot.off() 
 78          return True         
 79      # 
 80      # If in joystick mode --> punt to onEvent_joyMode 
 81      if self.joyMode: 
 82        if self.onEvent_joyMode(evt): 
 83          return 
 84      else: # else in gait mode --> punt to onEvent_gaitMode 
 85        if self.onEvent_gaitMode(evt): 
 86          return 
 87      # 
 88      # We reach this point if no previous handler processed the event. 
 89      #   If it is not a TIMEREVENT, we use the superclass onEvent handler 
 90      #   which prints a human readable description of the event object. 
 91      JoyApp.onEvent(self,evt) 
 92   
 93 -  def onEvent_joyMode( self, evt ): 
 94      # Joystick motions are pushed to the stick-filter 
 95      if evt.type==JOYAXISMOTION: 
 96        self.sf.push(evt) 
 97        return True 
 98      if evt.type==TIMEREVENT and : 
 99        lClaw = self.sf.getValue("joy0axis1") 
100        lAng = self.sf.getValue("joy0axis0") 
101        rClaw = self.sf.getValue("joy0axis3") 
102        rAng = self.sf.getValue("joy0axis2") 
103        body = self.sf.getValue("joy0axis4") 
104        self.robot.at.w1.set_pos(lClaw * 1000) 
105        self.robot.at.w2.set_pos(lAng * 1000) 
106        self.robot.at.w3.set_pos(body * 1000) 
107        self.robot.at.w4.set_pos(rAng * 1000) 
108        self.robot.at.w5.set_pos(rClaw * 1000)  
109        if self.oncePer(): 
110          progress("<<< %g %g < %g > %g %g >>>" % (lClaw,lAng,body,rClaw,rAng) ) 
111        return True 
112   
113 -  def onEvent_gaitMode(self, evt ): 
114      # All gait control happens via the keyboard; non KEYDOWN events are ignored 
115      if evt.type!=KEYDOWN: 
116        return False 
117      # 
118      # [enter] toggles gait on and off 
119      if evt.key==K_ENTER: 
120        if self.gait.isRunning(): 
121          self.gait.stop() 
122          progress( "Gait stopped" ) 
123          return True 
124        else: 
125          self.gait.start() 
126          progress( "Gait starting" ) 
127          return True 
128      # 
129      # [space] freezes cycling in place 
130      if evt.key==K_SPACE:  
131        self.gait.setFrequency(0) 
132        progress('Stopped motion') 
133        return True 
134      #  
135      # [up] and [down] change frequency 
136      if evt.key in (K_UP,K_DOWN): 
137        f = self.gait.getFrequency() 
138        # Change frequency up/down in range -limit..limit hz 
139        if evt.key==K_DOWN: 
140          f -= 0.03 
141        else: 
142          f += 0.03 
143        self.gait.setFrequency(f) 
144        progress('Frequency changed to %.2f Hz' % self.gait.getFrequency()) 
145        return True 
146      # 
147      # Digits jump to relative positions in the cycle 
148      if evt.key in range(K_0,K_9+1): 
149        self.gait.setFrequency(0) 
150        phi = ('1234567890'.find(chr(evt.key)))/9.0 
151        self.gait.moveToPhase( phi ) 
152        progress('Moved to phase %.2f' % phi) 
153        return True 
154         
155  if __name__=="__main__": 
156    print """ 
157    Inchworm Robot Demo 
158    ------------------- 
159     
160    This demo combines several JoyApp capabilities to give a full-fleged 
161    application for controlling a 5-segment worm-like robot. 
162     
163    The demo uses a gait stored in 'inchworm.csv', which can be generated 
164    by piping the output of demo-makeInchwormCSV.py, as follows: 
165     
166    $ python demos/demo-makeInchwormCSV.py > demos/inchworm.csv 
167     
168    To make this work with your own robot, make sure to name your modules 
169    w1,w2,w3,w4 and w5 in order (the nodeNames entry in JoyApp.yml), and to 
170    make sure that the line: 
171          signs = array([1,-1,1,1,-1]) 
172    is changed to reflect the rotation directions of your modules. 
173     
174    Operation 
175    --------- 
176    When the demo runs, the operation can switch between two modes: the "joystick" 
177    mode and the "gait" mode. In joystick mode, each of the first 5 game 
178    controller axes controls a robot DOF directly. In gait mode, the keyboard  
179    controls starting, stopping, stepping and jumping around in the gait cycle. 
180     
181    In addition, the first 4 game controller buttons are "panic" buttons that  
182    reset the robot to a known state. 
183     
184    All other controller buttons toggle between joystick and gait modes. 
185     
186    In gait mode, the following keys are used: 
187      [up] / [down] -- control frequency 
188      [space] -- freeze in place 
189      [1]...[9],[0] -- jump to a relative location in the cycle [1] start, [0] end 
190      [enter] -- toggle gait on and off. 
191    """ 
192    app=InchwormApp() 
193    app.run() 
194