autotest: remove copied-in duplicate of pymavlink rotmat

Tools/autotest/arducopter.py

@@ -16,8 +16,9 @@ import numpy
 
 from pymavlink import mavutil
 from pymavlink import mavextra
+from pymavlink import rotmat
 
-from pysim import util, rotmat
+from pysim import util
 
 from common import AutoTest
 from common import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException

Tools/autotest/pysim/aircraft.py

@@ -3,7 +3,7 @@ import random
 import time
 import util
 
-from rotmat import Vector3, Matrix3
+from pymavlink.rotmat import Vector3, Matrix3
 
 
 class Aircraft(object):

Tools/autotest/pysim/iris_ros.py

@@ -13,7 +13,7 @@ import rospy
 import sensor_msgs.msg as sensor_msgs
 
 from aircraft import Aircraft
-from rotmat import Vector3, Matrix3
+from pymavlink.rotmat import Vector3, Matrix3
 
 
 def quat_to_dcm(q1, q2, q3, q4):

Tools/autotest/pysim/rotmat.py

@@ -1,346 +0,0 @@
-#!/usr/bin/env python
-"""
- vector3 and rotation matrix classes
- This follows the conventions in the ArduPilot code,
- and is essentially a python version of the AP_Math library
-
- Andrew Tridgell, March 2012
-
- This library is free software; you can redistribute it and/or modify it
- under the terms of the GNU Lesser General Public License as published by the
- Free Software Foundation; either version 2.1 of the License, or (at your
- option) any later version.
-
- This library is distributed in the hope that it will be useful, but WITHOUT
- ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License
- for more details.
-
- You should have received a copy of the GNU Lesser General Public License
- along with this library; if not, write to the Free Software Foundation,
- Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
-
-"""
-from __future__ import print_function
-from math import acos, asin, atan2, cos, pi, radians, sin, sqrt
-
-
-class Vector3:
-    """A vector."""
-
-    def __init__(self, x=None, y=None, z=None):
-        if x is not None and y is not None and z is not None:
-            self.x = float(x)
-            self.y = float(y)
-            self.z = float(z)
-        elif x is not None and len(x) == 3:
-            self.x = float(x[0])
-            self.y = float(x[1])
-            self.z = float(x[2])
-        elif x is not None:
-            raise ValueError('bad initialiser')
-        else:
-            self.x = float(0)
-            self.y = float(0)
-            self.z = float(0)
-
-    def __repr__(self):
-        return 'Vector3(%.2f, %.2f, %.2f)' % (self.x,
-                                              self.y,
-                                              self.z)
-
-    def __add__(self, v):
-        return Vector3(self.x + v.x,
-                       self.y + v.y,
-                       self.z + v.z)
-
-    __radd__ = __add__
-
-    def __sub__(self, v):
-        return Vector3(self.x - v.x,
-                       self.y - v.y,
-                       self.z - v.z)
-
-    def __neg__(self):
-        return Vector3(-self.x, -self.y, -self.z)
-
-    def __rsub__(self, v):
-        return Vector3(v.x - self.x,
-                       v.y - self.y,
-                       v.z - self.z)
-
-    def __mul__(self, v):
-        if isinstance(v, Vector3):
-            """dot product"""
-            return self.x * v.x + self.y * v.y + self.z * v.z
-        return Vector3(self.x * v,
-                       self.y * v,
-                       self.z * v)
-
-    __rmul__ = __mul__
-
-    def __div__(self, v):
-        return Vector3(self.x / v,
-                       self.y / v,
-                       self.z / v)
-
-    def __mod__(self, v):
-        """Cross product."""
-        return Vector3(self.y * v.z - self.z * v.y,
-                       self.z * v.x - self.x * v.z,
-                       self.x * v.y - self.y * v.x)
-
-    def __copy__(self):
-        return Vector3(self.x, self.y, self.z)
-
-    copy = __copy__
-
-    def length(self):
-        return sqrt(self.x ** 2 + self.y ** 2 + self.z ** 2)
-
-    def zero(self):
-        self.x = self.y = self.z = 0
-
-    def angle(self, v):
-        """Return the angle between this vector and another vector."""
-        return acos(self * v) / (self.length() * v.length())
-
-    def normalized(self):
-        return self / self.length()
-
-    def normalize(self):
-        v = self.normalized()
-        self.x = v.x
-        self.y = v.y
-        self.z = v.z
-
-
-class Matrix3:
-    """A 3x3 matrix, intended as a rotation matrix."""
-
-    def __init__(self, a=None, b=None, c=None):
-        if a is not None and b is not None and c is not None:
-            self.a = a.copy()
-            self.b = b.copy()
-            self.c = c.copy()
-        else:
-            self.identity()
-
-    def __repr__(self):
-        return 'Matrix3((%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f))' % (
-            self.a.x, self.a.y, self.a.z,
-            self.b.x, self.b.y, self.b.z,
-            self.c.x, self.c.y, self.c.z)
-
-    def identity(self):
-        self.a = Vector3(1, 0, 0)
-        self.b = Vector3(0, 1, 0)
-        self.c = Vector3(0, 0, 1)
-
-    def transposed(self):
-        return Matrix3(Vector3(self.a.x, self.b.x, self.c.x),
-                       Vector3(self.a.y, self.b.y, self.c.y),
-                       Vector3(self.a.z, self.b.z, self.c.z))
-
-    def from_euler(self, roll, pitch, yaw):
-        """Fill the matrix from Euler angles in radians."""
-        cp = cos(pitch)
-        sp = sin(pitch)
-        sr = sin(roll)
-        cr = cos(roll)
-        sy = sin(yaw)
-        cy = cos(yaw)
-
-        self.a.x = cp * cy
-        self.a.y = (sr * sp * cy) - (cr * sy)
-        self.a.z = (cr * sp * cy) + (sr * sy)
-        self.b.x = cp * sy
-        self.b.y = (sr * sp * sy) + (cr * cy)
-        self.b.z = (cr * sp * sy) - (sr * cy)
-        self.c.x = -sp
-        self.c.y = sr * cp
-        self.c.z = cr * cp
-
-    def to_euler(self):
-        """Find Euler angles (321 convention) for the matrix."""
-        if self.c.x >= 1.0:
-            pitch = pi
-        elif self.c.x <= -1.0:
-            pitch = -pi
-        else:
-            pitch = -asin(self.c.x)
-        roll = atan2(self.c.y, self.c.z)
-        yaw = atan2(self.b.x, self.a.x)
-        return (roll, pitch, yaw)
-
-    def to_euler312(self):
-        """Find Euler angles (312 convention) for the matrix.
-        See http://www.atacolorado.com/eulersequences.doc
-        """
-        T21 = self.a.y
-        T22 = self.b.y
-        T23 = self.c.y
-        T13 = self.c.x
-        T33 = self.c.z
-        yaw = atan2(-T21, T22)
-        roll = asin(T23)
-        pitch = atan2(-T13, T33)
-        return (roll, pitch, yaw)
-
-    def from_euler312(self, roll, pitch, yaw):
-        """Fill the matrix from Euler angles in radians in 312 convention."""
-        c3 = cos(pitch)
-        s3 = sin(pitch)
-        s2 = sin(roll)
-        c2 = cos(roll)
-        s1 = sin(yaw)
-        c1 = cos(yaw)
-
-        self.a.x = c1 * c3 - s1 * s2 * s3
-        self.b.y = c1 * c2
-        self.c.z = c3 * c2
-        self.a.y = -c2 * s1
-        self.a.z = s3 * c1 + c3 * s2 * s1
-        self.b.x = c3 * s1 + s3 * s2 * c1
-        self.b.z = s1 * s3 - s2 * c1 * c3
-        self.c.x = -s3 * c2
-        self.c.y = s2
-
-    def __add__(self, m):
-        return Matrix3(self.a + m.a, self.b + m.b, self.c + m.c)
-
-    __radd__ = __add__
-
-    def __sub__(self, m):
-        return Matrix3(self.a - m.a, self.b - m.b, self.c - m.c)
-
-    def __rsub__(self, m):
-        return Matrix3(m.a - self.a, m.b - self.b, m.c - self.c)
-
-    def __mul__(self, other):
-        if isinstance(other, Vector3):
-            v = other
-            return Vector3(self.a.x * v.x + self.a.y * v.y + self.a.z * v.z,
-                           self.b.x * v.x + self.b.y * v.y + self.b.z * v.z,
-                           self.c.x * v.x + self.c.y * v.y + self.c.z * v.z)
-        elif isinstance(other, Matrix3):
-            m = other
-            return Matrix3(Vector3(self.a.x * m.a.x + self.a.y * m.b.x + self.a.z * m.c.x,
-                                   self.a.x * m.a.y + self.a.y * m.b.y + self.a.z * m.c.y,
-                                   self.a.x * m.a.z + self.a.y * m.b.z + self.a.z * m.c.z),
-                           Vector3(self.b.x * m.a.x + self.b.y * m.b.x + self.b.z * m.c.x,
-                                   self.b.x * m.a.y + self.b.y * m.b.y + self.b.z * m.c.y,
-                                   self.b.x * m.a.z + self.b.y * m.b.z + self.b.z * m.c.z),
-                           Vector3(self.c.x * m.a.x + self.c.y * m.b.x + self.c.z * m.c.x,
-                                   self.c.x * m.a.y + self.c.y * m.b.y + self.c.z * m.c.y,
-                                   self.c.x * m.a.z + self.c.y * m.b.z + self.c.z * m.c.z))
-        v = other
-        return Matrix3(self.a * v, self.b * v, self.c * v)
-
-    def __div__(self, v):
-        return Matrix3(self.a / v, self.b / v, self.c / v)
-
-    def __neg__(self):
-        return Matrix3(-self.a, -self.b, -self.c)
-
-    def __copy__(self):
-        return Matrix3(self.a, self.b, self.c)
-
-    copy = __copy__
-
-    def rotate(self, g):
-        """Rotate the matrix by a given amount on 3 axes."""
-        temp_matrix = Matrix3()
-        a = self.a
-        b = self.b
-        c = self.c
-        temp_matrix.a.x = a.y * g.z - a.z * g.y
-        temp_matrix.a.y = a.z * g.x - a.x * g.z
-        temp_matrix.a.z = a.x * g.y - a.y * g.x
-        temp_matrix.b.x = b.y * g.z - b.z * g.y
-        temp_matrix.b.y = b.z * g.x - b.x * g.z
-        temp_matrix.b.z = b.x * g.y - b.y * g.x
-        temp_matrix.c.x = c.y * g.z - c.z * g.y
-        temp_matrix.c.y = c.z * g.x - c.x * g.z
-        temp_matrix.c.z = c.x * g.y - c.y * g.x
-        self.a += temp_matrix.a
-        self.b += temp_matrix.b
-        self.c += temp_matrix.c
-
-    def normalize(self):
-        """Re-normalise a rotation matrix."""
-        error = self.a * self.b
-        t0 = self.a - (self.b * (0.5 * error))
-        t1 = self.b - (self.a * (0.5 * error))
-        t2 = t0 % t1
-        self.a = t0 * (1.0 / t0.length())
-        self.b = t1 * (1.0 / t1.length())
-        self.c = t2 * (1.0 / t2.length())
-
-    def trace(self):
-        """The trace of the matrix."""
-        return self.a.x + self.b.y + self.c.z
-
-
-def test_euler():
-    """Check that from_euler() and to_euler() are consistent."""
-    m = Matrix3()
-    from math import radians, degrees
-    for r in range(-179, 179, 3):
-        for p in range(-89, 89, 3):
-            for y in range(-179, 179, 3):
-                m.from_euler(radians(r), radians(p), radians(y))
-                (r2, p2, y2) = m.to_euler()
-                v1 = Vector3(r, p, y)
-                v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
-                diff = v1 - v2
-                if diff.length() > 1.0e-12:
-                    print('EULER ERROR:', v1, v2, diff.length())
-
-
-def test_euler312_single(r, p, y):
-    """Check that from_euler312() and to_euler312() are consistent for one set of values."""
-    from math import degrees, radians
-    m = Matrix3()
-    m.from_euler312(radians(r), radians(p), radians(y))
-    (r2, p2, y2) = m.to_euler312()
-    v1 = Vector3(r, p, y)
-    v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
-    diff = v1 - v2
-    if diff.length() > 1.0e-12:
-        print('EULER ERROR:', v1, v2, diff.length())
-
-
-def test_one_axis(r, p, y):
-    """Check that from_euler312() and from_euler() are consistent for one set of values on one axis."""
-    from math import degrees, radians
-    m = Matrix3()
-    m.from_euler312(radians(r), radians(p), radians(y))
-    (r2, p2, y2) = m.to_euler()
-    v1 = Vector3(r, p, y)
-    v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
-    diff = v1 - v2
-    if diff.length() > 1.0e-12:
-        print('EULER ERROR:', v1, v2, diff.length())
-
-
-def test_euler312():
-    """Check that from_euler312() and to_euler312() are consistent."""
-    m = Matrix3()
-
-    for x in range(-89, 89, 3):
-        test_one_axis(x, 0, 0)
-        test_one_axis(0, x, 0)
-        test_one_axis(0, 0, x)
-    for r in range(-89, 89, 3):
-        for p in range(-179, 179, 3):
-            for y in range(-179, 179, 3):
-                test_euler312_single(r, p, y)
-
-
-if __name__ == "__main__":
-    import doctest
-
-    doctest.testmod()
-    test_euler()
-    test_euler312()

Tools/autotest/pysim/testwind.py

@@ -5,7 +5,7 @@ simple test of wind generation code
 from __future__ import print_function
 import time
 import util
-from rotmat import Vector3
+from pymavlink.rotmat import Vector3
 
 wind = util.Wind('7,90,0.1')
 

Tools/autotest/pysim/util.py

@@ -15,7 +15,7 @@ from math import acos, atan2, cos, pi, sqrt
 
 import pexpect
 
-from . rotmat import Matrix3, Vector3
+from pymavlink.rotmat import Vector3, Matrix3
 
 if (sys.version_info[0] >= 3):
     ENCODING = 'ascii'

Tools/scripts/magfit_flashlog.py

@@ -19,7 +19,7 @@ parser.add_option("--repeat", type='int', default=1, help="number of repeats thr
 
 (opts, args) = parser.parse_args()
 
-from rotmat import Vector3
+from pymavlink.rotmat import Vector3
 
 if len(args) < 1:
     print("Usage: magfit_flashlog.py [options] <LOGFILE...>")