Annexe
Optimiser les matrices 4×4 en SCL (Siemens)

0. Règle d'or : mesurer d'abord

Sur automate, la contrainte n'est pas « aller vite » mais « tenir le temps de cycle » de façon déterministe. Avant d'optimiser une fonction, mesurez-la : sur une matrice 4×4, une multiplication naïve coûte quelques microsecondes sur un S7-1500 — souvent déjà négligeable. On optimise ce qui pèse réellement.

// Mesure du temps d'exécution avec l'instruction RUNTIME (renvoie des secondes, LREAL)
#t0    := RUNTIME(#memRuntime);        // amorce le chronomètre
"MatMul4x4"(A := #A, B := #B, C := #C); // le code à mesurer
#duree := RUNTIME(#memRuntime);        // durée écoulée, en secondes (ex : 3.1E-6 = 3,1 µs)

1. Représenter les matrices en SCL

On utilise des tableaux à deux dimensions. Deux choix structurent toute la suite :

• Type flottant. LREAL (64 bits) pour la cinématique : le chaînage de rotations accumule les erreurs d'arrondi, et le S7-1500 traite le LREAL efficacement. REAL (32 bits) est plus léger/rapide (utile sur S7-1200) mais moins précis.
• Passage des paramètres. Une matrice 4×4 de LREAL pèse 128 octets. En VAR_INPUT/VAR_OUTPUT, un tableau est copié à l'appel. En VAR_IN_OUT, il est passé par référence (pointeur) : aucune copie. Pour des matrices, préférez donc VAR_IN_OUT.

2. Multiplication 4×4 : du naïf à l'optimisé

2.1 Version naïve (triple boucle)

Correcte et lisible — le point de départ. Notez le passage par référence et l'accumulateur en TEMP.

FUNCTION "MatMul4x4" : Void
VAR_IN_OUT
    A : ARRAY[0..3, 0..3] OF LREAL;   // par référence : pas de copie
    B : ARRAY[0..3, 0..3] OF LREAL;
    C : ARRAY[0..3, 0..3] OF LREAL;   // résultat (⚠ doit être distinct de A et B)
END_VAR
VAR_TEMP
    i, j, k : INT;
    s : LREAL;
END_VAR
BEGIN
    FOR i := 0 TO 3 DO
        FOR j := 0 TO 3 DO
            s := 0.0;                          // accumulateur en pile locale
            FOR k := 0 TO 3 DO
                s := s + A[i, k] * B[k, j];
            END_FOR;
            C[i, j] := s;                      // une seule écriture dans C par case
        END_FOR;
    END_FOR;
END_FUNCTION

2.2 Optimisation : cacher la ligne et dérouler la boucle interne

Deux idées, à effort minime : (1) copier la ligne i de A dans quatre scalaires TEMP pour éviter de recalculer l'adresse A[i,k] à chaque itération ; (2) dérouler la boucle sur k (taille fixe = 4) pour supprimer le compteur, le test de fin et le contrôle de bornes.

VAR_TEMP
    i, j : INT;
    a0, a1, a2, a3 : LREAL;
END_VAR
BEGIN
    FOR i := 0 TO 3 DO
        a0 := A[i, 0];  a1 := A[i, 1];  a2 := A[i, 2];  a3 := A[i, 3];   // ligne cachée
        FOR j := 0 TO 3 DO
            C[i, j] := a0 * B[0, j] + a1 * B[1, j]
                     + a2 * B[2, j] + a3 * B[3, j];                      // boucle k déroulée
        END_FOR;
    END_FOR;
END_FUNCTION

On peut aller jusqu'au déroulage total (16 lignes C[0,0] := A[0,0]*B[0,0] + …) : gain maximal mais code verbeux et difficile à maintenir. La version ci-dessus est le bon compromis lisibilité / performance pour du 4×4.

3. Inverser une matrice 4×4 : exploiter la structure

3.1 Le piège : ne pas dégainer Gauss par réflexe

Inverser une 4×4 quelconque par élimination de Gauss-Jordan demande des dizaines d'opérations, des divisions, une recherche de pivot et des branchements — coûteux et sensible au bruit numérique. Or, en cinématique directe, on n'inverse jamais une matrice quelconque : toutes les transformations de repère sont homogènes.

3.2 Inverse d'une matrice homogène : formule analytique

Pour T = [R | p ; 0\,0\,0\,1] avec R orthonormée, l'inverse est exact et immédiat :

La rotation s'inverse par simple transposition, la translation par un petit produit. Aucune division, aucun pivot, aucun branchement.

FUNCTION "InvHomogene" : Void
VAR_IN_OUT
    T    : ARRAY[0..3, 0..3] OF LREAL;   // entrée : [R | p ; 0 0 0 1]
    Tinv : ARRAY[0..3, 0..3] OF LREAL;   // sortie
END_VAR
VAR_TEMP
    i, j : INT;
END_VAR
BEGIN
    // 1) Rotation : R_inv = R^T  (transposition du bloc 3×3)
    FOR i := 0 TO 2 DO
        FOR j := 0 TO 2 DO
            Tinv[i, j] := T[j, i];
        END_FOR;
    END_FOR;

    // 2) Translation : p_inv = -R^T * p   (R^T[i,:] = colonne i de R = T[.,i])
    FOR i := 0 TO 2 DO
        Tinv[i, 3] := -( T[0, i] * T[0, 3]
                       + T[1, i] * T[1, 3]
                       + T[2, i] * T[2, 3] );
    END_FOR;

    // 3) Dernière ligne (0 0 0 1)
    Tinv[3, 0] := 0.0;  Tinv[3, 1] := 0.0;  Tinv[3, 2] := 0.0;  Tinv[3, 3] := 1.0;
END_FUNCTION

Bilan : ≈ 9 multiplications et 6 additions, zéro division, temps d'exécution constant (déterministe). À comparer aux dizaines d'opérations et divisions d'un Gauss général.

4. Inverser un Jacobien : résoudre, ne pas inverser

Le Jacobien J relie vitesses articulaires et vitesse de l'effecteur : v = J·q̇. Pour la commande en vitesse (cinématique inverse instantanée) on veut q̇ = J^-1·v. Deux réflexes d'automaticien :

4.1 Résoudre le système au lieu de former l'inverse

Calculer explicitement J^-1 puis multiplier est plus coûteux et moins stable que résoudre directement J·q̇ = v par élimination de Gauss avec pivot partiel. La fonction ci-dessous résout un système A·x = b (ici A = J, b = v, x = q̇) et renvoie FALSE si la matrice est (quasi) singulière.

FUNCTION "ResoudreLin4" : BOOL   // renvoie FALSE si (quasi-)singulier
VAR_IN_OUT
    A : ARRAY[0..3, 0..3] OF LREAL;   // ⚠ modifiée : passez-en une COPIE de travail
    b : ARRAY[0..3] OF LREAL;         // ⚠ modifié également
    x : ARRAY[0..3] OF LREAL;         // solution
END_VAR
VAR_TEMP
    i, j, k, p : INT;
    maxv, coef, s, tmp : LREAL;
END_VAR
CONSTANT
    EPS : LREAL := 1.0E-9;            // seuil de singularité
END_CONSTANT
BEGIN
    FOR k := 0 TO 3 DO
        // --- recherche du pivot (plus grand |A[i,k]|) pour la stabilité ---
        p := k;  maxv := ABS(A[k, k]);
        FOR i := k + 1 TO 3 DO
            IF ABS(A[i, k]) > maxv THEN  maxv := ABS(A[i, k]);  p := i;  END_IF;
        END_FOR;
        IF maxv < EPS THEN
            "ResoudreLin4" := FALSE;     // singulier → on renonce proprement
            RETURN;
        END_IF;
        // --- échange des lignes k et p ---
        IF p <> k THEN
            FOR j := 0 TO 3 DO  tmp := A[k, j];  A[k, j] := A[p, j];  A[p, j] := tmp;  END_FOR;
            tmp := b[k];  b[k] := b[p];  b[p] := tmp;
        END_IF;
        // --- élimination sous le pivot ---
        FOR i := k + 1 TO 3 DO
            coef := A[i, k] / A[k, k];
            FOR j := k TO 3 DO  A[i, j] := A[i, j] - coef * A[k, j];  END_FOR;
            b[i] := b[i] - coef * b[k];
        END_FOR;
    END_FOR;
    // --- remontée (substitution arrière) ---
    FOR i := 3 TO 0 BY -1 DO
        s := b[i];
        FOR j := i + 1 TO 3 DO  s := s - A[i, j] * x[j];  END_FOR;
        x[i] := s / A[i, i];
    END_FOR;
    "ResoudreLin4" := TRUE;
END_FUNCTION

4.2 Près d'une singularité : amortir (Damped Least Squares)

Au voisinage d'une singularité, J devient mal conditionné : J^-1 « explose » et commande des vitesses articulaires énormes. La parade classique est l'inverse amorti (Damped Least Squares, Levenberg-Marquardt) :

Le terme λ²·I borne la solution : on troque un peu de précision de suivi contre des vitesses lisses et sûres. En pratique sur PLC : surveiller la manipulabilité w = √(det(J·J^T)), augmenter λ quand w chute, et toujours saturer la sortie q̇ avant de l'envoyer aux axes.

Exercices

FOR i := 0 TO 3 DO
    a0 := A[i,0]; a1 := A[i,1]; a2 := A[i,2]; a3 := A[i,3];   // 4 lectures
    FOR j := 0 TO 3 DO
        C[i,j] := a0*B[0,j] + a1*B[1,j] + a2*B[2,j] + a3*B[3,j];
    END_FOR;
END_FOR;

Récapitulatif