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_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_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)
def test_update2(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)
target = matrix([[0, 0,-1,-1],
[1, 2, 1, 2],
[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)
