def test_polyhedron(self):
b = Box(1, 2, 3)
A, b = b.get_polyhedron(np.eye(4))
Aa = np.array([[1, 0, 0], [-1, 0, 0], [0, 1, 0], [0, -1, 0], [0, 0, 1],
[0, 0, -1]])
ba = np.array([0.5, 0.5, 1, 1, 1.5, 1.5])
assert_almost_equal(A, Aa)
assert_almost_equal(b, ba)
def test_polyhedron_transformed(self):
b = Box(1, 2, 3)
tf = pose_z(0.3, 0.1, 0.2, -0.3)
A, b = b.get_polyhedron(tf)
Aa = np.array([[1, 0, 0], [-1, 0, 0], [0, 1, 0], [0, -1, 0], [0, 0, 1],
[0, 0, -1]])
ba = np.array([0.5, 0.5, 1, 1, 1.5, 1.5])
Aa = np.dot(Aa, tf[:3, :3].T)
ba = ba + np.dot(Aa, tf[:3, 3])
assert_almost_equal(A, Aa)
assert_almost_equal(b, ba)