Source Code for Script script-demo_dof_selector_py

  1  from joy import * 
  2   
  3 -class Simmod(object): 
  4 -  def __init__(self): 
  5      self.name = "Sim%02x" % id(self) 
  6      self.set_pos = lambda x : None 
  7      progress('Created %s ' % self.name)  
  8       
  9 -class DofSelectorApp( JoyApp ): 
 10     
 11 -  def __init__(self,robot=None,*arg,**kw): 
 12      #if robot is None: 
 13      #  raise ValueError('This application requires a robot') 
 14      JoyApp.__init__(self,*arg,robot=robot, 
 15        cfg=dict( buttonWait = 0.1, axisWait = 0.2, defaultGain=500 ), 
 16        **kw) 
 17       
 18 -  def onStart( self ): 
 19      self.dof = list(self.robot.itermodules()) 
 20      self.setter = [ m.set_pos for m in self.dof ] 
 21      self.btns = set() 
 22      self.gain = {} 
 23      self.ofs = {} 
 24      self.bind = {} 
 25      self.mode = None 
 26      self.btnTime = 0 
 27      self.axisTime = 0 
 28   
 29 -  def onClick(self,evt): 
 30      if self.mode == evt.button: 
 31        self.mode = None 
 32      else: 
 33        self.mode = evt.button 
 34      progress('[[[ MODE %s ]]]] -- mode changed' % str(self.mode)) 
 35      if not self.bind.has_key(self.mode): 
 36        progress("   >> no bindings defined")  
 37        return 
 38      for (joy,axis),dof in self.bind[self.mode].iteritems(): 
 39        progress("   >> joy %d axis %d -- module %s" 
 40          % (joy,axis,self.dof[dof].name) ) 
 41       
 42 -  def doMultiButton( self, evt ): 
 43      if evt.type==JOYBUTTONDOWN: 
 44        self.btns.add(evt.button) 
 45        self.btnTime = self.now + self.cfg.buttonWait 
 46        if 'b' in self.DEBUG: 
 47          debugMsg(self,'Buttons ' +str(self.btns) ) 
 48        return 
 49      elif evt.type==JOYBUTTONUP: 
 50        # If button press was short, and was a single button --> click 
 51        if (self.btns == set((evt.button,))  
 52            and self.now < self.btnTime): 
 53          self.onClick(evt) 
 54        self.btns.discard(evt.button) 
 55        if 'b' in self.DEBUG: 
 56          debugMsg(self,'Buttons ' +str(self.btns) ) 
 57        return 
 58   
 59 -  def getDofGainOfs( self, evt): 
 60      # Find binding table for current mode 
 61      dof = None 
 62      bnd = self.bind.get(self.mode,None) 
 63      if bnd: dof = bnd.get((evt.joy,evt.axis),None) 
 64      if dof is None: 
 65        return None,None,None 
 66      # Find gain table for DOF-s in this mode 
 67      g = self.gain.get(self.mode,None) 
 68      # If gain table not found, create an empty one 
 69      if g is None: 
 70        g = [self.cfg.defaultGain] * len(self.dof) 
 71        self.gain[self.mode] = g 
 72      # Find ofs table for DOF-s in this mode 
 73      o = self.ofs.get(self.mode,None) 
 74      # If ofs table not found, create an empty one 
 75      if o is None: 
 76        o = [0] * len(self.dof) 
 77        self.ofs[self.mode] = o 
 78      # Return dof number and tables for this mode 
 79      return dof,g,o     
 80         
 81 -  def doSelectDof( self, evt ): 
 82      bnd = self.bind.get(self.mode,None) 
 83      if bnd is None: 
 84        bnd = {} 
 85        self.bind[self.mode] = bnd 
 86      key = (evt.joy,evt.axis) 
 87       
 88      # Find binding for axis being moved 
 89      dof = bnd.get(key,0) 
 90      # Cycle through DOF 
 91      if evt.value>0: 
 92        dof = (1+dof) % len(self.dof) 
 93      else: 
 94        dof = (len(self.dof)-1+dof) % len(self.dof) 
 95      # Store back in bindings       
 96      bnd[key] = dof  
 97      progress("Selected module %s for axis %d"  
 98        % (self.dof[dof].name,evt.axis) ) 
 99      self.axisTime = self.now + self.cfg.axisWait 
100   
101 -  def doSelectGain( self, evt ): 
102      dof,g,o = self.getDofGainOfs(evt) 
103      if dof is None: 
104        progress("Could not find binding") 
105        return 
106      if evt.value>0: 
107        g[dof] *= 1.1 
108      else: 
109        g[dof] /= 1.1 
110      progress("Module %s gain set to %.2g (in mode %s)"  
111        % (self.dof[dof].name, g[dof],str(self.mode))) 
112      self.axisTime = self.now 
113       
114 -  def doSelectOfs( self, evt ): 
115      dof,g,o = self.getDofGainOfs(evt) 
116      if dof is None: 
117        progress("Could not find binding") 
118        return 
119      if evt.value>0: 
120        o[dof] += 0.05 
121      else: 
122        o[dof] -= 0.05 
123      progress("Module %s offset set to %.2g (in mode %s)"  
124        % (self.dof[dof].name, o[dof],str(self.mode))) 
125      self.axisTime = self.now 
126   
127 -  def onEvent(self, evt): 
128      if evt.type==QUIT or (evt.type==KEYDOWN and evt.key==ord('q')): 
129        self.stop() 
130        return 
131      #  
132      if evt.type in (JOYBUTTONUP,JOYBUTTONDOWN): 
133        self.doMultiButton(evt) 
134        return 
135      # 
136      if (len(self.btns)==2 
137          and self.now>self.btnTime  
138          and evt.type==JOYAXISMOTION  
139          and self.now>self.axisTime 
140          and abs(evt.value)>0.5): 
141        # 
142        if self.btns==set((4,6)): 
143          self.doSelectDof(evt) 
144          return 
145        # 
146        if self.btns==set((5,7)): 
147          self.doSelectGain(evt) 
148          return         
149        # 
150        if self.btns==set((6,7)): 
151          self.doSelectOfs(evt) 
152          return        
153      # 
154      if (evt.type==JOYAXISMOTION and not self.btns): 
155        dof,g,o = self.getDofGainOfs(evt) 
156        if dof is None: 
157          progress('Mode %s has no binding for %d'  
158            % (str(self.mode),evt.axis) ) 
159        else: 
160          val = (evt.value-o[dof])*g[dof] 
161          progress('Setting %s to %.2g' % (self.dof[dof].name, val )) 
162          self.setter[dof](val) 
163         
164  if __name__=="__main__": 
165    print """ 
166    """ 
167    import sys 
168    app = DofSelectorApp(robot={}) 
169    app.DEBUG[:]=['b'] 
170    app.run() 
171