Exemple #1
0
 def test_cov(self):
     config, robot_params = loadSystem()
     block = ChainSensor(
         config["chainA"],
         ChainMeasurement(chain_id="chainA",
                          chain_state=JointState(position=[0])), "boardA")
     block.update_config(robot_params)
     cov = block.compute_cov(None)
     print cov
 def test_cov(self):
     config, robot_params = loadSystem()
     block = ChainSensor(config["chainA"],
                         ChainMeasurement(chain_id="chainA",
                                          chain_state=JointState(position=[0]) ),
                         "boardA")
     block.update_config(robot_params)
     cov = block.compute_cov(None)
     print cov
Exemple #3
0
 def test_sparsity(self):
     config, robot_params = loadSystem()
     block = ChainSensor(config["chainA"],
                         ChainMeasurement(chain_id="chainA",
                                          chain_state=JointState(position=[numpy.pi / 2.0]) ),
                         "boardA")
     block.update_config(robot_params)
     sparsity = block.build_sparsity_dict()
     self.assertEqual(sparsity['transforms']['j0'], [1,1,1,1,1,1])
     self.assertEqual(sparsity['transforms']['j1'], [1,1,1,1,1,1])
     self.assertEqual(sparsity['chains']['chainA'], {'gearing':[1]})
Exemple #4
0
    def test_cov(self):
        config, robot_params = loadSystem()
        block = ChainSensor(config["chainA"],
                            ChainMeasurement(chain_id="chainA",
                                             chain_state=JointState(position=[0]) ),
                            "boardA")
        block.update_config(robot_params)
        cov = block.compute_cov(None)
        print cov

        self.assertAlmostEqual(cov[0,0], 0.0, 6)
        self.assertAlmostEqual(cov[1,0], 0.0, 6)
        self.assertAlmostEqual(cov[1,1], 1.0, 6)
        self.assertAlmostEqual(cov[4,4], 4.0, 6)
Exemple #5
0
    def test_update1(self):
        config, robot_params = loadSystem()
        block = ChainSensor(
            config["chainA"],
            ChainMeasurement(chain_id="chainA",
                             chain_state=JointState(position=[0])), "boardA")
        block.update_config(robot_params)

        target = matrix([[0.5, 1.5, 0.5, 1.5], [0.5, 0.5, -0.5, -0.5],
                         [0, 0, 0, 0], [1, 1, 1, 1]])

        h = block.compute_expected(target)
        z = block.get_measurement()
        r = block.compute_residual(target)

        self.assertAlmostEqual(numpy.linalg.norm(target - h), 0.0, 6)

        print "z=\n", z
        print "target=\n", target

        self.assertAlmostEqual(numpy.linalg.norm(target - z), 0.0, 6)
        self.assertAlmostEqual(numpy.linalg.norm(r - numpy.zeros([12])), 0.0,
                               6)
    def test_update3(self):
        config, robot_params = loadSystem()
        block = ChainSensor(config["chainB"],
                            ChainMeasurement(chain_id="chainB",
                                             chain_state=JointState(position=[0.0, 0.0]) ),
                            "boardA")
        block.update_config(robot_params)

        target = matrix([[2, 3, 2, 3],
                         [0, 0, 1, 1],
                         [0, 0, 0, 0],
                         [1, 1, 1, 1]])

        h = block.compute_expected(target)
        z = block.get_measurement()
        r = block.compute_residual(target)

        self.assertAlmostEqual(numpy.linalg.norm(target-h), 0.0, 6)

        print "z=\n",z
        print "target=\n",target

        self.assertAlmostEqual(numpy.linalg.norm(target-z), 0.0, 6)
        self.assertAlmostEqual(numpy.linalg.norm(r - numpy.zeros([12])), 0.0, 6)