def test_map_odometry_to_table(self):
odometry_pos = [1, 2, 3]
offset = [0.1, 0.2, 0.3]
table_pos = position.map_odometry_to_table(odometry_pos, offset)
self.assertAlmostEqual(cos(0.3) * 1 - sin(0.3) * 2 + 0.1, table_pos[0])
self.assertAlmostEqual(sin(0.3) * 1 + cos(0.3) * 2 + 0.2, table_pos[1])
self.assertAlmostEqual(3 + 0.3, table_pos[2])
def test_compute_odometry_offset(self):
table_pos = [1, 2, 3]
odometry_pos = [4, 5, 6]
offset = position.compute_odometry_offset(table_pos, odometry_pos)
np.testing.assert_almost_equal(table_pos, position.map_odometry_to_table(odometry_pos, offset))
