I have a 4x4 transoformation matrix T0 as a starting pose.
Now I want to rotate T0 with an 3x3 rotation matrix R around a center point to get a new pose T1.
import numpy as np
T0 = np.eye(4)
R = np.array([[0.98480775, 0., 0.17364818],
[0., 1., 0.],
[-0.17364818, 0., 0.98480775]])
center = np.array([-2.00628613e-02, -1.26855529e+00, -3.45331795e+01])
# T1 = ?
How to calculate T1?
The solution is described here. And the working code is below:
import numpy as np
T0 = np.eye(4)
R = np.array([[0.98480775, 0., 0.17364818],
[0., 1., 0.],
[-0.17364818, 0., 0.98480775]])
center = np.array([-2.00628613e-02, -1.26855529e+00, -3.45331795e+01])
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = center - np.matmul(R, center)
T1 = np.matmul(T, T0)