Dans cette partie, nous allons reprendre ce qui a été vu sur les rotations et transformations 3D et l’implémenter en Python.
Le code sera la suite de la partie 2D
Transformations élémentaires
On peut définir une translation par un vecteur étant comme étant:
def translation(x, y, z):
return np.array([[1, 0, 0, x],
[0, 1, 0, y],
[0, 0, 1, z],
[0, 0, 0, 1]])
Et les rotations autour des trois axes:
def Rx(alpha):
return np.array([[1, 0, 0, 0],
[0, np.cos(alpha), -np.sin(alpha), 0],
[0, np.sin(alpha), np.cos(alpha), 0],
[0, 0, 0, 1]])
def Ry(alpha):
return np.array([[np.cos(alpha), 0, np.sin(alpha), 0],
[0, 1, 0, 0],
[-np.sin(alpha), 0, np.cos(alpha), 0],
[0, 0, 0, 1]])
def Rz(alpha):
return np.array([[np.cos(alpha), -np.sin(alpha), 0, 0],
[np.sin(alpha), np.cos(alpha), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
Inverse
De manière très similaire à la version 2D, on peut inverser une matrice de transformation:
def frame_inv(T):
R = T[:3, :3] # On extrait la rotation
t = T[:3, 3:] # On extrait la translation
upper = np.hstack((R.T, -R.T @ t))
lower = np.array([0., 0., 0., 1.])
return np.vstack((upper, lower))
Rhoban
@RhobanRobots