🤖 Cinématique robotique · sur automate (PLC)

Chapitre 05
Cinématique directe sur PLC

Objectifs du chapitre

1. Le principe : la FK est un chaînage

La cinématique directe (forward kinematics, FK) répond à la question : pour un jeu de variables articulaires q = (q1, …, qn), quelle est la pose de l'effecteur ? La réponse est un simple produit de matrices homogènes : chaque liaison i fournit sa transformation i-1Ti (matrice DH du chapitre 3, ou MDH du chapitre 4), et on les compose de la base vers l'outil.

0Tn(q) = 0T1 · 1T2 · … · n-1Tn

Cette équation est la cinématique directe. Contrairement à la cinématique inverse, elle est déterministe et unique : à un q correspond exactement une pose, sans ambiguïté ni singularité de résolution. C'est un pur calcul de composition géométrique — idéal pour une tâche cyclique d'automate.

Deux propriétés doivent rester présentes à l'esprit. D'abord, le produit matriciel n'est pas commutatif : 0T1 · 1T2 ≠ 1T2 · 0T1. Une transformation, c'est d'abord une rotation puis une translation dans ce repère ; inverser l'ordre change le résultat. Ensuite, on parcourt la chaîne en post-multipliant : on part de l'identité et, à chaque liaison, on multiplie à droite la transformation accumulée par la matrice de la liaison courante :

T ← I,    puis pour i = 1..n :   T ← T · i-1Ti

À la fin de la boucle, T = 0Tn. Écrire T ← i-1Ti · T (pré-multiplication) composerait dans le mauvais sens et donnerait une pose fausse.

2. Liaison rotoïde vs prismatique

Dans la table DH, les paramètres θ, d, a, α stockés sont les offsets constants de la géométrie. La variable articulaire qi vient s'y ajouter — mais sur quel paramètre dépend de la nature de la liaison :

Les deux autres paramètres a (longueur de bras) et α (torsion) sont toujours constants : ils décrivent la structure mécanique, jamais le mouvement. Une petite fonction utilitaire calcule les paramètres effectifs d'une liaison à partir de sa ligne DH et de sa variable : 

// Calcule la matrice de liaison i-1 -> i pour une ligne DH et sa variable q.
// Aiguille q sur theta (rotoïde) ou sur d (prismatique).
// S'appuie sur DhTransform (chapitre 3) : DhTransform(theta, d, a, alpha, T).
FUNCTION LinkTransform : BOOL
VAR_IN_OUT
    dh : T_DH;      // ligne DH de la liaison (offsets constants)
    T  : T_MAT4;    // sortie : i-1 T i
END_VAR
VAR_INPUT
    q  : LREAL;     // variable articulaire de la liaison (rad ou m)
END_VAR
VAR
    thetaEff : LREAL;
    dEff     : LREAL;
END_VAR
    IF dh.isPrismatic THEN
        thetaEff := dh.theta;        // theta figé
        dEff     := dh.d + q;        // q s'ajoute à d
    ELSE
        thetaEff := dh.theta + q;    // q s'ajoute à theta
        dEff     := dh.d;            // d figé
    END_IF
    // a et alpha sont TOUJOURS constants.
    LinkTransform := DhTransform(thetaEff, dEff, dh.a, dh.alpha, T);
END_FUNCTION

Ne jamais oublier q sur une prismatique. L'erreur classique est de coder theta := offset + q pour tous les axes. Sur une liaison prismatique, θ est fixe et c'est d qui bouge : appliquer q au mauvais paramètre fait tourner l'axe au lieu de le faire coulisser. Le champ isPrismatic existe précisément pour aiguiller le calcul — utilisez-le à un seul endroit (ici LinkTransform) pour ne pas dupliquer la logique.

3. Algorithme général sur n liaisons

L'algorithme est le même quel que soit le nombre d'axes. On part de l'identité et l'on post-multiplie liaison après liaison. Le point crucial pour la suite du cours : on ne calcule pas seulement 0Tn, on mémorise au passage chaque transformation cumulée 0Ti dans un tableau ARRAY[0..N] OF T_MAT4.

T0i[0] = I,   T0i[1] = 0T1,   T0i[2] = 0T1·1T2,   …,   T0i[n] = 0Tn

Ces repères intermédiaires ne sont pas un luxe : le Jacobien géométrique (chapitre 6) a besoin, pour chaque axe i, de l'axe de rotation zi-1 (3ᵉ colonne de la rotation de 0Ti-1) et de la position d'origine pi-1 (colonne de translation). Les recalculer serait du gaspillage : on les produit gratuitement pendant la FK, on les range, et le Jacobien n'aura plus qu'à les lire. Retenez ce tableau T0i, c'est le lien direct avec le chapitre suivant.

En pseudo-code, en tenant compte de la contrainte que Mat4Mul exige un résultat distinct de ses entrées :

T0i[0] ← I
pour i = 1..n :
   L ← i-1Ti  (via LinkTransform)
   T0i[i] ← T0i[i-1] · L  (Mat4Mul, résultat dans une case neuve)
renvoyer T0i[n]

Comme T0i[i] et T0i[i-1] sont deux cases différentes du tableau, aucun aliasing n'est possible : on écrit dans une matrice qui n'est pas une entrée du produit. Le tableau nous rend service deux fois — il porte les repères pour le Jacobien et il fournit les buffers distincts que Mat4Mul réclame.

4. Implémentation : FB_ForwardKinematics

On encapsule la FK dans un FUNCTION_BLOCK (et non une simple FONCTION) car il porte des sorties structurées — la matrice, la pose, et surtout le tableau des repères cumulés qu'on veut conserver d'un cycle à l'autre. Les tableaux sont dimensionnés à une taille maximale N (constante globale, p. ex. 6) ; l'entrée n indique combien d'axes sont réellement utilisés. 

// Constante globale (GVL) : nombre MAXIMAL d'axes supportés.
VAR_GLOBAL CONSTANT
    N : INT := 6;
END_VAR

// -------------------------------------------------------------------
// FB_ForwardKinematics : cinématique directe d'un bras série n axes.
// Chaîne 0 T n = 0 T 1 * 1 T 2 * ... * (n-1) T n, en post-multipliant,
// et MÉMORISE chaque repère cumulé 0 T i (utile au Jacobien, ch. 6).
// -------------------------------------------------------------------
FUNCTION_BLOCK FB_ForwardKinematics
VAR_IN_OUT
    dh : ARRAY[0..N-1] OF T_DH;    // table DH (offsets constants)
    q  : ARRAY[0..N-1] OF LREAL;   // variables articulaires (rad ou m)
END_VAR
VAR_INPUT
    n  : INT;                      // nombre d'axes réellement utilisés (1..N)
END_VAR
VAR_OUTPUT
    T0n  : T_MAT4;                 // 0 T n : base -> effecteur
    pose : T_POSE;                 // même info, décomposée (x,y,z,R)
    T0i  : ARRAY[0..N] OF T_MAT4;  // repères cumulés : T0i[i] = 0 T i
END_VAR
VAR
    i   : INT;
    L   : T_MAT4;                  // i-1 T i (transformation de la liaison)
END_VAR

    // 1) Repère de base : T0i[0] = identité.
    Mat4Identity(M := T0i[0]);

    // 2) Chaînage : à chaque liaison, post-multiplication.
    FOR i := 1 TO n DO
        // Matrice de la liaison i (aiguille q sur theta ou d selon le type).
        LinkTransform(dh := dh[i-1], q := q[i-1], T := L);

        // T0i[i] := T0i[i-1] * L.  Résultat (T0i[i]) DISTINCT des entrées :
        // pas d'aliasing, Mat4Mul est utilisée en toute sécurité.
        Mat4Mul(A := T0i[i-1], B := L, C := T0i[i]);
    END_FOR

    // 3) Sorties : 0 T n et sa pose décomposée.
    Mat4Copy(src := T0i[n], dst := T0n);
    Mat4GetPose(T := T0n, p := pose);
END_FUNCTION_BLOCK

Deux remarques d'implémentation. D'abord, Mat4Mul écrit dans T0i[i], une case du tableau qui n'est ni A (T0i[i-1]) ni B (L) : la règle « résultat distinct des entrées » (chapitre 1) est respectée sans buffer supplémentaire. Ensuite, si l'on ne voulait pas conserver les repères intermédiaires, il faudrait deux buffers ping/pong et une recopie à chaque tour — ici le tableau T0i joue naturellement ce rôle. La petite fonction de recopie : 

// Recopie une matrice 4x4 (dst := src). Passage par VAR_IN_OUT : zéro copie superflue.
FUNCTION Mat4Copy : BOOL
VAR_IN_OUT
    src : T_MAT4;
    dst : T_MAT4;
END_VAR
VAR
    i, j : INT;
END_VAR
    FOR i := 0 TO 3 DO
        FOR j := 0 TO 3 DO
            dst[i, j] := src[i, j];
        END_FOR
    END_FOR
    Mat4Copy := TRUE;
END_FUNCTION

Pourquoi un FB et pas une FUNCTION ? En IEC 61131-3, une FUNCTION n'a pas de mémoire persistante entre appels : ses sorties disparaissent au retour. Un FUNCTION_BLOCK possède une instance avec état ; ses VAR_OUTPUT (dont le tableau T0i) restent lisibles après l'appel, tout le cycle durant. On déclare une instance unique fbFK : FB_ForwardKinematics; dans le programme, et on la rappelle chaque cycle — sans jamais rien allouer.

5. Extraction de la pose

La matrice 0Tn contient tout, mais on veut souvent en extraire séparément la position et l'orientation. La position est la colonne de translation (indices [i,3]) ; l'orientation est le bloc 3×3 supérieur gauche :

t = ( T[0,3], T[1,3], T[2,3] )    R = [ T[0,0] T[0,1] T[0,2] ; T[1,0] T[1,1] T[1,2] ; T[2,0] T[2,1] T[2,2] ] 
// Extrait la pose (position + rotation 3x3) d'une transformation homogène.
FUNCTION Mat4GetPose : BOOL
VAR_IN_OUT
    T : T_MAT4;
    p : T_POSE;
END_VAR
VAR
    i, j : INT;
END_VAR
    // Position = colonne de translation.
    p.x := T[0, 3];
    p.y := T[1, 3];
    p.z := T[2, 3];
    // Orientation = bloc rotation 3x3.
    FOR i := 0 TO 2 DO
        FOR j := 0 TO 2 DO
            p.R[i, j] := T[i, j];
        END_FOR
    END_FOR
    Mat4GetPose := TRUE;
END_FUNCTION

Pour une IHM ou un point de consigne, on veut fréquemment l'orientation sous forme de trois angles RPY (roulis, tangage, lacet) plutôt qu'une matrice 3×3. La conversion R → (roll, pitch, yaw) — et le piège du blocage de cardan à pitch = ±90° — a été traitée au chapitre 2 ; il suffit d'appeler cette fonction sur pose.R aux frontières du système (juste avant l'écriture vers l'IHM).

6. Repère base et repère outil (TCP)

0Tn place l'extrémité de la bride mécanique (repère n) dans le repère 0 du robot. En pratique, ce n'est presque jamais ce que l'on veut piloter :

La pose complète pilotable est alors l'encadrement de 0Tn par ces deux matrices :

baseTtcp = Tbase · 0Tn · Ttool

L'ordre reflète la géométrie : on part du monde, on descend au repère 0 du robot (Tbase), on traverse la chaîne jusqu'à la bride (0Tn), puis on va jusqu'au bout de l'outil (Ttool). En Structured Text, deux produits successifs (avec des temporaires distinctes) : 

// base T tcp := Tbase * (0 T n) * Ttool.
// tmp est distincte des entrées à chaque Mat4Mul (pas d'aliasing).
FUNCTION ApplyBaseTool : BOOL
VAR_IN_OUT
    Tbase : T_MAT4;   // repère 0 du robot dans le monde (montage)
    T0n   : T_MAT4;   // base robot -> bride
    Ttool : T_MAT4;   // bride -> TCP (outil)
    Tbtcp : T_MAT4;   // sortie : monde -> TCP
END_VAR
VAR
    tmp : T_MAT4;
END_VAR
    Mat4Mul(A := Tbase, B := T0n,  C := tmp);    // tmp := Tbase * 0Tn
    Mat4Mul(A := tmp,   B := Ttool, C := Tbtcp);  // Tbtcp := tmp * Ttool
    ApplyBaseTool := TRUE;
END_FUNCTION

Si le robot est monté « à l'origine » et sans outil, Tbase et Ttool valent l'identité et baseTtcp = 0Tn. C'est un bon cas de test : intercaler des identités ne doit rien changer. Séparer proprement base, robot et outil est ce qui permet, plus tard, de recalibrer l'un sans toucher aux autres.

7. Exemple travaillé : un bras anthropomorphe 6 axes

Prenons une table DH plausible d'un bras 6 axes de type industriel (poignet sphérique), toutes les liaisons rotoïdes. Les valeurs sont en mètres et radians ; les α valent ±π/2 ou 0, ce qui est typique de cette architecture.

iθi (offset)di (m)ai (m)αi (rad)type
100,3400,040−π/2rotoïde
2−π/200,3000rotoïde
3000,040−π/2rotoïde
400,3100+π/2rotoïde
5000−π/2rotoïde
600,08000rotoïde

On remplit la table une fois (données constantes), on charge la configuration articulaire q voulue, on appelle le FB, on lit la pose : 

PROGRAM P_ExempleFK6
VAR
    fbFK  : FB_ForwardKinematics;      // instance (état persistant)
    robot : ARRAY[0..N-1] OF T_DH;     // table DH du bras
    qcmd  : ARRAY[0..N-1] OF LREAL;    // configuration articulaire (rad)
    x, y, z : LREAL;
    initDone : BOOL := FALSE;
    PI : LREAL := 3.14159265358979;
END_VAR

// ---- Initialisation de la géométrie : UNE seule fois (constante) ----
IF NOT initDone THEN
    // theta(offset), d, a, alpha, isPrismatic
    robot[0].theta := 0.0;      robot[0].d := 0.340; robot[0].a := 0.040; robot[0].alpha := -PI/2.0; robot[0].isPrismatic := FALSE;
    robot[1].theta := -PI/2.0;  robot[1].d := 0.0;   robot[1].a := 0.300; robot[1].alpha := 0.0;     robot[1].isPrismatic := FALSE;
    robot[2].theta := 0.0;      robot[2].d := 0.0;   robot[2].a := 0.040; robot[2].alpha := -PI/2.0; robot[2].isPrismatic := FALSE;
    robot[3].theta := 0.0;      robot[3].d := 0.310; robot[3].a := 0.0;   robot[3].alpha := PI/2.0;  robot[3].isPrismatic := FALSE;
    robot[4].theta := 0.0;      robot[4].d := 0.0;   robot[4].a := 0.0;   robot[4].alpha := -PI/2.0; robot[4].isPrismatic := FALSE;
    robot[5].theta := 0.0;      robot[5].d := 0.080; robot[5].a := 0.0;   robot[5].alpha := 0.0;     robot[5].isPrismatic := FALSE;
    initDone := TRUE;
END_IF

// ---- Consigne articulaire (rad) ----
qcmd[0] := 0.0;
qcmd[1] := 0.3;
qcmd[2] := -0.5;
qcmd[3] := 0.0;
qcmd[4] := 0.8;
qcmd[5] := 0.0;

// ---- Cinématique directe ----
fbFK(dh := robot, q := qcmd, n := 6);

// ---- Lecture de la position de la bride (m), dans le repère 0 du robot ----
x := fbFK.pose.x;    // = fbFK.T0n[0,3]
y := fbFK.pose.y;    // = fbFK.T0n[1,3]
z := fbFK.pose.z;    // = fbFK.T0n[2,3]
// fbFK.pose.R contient l'orientation 3x3 ; fbFK.T0i[..] les repères cumulés.

La position (x, y, z) se lit dans la colonne de translation de fbFK.T0n (indices [0..2, 3]), reflétée par fbFK.pose.x/y/z. L'orientation est dans fbFK.pose.R (à convertir en RPY pour l'IHM, chapitre 2). Et — point clé — fbFK.T0i[0..6] est déjà rempli des six repères cumulés, prêt pour le Jacobien.

8. Validation

Un modèle de FK doit toujours être validé contre un oracle indépendant. Deux approches complémentaires, faciles à mettre en œuvre sur automate.

Oracle en formule fermée : le bras planaire 2R

Pour un bras planaire à deux liaisons rotoïdes (d = α = 0, ai = Li), la géométrie donne directement :

x = L1 c1 + L2 c12,    y = L1 s1 + L2 s12

avec c1 = cos q1, c12 = cos(q1+q2), etc. On compare la sortie du FB à cette formule fermée, à une tolérance près : 

PROGRAM P_Valide2R
VAR
    fbFK  : FB_ForwardKinematics;
    dh    : ARRAY[0..N-1] OF T_DH;
    q     : ARRAY[0..N-1] OF LREAL;
    L1, L2   : LREAL := 0.5;   // longueurs (m)
    xRef, yRef : LREAL;        // oracle (formule fermée)
    c1, s1, c12, s12 : LREAL;
    ok    : BOOL := TRUE;
    TOL   : LREAL := 1.0E-9;
    init  : BOOL := FALSE;
END_VAR

IF NOT init THEN
    // 2R planaire : theta variable, d=0, a=Li, alpha=0, rotoïde.
    dh[0].theta := 0.0; dh[0].d := 0.0; dh[0].a := L1; dh[0].alpha := 0.0; dh[0].isPrismatic := FALSE;
    dh[1].theta := 0.0; dh[1].d := 0.0; dh[1].a := L2; dh[1].alpha := 0.0; dh[1].isPrismatic := FALSE;
    init := TRUE;
END_IF

// Configuration de test.
q[0] := 0.6;   // rad
q[1] := -0.4;  // rad

// FK par produit de matrices.
fbFK(dh := dh, q := q, n := 2);

// Oracle : formule fermée.
c1  := COS(q[0]);              s1  := SIN(q[0]);
c12 := COS(q[0] + q[1]);       s12 := SIN(q[0] + q[1]);
xRef := L1 * c1 + L2 * c12;
yRef := L1 * s1 + L2 * s12;

// Comparaison à tolérance (jamais d'égalité stricte sur des flottants).
IF ABS(fbFK.pose.x - xRef) > TOL THEN ok := FALSE; END_IF
IF ABS(fbFK.pose.y - yRef) > TOL THEN ok := FALSE; END_IF
IF ABS(fbFK.pose.z)        > TOL THEN ok := FALSE; END_IF   // z doit rester nul
// ok = TRUE si le modèle matriciel coïncide avec l'oracle.

Pose de « home » connue

Deuxième contrôle, quasi gratuit : la configuration de home (q = 0 partout) donne une pose connue par construction de la table DH. Pour le 2R ci-dessus, q = 0 tend le bras le long de x : on attend (x, y) = (L1+L2, 0). Vérifier cette pose de référence au démarrage détecte immédiatement une table DH mal saisie ou une convention inversée.

Deux oracles valent mieux qu'un. La formule fermée valide la mécanique du produit de matrices sur toute une plage de q ; la pose de home valide la saisie de la table DH (offsets, signes). Gardez ces deux tests comme « auto-test » exécuté une fois au démarrage (front de bInit) : s'il échoue, on passe en défaut avant que le robot ne bouge. C'est peu de code pour beaucoup de sérénité.

9. Temps de cycle et déterminisme

La FK est bon marché. Chaque liaison, c'est un LinkTransform (deux SIN/COS et une poignée de produits) plus un produit 4×4 (64 multiplications, 48 additions). Six liaisons ≈ quelques µs sur un CPU d'automate moderne — négligeable devant une période de tâche de 250 µs à 1 ms.

Le coût dominant est souvent le SIN/COS. Or, dans une table DH, les α (et souvent une partie des θ offsets) sont constants : leurs cos α et sin α ne dépendent pas de q et peuvent être précalculés une fois (au front d'init) puis stockés à côté de la table DH. La boucle chaude n'évalue alors que les sin/cos des variables articulaires — le strict minimum.

Recette temps réel. (1) Précalculez tout ce qui est constant : cos α, sin α, offsets. (2) Zéro allocation : tout est en ARRAY de taille fixe, sur la mémoire statique du FB. (3) Boucle bornée : FOR i := 1 TO n, avec n ≤ N connu — pas de WHILE de longueur inconnue, pas d'appel bloquant. Résultat : un temps d'exécution constant et prévisible à chaque cycle. Le vrai risque sur PLC n'est pas la lenteur mais le non-déterminisme ; nos tableaux fixes et notre boucle bornée l'éliminent par construction.

10. Pièges récapitulés

Ordre de multiplication. On post-multiplie : Mat4Mul(A := Taccu, B := L, C := ...), jamais l'inverse. Inverser A et B compose dans le mauvais sens et produit une pose fausse — bug d'autant plus vicieux que le code compile et « ressemble » à un calcul matriciel correct.

Prismatique oubliée. Sur une liaison prismatique, q s'ajoute à d, pas à θ. Centraliser l'aiguillage dans LinkTransform (via isPrismatic) évite de l'oublier axe par axe.

Offsets et signes. Un θ offset ou un α de signe erroné passe souvent inaperçu à q = 0 mais fausse toute la plage. D'où l'importance de la pose de home ET de la formule fermée sur une plage d'angles.

DH classique vs MDH. La matrice de liaison n'a pas la même forme en convention DH distale (chapitre 3) et modifiée (MDH, chapitre 4). La FK doit savoir quelle convention décrit la table : appeler DhTransform sur une table MDH (ou l'inverse) mélange les repères et donne un résultat silencieusement faux. Fixez la convention par robot et ne la mélangez jamais dans une même chaîne.

Exercices

Exercice 1 — La boucle de FK sur n axes

Sans regarder la section 4, réécrivez le corps de FB_ForwardKinematics : partez de l'identité dans T0i[0], bouclez de 1 à n, construisez la matrice de liaison avec LinkTransform, et post-multipliez dans T0i[i]. Expliquez pourquoi aucun buffer temporaire supplémentaire n'est nécessaire pour Mat4Mul.

Voir la solution 
Mat4Identity(M := T0i[0]);
FOR i := 1 TO n DO
    LinkTransform(dh := dh[i-1], q := q[i-1], T := L);
    Mat4Mul(A := T0i[i-1], B := L, C := T0i[i]);   // résultat distinct des entrées
END_FOR
Mat4Copy(src := T0i[n], dst := T0n);
Mat4GetPose(T := T0n, p := pose);

Mat4Mul exige que C soit distinct de A et B. Ici C = T0i[i], tandis que les entrées sont T0i[i-1] (case précédente) et L (variable locale) : trois zones mémoire différentes, donc pas d'aliasing. Le tableau des repères cumulés fournit « gratuitement » les buffers distincts et conserve les 0Ti pour le Jacobien.

Exercice 2 — Valider un 2R contre la formule fermée

Écrivez un programme qui, pour un bras 2R (L1=0,4, L2=0,3) et q = (0,5 ; −0,7) rad, calcule la position par le FB puis la compare à x = L1c1 + L2c12, y = L1s1 + L2s12 à 10−9 près.

Voir la solution 
PROGRAM P_Ex2
VAR
    fbFK : FB_ForwardKinematics;
    dh   : ARRAY[0..N-1] OF T_DH;
    q    : ARRAY[0..N-1] OF LREAL;
    L1 : LREAL := 0.4;   L2 : LREAL := 0.3;
    xRef, yRef, c1, s1, c12, s12 : LREAL;
    ok   : BOOL := TRUE;
    init : BOOL := FALSE;
END_VAR

IF NOT init THEN
    dh[0].theta := 0.0; dh[0].d := 0.0; dh[0].a := L1; dh[0].alpha := 0.0; dh[0].isPrismatic := FALSE;
    dh[1].theta := 0.0; dh[1].d := 0.0; dh[1].a := L2; dh[1].alpha := 0.0; dh[1].isPrismatic := FALSE;
    init := TRUE;
END_IF

q[0] := 0.5;   q[1] := -0.7;
fbFK(dh := dh, q := q, n := 2);

c1  := COS(q[0]);           s1  := SIN(q[0]);
c12 := COS(q[0] + q[1]);    s12 := SIN(q[0] + q[1]);
xRef := L1 * c1 + L2 * c12;
yRef := L1 * s1 + L2 * s12;

IF ABS(fbFK.pose.x - xRef) > 1.0E-9 THEN ok := FALSE; END_IF
IF ABS(fbFK.pose.y - yRef) > 1.0E-9 THEN ok := FALSE; END_IF
// ok = TRUE si les deux modèles coïncident.

Toujours comparer à tolérance (1E-9), jamais avec = strict. Si le test échoue, suspectez d'abord l'ordre de multiplication ou un offset de la table DH.

Exercice 3 — Ajouter un repère outil (TCP)

Un outil est monté sur la bride : le TCP est décalé de 0,12 m le long de l'axe z de la bride (offset outil pur, sans rotation). Construisez Ttool et calculez 0Ttcp = 0Tn · Ttool (ici Tbase = I). Où lit-on le nouveau z du TCP ?

Voir la solution 
VAR
    Ttool, T0tcp, Ibase : T_MAT4;
    zTcp : LREAL;
END_VAR

// Toutefois d'abord la FK : fbFK(dh := ..., q := ..., n := 6);

// Ttool : translation pure de +0.12 m le long de z de la bride.
Mat4Identity(M := Ttool);
Ttool[2, 3] := 0.12;    // décalage z_tool

// Base à l'identité pour cet exemple.
Mat4Identity(M := Ibase);

// 0 T tcp := Ibase * (0 T n) * Ttool
ApplyBaseTool(Tbase := Ibase, T0n := fbFK.T0n, Ttool := Ttool, Tbtcp := T0tcp);

// La position du TCP est dans la colonne de translation.
zTcp := T0tcp[2, 3];    // z du TCP dans le repère 0

Le décalage outil est appliqué après 0Tn (post-multiplication) car il est exprimé dans le repère de la bride. Le TCP se lit dans la colonne [0..2, 3] de T0tcp ; l'orientation du TCP est identique à celle de la bride ici (l'outil n'a pas de rotation propre).

Exercice 4 — Bras 3R : extraire (x, y) et l'orientation planaire φ

Pour un bras planaire 3R (L = (0,4 ; 0,3 ; 0,2), q = (0,3 ; 0,4 ; −0,2) rad), calculez la pose par le FB, puis extrayez (x, y) et l'orientation planaire φ = θ1+θ2+θ3. Vérifiez que φ se retrouve aussi via ATAN2(R[1,0], R[0,0]) sur le bloc rotation.

Voir la solution 
PROGRAM P_Ex4
VAR
    fbFK : FB_ForwardKinematics;
    dh   : ARRAY[0..N-1] OF T_DH;
    q    : ARRAY[0..N-1] OF LREAL;
    L    : ARRAY[0..2] OF LREAL := [0.4, 0.3, 0.2];
    x, y, phiMat, phiSum, acc, xRef, yRef : LREAL;
    i    : INT;
    ok   : BOOL := TRUE;
    init : BOOL := FALSE;
END_VAR

IF NOT init THEN
    FOR i := 0 TO 2 DO
        dh[i].theta := 0.0; dh[i].d := 0.0; dh[i].a := L[i];
        dh[i].alpha := 0.0; dh[i].isPrismatic := FALSE;
    END_FOR
    init := TRUE;
END_IF

q[0] := 0.3;   q[1] := 0.4;   q[2] := -0.2;
fbFK(dh := dh, q := q, n := 3);

x := fbFK.pose.x;
y := fbFK.pose.y;

// Orientation planaire par le bloc rotation : atan2(r21, r11).
phiMat := ATAN2(fbFK.pose.R[1, 0], fbFK.pose.R[0, 0]);

// ... doit valoir la somme des angles (mécanisme planaire).
phiSum := q[0] + q[1] + q[2];

// Formule fermée nR : somme cumulative des angles.
acc := 0.0;   xRef := 0.0;   yRef := 0.0;
FOR i := 0 TO 2 DO
    acc  := acc + q[i];
    xRef := xRef + L[i] * COS(acc);
    yRef := yRef + L[i] * SIN(acc);
END_FOR

IF ABS(x - xRef)          > 1.0E-9 THEN ok := FALSE; END_IF
IF ABS(y - yRef)          > 1.0E-9 THEN ok := FALSE; END_IF
IF ABS(phiMat - phiSum)   > 1.0E-9 THEN ok := FALSE; END_IF
// ok = TRUE si position ET orientation coïncident.

Pour un mécanisme planaire, ATAN2(R[1,0], R[0,0]) redonne exactement la somme des angles articulaires : c'est l'angle de l'axe x de l'effecteur dans le plan. La formule fermée nR se généralise par la somme cumulative Σi Li cos(q1+…+qi).

Récapitulatif