(self, a, b, order, num_joints, ftype='lowpass')
Initializes filter.

    Either one per joint or same for all joints.

    Args:
      a: filter output history coefficients
      b: filter input coefficients
      order: filter order
      num_joints: robot DOF
      ftype: filter type. 'lowpass' or 'bandpass'
Initializes filter.
Initializes filter .
def __init__(self, a, b, order, num_joints, ftype='lowpass'):
    """Initializes filter.

    Either one per joint or same for all joints.

    Args:
      a: filter output history coefficients
      b: filter input coefficients
      order: filter order
      num_joints: robot DOF
      ftype: filter type. 'lowpass' or 'bandpass'
    """
    self.num_joints = num_joints
    if isinstance(a, list):
      self.a = a
      self.b = b
    else:
      self.a = [a]
      self.b = [b]

    # Either a set of parameters per joint must be specified as a list
    # Or one filter is applied to every joint
    if not ((len(self.a) == len(self.b) == num_joints) or (
        len(self.a) == len(self.b) == 1)):
      raise ValueError('Incorrect number of filter values specified')

    # Normalize by a[0]
    for i in range(len(self.a)):
      self.b[i] /= self.a[i][0]
      self.a[i] /= self.a[i][0]

    # Convert single filter to same format as filter per joint
    if len(self.a) == 1:
      self.a *= num_joints
      self.b *= num_joints
    self.a = np.stack(self.a)
    self.b = np.stack(self.b)

    if ftype == 'bandpass':
      assert len(self.b[0]) == len(self.a[0]) == 2 * order + 1
      self.hist_len = 2 * order
    elif ftype == 'lowpass':
      assert len(self.b[0]) == len(self.a[0]) == order + 1
      self.hist_len = order
    else:
      raise ValueError('%s filter type not supported' % (ftype))

    logging.info('Filter shapes: a: %s, b: %s', self.a.shape, self.b.shape)
    logging.info('Filter type:%s', ftype)

    self.yhist = collections.deque(maxlen=self.hist_len)
    self.xhist = collections.deque(maxlen=self.hist_len)
    self.reset()
def __init__ ( self , a , b , order , num_joints , ftype = 'lowpass' ) : self . num_joints = num_joints if isinstance ( a , list ) : self . a = a self . b = b else : self . a = [ a ] self . b = [ b ] # Either a set of parameters per joint must be specified as a list # Or one filter is applied to every joint if not ( ( len ( self . a ) == len ( self . b ) == num_joints ) or ( len ( self . a ) == len ( self . b ) == 1 ) ) : raise ValueError ( 'Incorrect number of filter values specified' ) # Normalize by a[0] for i in range ( len ( self . a ) ) : self . b [ i ] /= self . a [ i ] [ 0 ] self . a [ i ] /= self . a [ i ] [ 0 ] # Convert single filter to same format as filter per joint if len ( self . a ) == 1 : self . a *= num_joints self . b *= num_joints self . a = np . stack ( self . a ) self . b = np . stack ( self . b ) if ftype == 'bandpass' : assert len ( self . b [ 0 ] ) == len ( self . a [ 0 ] ) == 2 * order + 1 self . hist_len = 2 * order elif ftype == 'lowpass' : assert len ( self . b [ 0 ] ) == len ( self . a [ 0 ] ) == order + 1 self . hist_len = order else : raise ValueError ( '%s filter type not supported' % ( ftype ) ) logging . info ( 'Filter shapes: a: %s, b: %s' , self . a . shape , self . b . shape ) logging . info ( 'Filter type:%s' , ftype ) self . yhist = collections . deque ( maxlen = self . hist_len ) self . xhist = collections . deque ( maxlen = self . hist_len ) self . reset ( )