rotmat.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # vector3 and rotation matrix classes
00004 # This follows the conventions in the ArduPilot code,
00005 # and is essentially a python version of the AP_Math library
00006 #
00007 # Andrew Tridgell, March 2012
00008 #
00009 # This library is free software; you can redistribute it and/or modify it
00010 # under the terms of the GNU Lesser General Public License as published by the
00011 # Free Software Foundation; either version 2.1 of the License, or (at your
00012 # option) any later version.
00013 #
00014 # This library is distributed in the hope that it will be useful, but WITHOUT
00015 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00016 # FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License
00017 # for more details.
00018 #
00019 # You should have received a copy of the GNU Lesser General Public License
00020 # along with this library; if not, write to the Free Software Foundation,
00021 # Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
00022 
00023 '''rotation matrix class
00024 '''
00025 
00026 from math import sin, cos, sqrt, asin, atan2, pi, radians, acos, degrees
00027 
00028 class Vector3:
00029     '''a vector'''
00030     def __init__(self, x=None, y=None, z=None):
00031         if x != None and y != None and z != None:
00032             self.x = float(x)
00033             self.y = float(y)
00034             self.z = float(z)
00035         elif x != None and len(x) == 3:
00036             self.x = float(x[0])
00037             self.y = float(x[1])
00038             self.z = float(x[2])
00039         elif x != None:
00040             raise ValueError('bad initialiser')
00041         else:
00042             self.x = float(0)
00043             self.y = float(0)
00044             self.z = float(0)
00045 
00046     def __repr__(self):
00047         return 'Vector3(%.2f, %.2f, %.2f)' % (self.x,
00048                                               self.y,
00049                                               self.z)
00050 
00051     def __eq__(self, v):
00052         return self.x == v.x and self.y == v.y and self.z == v.z
00053 
00054     def __ne__(self, v):
00055         return not self == v
00056 
00057     def close(self, v, tol=1e-7):
00058         return abs(self.x - v.x) < tol and \
00059             abs(self.y - v.y) < tol and \
00060             abs(self.z - v.z) < tol
00061 
00062     def __add__(self, v):
00063         return Vector3(self.x + v.x,
00064                        self.y + v.y,
00065                        self.z + v.z)
00066 
00067     __radd__ = __add__
00068 
00069     def __sub__(self, v):
00070         return Vector3(self.x - v.x,
00071                        self.y - v.y,
00072                        self.z - v.z)
00073 
00074     def __neg__(self):
00075         return Vector3(-self.x, -self.y, -self.z)
00076 
00077     def __rsub__(self, v):
00078         return Vector3(v.x - self.x,
00079                        v.y - self.y,
00080                        v.z - self.z)
00081 
00082     def __mul__(self, v):
00083         if isinstance(v, Vector3):
00084             '''dot product'''
00085             return self.x*v.x + self.y*v.y + self.z*v.z
00086         return Vector3(self.x * v,
00087                        self.y * v,
00088                        self.z * v)
00089 
00090     __rmul__ = __mul__
00091 
00092     def __div__(self, v):
00093         return Vector3(self.x / v,
00094                        self.y / v,
00095                        self.z / v)
00096 
00097     def __mod__(self, v):
00098         '''cross product'''
00099         return Vector3(self.y*v.z - self.z*v.y,
00100                        self.z*v.x - self.x*v.z,
00101                        self.x*v.y - self.y*v.x)
00102 
00103     def __copy__(self):
00104         return Vector3(self.x, self.y, self.z)
00105 
00106     copy = __copy__
00107 
00108     def length(self):
00109         return sqrt(self.x**2 + self.y**2 + self.z**2)
00110 
00111     def zero(self):
00112         self.x = self.y = self.z = 0
00113 
00114     def angle(self, v):
00115         '''return the angle between this vector and another vector'''
00116         return acos((self * v) / (self.length() * v.length()))
00117 
00118     def normalized(self):
00119         return self.__div__(self.length())
00120 
00121     def normalize(self):
00122         v = self.normalized()
00123         self.x = v.x
00124         self.y = v.y
00125         self.z = v.z
00126 
00127 class Matrix3:
00128     '''a 3x3 matrix, intended as a rotation matrix'''
00129     def __init__(self, a=None, b=None, c=None):
00130         if a is not None and b is not None and c is not None:
00131             self.a = a.copy()
00132             self.b = b.copy()
00133             self.c = c.copy()
00134         else:
00135             self.identity()
00136 
00137     def __repr__(self):
00138         return 'Matrix3((%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f))' % (
00139             self.a.x, self.a.y, self.a.z,
00140             self.b.x, self.b.y, self.b.z,
00141             self.c.x, self.c.y, self.c.z)
00142 
00143     def identity(self):
00144         self.a = Vector3(1,0,0)
00145         self.b = Vector3(0,1,0)
00146         self.c = Vector3(0,0,1)
00147 
00148     def transposed(self):
00149         return Matrix3(Vector3(self.a.x, self.b.x, self.c.x),
00150                        Vector3(self.a.y, self.b.y, self.c.y),
00151                        Vector3(self.a.z, self.b.z, self.c.z))
00152 
00153 
00154     def from_euler(self, roll, pitch, yaw):
00155         '''fill the matrix from Euler angles in radians'''
00156         cp = cos(pitch)
00157         sp = sin(pitch)
00158         sr = sin(roll)
00159         cr = cos(roll)
00160         sy = sin(yaw)
00161         cy = cos(yaw)
00162 
00163         self.a.x = cp * cy
00164         self.a.y = (sr * sp * cy) - (cr * sy)
00165         self.a.z = (cr * sp * cy) + (sr * sy)
00166         self.b.x = cp * sy
00167         self.b.y = (sr * sp * sy) + (cr * cy)
00168         self.b.z = (cr * sp * sy) - (sr * cy)
00169         self.c.x = -sp
00170         self.c.y = sr * cp
00171         self.c.z = cr * cp
00172 
00173 
00174     def to_euler(self):
00175         '''find Euler angles (321 convention) for the matrix'''
00176         if self.c.x >= 1.0:
00177             pitch = pi
00178         elif self.c.x <= -1.0:
00179             pitch = -pi
00180         else:
00181             pitch = -asin(self.c.x)
00182         roll = atan2(self.c.y, self.c.z)
00183         yaw  = atan2(self.b.x, self.a.x)
00184         return (roll, pitch, yaw)
00185 
00186 
00187     def to_euler312(self):
00188         '''find Euler angles (312 convention) for the matrix.
00189         See http://www.atacolorado.com/eulersequences.doc
00190         '''
00191         T21 = self.a.y
00192         T22 = self.b.y
00193         T23 = self.c.y
00194         T13 = self.c.x
00195         T33 = self.c.z
00196         yaw = atan2(-T21, T22)
00197         roll = asin(T23)
00198         pitch = atan2(-T13, T33)
00199         return (roll, pitch, yaw)
00200 
00201     def from_euler312(self, roll, pitch, yaw):
00202         '''fill the matrix from Euler angles in radians in 312 convention'''
00203         c3 = cos(pitch)
00204         s3 = sin(pitch)
00205         s2 = sin(roll)
00206         c2 = cos(roll)
00207         s1 = sin(yaw)
00208         c1 = cos(yaw)
00209 
00210         self.a.x = c1 * c3 - s1 * s2 * s3
00211         self.b.y = c1 * c2
00212         self.c.z = c3 * c2
00213         self.a.y = -c2*s1
00214         self.a.z = s3*c1 + c3*s2*s1
00215         self.b.x = c3*s1 + s3*s2*c1
00216         self.b.z = s1*s3 - s2*c1*c3
00217         self.c.x = -s3*c2
00218         self.c.y = s2
00219 
00220     def __add__(self, m):
00221         return Matrix3(self.a + m.a, self.b + m.b, self.c + m.c)
00222 
00223     __radd__ = __add__
00224 
00225     def __sub__(self, m):
00226         return Matrix3(self.a - m.a, self.b - m.b, self.c - m.c)
00227 
00228     def __rsub__(self, m):
00229         return Matrix3(m.a - self.a, m.b - self.b, m.c - self.c)
00230 
00231     def __mul__(self, other):
00232         if isinstance(other, Vector3):
00233             v = other
00234             return Vector3(self.a.x * v.x + self.a.y * v.y + self.a.z * v.z,
00235                            self.b.x * v.x + self.b.y * v.y + self.b.z * v.z,
00236                            self.c.x * v.x + self.c.y * v.y + self.c.z * v.z)
00237         elif isinstance(other, Matrix3):
00238             m = other
00239             return Matrix3(Vector3(self.a.x * m.a.x + self.a.y * m.b.x + self.a.z * m.c.x,
00240                                    self.a.x * m.a.y + self.a.y * m.b.y + self.a.z * m.c.y,
00241                                    self.a.x * m.a.z + self.a.y * m.b.z + self.a.z * m.c.z),
00242                            Vector3(self.b.x * m.a.x + self.b.y * m.b.x + self.b.z * m.c.x,
00243                                    self.b.x * m.a.y + self.b.y * m.b.y + self.b.z * m.c.y,
00244                                    self.b.x * m.a.z + self.b.y * m.b.z + self.b.z * m.c.z),
00245                            Vector3(self.c.x * m.a.x + self.c.y * m.b.x + self.c.z * m.c.x,
00246                                    self.c.x * m.a.y + self.c.y * m.b.y + self.c.z * m.c.y,
00247                                    self.c.x * m.a.z + self.c.y * m.b.z + self.c.z * m.c.z))
00248         v = other
00249         return Matrix3(self.a * v, self.b * v, self.c * v)
00250 
00251     def __div__(self, v):
00252         return Matrix3(self.a / v, self.b / v, self.c / v)
00253 
00254     def __neg__(self):
00255         return Matrix3(-self.a, -self.b, -self.c)
00256 
00257     def __copy__(self):
00258         return Matrix3(self.a, self.b, self.c)
00259 
00260     copy = __copy__
00261 
00262     def rotate(self, g):
00263         '''rotate the matrix by a given amount on 3 axes'''
00264         temp_matrix = Matrix3()
00265         a = self.a
00266         b = self.b
00267         c = self.c
00268         temp_matrix.a.x = a.y * g.z - a.z * g.y
00269         temp_matrix.a.y = a.z * g.x - a.x * g.z
00270         temp_matrix.a.z = a.x * g.y - a.y * g.x
00271         temp_matrix.b.x = b.y * g.z - b.z * g.y
00272         temp_matrix.b.y = b.z * g.x - b.x * g.z
00273         temp_matrix.b.z = b.x * g.y - b.y * g.x
00274         temp_matrix.c.x = c.y * g.z - c.z * g.y
00275         temp_matrix.c.y = c.z * g.x - c.x * g.z
00276         temp_matrix.c.z = c.x * g.y - c.y * g.x
00277         self.a += temp_matrix.a
00278         self.b += temp_matrix.b
00279         self.c += temp_matrix.c
00280 
00281     def normalize(self):
00282         '''re-normalise a rotation matrix'''
00283         error = self.a * self.b
00284         t0 = self.a - (self.b * (0.5 * error))
00285         t1 = self.b - (self.a * (0.5 * error))
00286         t2 = t0 % t1
00287         self.a = t0 * (1.0 / t0.length())
00288         self.b = t1 * (1.0 / t1.length())
00289         self.c = t2 * (1.0 / t2.length())
00290 
00291     def trace(self):
00292         '''the trace of the matrix'''
00293         return self.a.x + self.b.y + self.c.z
00294 
00295     def from_axis_angle(self, axis, angle):
00296         '''create a rotation matrix from axis and angle'''
00297         ux = axis.x
00298         uy = axis.y
00299         uz = axis.z
00300         ct = cos(angle)
00301         st = sin(angle)
00302         self.a.x = ct + (1-ct) * ux**2
00303         self.a.y = ux*uy*(1-ct) - uz*st
00304         self.a.z = ux*uz*(1-ct) + uy*st
00305         self.b.x = uy*ux*(1-ct) + uz*st
00306         self.b.y = ct + (1-ct) * uy**2
00307         self.b.z = uy*uz*(1-ct) - ux*st
00308         self.c.x = uz*ux*(1-ct) - uy*st
00309         self.c.y = uz*uy*(1-ct) + ux*st
00310         self.c.z = ct + (1-ct) * uz**2
00311 
00312 
00313     def from_two_vectors(self, vec1, vec2):
00314         '''get a rotation matrix from two vectors.
00315            This returns a rotation matrix which when applied to vec1
00316            will produce a vector pointing in the same direction as vec2'''
00317         angle = vec1.angle(vec2)
00318         cross = vec1 % vec2
00319         if cross.length() == 0:
00320             # the two vectors are colinear
00321             return self.from_euler(0,0,angle)
00322         cross.normalize()
00323         return self.from_axis_angle(cross, angle)
00324 
00325     def close(self, m, tol=1e-7):
00326         return self.a.close(m.a) and self.b.close(m.b) and self.c.close(m.c)
00327 
00328 class Plane:
00329     '''a plane in 3 space, defined by a point and a vector normal'''
00330     def __init__(self, point=None, normal=None):
00331         if point is None:
00332             point = Vector3(0,0,0)
00333         if normal is None:
00334             normal = Vector3(0, 0, 1)
00335         self.point = point
00336         self.normal = normal
00337 
00338 class Line:
00339     '''a line in 3 space, defined by a point and a vector'''
00340     def __init__(self, point=None, vector=None):
00341         if point is None:
00342             point = Vector3(0,0,0)
00343         if vector is None:
00344             vector = Vector3(0, 0, 1)
00345         self.point = point
00346         self.vector = vector
00347 
00348     def plane_intersection(self, plane, forward_only=False):
00349         '''return point where line intersects with a plane'''
00350         l_dot_n = self.vector * plane.normal
00351         if l_dot_n == 0.0:
00352             # line is parallel to the plane
00353             return None
00354         d = ((plane.point - self.point) * plane.normal) / l_dot_n
00355         if forward_only and d < 0:
00356             return None
00357         return (self.vector * d) + self.point
00358 
00359 
00360 
00361 def test_euler():
00362     '''check that from_euler() and to_euler() are consistent'''
00363     m = Matrix3()
00364     from math import radians, degrees
00365     for r in range(-179, 179, 3):
00366         for p in range(-89, 89, 3):
00367             for y in range(-179, 179, 3):
00368                 m.from_euler(radians(r), radians(p), radians(y))
00369                 (r2, p2, y2) = m.to_euler()
00370                 v1 = Vector3(r,p,y)
00371                 v2 = Vector3(degrees(r2),degrees(p2),degrees(y2))
00372                 diff = v1 - v2
00373                 if diff.length() > 1.0e-12:
00374                     print('EULER ERROR:', v1, v2, diff.length())
00375 
00376 
00377 def test_two_vectors():
00378     '''test the from_two_vectors() method'''
00379     import random
00380     for i in range(1000):
00381         v1 = Vector3(1, 0.2, -3)
00382         v2 = Vector3(random.uniform(-5,5), random.uniform(-5,5), random.uniform(-5,5))
00383         m = Matrix3()
00384         m.from_two_vectors(v1, v2)
00385         v3 = m * v1
00386         diff = v3.normalized() - v2.normalized()
00387         (r, p, y) = m.to_euler()
00388         if diff.length() > 0.001:
00389             print('err=%f' % diff.length())
00390             print("r/p/y = %.1f %.1f %.1f" % (
00391                 degrees(r), degrees(p), degrees(y)))
00392             print(v1.normalized(), v2.normalized(), v3.normalized())
00393 
00394 def test_plane():
00395     '''testing line/plane intersection'''
00396     print("testing plane/line maths")
00397     plane = Plane(Vector3(0,0,0), Vector3(0,0,1))
00398     line = Line(Vector3(0,0,100), Vector3(10, 10, -90))
00399     p = line.plane_intersection(plane)
00400     print(p)
00401 
00402 if __name__ == "__main__":
00403     import doctest
00404     doctest.testmod()
00405     test_euler()
00406     test_two_vectors()


mavlink
Author(s): Lorenz Meier
autogenerated on Thu Jun 6 2019 19:01:57