How to use the pymavlink.quaternion.QuaternionBase function in pymavlink

To help you get started, we’ve selected a few pymavlink examples, based on popular ways it is used in public projects.

Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.

github ArduPilot / pymavlink / tests / test_quaternion.py View on Github external
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))
github ArduPilot / pymavlink / tests / test_quaternion.py View on Github external
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)
github ArduPilot / pymavlink / tests / test_quaternion.py View on Github external
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)
github ArduPilot / pymavlink / tests / test_quaternion.py View on Github external
"""
        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))
github ArduPilot / pymavlink / tests / test_quaternion.py View on Github external
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))
github ArduPilot / pymavlink / tests / test_quaternion.py View on Github external
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)
github ArduPilot / pymavlink / tests / test_quaternion.py View on Github external
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)
github ArduPilot / pymavlink / tests / test_quaternion.py View on Github external
# 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))
github ArduPilot / pymavlink / tests / test_quaternion.py View on Github external
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)
github morse-simulator / morse / src / morse / middleware / mavlink / read_attitude_target.py View on Github external
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