def test_isclose():
a = Quaternion(1.23e10)
b = a + 0.001
assert a != b
assert quaternion.isclose(a, b)
assert quaternion.isclose(a, a)
def check(rfn, cfn, qfn, q):
""" compares the Quaternion function (qfn) with the equivilent
real (rfn) and complex (cfn) functions
"""
a = Quaternion(rfn(q.real))
b = qfn(Quaternion(q.real))
assert quaternion.isclose(a, b)
a = Quaternion(cfn(q.complex))
b = qfn(Quaternion(q.complex))
assert quaternion.isclose(a, b)
# For single value functions, we can create a z with same r and phi as the q
# Call the complex function, then create a Quaternion with f(r, phi) of the
# complex result merged in with the original axis.
#
q_polar = quaternion.polar(q)
z = cmath.rect(q_polar[0], q_polar[1])
fz = cfn(z)
fz_polar = cmath.polar(fz)
fi = quaternion.rect(fz_polar[0], fz_polar[1], q_polar[2])
fd = qfn(q)
assert quaternion.isclose(fi, fd)
# Test cut'n'paste errors referenceing imaginary components
# by rotating the axis,
#
a = qfn(qfoo(q))
b = qfoo(qfn(q))
assert quaternion.isclose(a, b)
a = qfn(qbar(q))
b = qbar(qfn(q))
assert quaternion.isclose(a, b)
def test_tan():
for q in qlist:
s = quaternion.sin(q)
c = quaternion.cos(q)
t = s / c
r = quaternion.tan(q)
assert quaternion.isclose(r, t)
def __eq__(self, other: object) -> bool:
if not isinstance(other, Orientation):
return False
return cast(
bool,
quaternion.isclose(self.as_quaternion(),
other.as_quaternion(),
rtol=1.0e-6)[0])
def test_local_pose(self):
shape = Shape.create_sphere(1., Material())
shape.set_local_pose([0, 2, 1])
p, q = shape.get_local_pose()
np.testing.assert_almost_equal(p, [0, 2, 1])
self.assertTrue(npq.isclose(q, npq.one))
shape.set_local_pose(([0, 2, 1], [1, 0, 0, 1]))
p, q = shape.get_local_pose()
np.testing.assert_almost_equal(p, [0, 2, 1])
np.testing.assert_almost_equal(npq.as_float_array(q), np.array([1, 0, 0, 1]) / np.sqrt(2)) # is normalized
def test_sqrt():
a = Quaternion(4.0, 0.0, 0.0, 0.0)
b = Quaternion(2.0, 0.0, 0.0, 0.0)
c = quaternion.sqrt(a)
assert abs(b - c) < 1.0e-9
a = Quaternion(1.23, 4.56, -7.89, 2.456)
b = quaternion.sqrt(a)
c = b * b
assert abs(a - c) < 1.0e-9
for q in qlist:
a = q * q
b = quaternion.sqrt(a)
assert quaternion.isclose(q, b)
a = quaternion.sqrt(q)
b = a * a
assert quaternion.isclose(q, b)
def test_casting_from_pose(self):
pref = [5., 3., 6.]
qref = npq.from_rotation_vector([3., 2., 3])
self.assertTrue(type(qref) == npq.quaternion)
p, q = cast_transformation((pref, qref))
self.assertTrue(np.isclose(pref, p).all())
self.assertTrue(npq.isclose(q, qref, rtol=1e-5))
p, q = cast_transformation((pref, [qref.w, qref.x, qref.y, qref.z]))
self.assertTrue(np.isclose(pref, p).all())
self.assertTrue(npq.isclose(q, qref, rtol=1e-5))
p, q = cast_transformation((pref, (qref.w, qref.x, qref.y, qref.z)))
self.assertTrue(np.isclose(pref, p).all())
self.assertTrue(npq.isclose(q, qref, rtol=1e-5))
p, q = cast_transformation(
[5., 3., 6., qref.w, qref.x, qref.y, qref.z])
self.assertTrue(np.isclose(pref, p).all())
self.assertTrue(npq.isclose(q, qref, rtol=1e-5))
def test_global_pose(self):
actor = RigidDynamic()
actor.set_global_pose(([0, 2, 1], npq.one))
p, q = actor.get_global_pose()
np.testing.assert_almost_equal(p, [0, 2, 1])
self.assertTrue(npq.isclose(q, npq.one))
actor.set_global_pose(([0, 2, 1], [1, 0, 0, 1]))
p, q = actor.get_global_pose()
np.testing.assert_almost_equal(p, [0, 2, 1])
np.testing.assert_almost_equal(npq.as_float_array(q),
np.array([1, 0, 0, 1]) /
np.sqrt(2)) # is normalized
def test_exp():
for q in qlist:
# Do special for the zeroth term
#
t = quaternion.one
terms = [t]
for j in range(1, 1000):
t = t * q / j
terms.append(t)
if abs(t) < 1.0E-200:
break
# Add in reverse - smallest terms first
#
s = Quaternion(0.0)
m = len(terms) - 1
for j in range(m, -1, -1):
s += terms[j]
r = quaternion.exp(q)
assert quaternion.isclose(r, s)
def test_cos():
for q in qlist:
check(math.cos, cmath.cos, quaternion.cos, q)
# Do special for the zeroth term
#
t = quaternion.one
terms = [t]
for j in range(2, 1000, 2):
t = -t * q * q / (float(j) * (j - 1))
terms.append(t)
if abs(t) < 1.0E-200:
break
# Add in reverse - smallest terms first
#
s = Quaternion(0.0)
m = len(terms) - 1
for j in range(m, -1, -1):
s += terms[j]
r = quaternion.cos(q)
assert quaternion.isclose(r, s)
def __eq__(self, other):
return (np.all(quaternion.isclose(self.q_r, other.q_r)) or np.all(quaternion.isclose(self.q_r,-other.q_r)))\
and (np.all(quaternion.isclose(self.q_d, other.q_d)) or np.all(quaternion.isclose(self.q_d,-other.q_d)))
def __eq__(self, other) -> bool:
if not isinstance(other, Orientation):
return False
return quaternion.isclose(self.as_quaternion(), other.as_quaternion(), rtol=1.e-8)[0]
def test_casting_from_position(self):
pref = [1., 2., 3.]
for pin in [([1, 2, 3], ), [1, 2, 3], (1, 2, 3), np.array([1, 2, 3])]:
p, q = cast_transformation(pin)
self.assertTrue(np.isclose(pref, p).all())
self.assertTrue(npq.isclose(q, npq.one))
