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:
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?
