def testFromRPY(self):
for i in range(100):
rpy = np.random.uniform(-np.pi, np.pi, size=3)
R = SO3.fromRPY(rpy).R
q = Quaternion.fromRPY(rpy)
np.testing.assert_allclose(R, q.R.T)
def test_from_RPY_and_trans(self):
rpy = [np.random.uniform(-np.pi, np.pi, size=3) for i in range(100)]
trans = [np.random.uniform(-10.0, 10.0, size=3) for i in range(100)]
for (eul, t) in zip(rpy, trans):
T = SE3.fromRPYandt(eul, t)
R = SO3.fromRPY(eul).R
# q = Quaternion.fromRotationMatrix(R.T)
q = Quaternion.fromRotationMatrix(R)
np.testing.assert_allclose(q.q, T.q_arr)
np.testing.assert_allclose(t, T.t)
def testConstructor(self):
for i in range(100):
angles = np.random.uniform(-np.pi, np.pi, size=3)
R_ex = SO3.fromRPY(angles)
cp = np.cos(angles[0])
sp = np.sin(angles[0])
R1 = np.array([[1, 0, 0], [0, cp, -sp], [0, sp, cp]])
ct = np.cos(angles[1])
st = np.sin(angles[1])
R2 = np.array([[ct, 0, st], [0, 1, 0], [-st, 0, ct]])
cps = np.cos(angles[2])
sps = np.sin(angles[2])
R3 = np.array([[cps, -sps, 0], [sps, cps, 0], [0, 0, 1]])
R_true = Rotation.from_euler(
'ZYX', [angles[2], angles[1], angles[0]]).as_matrix()
R_ex2 = SO3(R_true)
np.testing.assert_allclose(R_true, R_ex.arr)
np.testing.assert_allclose(R_true, R_ex2.arr)
def test_euler(self):
for i in range(100):
R1 = SO3.random()
rpy = R1.euler
R2 = SO3.fromRPY(rpy)
np.testing.assert_allclose(R1.R, R2.R, rtol=1e-5)