Rotate a frame by the given theta about an axis of another frame

36 Views Asked by At

I have two frames ($F_1$ and $F_2$) both are represented wrt frame $O$ (origin frame). Therefore, $T_1$ and $T_2$ are known. Also, I know the frame $F_2$ relative to $F_1$, i.e., $T_{rel}$ is known. Please see a representative figure below:

enter image description here

My goal is to rotate frame $F_2$ by 30 degrees about z-axis of frame $F_1$. However, the result seems incorrect. Please see below a demonstration:

# Define T1 and T2
T_1 = np.array([[1,      0,       0, 0.3000],
                [0, 0.9397, -0.3420,      0],
                [0, 0.3420,  0.9397,      0],
                [0,      0,       0,      1]])

T_2 = np.array([[      0,      0, 1,  0.3000],
                [ 0.6434, 0.7655, 0, -0.0572],
                [-0.7655, 0.6434, 0, -0.0474],
                [      0,      0, 0,       1]])

# Calculate the relative transformation matrix
T_rel = np.linalg.inv(T_2).dot(T_1)

# Convert 30 degrees to radians
theta = np.deg2rad(30)

# Define our rotation matrix
T_rotation = np.array([[np.cos(theta), -np.sin(theta), 0, 0],
                       [np.sin(theta),  np.cos(theta), 0, 0],
                       [            0,              0, 1, 0],
                       [            0,              0, 0, 1]])

# Apply the rotation
T_new = T_rotation.dot(T_rel)

# Print the result
T_new
[[ 0, -0.1729, -0.9849, -0.0367]
 [-0,  0.9849, -0.1729,  0.0646]
 [ 1,       0,       0,       0]
 [ 0,       0,       0,       1]]

The result $T_{new}$ is incorrect because the $x$ cooridnate is negative. Upon visulazation, the frame seems rotated about origin $O$, which is incorrect.

Update

I used quaternion instead of rotation matrix as shown below:

# Define T1 and Trel
T_1 = [0.3000, 0, 0, 0.1736, 0, 0, 0.9848]
T_rel = [0, -0.0700, -0.0250, 0.1230, 0.6960, 0.1230, 0.6960]

# Define our rotated quaternion
Q_z = quaternion_about_axis(np.deg2rad(30), z_axis)

# Apply the rotation on quaternion
T_rel[3:] = T_rel[3:] * Q_z

T_new = T_1.dot(T_rel)

# Print the result
T_new
[0.3000, -0.0572, -0.0474, 0.4058, 0.5791, 0.4058, 0.5791]

In this case, the $x$ coordinate is postive. Upon visualization, I noticed that the frame $F_2$ has rotated but position remain unchanged. In other words, the frame $F_2$ has rotated about its own $z$ axis. I wanted to rotate is about the $z$ axis of $F_1$ though.

Question

How to rotate a frame by the given theta about an axis of another frame?