Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
q_euler = QuaternionBase(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from a euler angles
q_test = QuaternionBase(euler)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(euler)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(euler)
q_euler = QuaternionBase(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from dcm
q_test = QuaternionBase(dcm)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(dcm)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(dcm)
q_euler = QuaternionBase(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
def test_conversion(self):
"""
Tests forward and backward conversions
"""
for q in self.quaternions:
# quaternion -> euler -> quaternion
q0 = q
e = QuaternionBase(q.q).euler
q1 = QuaternionBase(e)
assert q0.close(q1)
# quaternion -> dcm -> quaternion
q0 = q
dcm = QuaternionBase(q.q).dcm
q1 = QuaternionBase(dcm)
assert q0.close(q1)
Calls constructor for the quaternion from q euler and dcm and checks
if the resulting converions are equivalent to the arguments.
The test for the euler angles is weak as the solution is not unique
:param q: quaternion 4x1, [w, x, y, z]
:param euler: [roll, pitch, yaw], needs to be equivalent to q
:param q: dcm 3x3, needs to be equivalent to q
"""
# construct q from a QuaternionBase
quaternion_instance = QuaternionBase(q)
q_test = QuaternionBase(quaternion_instance)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(quaternion_instance)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(quaternion_instance)
q_euler = QuaternionBase(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from a quaternion
q_test = QuaternionBase(q)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(q)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(q)
q_euler = QuaternionBase(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from a euler angles
q_test = QuaternionBase(euler)
np.testing.assert_almost_equal(q_test.q, q)
"""
Helper function for constructor test
Calls constructor for the quaternion from q euler and dcm and checks
if the resulting converions are equivalent to the arguments.
The test for the euler angles is weak as the solution is not unique
:param q: quaternion 4x1, [w, x, y, z]
:param euler: [roll, pitch, yaw], needs to be equivalent to q
:param q: dcm 3x3, needs to be equivalent to q
"""
# construct q from a QuaternionBase
quaternion_instance = QuaternionBase(q)
q_test = QuaternionBase(quaternion_instance)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(quaternion_instance)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(quaternion_instance)
q_euler = QuaternionBase(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from a quaternion
q_test = QuaternionBase(q)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(q)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(q)
q_euler = QuaternionBase(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
np.allclose(q_test.q, q_euler.q))
# construct q from a euler angles
q_test = QuaternionBase(euler)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(euler)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(euler)
q_euler = QuaternionBase(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from dcm
q_test = QuaternionBase(dcm)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(dcm)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(dcm)
q_euler = QuaternionBase(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
def test_transform(self):
"""Test transform"""
for q in self.quaternions:
q_inv = q.inversed
v = np.array([1, 2, 3])
v1 = q.transform(v)
v1_dcm = np.dot(q.dcm, v)
np.testing.assert_almost_equal(v1, v1_dcm)
# test versus slower solution using multiplication
v1_mul = q * QuaternionBase(np.hstack([0, v])) * q.inversed
np.testing.assert_almost_equal(v1, v1_mul[1:4])
v2 = q_inv.transform(v1)
np.testing.assert_almost_equal(v, v2)
def test_conversion(self):
"""
Tests forward and backward conversions
"""
for q in self.quaternions:
# quaternion -> euler -> quaternion
q0 = q
e = QuaternionBase(q.q).euler
q1 = QuaternionBase(e)
assert q0.close(q1)
# quaternion -> dcm -> quaternion
q0 = q
dcm = QuaternionBase(q.q).dcm
q1 = QuaternionBase(dcm)
assert q0.close(q1)
# construct q from a quaternion
q_test = QuaternionBase(q)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(q)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(q)
q_euler = QuaternionBase(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from a euler angles
q_test = QuaternionBase(euler)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(euler)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(euler)
q_euler = QuaternionBase(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from dcm
q_test = QuaternionBase(dcm)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(dcm)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(dcm)
q_euler = QuaternionBase(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
quaternion_instance = QuaternionBase(q)
q_test = QuaternionBase(quaternion_instance)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(quaternion_instance)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(quaternion_instance)
q_euler = QuaternionBase(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from a quaternion
q_test = QuaternionBase(q)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(q)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(q)
q_euler = QuaternionBase(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from a euler angles
q_test = QuaternionBase(euler)
np.testing.assert_almost_equal(q_test.q, q)
q_test = QuaternionBase(euler)
np.testing.assert_almost_equal(q_test.dcm, dcm)
q_test = QuaternionBase(euler)
q_euler = QuaternionBase(q_test.euler)
assert(np.allclose(q_test.euler, euler) or
np.allclose(q_test.q, q_euler.q))
# construct q from dcm
q_test = QuaternionBase(dcm)
def process_msg(self):
# the actuator assumes ned control, so don't do any transformation
q = QuaternionBase(self._msg.q)
self.data['roll'] = q.euler[0]
self.data['pitch'] = q.euler[1]
self.data['yaw'] = q.euler[2]
self.data['thrust'] = self._msg.thrust