Exemple #1
0
 def setup(self):
    """
       Initialization function called by the framework
       Define the fuzzy rules
    """
    self.sfr_rot = SugenoFuzzyRuler()
    self.sfr_trans = SugenoFuzzyRuler()
    self.D_OK = (0.2,0.4,0.6,0.7) # distance ok fuzzy set
    self.D_LARGE = (0.6,0.7,100,100) # distance large fuzzy set
    self.D_SMALL = (0,0,0.1,0.3) # distance small fuzzy set
    # wall dist ok and no obstacle =>  go fast
    self.sfr_rot.addRule( ( [ (None,'minRightSide',self.D_OK), ('and','s6',self.D_OK) , ('and','minFront',self.D_LARGE)] , 0.0) )
    self.sfr_trans.addRule( ( [ (None,'minRightSide',self.D_OK), ('and','s6',self.D_OK), ('and','minFront',self.D_LARGE) ] , 1) )
    # too close to wall  => turn left
    self.sfr_rot.addRule( ( [ (None,'minRightSide',self.D_SMALL) ] , 0.3) )
    self.sfr_trans.addRule( ( [ (None,'minRightSide',self.D_SMALL) ] , 0.3) )
    # too far => turn right
    self.sfr_rot.addRule( ( [ (None,'minRightSide',self.D_LARGE) ] , -0.3) )
    self.sfr_trans.addRule( ( [ (None,'minRightSide',self.D_LARGE) ] , 0.3) )
    # obstacle => turn left
    self.sfr_rot.addRule( ( [ (None,'minFront',self.D_SMALL) ] , 1) )
    self.sfr_trans.addRule( ( [ (None,'minFront',self.D_SMALL) ] , 0.05) )
    self.sfr_rot.addRule( ( [ (None,'minFront',self.D_OK) ] , 0.8) )
    self.sfr_trans.addRule( ( [ (None,'minFront',self.D_OK) ] , 0.1) )
    # s5 large and s6 ok => turn a bit right
    self.sfr_rot.addRule( ( [ (None,'s5',self.D_LARGE), ('and','s6',self.D_OK) ] , -0.2) )
    self.sfr_trans.addRule( ( [ (None,'s5',self.D_LARGE), ('and','s6',self.D_OK) ] , 0.2) )
    # s5 large and s6 ok => turn a bit right
    self.sfr_rot.addRule( ( [ (None,'s5',self.D_LARGE), ('and','s6',self.D_LARGE) ] , -0.9) )
    self.sfr_trans.addRule( ( [ (None,'s5',self.D_LARGE), ('and','s6',self.D_LARGE) ] , 0.1) )
Exemple #2
0
 def setup(self):
    """
       Initialization function called by the framework
       Define the fuzzy rules
    """
    self.sfr_rot = SugenoFuzzyRuler()
    self.sfr_trans = SugenoFuzzyRuler()
    self.D_OK = (0.2,0.4,0.6,0.7) # distance ok fuzzy set
    self.D_LARGE = (0.6,0.7,100,100) # distance large fuzzy set
    self.D_SMALL = (0,0,0.1,0.3) # distance small fuzzy set
    #Rules for rotation then translation
    self.setRules()
    self.states = {'followLeft': self.followLeft,\
                   'wander': self.wander,\
                   'followRight': self.followRight}
    self.state = 'wander'
Exemple #3
0
class FuzzySugenoBrain(Brain):
   """
      A demo brain for a pioner with a complete ring of 16 sonars.
      Demonstrate a wall following on the right behavior.
   """

   
   def setup(self):
      """
         Initialization function called by the framework
         Define the fuzzy rules
      """
      self.sfr_rot = SugenoFuzzyRuler()
      self.sfr_trans = SugenoFuzzyRuler()
      self.D_OK = (0.2,0.4,0.6,0.7) # distance ok fuzzy set
      self.D_LARGE = (0.6,0.7,100,100) # distance large fuzzy set
      self.D_SMALL = (0,0,0.1,0.3) # distance small fuzzy set
      # wall dist ok and no obstacle =>  go fast
      self.sfr_rot.addRule( ( [ (None,'minRightSide',self.D_OK), ('and','s6',self.D_OK) , ('and','minFront',self.D_LARGE)] , 0.0) )
      self.sfr_trans.addRule( ( [ (None,'minRightSide',self.D_OK), ('and','s6',self.D_OK), ('and','minFront',self.D_LARGE) ] , 1) )
      # too close to wall  => turn left
      self.sfr_rot.addRule( ( [ (None,'minRightSide',self.D_SMALL) ] , 0.3) )
      self.sfr_trans.addRule( ( [ (None,'minRightSide',self.D_SMALL) ] , 0.3) )
      # too far => turn right
      self.sfr_rot.addRule( ( [ (None,'minRightSide',self.D_LARGE) ] , -0.3) )
      self.sfr_trans.addRule( ( [ (None,'minRightSide',self.D_LARGE) ] , 0.3) )
      # obstacle => turn left
      self.sfr_rot.addRule( ( [ (None,'minFront',self.D_SMALL) ] , 1) )
      self.sfr_trans.addRule( ( [ (None,'minFront',self.D_SMALL) ] , 0.05) )
      self.sfr_rot.addRule( ( [ (None,'minFront',self.D_OK) ] , 0.8) )
      self.sfr_trans.addRule( ( [ (None,'minFront',self.D_OK) ] , 0.1) )
      # s5 large and s6 ok => turn a bit right
      self.sfr_rot.addRule( ( [ (None,'s5',self.D_LARGE), ('and','s6',self.D_OK) ] , -0.2) )
      self.sfr_trans.addRule( ( [ (None,'s5',self.D_LARGE), ('and','s6',self.D_OK) ] , 0.2) )
      # s5 large and s6 ok => turn a bit right
      self.sfr_rot.addRule( ( [ (None,'s5',self.D_LARGE), ('and','s6',self.D_LARGE) ] , -0.9) )
      self.sfr_trans.addRule( ( [ (None,'s5',self.D_LARGE), ('and','s6',self.D_LARGE) ] , 0.1) )
      
          

   def step(self):
      """
         Step function called at each iteration of the simulator.
      """
      # Gather sensor values
      self.sonar_sense()
      # create a dict of inputs for the fuzzy rules
      dx = {'minRightSide':min(self.sv[5:10]),'minFront':min(self.sv[3:5]),\
            's5':self.sv[5]*1.60,'s6':self.sv[6]*1.10} # normalize sonar 5 and 6
      t_speed = self.sfr_trans.output(dx,False)   # to display debug information pass 'True' instead of 'False'
      r_speed = self.sfr_rot.output(dx,False)  # to display debug information pass 'True' instead of 'False'
      print '(t_speed, r_speed) = ({0:.2f},{1:.2f})'.format(t_speed, r_speed)
      self.robot.move(t_speed, r_speed)



   def sonar_sense(self):
      """
         Compute the sonar vector self.sv, its minimum self.minDist and the index
         of minDist.
      """
      self.sv = [s.distance() for s in self.robot.range]
Exemple #4
0
class FuzzySugenoBrain(Brain):
   """
      A demo brain for a pioner with a complete ring of 16 sonars.
      Demonstrate a wall following on the right behavior.
   """

   
   def setup(self):
      """
         Initialization function called by the framework
         Define the fuzzy rules
      """
      self.sfr_rot = SugenoFuzzyRuler()
      self.sfr_trans = SugenoFuzzyRuler()
      self.D_OK = (0.2,0.4,0.6,0.7) # distance ok fuzzy set
      self.D_LARGE = (0.6,0.7,100,100) # distance large fuzzy set
      self.D_SMALL = (0,0,0.1,0.3) # distance small fuzzy set
      #Rules for rotation then translation
      self.setRules()
      self.states = {'followLeft': self.followLeft,\
                     'wander': self.wander,\
                     'followRight': self.followRight}
      self.state = 'wander'
      
      
      
   def step(self):
      self.sonar_sense()
      self.getState()
      t_speed, r_speed = self.states[self.state]()
      self.robot.move(t_speed, r_speed)
     # print self.state



   def sonar_sense(self):
      self.sv = [s.distance() for s in self.robot.range]
      #print self.sv

   def getState(self):
      if ( 0.2 <= self.sv[0] <= 0.8) and ( 0.8 <= self.sv[1] <= 100) :
         #print 'wall ending on left side'
         self.state = 'followLeft'
         return
      if ( 0.2 <= self.sv[7] <= 0.8) and ( 0.8 <= self.sv[6] <= 100) :
         #print 'wall ending on right side'
         self.state = 'followRight'
         return
      if (self.state == 'followLeft') and not (0.1 <= self.sv[0] <= 0.7):
         #print 'I was following left and now my first sensor is large'
         self.state = 'followLeft'
         return
      if (self.state == 'followRight') and not (0.1 <= self.sv[7] <= 0.7):
         self.state = 'followRight'
         #print 'I was following right and now my last sensor is large'
         return

      self.state = 'wander'
      
      
   def followLeft(self):
      #print 'going left'
      return 0.4, 0.8
   
   def followRight(self):
      #print 'going right'
      return 0.4, -0.8
   
   def wander(self):
      self.sonar_sense()
      dx = {'minRightSide':min(self.sv[5:10]),'maxFront':max(self.sv[3:5]), 'minFront':min(self.sv[3:5]),'minLeftSide':min(self.sv[0:4]),'maxRightSide':max(self.sv[5:10]), 'maxLeftSide':max(self.sv[0:4]),\
            's0':self.sv[0],'s2':self.sv[1], 's5':self.sv[6],'s7':self.sv[7]}
      t_speed = self.sfr_trans.output(dx,False)   # to display debug information pass 'True' instead of 'False'
      r_speed = self.sfr_rot.output(dx,False)  # to display debug information pass 'True' instead of 'False'
      print '(t_speed, r_speed) = ({0:.2f},{1:.2f})'.format(t_speed, r_speed)
      return t_speed, r_speed

   def setRules(self):
      #self.sfr_rot.addRule( ( [ (None,'minRightSide',self.D_SMALL), ('and','minLeftSide',self.D_SMALL) , ('and','minFront',self.D_SMALL)] , 0.8) )
      #self.sfr_rot.addRule( ( [ (None,'minRightSide',self.D_LARGE), ('and','minLeftSide',self.D_LARGE) , ('and','minFront',self.D_OK)] , 0.2) )
      #self.sfr_rot.addRule( ( [ (None,'minRightSide',self.D_LARGE), ('and','minLeftSide',self.D_LARGE) , ('and','minFront',self.D_LARGE)] , 0.2) )
      #self.sfr_trans.addRule( ( [ (None,'minRightSide',self.D_LARGE), ('and','minLeftSide',self.D_LARGE), ('and','minFront',self.D_LARGE) ] , 0.5) )
      self.sfr_rot.addRule( ( [ (None,'minRightSide',self.D_LARGE), ('and','minLeftSide',self.D_LARGE) , ('and','minFront',self.D_LARGE)] , 0.2) )
      self.sfr_trans.addRule( ( [ (None,'minRightSide',self.D_LARGE), ('and','minLeftSide',self.D_LARGE), ('and','minFront',self.D_LARGE) ] , 0.5) )
      self.sfr_trans.addRule( ( [ (None,'minFront',self.D_LARGE)], 0.5))
      self.sfr_rot.addRule( ( [ (None,'minFront',self.D_LARGE)], 0.0))


      #wall in front
      self.sfr_rot.addRule(([(None, 'minFront', self.D_SMALL), ('and', 'maxRightSide', self.D_LARGE), ('and', 'maxLeftSide', self.D_LARGE)], 0.9))
      self.sfr_trans.addRule(([(None, 'minFront', self.D_SMALL), ('and', 'maxRightSide', self.D_LARGE), ('and', 'maxLeftSide', self.D_LARGE)], 0.1))
      self.sfr_rot.addRule(([(None, 'minFront', self.D_OK), ('and', 'maxRightSide', self.D_LARGE), ('and', 'maxLeftSide', self.D_LARGE)], 0.9))
      self.sfr_trans.addRule(([(None, 'minFront', self.D_OK), ('and', 'maxRightSide', self.D_LARGE), ('and', 'maxLeftSide', self.D_LARGE)], 0.1))
      #Wall on right and front
      self.sfr_rot.addRule(([(None, 'minFront', self.D_SMALL), ('and', 'maxRightSide', self.D_OK), ('and', 'maxLeftSide', self.D_LARGE)], 0.9))
      self.sfr_trans.addRule(([(None, 'minFront', self.D_SMALL), ('and', 'maxRightSide', self.D_OK), ('and', 'maxLeftSide', self.D_LARGE)], 0.1))
      self.sfr_rot.addRule(([(None, 'minFront', self.D_OK), ('and', 'maxRightSide', self.D_OK), ('and', 'maxLeftSide', self.D_LARGE)], 0.9))
      self.sfr_trans.addRule(([(None, 'minFront', self.D_OK), ('and', 'maxRightSide', self.D_OK), ('and', 'maxLeftSide', self.D_LARGE)], 0.1))
      #wall on left and front
      self.sfr_rot.addRule(([(None, 'minFront', self.D_SMALL), ('and', 'maxLeftSide', self.D_OK), ('and', 'maxRightSide', self.D_LARGE)], -0.9))
      self.sfr_trans.addRule(([(None, 'minFront', self.D_SMALL), ('and', 'maxLeftSide', self.D_OK), ('and', 'maxRightSide', self.D_LARGE)], 0.1))
      self.sfr_rot.addRule(([(None, 'minFront', self.D_OK), ('and', 'maxLeftSide', self.D_OK), ('and', 'maxRightSide', self.D_LARGE)], -0.9))
      self.sfr_trans.addRule(([(None, 'minFront', self.D_OK), ('and', 'maxLeftSide', self.D_OK), ('and', 'maxRightSide', self.D_LARGE)], 0.1))
      
      
      #Left
         #Too close
      self.sfr_rot.addRule( ( [ (None,'minLeftSide',self.D_SMALL)], -0.4))
      self.sfr_trans.addRule(( [ (None,'minLeftSide',self.D_SMALL)], 0.2))
      #Right
         #Too Close
      self.sfr_rot.addRule( ( [ (None,'minRightSide',self.D_SMALL)], 0.4))
      self.sfr_trans.addRule(([ (None,'minRightSide',self.D_SMALL)], 0.2))

      #Wall following
      #Left | Wall ending
      self.sfr_rot.addRule( ( [ (None,'s0',self.D_SMALL), ('and','s2',self.D_LARGE)], 0.99))
      self.sfr_trans.addRule( ( [ (None,'s0',self.D_SMALL), ('and','s2',self.D_LARGE)], 0.1))
      self.sfr_rot.addRule( ( [ (None,'s0',self.D_OK), ('and','s2',self.D_LARGE)], 0.99))
      self.sfr_trans.addRule( ( [ (None,'s0',self.D_OK), ('and','s2',self.D_LARGE)], 0.1))
      #Right | Wall ending
      self.sfr_rot.addRule( ( [ (None,'s5',self.D_LARGE), ('and','s7',self.D_SMALL)], -0.99))
      self.sfr_trans.addRule( ( [ (None,'s5',self.D_LARGE), ('and','s7',self.D_SMALL)], 0.1))
      self.sfr_rot.addRule( ( [ (None,'s5',self.D_LARGE), ('and','s7',self.D_OK)], -0.99))
      self.sfr_trans.addRule( ( [ (None,'s5',self.D_LARGE), ('and','s7',self.D_OK)], 0.1))