Add function to estimate the transformation between two axes

src/baldor/transform.py

@@ -38,6 +38,33 @@ def are_equal(T1, T2, rtol=1e-5, atol=1e-8):
   M2 /= M2[3,3]
   return np.allclose(M1, M2, rtol, atol)
 
+def between_axes(axis_a, axis_b):
+  """
+  Compute the transformation that aligns two vectors/axes.
+
+  Parameters
+  ----------
+  axis_a: array_like
+    The initial axis
+  axis_b: array_like
+    The goal axis
+
+  Returns
+  -------
+  transform: array_like
+    The transformation that transforms `axis_a` into `axis_b`
+  """
+  a_unit = br.vector.unit(axis_a)
+  b_unit = br.vector.unit(axis_b)
+  c = np.dot(a_unit, b_unit)
+  angle = np.arccos(c)
+  if np.isclose(c, -1.0) or np.allclose(a_unit, b_unit):
+    axis = br.vector.perpendicular(b_unit)
+  else:
+    axis = br.vector.unit(np.cross(a_unit, b_unit))
+  transform = br.axis_angle.to_transform(axis, angle)
+  return transform
+
 def inverse(transform):
   """
   Compute the inverse of an homogeneous transformation.

src/baldor/vector.py

@@ -73,13 +73,12 @@ def perpendicular(vector):
   result: array_like
     The perpendicular vector
   """
-  if np.allclose(vector[:2], np.zeros(2)):
-    if np.isclose(vector[2], 0.):
-      # unit is (0, 0, 0)
-      raise ValueError('Input vector cannot be a zero vector')
-    # unit is (0, 0, Z)
-    return br.Y_AXIS
+  if np.allclose(vector, np.zeros(3)):
+    # vector is [0, 0, 0]
+    raise ValueError('Input vector cannot be a zero vector')
   u = unit(vector)
+  if np.allclose(u[:2], np.zeros(2)):
+    return br.Y_AXIS
   result = np.array([-u[1], u[0], 0], dtype=np.float64)
   return result
 

tests/test_transform.py

@@ -14,6 +14,27 @@ def test_are_equal(self):
     T3 = br.quaternion.to_transform([-1, 0, 0, 0])
     self.assertTrue(br.quaternion.are_equal(T2, T3))
 
+  def test_between_axes(self):
+    # Random axis / angle
+    np.random.seed(123)
+    axis = br.vector.unit(np.random.randn(3))
+    angle = np.deg2rad(45)
+    transform = br.axis_angle.to_transform(axis, angle)
+    newaxis = np.dot(transform[:3,:3], br.Z_AXIS)
+    est_transform = br.transform.between_axes(br.Z_AXIS, newaxis)
+    np.testing.assert_allclose(transform[:3,2], est_transform[:3,2])
+    # Edge case 1
+    newaxis = -br.Z_AXIS
+    transform = br.transform.between_axes(br.Z_AXIS, newaxis)
+    _, angle, _ = br.transform.to_axis_angle(transform)
+    np.testing.assert_allclose(angle, np.pi)
+    # Edge case 2
+    newaxis = br.Z_AXIS
+    transform = br.transform.between_axes(-br.Z_AXIS, newaxis)
+    _, angle, _ = br.transform.to_axis_angle(transform)
+    np.testing.assert_allclose(angle, np.pi)
+
+
   def test_inverse(self):
     q = br.quaternion.random()
     T = br.quaternion.to_transform(q)