🦀 Rust pour la robotique · temps réel

Chapitre 11
Cinématique directe

Objectifs du chapitre

1. Qu'est-ce que la cinématique directe ?

La cinématique directe (forward kinematics, FK) répond à une question géométrique précise : connaissant la valeur de chaque variable articulaire d'un robot (angle d'une liaison rotoïde, déplacement d'une liaison prismatique), où se trouve l'effecteur terminal et comment est-il orienté ?

On passe donc de l'espace articulaire — le vecteur des n coordonnées articulaires q = (q1, …, qn) — vers l'espace cartésien : la pose de l'effecteur, c'est-à-dire sa position t ∈ ℝ³ et son orientation R ∈ SO(3).

f : ℝn → SE(3),    q ↦ 0Tn(q) = [ R(q)   t(q) ;   0 0 0   1 ]

Contrairement à la cinématique inverse (chapitre 12), la FK est déterministe et toujours définie : à un jeu d'angles correspond une pose unique. C'est un simple calcul de composition géométrique, sans ambiguïté ni singularité de résolution.

2. Matrices de transformation homogènes 4×4

Une transformation rigide (rotation + translation) entre deux repères s'écrit de manière compacte avec les coordonnées homogènes. Un point p = (x, y, z) devient (x, y, z, 1)T, et une transformation devient une matrice 4×4 :

T = [ R   t ;   0T   1 ] = [ r11 r12 r13 tx ; r21 r22 r23 ty ; r31 r32 r33 tz ; 0   0   0   1 ]

où R est une matrice de rotation 3×3 (orthogonale, RTR = I, det R = +1) et t le vecteur translation 3×1. La transformation d'un point s'écrit :

p' = T · p   ⇔    p'3×1 = R · p3×1 + t

L'intérêt majeur : la composition de repères successifs se fait par simple produit matriciel. Si 0T1 place le repère 1 dans le repère 0, et 1T2 le repère 2 dans le repère 1, alors :

0T2 = 0T1 · 1T2    et    0Tn = 0T1 · 1T2 · … · n-1Tn

Cette dernière équation est la cinématique directe : on chaîne les transformations d'une liaison à la suivante, depuis la base jusqu'à l'effecteur.

3. Rotations élémentaires

Les rotations autour des axes principaux sont les briques de base. Avec la convention main droite (angle positif = sens trigonométrique vu depuis l'axe positif) :

Rz(θ) = [ cos θ   −sin θ   0 ; sin θ   cos θ   0 ; 0   0   1 ]
Ry(θ) = [ cos θ   0   sin θ ; 0   1   0 ; −sin θ   0   cos θ ]
Rx(θ) = [ 1   0   0 ; 0   cos θ   −sin θ ; 0   sin θ   cos θ ]

Notez le signe du sin dans Ry : il est inversé par rapport aux deux autres, conséquence directe de l'orientation cyclique des axes (x→y→z→x). C'est une erreur classique lorsqu'on écrit ces matrices de mémoire.

Radians, toujours des radians. Les fonctions f64::sin / f64::cos de Rust (comme sin/cos de la libm en C) travaillent en radians. Si vos angles arrivent en degrés depuis un fichier de config ou une IHM, convertissez-les explicitement avec x.to_radians(). Un cos(90.0) qui vaut −0.448 au lieu de 0.0 est le symptôme immédiat de cet oubli.

4. Convention de Denavit-Hartenberg

Écrire à la main chaque i-1Ti est fastidieux et source d'erreurs. La convention de Denavit-Hartenberg (DH) normalise le placement des repères sur une chaîne cinématique série : la transformation entre deux repères consécutifs ne dépend plus que de quatre paramètres.

ParamètreNomSignification
θiangle articulairerotation autour de zi-1 (variable si liaison rotoïde)
didécalage (offset)translation le long de zi-1 (variable si liaison prismatique)
ailongueur du brastranslation le long de xi
αitorsion (twist)rotation autour de xi

La matrice DH standard (convention distale, la plus répandue) résulte de la composition Rotz(θ) · Transz(d) · Transx(a) · Rotx(α) :

i-1Ti = [ cos θ   −sin θ cos α   sin θ sin α   a cos θ ; sin θ   cos θ cos α   −cos θ sin α   a sin θ ; 0   sin α   cos α   d ; 0   0   0   1 ]

On décrit alors le robot entier par une simple table DH, une ligne par liaison. Exemple d'un bras planaire 2R (voir section 5), avec les angles θ1, θ2 variables :

iθidiaiαi
1θ10L10
2θ20L20

5. Exemple : le bras planaire 2R

Considérons un bras à deux liaisons rotoïdes dans le plan, de longueurs L1 et L2. La géométrie élémentaire donne directement la formule fermée de la position de l'effecteur :

x = L1 cos θ1 + L2 cos(θ1 + θ2)
y = L1 sin θ1 + L2 sin(θ1 + θ2)

L'orientation de l'effecteur dans le plan est simplement φ = θ1 + θ2. On retrouve exactement ce résultat par le produit des deux matrices DH ci-dessus. Avec α = 0 et d = 0, chaque i-1Ti se réduit à une rotation Rz suivie d'une translation Li le long de x :

0T2 = 0T1 · 1T2,    colonne de translation = ( L1c1 + L2c12,  L1s1 + L2s12,  0 )

avec la notation usuelle c1 = cos θ1, c12 = cos(θ1+θ2), etc. La formule fermée et le produit matriciel coïncident : c'est notre test de validation en Rust.

La formule fermée est spécifique à une géométrie donnée et se dérive à la main ; l'approche DH par produit de matrices est générique et se code une fois pour toutes, quel que soit le nombre de liaisons. En robotique on garde souvent les deux : la formule fermée pour la vitesse et comme oracle de test, le produit matriciel pour la généralité.

6. Implémentation en Rust avec nalgebra

nalgebra fournit des matrices de taille fixe allouées sur la pile (Matrix4<f64> est un tableau [[f64; 4]; 4]), parfaites pour le temps réel. On définit d'abord la structure des paramètres DH, puis la matrice DH, puis la FK. 

use nalgebra::{Matrix4, Vector3};

/// Paramètres de Denavit-Hartenberg d'une liaison.
/// Ici `theta` et `d` sont les valeurs *constantes* ; la partie
/// variable (angle articulaire) est fournie séparément à la FK.
#[derive(Clone, Copy, Debug)]
pub struct ParamDH {
    pub theta: f64, // rad  (offset constant, souvent 0)
    pub d:     f64, // m
    pub a:     f64, // m
    pub alpha: f64, // rad
}

/// Matrice de transformation homogène DH standard (convention distale)
/// entre le repère i-1 et le repère i.
pub fn dh_transform(theta: f64, d: f64, a: f64, alpha: f64) -> Matrix4<f64> {
    let (st, ct) = theta.sin_cos();
    let (sa, ca) = alpha.sin_cos();

    // Matrix4::new remplit ligne par ligne (row-major).
    Matrix4::new(
        ct, -st * ca,  st * sa, a * ct,
        st,  ct * ca, -ct * sa, a * st,
        0.0,      sa,       ca,      d,
        0.0,     0.0,      0.0,    1.0,
    )
}

La cinématique directe chaîne les transformations. On part de l'identité et on multiplie, dans l'ordre, chaque matrice de liaison. L'angle articulaire angles[i] s'ajoute à l'offset constant params[i].theta. 

/// Cinématique directe : renvoie la transformation base -> effecteur.
/// `angles[i]` est la variable articulaire ajoutée à l'offset theta de la liaison i.
pub fn fk(angles: &[f64], params: &[ParamDH]) -> Matrix4<f64> {
    assert_eq!(angles.len(), params.len(), "un angle par liaison");

    let mut t = Matrix4::identity();
    for (q, p) in angles.iter().zip(params.iter()) {
        // Ordre crucial : T = T01 * T12 * ... ; on post-multiplie.
        t *= dh_transform(p.theta + q, p.d, p.a, p.alpha);
    }
    t
}

/// Extrait la position (colonne de translation) d'une transformation homogène.
pub fn position(t: &Matrix4<f64>) -> Vector3<f64> {
    Vector3::new(t[(0, 3)], t[(1, 3)], t[(2, 3)])
}

Version formule fermée du bras 2R, qui sert d'oracle pour valider la version matricielle : 

/// FK analytique du bras planaire 2R -> (x, y).
pub fn fk_2r_closed_form(l1: f64, l2: f64, theta1: f64, theta2: f64) -> (f64, f64) {
    let x = l1 * theta1.cos() + l2 * (theta1 + theta2).cos();
    let y = l1 * theta1.sin() + l2 * (theta1 + theta2).sin();
    (x, y)
}

fn main() {
    let (l1, l2) = (1.0_f64, 0.5_f64);
    let (t1, t2) = (30.0_f64.to_radians(), 45.0_f64.to_radians());

    // Table DH du bras 2R : theta variable, d=0, a=Li, alpha=0.
    let params = [
        ParamDH { theta: 0.0, d: 0.0, a: l1, alpha: 0.0 },
        ParamDH { theta: 0.0, d: 0.0, a: l2, alpha: 0.0 },
    ];

    let t = fk(&[t1, t2], &params);
    let p = position(&t);
    let (xc, yc) = fk_2r_closed_form(l1, l2, t1, t2);

    println!("matrices  : x = {:.6}, y = {:.6}", p.x, p.y);
    println!("analytique : x = {:.6}, y = {:.6}", xc, yc);

    // Validation : les deux doivent coïncider à l'epsilon près.
    assert!((p.x - xc).abs() < 1e-9);
    assert!((p.y - yc).abs() < 1e-9);
    println!("FK validee ✔");
}

f64::sin_cos() calcule sinus et cosinus en un seul appel, ce qui est à la fois plus rapide (une seule réduction d'argument) et plus lisible que deux appels séparés. À privilégier systématiquement dans les matrices de rotation.

Temps réel. Toutes les matrices ici sont de taille fixe (Matrix4<f64>) : elles vivent sur la pile, sans malloc ni Vec, donc sans latence imprévisible ni fragmentation du tas. Dans une boucle de contrôle à 1 kHz : (1) précalculez tout ce qui est constant (les offsets DH, les cos α / sin α qui ne dépendent pas des angles) ; (2) évitez toute allocation dans la boucle chaude ; (3) préférez les tableaux fixes [ParamDH; N] aux slices dynamiques quand N est connu à la compilation. La FK d'un bras 6 axes se réduit alors à quelques dizaines de multiplications flottantes, parfaitement déterministe.

L'ordre des multiplications n'est pas commutatif. Une transformation, c'est d'abord une rotation puis une translation dans ce repère ; inverser l'ordre change le résultat. La chaîne s'écrit 0Tn = 0T1 · 1T2 · … · n-1Tn, de la base vers l'effecteur, en post-multipliant. Écrire t = dh * t au lieu de t *= dh compose dans le mauvais sens et donne une pose fausse — l'un des bugs les plus insidieux car le code compile et « ressemble » à du calcul matriciel correct.

Exercices

Exercice 1 — FK du bras 2R en formule fermée

Écrivez une fonction fk_2r(l1, l2, t1, t2) -> (f64, f64) retournant la position (x, y) de l'effecteur d'un bras planaire 2R. Testez-la avec L1=1, L2=1, θ1=θ2=0 (le bras est tendu, on attend (2, 0)).

Voir la solution 
pub fn fk_2r(l1: f64, l2: f64, t1: f64, t2: f64) -> (f64, f64) {
    let x = l1 * t1.cos() + l2 * (t1 + t2).cos();
    let y = l1 * t1.sin() + l2 * (t1 + t2).sin();
    (x, y)
}

fn main() {
    let (x, y) = fk_2r(1.0, 1.0, 0.0, 0.0);
    assert!((x - 2.0).abs() < 1e-12 && y.abs() < 1e-12);

    // Bras replié à angle droit : L1 vers +x, L2 vers +y.
    let (x, y) = fk_2r(1.0, 1.0, 0.0, std::f64::consts::FRAC_PI_2);
    assert!((x - 1.0).abs() < 1e-12 && (y - 1.0).abs() < 1e-12);
    println!("OK");
}

Le deuxième cas vérifie le terme θ1+θ2 : avec θ2=π/2 le second segment part perpendiculairement au premier, d'où (1, 1).

Exercice 2 — Matrice de rotation Rz en 4×4

Écrivez rot_z(theta) -> Matrix4<f64> construisant la rotation homogène autour de z (translation nulle). Vérifiez que rot_z(PI) envoie (1, 0, 0, 1) sur (−1, 0, 0, 1).

Voir la solution 
use nalgebra::{Matrix4, Vector4};

pub fn rot_z(theta: f64) -> Matrix4<f64> {
    let (s, c) = theta.sin_cos();
    Matrix4::new(
        c,  -s,  0.0, 0.0,
        s,   c,  0.0, 0.0,
        0.0, 0.0, 1.0, 0.0,
        0.0, 0.0, 0.0, 1.0,
    )
}

fn main() {
    let p = Vector4::new(1.0, 0.0, 0.0, 1.0);
    let q = rot_z(std::f64::consts::PI) * p;
    assert!((q.x + 1.0).abs() < 1e-12);
    assert!(q.y.abs() < 1e-12);
    println!("Rz OK");
}

Attention au signe : c'est −sin θ en position (0,1) et +sin θ en position (1,0). L'inverser produit une rotation dans le mauvais sens (équivalente à Rz(−θ)).

Exercice 3 — Écrire dh_transform

Implémentez dh_transform(theta, d, a, alpha) -> Matrix4<f64> selon la matrice DH standard. Vérifiez le cas dégénéré d=a=α=0 : on doit retrouver exactement Rz(θ) homogène.

Voir la solution 
use nalgebra::Matrix4;

pub fn dh_transform(theta: f64, d: f64, a: f64, alpha: f64) -> Matrix4<f64> {
    let (st, ct) = theta.sin_cos();
    let (sa, ca) = alpha.sin_cos();
    Matrix4::new(
        ct, -st * ca,  st * sa, a * ct,
        st,  ct * ca, -ct * sa, a * st,
        0.0,      sa,       ca,      d,
        0.0,     0.0,      0.0,    1.0,
    )
}

fn main() {
    let t = dh_transform(0.7, 0.0, 0.0, 0.0);
    // Avec d=a=alpha=0, la matrice DH se réduit à Rz(theta).
    let (s, c) = 0.7_f64.sin_cos();
    assert!((t[(0, 0)] - c).abs() < 1e-12);
    assert!((t[(0, 1)] + s).abs() < 1e-12);
    assert!((t[(2, 2)] - 1.0).abs() < 1e-12);
    println!("dh_transform OK");
}

Le facteur cos α / sin α couple les colonnes 2 et 3 de la partie rotation : c'est la torsion qui fait sortir le mécanisme du plan. Avec α=0 ce couplage disparaît et l'on reste dans un mouvement planaire.

Exercice 4 — Pose d'un bras 3R et extraction de (x, y)

Toujours dans le plan, ajoutez un troisième segment L3. En réutilisant fk et dh_transform, calculez la pose complète pour L=(1, 0.5, 0.3) et θ=(30°, 45°, −20°), puis extrayez (x, y) et l'orientation planaire φ = θ1+θ2+θ3. Comparez à la formule fermée.

Voir la solution 
use nalgebra::Matrix4;

// on réutilise ParamDH, dh_transform, fk et position vus plus haut.

fn main() {
    let l = [1.0_f64, 0.5, 0.3];
    let th = [30.0_f64.to_radians(), 45.0_f64.to_radians(), (-20.0_f64).to_radians()];

    let params = [
        ParamDH { theta: 0.0, d: 0.0, a: l[0], alpha: 0.0 },
        ParamDH { theta: 0.0, d: 0.0, a: l[1], alpha: 0.0 },
        ParamDH { theta: 0.0, d: 0.0, a: l[2], alpha: 0.0 },
    ];

    let t = fk(&th, &params);
    let p = position(&t);

    // Orientation planaire : angle de l'axe x du repère effecteur.
    let phi = t[(1, 0)].atan2(t[(0, 0)]);

    // Formule fermée pour un bras planaire nR (somme cumulative des angles).
    let (mut xc, mut yc, mut acc) = (0.0, 0.0, 0.0);
    for i in 0..3 {
        acc += th[i];
        xc += l[i] * acc.cos();
        yc += l[i] * acc.sin();
    }

    println!("x = {:.6}, y = {:.6}, phi = {:.6} rad", p.x, p.y, phi);
    assert!((p.x - xc).abs() < 1e-9);
    assert!((p.y - yc).abs() < 1e-9);
    assert!((phi - (th[0] + th[1] + th[2])).abs() < 1e-9);
    println!("FK 3R validee ✔");
}

L'orientation se lit dans la partie rotation de 0T3 : pour un mécanisme planaire, atan2(r21, r11) redonne exactement la somme des angles. La formule fermée nR se généralise via la somme cumulative Σi Li cos(θ1+…+θi).

Récapitulatif