Source Code for Module geometry.yaml

  1   
  2  from . import contract, np, logger 
  3  from .manifolds import DifferentiableManifold 
  4  from contracts import describe_value, describe_type 
  5   
  6  # 
  7  #def array_to_lists(x): 
  8  #    return x.tolist() 
  9  # 
 10  #def packet(space, rep, value): 
 11  #    return {'space': space, 'repr': rep, 'value': value} 
 12  # 
 13  #@contract(x='SE3') 
 14  #def yaml_from_SE3(x): 
 15  #    return packet('SE3', 'matrix', array_to_lists(x)) 
 16  # 
 17  #@contract(x='se3') 
 18  #def yaml_from_se3(x): 
 19  #    return packet('se3', 'matrix', array_to_lists(x)) 
 20  #     
 21  ## what about user-centered? 
 22  #def yaml_from_TSE3(x): 
 23  #    pose, vel = x     
 24  #    return packet('TSE3', 'base-tangent', 
 25  #                  [yaml_from_SE3(pose), yaml_from_se3(vel)]) 
 26   
 27   
 28  converters = {} 
 29  default_representation = {} 
 30   
 31   
 32 -def register_yaml_converter(manifold_name, representation, converter): 
 33      if not manifold_name in default_representation: 
 34          default_representation[manifold_name] = representation 
 35      key = (manifold_name, representation) 
 36      assert not key in converters 
 37      converters[key] = converter 
 38   
 39   
 40 -def get_default_representation(manifold): 
 41      if isinstance(manifold, DifferentiableManifold): 
 42          key = str(manifold) 
 43      else: 
 44          key = manifold 
 45   
 46      if not key in default_representation: 
 47          raise Exception('Cannot find representation for %s.' % manifold) 
 48   
 49      return default_representation[key] 
 50   
 51   
 52  @contract(returns='list[2]') 
 53 -def to_yaml(manifold, value, representation=None): 
 54      if representation is None: 
 55          representation = get_default_representation(manifold) 
 56      key = (manifold, representation) 
 57      if not key in converters: 
 58          raise ValueError('Unknown format %s; I know %s.' % 
 59                           (key, converters.keys())) 
 60      conv = converters[key] 
 61      try: 
 62          x = conv.to_yaml(value) 
 63      except: 
 64          msg = 'Error while trying to convert %s' % describe_value(value) 
 65          logger.error(msg) 
 66          raise 
 67      return ['%s:%s' % (manifold, representation), x] 
 68   
 69   
 70  @contract(x='list[2]') 
 71 -def from_yaml(x): 
 72      if not isinstance(x, list): 
 73          raise ValueError('I expect a list with two elements.') 
 74      form = x[0] 
 75      if not isinstance(form, str): 
 76          raise ValueError('I expect a string describing the format,' 
 77                           ' not %s, while decoding %s' % 
 78                           (describe_type(form), describe_value(x))) 
 79      value = x[1] 
 80      space, representation = form.split(':') 
 81   
 82      key = (space, representation) 
 83      if not key in converters: 
 84          raise ValueError('Unknown format %s; I know %s.' % 
 85                           (key, converters.keys())) 
 86      conv = converters[key] 
 87      return conv.from_yaml(value) 
 88   
 89   
 90 -class Representation: 
 91 -    def to_yaml(self, x): 
 92          pass 
 93   
 94 -    def from_yaml(self, y): 
 95          pass 
 96   
 97   
 98 -class SE3_m44(Representation): 
 99      @staticmethod 
100      @contract(x='SE3', returns='list[4](list[4](float))') 
101 -    def to_yaml(x): 
102          return x.tolist() 
103   
104      @staticmethod 
105      @contract(y='list[4](list[4](float))', returns='SE3') 
106 -    def from_yaml(y): 
107          return np.array(y) 
108   
109  register_yaml_converter('SE3', 'm44', SE3_m44) 
110   
111   
112 -class se3_m44(Representation): 
113      @staticmethod 
114 -    def to_yaml(x): 
115          return x.tolist() 
116   
117      @staticmethod 
118 -    def from_yaml(y): 
119          return np.array(y) 
120   
121   
122 -class TSE3_bt(Representation): 
123      @staticmethod 
124 -    def to_yaml(x): 
125          a, b = x 
126          return [SE3_m44.to_yaml(a), se3_m44.to_yaml(b)] 
127   
128      @staticmethod 
129 -    def from_yaml(y): 
130          return (SE3_m44.from_yaml(y[0]), 
131                  se3_m44.from_yaml(y[1])) 
132   
133   
134  register_yaml_converter('TSE3', 'bt', TSE3_bt) 
135