def test_vee(): rho_vec = np.array([[4, 3, 2]]).T theta_vec = 2 * np.pi / 3 * np.array([[2, 1, 1]]).T / np.sqrt(6) xi_vec = np.vstack((rho_vec, theta_vec)) xi_hat = SE3.hat(xi_vec) np.testing.assert_equal(SE3.vee(xi_hat), xi_vec)
def test_hat(): rho_vec = np.array([[1, 2, 3]]).T theta_vec = np.pi / 3 * np.array([[1, -1, 2]]).T / np.sqrt(6) xi_vec = np.vstack((rho_vec, theta_vec)) xi_hat_expected = np.block([[SO3.hat(theta_vec), rho_vec], [np.zeros((1, 4))]]) np.testing.assert_equal(SE3.hat(xi_vec), xi_hat_expected)