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)