def test_cov(self): config, robot_params = self.load() block = ChainSensor( config, ChainMeasurement(chain_id="chainA", chain_state=JointState(position=[0])), "boardA") block.update_config(robot_params) cov = block.compute_cov(None) 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)
def test_sparsity(self): config, robot_params = self.load() block = ChainSensor( config, 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']['transformA'], [1, 1, 1, 1, 1, 1]) self.assertEqual(sparsity['transforms']['transformB'], [1, 1, 1, 1, 1, 1]) self.assertEqual(sparsity['dh_chains']['chainA'], { 'dh': [[1, 1, 1, 1]], 'gearing': [1] })
def test_update1(self): config, robot_params = self.load() block = ChainSensor( config, ChainMeasurement(chain_id="chainA", chain_state=JointState(position=[0])), "boardA") block.update_config(robot_params) target = matrix([[1, 2, 1, 2], [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)