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
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)
