ENH: Create Rotations by aligning a primary axis and best-fitting a secondary axis

[scottshambaugh]

Is your feature request related to a problem? Please describe.

In robotics applications, I often have the need to align a primary and secondary base direction with a primary and secondary reference direction, such that the primary base points directly along the primary reference, and the secondary base is pointed as close as possible to the secondary reference direction by rotating around that primary axis. For example, I might want a camera mounted as eyes on a head to point at an object, while I rotate the head about that viewing axis to point the side-mounted antenna towards a receiver.

I've found that this is known as an "align-constrain" formulation in spacecraft attitude dynamics, and a variation on the below algorithm is presented in Magner, "Extending Target Tracking Capabilities through Trajectory and Momentum Setpoint Optimization", 2018. This is also equivalent to using the Gram-Schmidt process to orthogonalize the pairs of vectors and finding the rotation matrix between them.

Describe the solution you'd like.

I would want a classmethod that does this. The user interface would be the same as align_vectors, but would require pairs of primary and secondary axes rather than an arbitrary N. Rotation.align_constrain() might be a good name, but I'm open to suggestions.

Algorithm:

1. Create orthonormal frames from the base and reference vectors. E.g for primary and secondary unit vectors ax_1 and ax_2:

# Do the below for R_ref and R_base
ax_3 = np.cross(ax_1, ax_2)
ax_2 = np.cross(ax_3, ax_1)
R_ax = Rotation.from_matrix([ax_1, ax_2, ax_3])

2. Compute the rotation between them, eg: R = R_ref * R_base.inv()

Example of behavior:

# Align the +X base with the +Y reference, and rotate about it such that the
# +Y base (residual of the [1, 1, 0] secondary base vector) is aligned with
# the +Z reference (residual of the [0, 1, 1] secondary reference vector)
base = [[1, 0, 0], [1, 1, 0]]
ref  = [[0, 1, 0], [0, 1, 1]]
R.align_constrain(base, ref)
print(R.as_matrix())
# array([[0, 0, 1]
#        [1, 0, 0]
#        [0, 1, 0])

Note that this will be undefined for primary and secondary axes that are parallel, and likely requires a parallelism check to some tolerance. We should think about behavior in that instance - we could default to arbitrary secondary axes (say [1, 0, 0] followed by [0, 1, 0] if the former is also parallel), or require the user to give backup directions. (How does align_vectors handle this right now?)

Describe alternatives you've considered.

This is essentially equivalent to the align_vectors classmethod with two pairs of vectors, where the first vector is infinitely preferred over the second. However, there is no way to assign weights to those vectors for infinite primary preference while keeping the secondary weight nonzero. Weights of [1, epsilon] for an epsilon close to zero would get close to the desired answer, but is not exact and could be numerically unstable. The algorithm above is exact and should be faster.

Additional context (e.g. screenshots, GIFs)

No response