Complete tests

doc/index.rst

@@ -1,5 +1,36 @@
-baldor Package
-==============
+Homogeneous Transformation Matrices and Quaternions
 
-.. toctree::
-  reference.rst
+Axis-Angle Representation
+=========================
+.. automodule:: baldor.axis_angle
+  :members:
+  :undoc-members:
+  :show-inheritance:
+
+Euler Angles
+============
+.. automodule:: baldor.euler
+  :members:
+  :undoc-members:
+  :show-inheritance:
+
+Quaternions
+===========
+.. automodule:: baldor.quaternion
+  :members:
+  :undoc-members:
+  :show-inheritance:
+
+Homogeneous Transformations
+===========================
+.. automodule:: baldor.transform
+  :members:
+  :undoc-members:
+  :show-inheritance:
+
+Vectors
+=======
+.. automodule:: baldor.vector
+  :members:
+  :undoc-members:
+  :show-inheritance:

src/baldor/quaternion.py

@@ -6,9 +6,11 @@
 """
 import math
 import numpy as np
+# Local modules
+import baldor as br
 
 
-def almost_equal(q1, q2, rtol=1e-5, atol=1e-8):
+def are_equal(q1, q2, rtol=1e-5, atol=1e-8):
   """
   Returns True if two quaternions are equal within a tolerance.
 
@@ -40,18 +42,17 @@ def almost_equal(q1, q2, rtol=1e-5, atol=1e-8):
   --------
   >>> import baldor as br
   >>> q1 = [1, 0, 0, 0]
-  >>> br.quaternion.almost_equal(q1, [0, 1, 0, 0])
+  >>> br.quaternion.are_equal(q1, [0, 1, 0, 0])
   False
-  >>> br.quaternion.almost_equal(q1, [1, 0, 0, 0])
+  >>> br.quaternion.are_equal(q1, [1, 0, 0, 0])
   True
-  >>> br.quaternion.almost_equal(q1, [-1, 0, 0, 0])
+  >>> br.quaternion.are_equal(q1, [-1, 0, 0, 0])
   True
   """
   if np.allclose(q1, q2, rtol, atol):
     return True
   return np.allclose(np.array(q1)*-1, q2, rtol, atol)
 
-
 def conjugate(q):
   """
   Compute the conjugate of a quaternion.
@@ -164,7 +165,7 @@ def norm(q):
 
 def random(rand=None):
   """
-  Generate a uniform random unit quaternion.
+  Generate an uniform random unit quaternion.
 
   Parameters
   ----------
@@ -200,13 +201,13 @@ def random(rand=None):
   t2 = pi2 * rand[2]
   return np.array([np.cos(t2)*r2, np.sin(t1)*r1, np.cos(t1)*r1, np.sin(t2)*r2])
 
-def to_axis_angle(q, identity_thresh=None):
+def to_axis_angle(quaternion, identity_thresh=None):
   """
   Return axis-angle rotation from a quaternion
 
   Parameters
   -------
-  q : array_like
+  quaternion: array_like
     Input quaternion (4 element sequence)
   identity_thresh : None or scalar, optional
     Threshold below which the norm of the vector part of the quaternion (x,
@@ -225,7 +226,7 @@ def to_axis_angle(q, identity_thresh=None):
   -----
   Quaternions :math:`w + ix + jy + kz` are represented as :math:`[w, x, y, z]`.
   A quaternion for which x, y, z are all equal to 0, is an identity rotation.
-  In this case we return a angle=0 and axis=[1, 0, 0]. This is an arbitrary
+  In this case we return a `angle=0` and `axis=[1, 0, 0]``. This is an arbitrary
   vector.
 
   Examples
@@ -238,16 +239,16 @@ def to_axis_angle(q, identity_thresh=None):
   >>> angle
   1.5
   """
-  w, x, y, z = quat
-  Nq = norm(quat)
+  w, x, y, z = quaternion
+  Nq = norm(quaternion)
   if not np.isfinite(Nq):
     return np.array([1.0, 0, 0]), float('nan')
   if identity_thresh is None:
     try:
       identity_thresh = np.finfo(Nq.type).eps * 3
     except (AttributeError, ValueError): # Not a numpy type or not float
-      identity_thresh = _FLOAT_EPS * 3
-  if Nq < _FLOAT_EPS ** 2:  # Results unreliable after normalization
+      identity_thresh = br._FLOAT_EPS * 3
+  if Nq < br._FLOAT_EPS ** 2:  # Results unreliable after normalization
     return np.array([1.0, 0, 0]), 0.0
   if Nq != 1:  # Normalize if not normalized
     s = math.sqrt(Nq)
@@ -259,3 +260,81 @@ def to_axis_angle(q, identity_thresh=None):
   # Make sure w is not slightly above 1 or below -1
   theta = 2 * math.acos(max(min(w, 1), -1))
   return  np.array([x, y, z]) / math.sqrt(len2), theta
+
+def to_euler(quaternion, axes='sxyz'):
+  """
+  Return Euler angles from a quaternion using the specified axis sequence.
+
+  Parameters
+  ----------
+  q : array_like
+    Input quaternion (4 element sequence)
+  axes: str, optional
+    Axis specification; one of 24 axis sequences as string or encoded tuple
+
+  Returns
+  -------
+  ai: float
+    First rotation angle (according to axes).
+  aj: float
+    Second rotation angle (according to axes).
+  ak: float
+    Third rotation angle (according to axes).
+
+  Notes
+  -----
+  Many Euler angle triplets can describe the same rotation matrix
+  Quaternions :math:`w + ix + jy + kz` are represented as :math:`[w, x, y, z]`.
+
+  Examples
+  --------
+  >>> import numpy as np
+  >>> import baldor as br
+  >>> ai, aj, ak = br.quaternion.to_euler([0.99810947, 0.06146124, 0, 0])
+  >>> np.allclose([ai, aj, ak], [0.123, 0, 0])
+  True
+  """
+  return br.transform.to_euler(to_transform(quaternion), axes)
+
+def to_transform(quaternion):
+  """
+  Return Euler angles from a quaternion using the specified axis sequence.
+
+  Parameters
+  ----------
+  quaternion: array_like
+    Input quaternion (4 element sequence)
+  axes: str, optional
+    Axis specification; one of 24 axis sequences as string or encoded tuple
+
+  Returns
+  -------
+  T: array_like
+    Homogeneous transformation (4x4)
+
+  Notes
+  -----
+  Quaternions :math:`w + ix + jy + kz` are represented as :math:`[w, x, y, z]`.
+
+  Examples
+  --------
+  >>> import numpy as np
+  >>> import baldor as br
+  >>> M = br.quaternion.to_transform([1, 0, 0, 0]) # Identity quaternion
+  >>> np.allclose(M, np.eye(3))
+  True
+  >>> M = br.quaternion.to_transform([0, 1, 0, 0]) # 180 degree rot around X
+  >>> np.allclose(M, np.diag([1, -1, -1]))
+  True
+  """
+  q = np.array(quaternion, dtype=np.float64, copy=True)
+  n = np.dot(q, q)
+  if n < br._EPS:
+    return np.identity(4)
+  q *= math.sqrt(2.0 / n)
+  q = np.outer(q, q)
+  return np.array([
+      [1.0-q[2,2]-q[3,3],     q[1,2]-q[3,0],      q[1,3]+q[2,0], 0.0],
+      [    q[1,2]+q[3,0], 1.0-q[1,1]-q[3,3],      q[2,3]-q[1,0], 0.0],
+      [    q[1,3]-q[2,0],     q[2,3]+q[1,0], 1.0-q[1,1]-q[2, 2], 0.0],
+      [              0.0,               0.0,                0.0, 1.0]])

src/baldor/transform.py

@@ -8,7 +8,7 @@
 import baldor as br
 
 
-def almost_equal(T1, T2, rtol=1e-5, atol=1e-8):
+def are_equal(T1, T2, rtol=1e-5, atol=1e-8):
   """
   Returns True if two homogeneous transformation are equal within a tolerance.
 
@@ -79,6 +79,19 @@ def to_axis_angle(transform):
     angle of rotation
   point: array_like
     point around which the rotation is performed
+
+  Examples
+  --------
+  >>> import numpy as np
+  >>> import baldor as br
+  >>> axis = np.random.sample(3) - 0.5
+  >>> angle = (np.random.sample(1) - 0.5) * (2*np.pi)
+  >>> point = np.random.sample(3) - 0.5
+  >>> T0 = br.axis_angle.to_transform(axis, angle, point)
+  >>> axis, angle, point = br.transform.to_axis_angle(T0)
+  >>> T1 = br.axis_angle.to_transform(axis, angle, point)
+  >>> br.transform.are_equal(T0, T1)
+  True
   """
   R = np.array(transform, dtype=np.float64, copy=False)
   R33 = R[:3,:3]
@@ -135,7 +148,7 @@ def to_euler(transform, axes='sxyz'):
   --------
   >>> import numpy as np
   >>> import baldor as br
-  >>> T0 = euler_matrix(1, 2, 3, 'syxz')
+  >>> T0 = br.euler.to_transform(1, 2, 3, 'syxz')
   >>> al, be, ga = br.transform.to_euler(T0, 'syxz')
   >>> T1 = br.euler.to_transform(al, be, ga, 'syxz')
   >>> np.allclose(T0, T1)
@@ -210,7 +223,7 @@ def to_quaternion(transform, isprecise=False):
   >>> q = br.transform.to_quaternion(np.diag([1, -1, -1, 1]))
   >>> np.allclose(q, [0, 1, 0, 0]) or np.allclose(q, [0, -1, 0, 0])
   True
-  >>> T = br.axis_angle.to_transform(0.123, (1, 2, 3))
+  >>> T = br.axis_angle.to_transform((1, 2, 3), 0.123)
   >>> q = br.transform.to_quaternion(T, True)
   >>> np.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786])
   True

src/baldor/vector.py

@@ -4,6 +4,8 @@
 """
 import math
 import numpy as np
+# Local modules
+import baldor as br
 
 
 def unit(vector):
@@ -18,7 +20,7 @@ def unit(vector):
   Returns
   -------
   unit : array_like
-     vector divided by L2 norm
+    Vector divided by L2 norm
 
   Examples
   --------
@@ -44,7 +46,7 @@ def norm(vector):
   Returns
   -------
   norm: float
-   The computed norm
+    The computed norm
 
   Examples
   --------
@@ -77,10 +79,30 @@ def perpendicular(vector):
       # unit is (0, 0, 0)
       raise ValueError('Input vector cannot be a zero vector')
     # unit is (0, 0, Z)
-    result = np.array(br.Y_AXIS, dtype=np.float64, copy=True)
-  result = np.array([-unit[1], unit[0], 0], dtype=np.float64)
+    return br.Y_AXIS
+  result = np.array([-u[1], u[0], 0], dtype=np.float64)
   return result
 
+def skew(vector):
+  """
+  Returns the 3x3 skew matrix of the input vector.
+
+  The skew matrix is a square matrix `R` whose transpose is also its negative;
+  that is, it satisfies the condition :math:`-R = R^T`.
+
+  Parameters
+  ----------
+  vector: array_like
+    The input array
+
+  Returns
+  -------
+  R: array_like
+    The resulting 3x3 skew matrix
+  """
+  skv = np.roll(np.roll(np.diag(np.asarray(vector).flatten()), 1, 1), -1, 0)
+  return (skv - skv.T)
+
 def transform_between_vectors(vector_a, vector_b):
   """
   Compute the transformation that aligns two vectors
@@ -97,13 +119,13 @@ def transform_between_vectors(vector_a, vector_b):
   transform: array_like
     The transformation between `vector_a` a `vector_b`
   """
-  ua = unit(vector_a)
-  ub = unit(vector_b)
-  c = np.dot(ua, ub)
+  newaxis = unit(vector_b)
+  oldaxis = unit(vector_a)
+  c = np.dot(oldaxis, newaxis)
   angle = np.arccos(c)
-  if np.isclose(c, -1.0) or np.allclose(ua, ub):
-    axis = perpendicular(ub)
+  if np.isclose(c, -1.0) or np.allclose(newaxis, oldaxis):
+    axis = perpendicular(newaxis)
   else:
-    axis = unit(np.cross(ua, ub))
+    axis = unit(np.cross(oldaxis, newaxis))
   transform = br.axis_angle.to_transform(axis, angle)
   return transform

tests/test_axis_angle.py

@@ -28,10 +28,10 @@ def test_to_transform(self):
     point = np.random.sample(3) - 0.5
     T0 = br.axis_angle.to_transform(axis, angle, point)
     T1 = br.axis_angle.to_transform(axis, angle-2*np.pi, point)
-    self.assertTrue(br.transform.almost_equal(T0, T1))
+    self.assertTrue(br.transform.are_equal(T0, T1))
     T0 = br.axis_angle.to_transform(axis, angle, point)
     T1 = br.axis_angle.to_transform(-axis, -angle, point)
-    self.assertTrue(br.transform.almost_equal(T0, T1))
+    self.assertTrue(br.transform.are_equal(T0, T1))
     T = br.axis_angle.to_transform(axis, np.pi*2)
     np.testing.assert_allclose(T, np.eye(4), rtol=1e-7, atol=1e-8)
     T = br.axis_angle.to_transform(axis, np.pi/2., point)