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)
