I have a 4×4 transoformation matrix T0
as a starting pose.
Now I want to rotate T0
with an 3×3 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
?
Advertisement
Answer
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)