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))