class RPCProxy(object):
    
    def __init__(self, uri, method=None, namespaces=None, sid=''):
        self.__serviceURL = uri
        self.__serviceName = method
        self.namespaces = isinstance(namespaces, ClassLoader) and namespaces or \
                              ClassLoader(namespaces or [])
        self._seril = Serializer(self.namespaces)
        self.sid = sid
        self.logger = None
        self.start_call_listener = []
        self.end_call_listener = []
    
    def __call__(self, *args, **kw):
        args = self._seril.serialize((args, kw))
        
        post_data = {"method": self.__serviceName, 
                     'params': args, 
                     'id':'httprpc',
                     'sid':self.sid}
        
        #@todo xx
        for l in self.start_call_listener:
            l(name=self.__serviceName, args=args, kw_args=kw)
        
        rpc_response = self.post_request(self.__serviceURL, post_data)
        try:
            respdata = rpc_response.read()
            ret, e = self._seril.deserialize(respdata)
        except Exception, e:
            raise RPCException("Failed to deserialize response data:%s, exception:%s" % 
                               (respdata, e))
        finally:

 def testSimpleSerialization(self):
     m = self.createTestMessage()
     serialized = StringIO(Serializer.serialize(m))
     
     deserialized = Serializer.deserialize(serialized, TestMessage1)
     
     self.assertDictEqual(m.__dict__, deserialized.__dict__)

class RPCStub(object):
    def __init__(self, uri, stub, namespace=None):
        self.uri = uri
        self.stub = stub
        self.namespace = namespace
        self._seril = Serializer(namespace)
        self.logger = None
        
    def process(self, method, data):
        ret = exception = None
        try:
            args, kw = self._seril.deserialize(data)
            try:
                self.logger and self.logger.info(u"method:%s, args:%s, kw:%s" % (method,
                                                args, kw))
            except Exception, e:
                #@todo:  fix the decode problem.
                self.logger and self.logger.info(str(e))
                
            
            h = self._local_service(self.stub, method)
            if h is None: raise RPCException(u"Not found interface '%s'" % method)
            
            ret = h(*args, **kw)
            
            self.logger and self.logger.info("return:%s" % (ret, ))
        except BaseException, e:
            exception = e
            self.logger and self.logger.exception(e)

class NodeMaker:
    def __init__(self):
        self.ser = Serializer()
        self.words = defaultdict(int)
        self.graph = Graph()
    def PosNo(self, morph):
        for i, p in enumerate([u'形容詞', u'名詞']):
            if p in morph.pos():
                return i+2
        return 0
    def regist(self, text):
        lines = text.split('\n')
        lst = []
        for lnum, line in enumerate(lines):
            morphs = wakachi.parse(text)
            for morph in morphs:
                if self.PosNo(morph):
                    lst.append(morph)
                    self.words[(morph.posid, morph.original)] += 1
                else:
                    lst.append(None)
            lst += [None]*5
            if line == '':
                self.consume(lst)
                lst = []
        self.consume(lst)
    def consume(self, lst, back=3, fore=10): #0:N, 1:V, 2:Y
        size = len(lst)
        for i in xrange(size):
            if lst[i] is None: continue
            posno = self.PosNo(lst[i])
            node = []
            for x in xrange(posno):
                node.append(self.ser.encode((lst[i].posid, lst[i].original(), x)))
                self.graph.registerNode(node[x])
            #for node = V
            for j in xrange(max(0,i-fore), min(size,i+back)):
                if lst[j] is None or self.PosNo(lst[j]) == 2: continue
                ny = self.ser.encode((lst[j].posid, lst[j].original(), 2))
                self.graph.addEdge(node[1], ny)
            #for node = Y
            if posno == 3:
                for j in xrange(max(0,i-back), min(size,i+fore)):
                    if lst[j] is None: continue
                    nv = self.ser.encode((lst[j].posid, lst[j].original(), 1))
                    self.graph.addEdge(node[2],nv)

 def __init__(self, problem, config):
     self.abs_path = os.path.dirname(os.path.abspath(__file__))
     self.serializer = Serializer()
     self.config_file = config
     self.config = self.serializer.read_config(self.abs_path + "/" + config)        
     self.ik_solution_generator = IKSolutionGenerator()        
     self.clear_stats(problem)
     self.run(problem)

    def _fetch_metadata_by_json(self, json_obj):
        """ parses incoming json object representation and fetches related 
        object metadata from the server. Returns None or the Metadata object.

        This method is called only when core object is not found in cache, and 
        if metadata should be requested anyway, it could be done faster with 
        this method that uses GET /neo/<obj_type>/198272/metadata/

        Currently not used, because inside the pull function (in cascade mode) 
        it may be faster to collect all metadata after the whole object tree is 
        fetched. For situations, when, say, dozens of of objects tagged with the
        same value are requested, it's faster to fetch this value once at the 
        end rather than requesting related metadata for every object.
        """
        if not json_obj['fields'].has_key('metadata') or \
            not json_obj['fields']['metadata']:
            return None # no metadata field or empty metadata

        url = json_obj['permalink']

        # TODO move requests to the remote class
        resp = requests.get( url + 'metadata' , cookies=self._meta.cookie_jar )
        raw_json = get_json_from_response( resp )

        if not resp.status_code == 200:
            message = '%s (%s)' % (raw_json['message'], raw_json['details'])
            raise errors.error_codes[resp.status_code]( message )

        if not raw_json['metadata']: # if no objects exist return empty result
            return None

        mobj = Metadata()
        for p, v in raw_json['metadata']:
            prp = Serializer.deserialize(p, self)
            val = Serializer.deserialize(v, self)
            prp.append( val )

            # save both objects to cache
            self._cache.add_object( prp )
            self._cache.add_object( val )

            setattr( mobj, prp.name, prp )

        return mobj # Metadata object with list of properties (tags)

 def __init__(self, uri, method=None, namespaces=None, sid=''):
     self.__serviceURL = uri
     self.__serviceName = method
     self.namespaces = isinstance(namespaces, ClassLoader) and namespaces or \
                           ClassLoader(namespaces or [])
     self._seril = Serializer(self.namespaces)
     self.sid = sid
     self.logger = None
     self.start_call_listener = []
     self.end_call_listener = []

 def add(self, product_id, unit_cost, quantity=1, **extra_data_dict):
     """
         Returns True if the addition of the product of the given product_id and unit_cost with given quantity 
         is succesful else False.
         Can also add extra details in the form of dictionary.
     """
     product_dict = self.__product_dict(
         unit_cost, quantity, extra_data_dict)
     self.redis_connection.hset(
         self.user_redis_key, product_id, Serializer.dumps(product_dict))
     self.user_cart_exists = self.cart_exists(self.user_id)
     self.set_ttl()

 def get_product(self, product_id):
     """
         Returns the cart details as a Dictionary for the given product_id
     """
     if self.user_cart_exists:
         product_string = self.redis_connection.hget(
             self.user_redis_key, product_id)
         if product_string:
             return Serializer.loads(product_string)
         else:
             return {}
     else:
         raise ErrorMessage("The user cart is Empty")

    def write(self, buf, offset=0):
        """
        Updates object information 

        :param buf:     a YAML representation of an object.
        :type buf:      str

        :return:        0 on success or and negative error code.
        """
        try:
            new = Serializer.deserialize(self.model_instance.__class__, buf)
            new.id = self.model_instance.id

        except Exception, e:
            return -1 # TODO find a way to handle expceptions better..

 def __init__(self, model, **kwargs):
     self.model = model
     self.fields = kwargs.get("fields") or self.model._meta.get_all_field_names()
     self.name = kwargs.get("name")
     self.parent_name = kwargs.get("parent_name", False)
     if self.parent_name:
         self.parent_id = self.parent_name + "_id"
     else:
         self.parent_id = ""
     self.id = self.name + "_id"
     self.set_name = self.name + "_set"
     self.relations = kwargs.get("relations") or [
         rel.get_accessor_name() for rel in model._meta.get_all_related_objects()
     ]
     self.parent_model = kwargs.get("parent_model")
     self.serialize = Serializer(self.fields)

    def __push_data_from_obj(self, obj):
        """ saves array data to disk in HDF5s and uploads new datafiles to the 
        server according to the arrays of the given obj. Saves datafile objects 
        to cache """
        data_refs = {} # collects all references to the related data - output
        model_name = self._meta.get_type_by_obj( obj )

        data_attrs = self._meta.get_array_attr_names( model_name )
        attrs_to_sync = self._cache.detect_changed_data_fields( obj )

        for attr in data_attrs: # attr is like 'times', 'signal' etc.

            arr = None
            if attr in attrs_to_sync:
                # 1. get current array and units
                fname = self._meta.app_definitions[model_name]['data_fields'][attr][2]
                if fname == 'self':
                    arr = obj # some NEO objects like signal inherit array
                else:
                    arr = getattr(obj, fname)

            if not type(arr) == type(None): # because of NEO __eq__
                units = Serializer.parse_units(arr)

                datapath = self._cache.save_data(arr)
                json_obj = self._remote.save_data(datapath)

                # update cache data map
                datalink = json_obj['permalink']
                fid = str(get_id_from_permalink( datalink ))

                folder, tempname = os.path.split(datapath)
                new_path = os.path.join(folder, fid + tempname[tempname.find('.'):])
                os.rename(datapath, new_path)
                self._cache.update_data_map(fid, new_path)

                data_refs[ attr ] = {'data': datalink, 'units': units}

            else:
                data_refs[ attr ] = None

        return data_refs

 def __init__(self, save):
     logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO)
     if not os.path.isdir("stats"):
         os.makedirs("stats")      
     self.save = save
     self.utils = Utils()
     logging.info("Loading cartesian coordinates...") 
     serializer = Serializer()
     
     rave_env = Environment()
     f = "stats/model/test.urdf"
     rave_env.initializeEnvironment(f)
     self.link_dimensions = rave_env.getRobotLinkDimensions()
     rave_env.destroy()
     
     self.setup_kinematics(serializer, "stats")
     self.process_covariances = []
     for logfile in glob.glob("stats" + "/*.log"):
         self.process_covariances.append(serializer.read_process_covariance(logfile))
     #arr = serializer.deserialize_joint_angles(path="stats", file="state_path1.txt")        
     #self.plot_state_path(serializer, arr)
     #sleep()
     
     self.create_video(serializer, "stats")
     logging.info("plotting paths")  
     try:  
         self.plot_paths(serializer)
     except:
         logging.warn("Paths could not be plotted")        
     logging.info("Reading config") 
     config = serializer.read_config("config.yaml", path="stats")
         
     #try:
     #self.plot_end_effector_paths(serializer, plot_scenery=True, plot_manipulator=True)
     #except:
     #    print "Error. End effector paths could not be plotted"
     
     logging.info("Plotting mean planning times")
     self.plot_mean_planning_times(serializer, 
                                   dir="stats", 
                                   filename="mean_planning_times_per_step*.yaml", 
                                   output="mean_planning_times_per_step.pdf")
     self.plot_mean_planning_times(serializer, 
                                   dir="stats", 
                                   filename="mean_planning_times_per_run*.yaml", 
                                   output="mean_planning_times_per_run.pdf")
     
     self.plot_particles(serializer, particle_limit=config['particle_plot_limit'])
     
     try:
         self.plot_paths(serializer, best_paths=True)
     except:
         logging.warn("Best_paths could not be plotted")
         
     try:
         cart_coords = serializer.load_cartesian_coords(path="stats")
     except:
         logging.warn("Cartesian_coords could not be plotted")
     
     logging.info("Plotting average distance to goal" )       
     try:
         self.plot_average_dist_to_goal(serializer, cart_coords)
     except:
         logging.warn("Average distance to goal could not be plotted")
     
     logging.info("plotting mean rewards")
     try:
         self.plot_mean_rewards(serializer)
     except Exception as e:
         logging.warn("Mean rewards could not be plotted: " + str(e))       
     
     logging.info("Plotting EMD graph...")       
     try:
         self.plot_emd_graph(serializer, cart_coords) 
     except:
         logging.warn("EMD graphs could not be plotted")
     
     logging.info("Plotting histograms...") 
     try:            
         self.save_histogram_plots(serializer, cart_coords) 
     except:
         logging.warn("Histograms could not be plotted")  

class TestSerialize(unittest.TestCase):

    @classmethod
    def _clearMsgDefs(self):
        # Clear all non-test messages
        try:
            while True:
                messages.MESSAGES.pop()
        except IndexError:
            pass

    @classmethod
    def setUpClass(self):
        self._clearMsgDefs()
        messages.MESSAGES.append(__import__("test_pb2", globals(), locals(), [], -1))
        self.blank_args = [1, 0, 2, 0, 3, 0, 4, 0, 5, 0, 6, 0, 7, 0, 8, 0, 9, 0, 10, 0, 11, 0, 12, 0, True, 0, "a", "b", "c", "d"]
        self.neg_args = [-1.0, 0, -1.0, 0, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, 1, 1, "", "", "", ""]
        self.illegal_len_args = [
                                [0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", "", ""],
                                [0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", "", ""]
                            ]
        self.too_long_args = [
                                [0, 0, 0, 0, 0, 32768, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", "", ""],
                                [0, 0, 0, 0, 0, 0, 0, 32768, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", "", ""],
                                [0, 0, 0, 0, 0, 0, 0, 0, 0, 32768, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", "", ""],
                                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32768, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", "", ""],
                                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32768, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", "", ""],
                                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32768, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", "", ""],
                                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32768, 0, 0, 0, 0, 0, 0, 0, 0, "", "", "", ""],
                                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32768, 0, 0, 0, 0, 0, 0, "", "", "", ""],
                                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32768, 0, 0, 0, 0, "", "", "", ""],
                                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32768, 0, 0, "", "", "", ""],
                                [0, 0, 0, 0, 0, -32769, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", "", ""],
                                [0, 0, 0, 0, 0, 0, 0, -32769, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", "", ""],
                                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -32769, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", "", ""],
                                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -32769, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", "", ""],
                                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -32769, 0, 0, 0, 0, "", "", "", ""],
                                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -32769, 0, 0, "", "", "", ""],
                                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "aaa", "", ""],
                                [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", "", "aaa"]
                            ]

    def setUp(self):
        self.s = Serializer()

    def test_id(self):
        enc = self.s.serialize("Test", *self.blank_args)
        (name, ) = struct.unpack_from('20s', enc)

        self.assertTrue(name.startswith('test_pb2Test'))

    def test_values(self):
        enc = self.s.serialize("Test", *self.blank_args)

        def f(obj):
            self.assertTrue(obj.IsInitialized())
            for (_, value) in obj.ListFields():
                self.assertIn(value, self.blank_args)

        self.s.add_handler("Test", f)
        self.s.unserialize(enc)

    def test_explicit_call(self):
        enc = self.s.serialize("test.Test", *self.blank_args)

        def f(obj):
            self.assertTrue(obj.IsInitialized())
            for (_, value) in obj.ListFields():
                self.assertIn(value, self.blank_args)

        self.s.add_handler("Test", f)
        self.s.unserialize(enc)

    def test_negative_values(self):
        enc = self.s.serialize("Test", *self.neg_args)

        def f(obj):
            self.assertTrue(obj.IsInitialized())
            for (_, value) in obj.ListFields():
                self.assertIn(value, self.neg_args)

        self.s.add_handler("Test", f)
        self.s.unserialize(enc)

    def test_illegal_length_option(self):
        for args in self.illegal_len_args:
            with self.assertRaises(FieldLengthUnsupportedException):
                self.s.serialize("Test", *args)

    def test_too_long_option(self):
        for args in self.too_long_args:
            with self.assertRaises(FieldTooLongException):
                self.s.serialize("Test", *args)

    def test_unknown_message(self):
        with self.assertRaises(UnknownMessageException):
            self.s.serialize("Unknown")

    def test_unknown_message_explicit(self):
        with self.assertRaises(UnknownMessageException):
#.........这里部分代码省略.........

 def setUp(self):
     self.s = Serializer()

 def init_serializer(self):        
     self.serializer = Serializer()
     self.serializer.create_temp_dir(self.abs_path, "mpc") 

#.........这里部分代码省略.........
            cjvels = v_double()
            cjvals_arr = [x[i] for i in xrange(len(x) / 2)]
            cjvels_arr = [x[i] for i in xrange(len(x) / 2, len(x))]
            cjvals[:] = cjvals_arr
            cjvels[:] = cjvels_arr
            particle_joint_values = v2_double()
            if show_viewer:
                self.robot.updateViewerValues(cjvals, 
                                              cjvels,
                                              particle_joint_values,
                                              particle_joint_values)
            time.sleep(0.03) 
            
            y += 1
            print y
    
    def setup_scene(self,                    
                    environment_file,
                    robot):
        """ Load the obstacles """         
        self.obstacles = self.utils.loadObstaclesXML(self.abs_path + "/" + environment_file)      
        
        """ Load the goal area """
        goal_area = v_double()
        self.utils.loadGoalArea(self.abs_path + "/" + environment_file, goal_area)
        if len(goal_area) == 0:
            print "ERROR: Your environment file doesn't define a goal area"
            return False
        self.goal_position = [goal_area[i] for i in xrange(0, 3)]
        self.goal_radius = goal_area[3]
        return True
                
    def init_serializer(self):        
        self.serializer = Serializer()
        self.serializer.create_temp_dir(self.abs_path, "mpc") 
                
    def clear_stats(self, dir):
        if os.path.isdir(dir):
            cmd = "rm -rf " + dir + "/*"            
            os.system(cmd)
        else:
            os.makedirs(dir)
                
    def get_average_distance_to_goal_area(self, goal_position, goal_radius, cartesian_coords):        
        avg_dist = 0.0
        goal_pos = np.array(goal_position)        
        for i in xrange(len(cartesian_coords)):            
            cart = np.array(cartesian_coords[i])            
            dist = np.linalg.norm(goal_pos - cart)            
            if dist < goal_radius:
                dist = 0.0
            avg_dist += dist
        if avg_dist == 0.0:
            return avg_dist        
        return np.asscalar(avg_dist) / len(cartesian_coords)
            
    def calc_covariance_value(self, robot, error, covariance_matrix, covariance_type='process'):
        active_joints = v_string()
        robot.getActiveJoints(active_joints)        
        if covariance_type == 'process':
            if self.dynamic_problem:            
                torque_limits = v_double()
                robot.getJointTorqueLimits(active_joints, torque_limits)
                torque_limits = [torque_limits[i] for i in xrange(len(torque_limits))]
                for i in xrange(len(torque_limits)):
                    torque_range = 2.0 * torque_limits[i]

def fix_peewee_obj(obj):
    """
    Serializes a single peewee object
    """
    ser = Serializer()
    return ser.serialize_object(obj)

class TestSerialize(unittest.TestCase):

    @classmethod
    def _clearMsgDefs(self):
        # Clear all non-test messages
        try:
            while True:
                messages.MESSAGES.pop()
        except IndexError:
            pass

    @classmethod
    def setUpClass(self):
        self._clearMsgDefs()
        messages.MESSAGES.append(__import__("test_complex_pb2", globals(), locals(), [], -1))

    def setUp(self):
        self.calls = 0
        self.s = Serializer()

    def test_unnamed_serialize(self):
        enc = self.s.serialize("TestComplex", [("normal", ["m1", "m2"])])        

        def f(obj):
            self.assertTrue(obj.IsInitialized())
            self.assertEqual(obj.nesteds[0].normal, "normal")
            self.assertEqual(obj.nesteds[0].multiple[0], "m1")
            self.assertEqual(obj.nesteds[0].multiple[1], "m2")
            self.calls += 1
        
        self.s.add_handler("TestComplex", f)
        self.s.unserialize(enc)
        self.assertEqual(self.calls, 1)

    def test_named_serialize(self):
        enc = self.s.serialize("TestComplex",
                            nesteds = [{
                                            "normal": "normal",
                                            "multiple": [
                                                        "m1", 
                                                        "m2"
                                                        ]
                                        }]
                            )        

        def f(obj):
            self.assertTrue(obj.IsInitialized())
            self.assertEqual(obj.nesteds[0].normal, "normal")
            self.assertEqual(obj.nesteds[0].multiple[0], "m1")
            self.assertEqual(obj.nesteds[0].multiple[1], "m2")
            self.calls += 1
        
        self.s.add_handler("TestComplex", f)
        self.s.unserialize(enc)
        self.assertEqual(self.calls, 1)

    def test_nested_length(self):
        with self.assertRaises(FieldTooLongException):
            self.s.serialize("TestComplex", [("", ["longerthantwentycharacters"])])

    def test_obj_onto_repeated(self):
        with self.assertRaises(FieldWrongTypeException):
            self.s.serialize("TestComplex", [("", ("m1",))])

    def test_repeated_onto_obj(self):
        with self.assertRaises(FieldWrongTypeException):
            self.s.serialize("TestComplex", [["", ["m1"]]])

    def test_unicode_strings(self):
        sbs = '}\xc8A\xc1\x8a}D\xe8\xea\x93}'
        enc = self.s.serialize("TestComplex", [(sbs, ["m1", "m2"])])

        def f(obj):
            self.assertTrue(obj.IsInitialized())
            for i in range(len(sbs)):
                self.assertEqual(ord(obj.nesteds[0].normal[i]), ord(sbs[i]))
            self.assertEqual(obj.nesteds[0].multiple[0], "m1")
            self.assertEqual(obj.nesteds[0].multiple[1], "m2")
            self.calls += 1

        self.s.add_handler("TestComplex", f)
        self.s.unserialize(enc)
        self.assertEqual(self.calls, 1)

    def test_optional_missing(self):
        enc = self.s.serialize("TestOptional")

        def f(obj):
            self.assertTrue(obj.IsInitialized())
            self.assertTrue(not obj.field)
            self.calls += 1

        self.s.add_handler("TestOptional", f)
        self.s.unserialize(enc)
        self.assertEqual(self.calls, 1)

    def test_optional_set(self):
        enc = self.s.serialize("TestOptional", "test")

        def f(obj):
#.........这里部分代码省略.........

#.........这里部分代码省略.........
            if show_viewer:
                self.robot.updateViewerValues(cjvals, 
                                              cjvels,
                                              particle_joint_values,
                                              particle_joint_values)
            #time.sleep(0.03)
            time.sleep(0.03) 
            
            y += 1
            print y
            
    """
    ================================================================
    """
        
        
    def setup_scene(self,                    
                    environment_file,
                    robot):
        """ Load the obstacles """         
        self.obstacles = self.utils.loadObstaclesXML(self.abs_path + "/" + environment_file)      
        
        """ Load the goal area """
        goal_area = v_double()
        self.utils.loadGoalArea(self.abs_path + "/" + environment_file, goal_area)
        if len(goal_area) == 0:
            print "ERROR: Your environment file doesn't define a goal area"
            return False
        self.goal_position = [goal_area[i] for i in xrange(0, 3)]
        self.goal_radius = goal_area[3]
        return True
            
    def init_serializer(self):
        self.serializer = Serializer()
        self.serializer.create_temp_dir(self.abs_path, "lqg")        
            
    def clear_stats(self, dir):        
        if os.path.isdir(dir):
            cmd = "rm -rf " + dir + "/*"            
            os.system(cmd)
        else:
            os.makedirs(dir)
            
    def get_average_distance_to_goal_area(self, goal_position, goal_radius, cartesian_coords):        
        avg_dist = 0.0
        goal_pos = np.array(goal_position)        
        for i in xrange(len(cartesian_coords)):            
            cart = np.array(cartesian_coords[i])            
            dist = np.linalg.norm(goal_pos - cart)            
            if dist < goal_radius:
                dist = 0.0
            avg_dist += dist
        if avg_dist == 0.0:
            return avg_dist        
        return np.asscalar(avg_dist) / len(cartesian_coords)          
            
    def problem_setup(self, delta_t, num_links):
        A = np.identity(num_links * 2)
        H = np.identity(num_links * 2)
        W = np.identity(num_links * 2)
        C = self.path_deviation_cost * np.identity(num_links * 2)
        
        '''B = delta_t * np.identity(num_links * 2)                
        V = np.identity(num_links * 2)
        D = self.control_deviation_cost * np.identity(num_links * 2)        
        M_base = np.identity(2 * self.robot_dof)