Chapitre 02
Transformations rigides & orientation

1. La matrice de transformation homogène 4×4

Une transformation rigide combine une rotation et une translation entre deux repères. En coordonnées homogènes, un point p = (x, y, z) devient (x, y, z, 1)^T, et la transformation prend la forme compacte d'une matrice 4×4 :

Le bloc R ∈ SO(3) (rotation 3×3, orthogonale, R^TR = I, det R = +1) porte l'orientation ; le vecteur t ∈ ℝ³ porte la position. La dernière ligne (0 0 0 1) est constante : elle rend le produit associatif et l'inverse propre. Transformer un point s'écrit alors comme un simple produit matrice-vecteur :

La partie rotation tourne le vecteur ; la partie translation le décale. Le 1 homogène « active » la translation — un vecteur direction (dernière coordonnée 0) subit la rotation mais pas la translation, ce qui est exactement le comportement voulu pour un axe ou une vitesse.

1.1 Composition en chaîne

L'intérêt majeur des coordonnées homogènes : la composition de repères se fait par produit matriciel. Si ⁰T₁ place le repère 1 dans le repère 0 et ¹T₂ le repère 2 dans le repère 1, alors :

Les indices s'« annihilent » deux à deux (…₁·¹…) : c'est un excellent garde-fou pour vérifier l'ordre d'une chaîne. Cette dernière équation est la cinématique directe (chapitres 4-5), écrite de la base vers l'effecteur.

1.2 Non-commutativité

Le produit matriciel n'est pas commutatif : A · B ≠ B · A en général. Géométriquement, « tourner puis translater » ne donne pas la même pose que « translater puis tourner ». C'est la source de bugs la plus fréquente en manipulation de repères — le code compile, les dimensions collent, mais le robot va au mauvais endroit.

2. Inverse d'une transformation rigide

On a souvent besoin de renverser une transformation : si ⁱT_j place j dans i, alors ^jT_i = (ⁱT_j)^⁻¹ place i dans j. Inverser une matrice 4×4 quelconque coûte cher (élimination de Gauss, ≈ 60-80 opérations, sans parler des risques numériques). Mais une transformation rigide a une structure particulière : sa partie rotation est orthogonale, donc son inverse est sa transposée (R^⁻¹ = R^T). On en déduit la formule fermée :

La vérification est immédiate : T^⁻¹ · T a pour bloc rotation R^TR = I et pour translation R^Tt − R^Tt = 0. On ne calcule donc aucune inversion : juste une transposition (recopie d'indices) et un produit matrice-vecteur de 3 lignes.

// Inverse d'une transformation RIGIDE : Tinv := T^-1 = [ R^T | -R^T t ].
// Suppose que T a bien la structure [R t ; 0 0 0 1] (R orthogonale).
// Tinv doit être DISTINCTE de T.
FUNCTION Mat4InvRigid : BOOL
VAR_IN_OUT
    T    : T_MAT4;   // entrée : transformation rigide
    Tinv : T_MAT4;   // sortie : son inverse
END_VAR
VAR
    i, j : INT;
    t    : ARRAY[0..2] OF LREAL;   // translation d'origine (colonne 3)
END_VAR
    // 1) Transposer le bloc rotation 3x3 : Rinv[i,j] := R[j,i].
    FOR i := 0 TO 2 DO
        FOR j := 0 TO 2 DO
            Tinv[i, j] := T[j, i];
        END_FOR
    END_FOR

    // 2) Nouvelle translation : t' := -R^T * t.
    //    On lit d'abord t (colonne 3) puis on projette sur les lignes de R^T.
    FOR i := 0 TO 2 DO
        t[i] := T[i, 3];
    END_FOR
    FOR i := 0 TO 2 DO
        Tinv[i, 3] := -( Tinv[i, 0] * t[0]
                       + Tinv[i, 1] * t[1]
                       + Tinv[i, 2] * t[2] );
    END_FOR

    // 3) Dernière ligne homogène.
    Tinv[3, 0] := 0.0;  Tinv[3, 1] := 0.0;  Tinv[3, 2] := 0.0;  Tinv[3, 3] := 1.0;
    Mat4InvRigid := TRUE;
END_FUNCTION

3. Rotations élémentaires

Les rotations autour des trois axes principaux sont les briques de toute orientation. Avec la convention main droite (angle θ positif = sens trigonométrique vu depuis l'extrémité positive de l'axe), en notant c = cos θ, s = sin θ :

Notez le signe inversé du sin dans R_y : le −s est en bas à gauche et le +s en haut à droite, l'exact opposé de R_x et R_z. Ce n'est pas une coquille : c'est la conséquence de l'orientation cyclique des axes (x → y → z → x). R_y tourne dans le plan (z, x) — l'ordre est « à rebours » du couple (x, y) —, d'où le renversement. C'est l'erreur la plus classique quand on écrit ces matrices de mémoire.

4. Composition d'orientations : pré- vs post-multiplication

Quand on enchaîne deux rotations, le résultat dépend du repère par rapport auquel chaque rotation est exprimée. Deux conventions coexistent :

• Post-multiplication (R · R') : la seconde rotation R' est exprimée dans le repère courant (déjà tourné). On multiplie à droite. C'est la logique d'un enchaînement de liaisons : chaque axe tourne dans le repère du segment précédent.
• Pré-multiplication (R' · R) : la seconde rotation est exprimée dans le repère fixe (le monde). On multiplie à gauche.

Autrement dit : une rotation exprimée dans le repère courant se post-multiplie, une rotation exprimée dans le repère fixe se pré-multiplie. Les deux lectures d'une même séquence sont valides mais donnent des matrices différentes — sauf cas particulier commutatif (rotations autour du même axe).

5. Représentations de l'orientation

Une orientation de SO(3) a 3 degrés de liberté, mais on peut la coder de plusieurs façons, chacune avec ses compromis. Aucune n'est parfaite — le choix dépend de l'usage (calcul interne, interface humaine, interpolation).

6. Conversions RPY (Euler ZYX)

La convention RPY (roll-pitch-yaw) décrit l'orientation par trois angles : roll φ autour de x, pitch θ autour de y, yaw ψ autour de z. La matrice correspondante, en repère fixe (donc appliquée dans l'ordre x, y, z et écrite en pré-multipliant), est :

En développant le produit (avec c_φ = cos φ, s_φ = sin φ, etc.), on obtient explicitement :

Pour l'extraction inverse (retrouver les angles depuis une matrice R connue), on utilise atan2, qui gère les quatre quadrants et le signe correctement — jamais asin seul, qui perd de l'information. En lisant les bons coefficients :

(indices à partir de 1 dans les formules ; en ST, où les tableaux commencent à 0, r₂₁ devient R[1,0], etc.)

7. Implémentation en Structured Text

On réutilise les types et briques du chapitre 1 (T_MAT3, T_MAT4, Mat4Identity, Mat4Mul). Les rotations élémentaires renvoient une T_MAT4 (bloc rotation + identité ailleurs) pour se composer directement avec les transformations homogènes. On passe le résultat en VAR_IN_OUT pour éviter toute copie.

// ---- Rotations élémentaires en 4x4 (bloc rotation + identité homogène) ----

// Rz(theta) : rotation autour de z. c en (0,0)(1,1), -s en (0,1), +s en (1,0).
FUNCTION RotZ : BOOL
VAR_IN_OUT
    M : T_MAT4;
END_VAR
VAR_INPUT
    theta : LREAL;   // rad
END_VAR
VAR
    c, s : LREAL;
END_VAR
    c := COS(theta);
    s := SIN(theta);
    Mat4Identity(M := M);        // met tout à I, on écrase le bloc 3x3 utile
    M[0,0] := c;   M[0,1] := -s;
    M[1,0] := s;   M[1,1] :=  c;
    RotZ := TRUE;
END_FUNCTION

// Ry(theta) : ATTENTION au signe inversé du sin (-s en (2,0), +s en (0,2)).
FUNCTION RotY : BOOL
VAR_IN_OUT
    M : T_MAT4;
END_VAR
VAR_INPUT
    theta : LREAL;
END_VAR
VAR
    c, s : LREAL;
END_VAR
    c := COS(theta);
    s := SIN(theta);
    Mat4Identity(M := M);
    M[0,0] :=  c;   M[0,2] := s;
    M[2,0] := -s;   M[2,2] := c;
    RotY := TRUE;
END_FUNCTION

// Rx(theta) : rotation autour de x. -s en (1,2), +s en (2,1).
FUNCTION RotX : BOOL
VAR_IN_OUT
    M : T_MAT4;
END_VAR
VAR_INPUT
    theta : LREAL;
END_VAR
VAR
    c, s : LREAL;
END_VAR
    c := COS(theta);
    s := SIN(theta);
    Mat4Identity(M := M);
    M[1,1] := c;   M[1,2] := -s;
    M[2,1] := s;   M[2,2] :=  c;
    RotX := TRUE;
END_FUNCTION

La construction d'une matrice depuis les angles RPY se fait par la composition R_z(ψ)·R_y(θ)·R_x(φ). On enchaîne deux produits via Mat4Mul, en veillant à des matrices temporaires distinctes :

// R := Rz(psi) * Ry(theta) * Rx(phi)   (RPY, repère fixe / Euler ZYX)
// Écrit une transformation homogène 4x4 (translation nulle) dans M.
FUNCTION RpyToMat : BOOL
VAR_IN_OUT
    M : T_MAT4;    // sortie
END_VAR
VAR_INPUT
    phi   : LREAL;   // roll  (autour de x), rad
    theta : LREAL;   // pitch (autour de y), rad
    psi   : LREAL;   // yaw   (autour de z), rad
END_VAR
VAR
    Rz, Ry, Rx, tmp : T_MAT4;
END_VAR
    RotZ(M := Rz, theta := psi);
    RotY(M := Ry, theta := theta);
    RotX(M := Rx, theta := phi);

    Mat4Mul(A := Ry, B := Rx,  C := tmp);   // tmp := Ry * Rx
    Mat4Mul(A := Rz, B := tmp, C := M);     // M   := Rz * (Ry * Rx)
    RpyToMat := TRUE;
END_FUNCTION

L'extraction inverse applique les trois atan2, avec détection du cas dégénéré θ = ±90°. La fonction ATAN2(y, x) et SQRT sont standard IEC 61131-3.

// Extrait les angles RPY (phi, theta, psi) d'une matrice de rotation.
// Convention Euler ZYX. Gère le blocage de cardan à theta = +/- 90°.
FUNCTION MatToRpy : BOOL
VAR_IN_OUT
    M : T_MAT4;      // entrée : rotation (bloc 3x3 utilisé)
END_VAR
VAR_OUTPUT
    phi   : LREAL;   // roll
    theta : LREAL;   // pitch
    psi   : LREAL;   // yaw
END_VAR
VAR
    cy : LREAL;      // sqrt(r11^2 + r21^2) = |cos(theta)|
END_VAR
    // r11 = M[0,0], r21 = M[1,0], r31 = M[2,0], r32 = M[2,1], r33 = M[2,2].
    cy := SQRT(M[0,0]*M[0,0] + M[1,0]*M[1,0]);

    IF cy > 1E-9 THEN
        // Cas régulier.
        psi   := ATAN2(M[1,0], M[0,0]);   // atan2(r21, r11)
        theta := ATAN2(-M[2,0], cy);      // atan2(-r31, cy)
        phi   := ATAN2(M[2,1], M[2,2]);   // atan2(r32, r33)
    ELSE
        // Blocage de cardan : cos(theta) ≈ 0, theta = +/- 90°.
        // Roll et yaw non séparables : on fixe psi = 0 par convention.
        psi   := 0.0;
        theta := ATAN2(-M[2,0], cy);      // +/- PI/2 selon le signe de r31
        phi   := ATAN2(-M[1,2], M[1,1]);  // report de la rotation sur phi
    END_IF
    MatToRpy := TRUE;
END_FUNCTION

8. Pièges à connaître

• Ordre des angles d'Euler. ZYX (RPY) est répandu, mais chaque constructeur a sa convention : ZYZ, XYZ, angles fixes ou courants, degrés ou radians… Un jeu d'angles n'a de sens qu'avec sa convention. Vérifiez la documentation du robot cible avant tout échange, sous peine d'orientation fausse mais « plausible ».
• Dérive de l'orthonormalité de R. À force de multiplier des matrices en flottant, R^TR s'éloigne lentement de I : les colonnes cessent d'être exactement unitaires et orthogonales. Sur une longue chaîne ou une intégration répétée, il faut ré-orthonormaliser périodiquement (voir exercice 4).
• Radians, toujours. SIN, COS, ATAN2 travaillent en radians. Convertissez aux frontières (rad := deg * PI / 180.0) et raisonnez en SI au cœur du calcul, comme fixé au chapitre 1.

Exercices

PROGRAM P_TestRotZPi
VAR
    R          : T_MAT4;
    px, py, pz : LREAL;
    qx, qy, qz : LREAL;
    ok         : BOOL := TRUE;
    PI         : LREAL := 3.14159265358979;
END_VAR

// Point de départ (1, 0, 0).
px := 1.0;  py := 0.0;  pz := 0.0;

RotZ(M := R, theta := PI);

// q := R * p  (partie 3x3 ; translation nulle ici).
qx := R[0,0]*px + R[0,1]*py + R[0,2]*pz;
qy := R[1,0]*px + R[1,1]*py + R[1,2]*pz;
qz := R[2,0]*px + R[2,1]*py + R[2,2]*pz;

IF ABS(qx + 1.0) > 1E-9 THEN ok := FALSE; END_IF   // attendu -1
IF ABS(qy)       > 1E-9 THEN ok := FALSE; END_IF   // attendu  0
IF ABS(qz)       > 1E-9 THEN ok := FALSE; END_IF   // attendu  0
// ok = TRUE si Rz(PI) envoie (1,0,0) sur (-1,0,0)

PROGRAM P_TestNonCommute
VAR
    Rz, Rx, AB, BA : T_MAT4;
    i, j           : INT;
    identiques     : BOOL := TRUE;
    HALFPI         : LREAL := 1.5707963267949;   // PI/2
END_VAR

RotZ(M := Rz, theta := HALFPI);
RotX(M := Rx, theta := HALFPI);

Mat4Mul(A := Rz, B := Rx, C := AB);   // AB := Rz * Rx
Mat4Mul(A := Rx, B := Rz, C := BA);   // BA := Rx * Rz

FOR i := 0 TO 3 DO
    FOR j := 0 TO 3 DO
        IF ABS(AB[i,j] - BA[i,j]) > 1E-9 THEN
            identiques := FALSE;   // au moins un coefficient diffère
        END_IF
    END_FOR
END_FOR
// identiques = FALSE : les deux ordres donnent des rotations différentes

PROGRAM P_TestRpyAllerRetour
VAR
    R                : T_MAT4;
    phi0, the0, psi0 : LREAL;
    phi1, the1, psi1 : LREAL;
    ok               : BOOL := TRUE;
END_VAR

phi0 := 0.1;  the0 := 0.2;  psi0 := 0.3;

RpyToMat(M := R, phi := phi0, theta := the0, psi := psi0);
MatToRpy(M := R, phi => phi1, theta => the1, psi => psi1);

IF ABS(phi1 - phi0) > 1E-9 THEN ok := FALSE; END_IF
IF ABS(the1 - the0) > 1E-9 THEN ok := FALSE; END_IF
IF ABS(psi1 - psi0) > 1E-9 THEN ok := FALSE; END_IF
// ok = TRUE : la conversion RPY -> R -> RPY est l'identité (hors singularité)

// Ré-orthonormalise le bloc rotation 3x3 de M par Gram-Schmidt sur colonnes.
// Corrige la dérive numérique accumulée au fil des produits de matrices.
FUNCTION ReOrthoNorm : BOOL
VAR_IN_OUT
    M : T_MAT4;
END_VAR
VAR
    cx, cy, cz : T_VEC3;   // colonnes 0, 1, 2 de R
    dot, n     : LREAL;
    i          : INT;
END_VAR
    // Lecture des colonnes.
    FOR i := 0 TO 2 DO
        cx[i] := M[i,0];  cy[i] := M[i,1];  cz[i] := M[i,2];
    END_FOR

    // 1) Normaliser cx.
    n := SQRT(cx[0]*cx[0] + cx[1]*cx[1] + cx[2]*cx[2]);
    FOR i := 0 TO 2 DO cx[i] := cx[i] / n; END_FOR

    // 2) Rendre cy orthogonale à cx (cy := cy - (cy·cx) cx), puis normaliser.
    dot := cy[0]*cx[0] + cy[1]*cx[1] + cy[2]*cx[2];
    FOR i := 0 TO 2 DO cy[i] := cy[i] - dot * cx[i]; END_FOR
    n := SQRT(cy[0]*cy[0] + cy[1]*cy[1] + cy[2]*cy[2]);
    FOR i := 0 TO 2 DO cy[i] := cy[i] / n; END_FOR

    // 3) cz := cx × cy  (produit vectoriel -> repère direct garanti).
    cz[0] := cx[1]*cy[2] - cx[2]*cy[1];
    cz[1] := cx[2]*cy[0] - cx[0]*cy[2];
    cz[2] := cx[0]*cy[1] - cx[1]*cy[0];

    // Réécriture des colonnes dans M.
    FOR i := 0 TO 2 DO
        M[i,0] := cx[i];  M[i,1] := cy[i];  M[i,2] := cz[i];
    END_FOR
    ReOrthoNorm := TRUE;
END_FUNCTION

Récapitulatif