Rotation of a known matrix into another known matrix

Question

I have a 6x3 Matrix M_start which provides coordinates in xyz. I also have an end Matrix 6x3 M_end, which provides the coordinates of the desired matrix. I now need to know which rotations around x y and z have to be applied to get from M_start to M_end. The Equation is [RzRyRx][M_start]=[M_end]

For this, I tried to use the 3x3 RotationMatrix RzRyRx, which Wikipedia provides and used it on M_start to get a set of 18 Equations. Since i have only 3 variables only 3 equations are needed. These Equations i tried to solve with SymPy's solve() and nonlinsolve(). Sadly, the solution takes forever in the case of solve, and nonlinsolve provides an output which i dont really understand.

"ConditionSet((a, b, c), Eq((2.02015sin(c) - 0.708642cos(c))cos(b) + 0.6677sin(b) - 1.0154, 0) & Eq(-0.1967*((sin(c) - 1.122cos(c))sin(b) - 0.6853cos(b))sin(a) - 0.1967(1.122sin(c) + cos(c))cos(a) - 0.0285, 0) & Eq(0.2201sin(a)sin(c) + 0.1967sin(a)cos(c) - 0.1967sin(b)*sin(c)cos(a) + 0.2207sin(b)*cos(a)cos(c) + 0.1348cos(a)*cos(b) - 0.323, 0), ProductSet(Complexes, Complexes, Complexes"

Here is my code:

from sympy import symbols, cos, sin, Eq, solve, nonlinsolve

a, b, c = symbols('a b c')
# Gleichungen für die Werte
eq1 = Eq(0.2201*sin(a)*sin(c) + 0.1967*sin(a)*cos(c) - 0.1967*sin(b)*sin(c)*cos(a) + 0.2207*sin(b)*cos(a)*cos(c) + 0.1348*cos(a)*cos(b), 0.323)
eq2 = Eq(-0.1967*(cos(a)*(cos(c) + 1.122*sin(c)) + sin(a)*(-0.6853*cos(b) + sin(b)*(-1.122*cos(c) + sin(c)))), 0.0285)
eq3 = Eq(0.6677*sin(b) + cos(b)*(-0.708642*cos(c) + 2.02015*sin(c)), 1.0154)


solution = nonlinsolve((eq1,eq2,eq3), (a, b, c))

print(solution)

Do you have any other ideas how to solve for the angles a b c or what I am making wrong with this method?

Answer 1

Here is a more generic solution to your problem.

First, let's rename your matrices m1 and m2. You can see each of these matrices as a could of 3D points. You want to find a rotation R which maximises correlation between these two sets (in your specific case, you assume this correlation can be perfect because you know m1 is a rotation of m2). This problem is called the Procustes problem, and can be solved using the Kabsch algorithm.

You can simply compute the desired matrix using a solver for this problem in scipy:

import numpy as np
import scipy

m1 = np.array(
    [
        [0.134819278, -0.196742231, 0.220766771],
        [-0.874283722, -0.544579231, 1.553994771],
        [-0.587264722, 0.982924769, -1.159154229],
        [-0.667699722, 2.020153769, -0.708642229],
        [-1.598599722, 0.497746769, -1.396776229],
        [0.117048278, 0.938185769, -2.045812229],
    ]
)

m2 = np.array(
    [
        [0.323738425, 0.028548749, 0.000334069],
        [1.15558905, -1.46302634, 0.002484488],
        [-1.62881025, -0.040371652, 0.002605095],
        [-1.95174685, -0.434232317, 1.01545285],
        [-1.85352078, -0.75189964, -0.86796569],
        [-2.0051881, 1.00801092, -0.205852296],
    ]
)

# Compute rotation
r, _ = scipy.spatial.transform.Rotation.align_vectors(m1.T, m2.T)

# Check results
print(r.as_matrix())
assert np.allclose(m1 @ r.as_matrix(), m2)

For more details on how this works, you can read this subsection of a blog post on the Procrustes problem. It explains that you can solve the Procrustes problem by using the Singular Value Decomposition of the correlation matrix of m1 and m2 and gives an analytical proof for that.