def test_vee():
rho_vec = np.array([[4, 3]]).T
theta = 2 * np.pi / 3
xi_vec = np.vstack((rho_vec, theta))
xi_hat = SE2.hat(xi_vec)
np.testing.assert_equal(SE2.vee(xi_hat), xi_vec)
def test_hat():
rho_vec = np.array([[1, 2]]).T
theta = np.pi / 3
xi_vec = np.vstack((rho_vec, theta))
xi_hat_expected = np.block([[SO2.hat(theta), rho_vec], [np.zeros((1, 3))]])
np.testing.assert_equal(SE2.hat(xi_vec), xi_hat_expected)