Salut tout le monde !

Alors, suite de notre aventure dans le monde merveilleux (et parfois un peu tordu) de la robotique avec mon projet Robo-Pointer Visuel. Dans le premier article, on avait laissé notre bras robot SO-100 : il arrivait à suivre un objet rouge de gauche à droite (contrôle "Pan"). Petite victoire, mais grosse satisfaction !

Sauf que... un objet, ça ne fait pas que bouger de gauche à droite. Ça monte, ça descend. Et là, notre contrôle simpliste ne suffisait plus du tout.

Le Nouveau Défi : Coordonner le Mouvement Vertical

Imaginez : la caméra est sur le "poignet" du robot. Si je veux qu'elle suive un objet qui monte, je ne peux pas juste dire au moteur de l'épaule (ID 2) de se lever un peu. Si je fais ça, le poignet va décrire un arc de cercle, et la caméra va non seulement monter, mais aussi s'avancer ou reculer. Pas top pour garder la cible au centre !

Pour un mouvement vertical plus contrôlé, il faut être plus malin. Il faut jouer sur deux articulations en même temps : l'épaule (ID 2, qui lève le premier segment du bras) et le coude (ID 3, qui plie le deuxième segment). C'est en combinant finement leurs mouvements qu'on peut espérer positionner le poignet (et donc la caméra) à la bonne hauteur tout en restant (à peu près) à la même distance horizontale.

Et c'est là qu'on met les pieds dans un plat fondamental de la robotique : la cinématique. Derrière ce mot un peu barbare, il y a deux questions essentielles :

Cinématique Directe (FK - Forward Kinematics) : Facile à comprendre. Si je connais les angles de mes moteurs (épaule et coude), où exactement se trouve mon poignet dans l'espace (ses coordonnées X et Y) ?
Cinématique Inverse (IK - Inverse Kinematics) : Le problème inverse, et souvent celui qui nous intéresse le plus pour le contrôle. Si je sais où je veux que mon poignet aille (une cible X, Y dans le plan vertical), quels angles je dois donner à mes moteurs d'épaule et de coude pour qu'il y arrive ?

Pour mon projet, j'avais clairement besoin de l'IK pour calculer les commandes moteurs. Mais comme souvent, pour bien comprendre et valider l'IK, il faut d'abord maîtriser la FK.

Étape 1 : La FK - Juste Savoir où On Est (Théoriquement)

Commençons par le plus simple. Pour mon bras SO-100, je me suis concentré sur les deux moteurs qui gèrent le plan vertical : ID 2 (épaule, angle theta1) et ID 3 (coude, angle theta2). Première mission : mesurer les longueurs des bras sur le modèle 3D. J'ai trouvé :

L1 (épaule -> coude) : 0.116 m
L2 (coude -> poignet) : 0.135 m

Avec ces deux longueurs et les deux angles, c'est juste un peu de trigonométrie de base (souvenirs de lycée !) pour trouver la position du poignet (Xw, Yw). On utilise les cosinus et les sinus pour projeter les longueurs sur les axes X et Y.

J'ai mis ça dans une fonction Python calculate_fk_wrist dans un fichier kinematics.py. En gros, tu lui donnes les angles en degrés, elle te sort les coordonnées X, Y en mètres. Plutôt direct, et ça permet déjà de visualiser la zone de travail du robot.

# Extrait de kinematics.py
import math

L1 = 0.116
L2 = 0.135

def calculate_fk_wrist(theta1_deg, theta2_deg):
    """ Calcule la position du poignet (X, Y) à partir des angles d'épaule et coude. """
    theta1_rad = math.radians(theta1_deg)
    theta2_rad = math.radians(theta2_deg) # Angle interne coude

    # Position Poignet = Position Coude + Projection L2
    wrist_x = L1 * math.cos(theta1_rad) + L2 * math.cos(theta1_rad + theta2_rad)
    wrist_y = L1 * math.sin(theta1_rad) + L2 * math.sin(theta1_rad + theta2_rad)
    return wrist_x, wrist_y

Étape 2 : L'IK - Le Cerveau de la Manœuvre (Calculer les Angles)

Maintenant, le cœur du problème : on a une cible (Xw, Yw) où on veut positionner le poignet. Comment trouver les bons angles theta1 (épaule) et theta2 (coude) ? C'est l'IK.

Pour un bras simple comme celui-ci (2 degrés de liberté dans un plan), l'approche la plus classique est géométrique. On dessine un triangle :

Sommet A : La base du robot (l'axe de l'épaule, qu'on place à l'origine 0,0).
Sommet B : L'axe du coude.
Sommet C : Le poignet cible (notre fameux Xw, Yw).

On connaît les longueurs des côtés AB (c'est L1) et BC (c'est L2). Le troisième côté, AC, c'est la distance directe entre la base et la cible, qu'on appelle D.

Le plan d'attaque se déroule comme ça :

Calculer D : Un coup de Pythagore et c'est réglé : D = sqrt(Xw**2 + Yw**2).
Atteignable ou Pas ? Le bras a une portée limitée. Si la distance D est plus petite que abs(L1 - L2) (distance min) ou plus grande que L1 + L2 (distance max), on ne peut pas atteindre la cible. Important de vérifier ça en premier !
Trouver l'Angle du Coude (theta2) : On utilise la fameuse Loi des Cosinus (ou Al-Kashi) dans notre triangle (L1, L2, D). Elle nous permet de trouver l'angle interne au niveau du coude, appelons-le gamma. Petite subtilité : la convention habituelle en robotique, c'est que theta2 = 0 quand le bras est tendu. Notre gamma fait l'inverse (il vaut 180° quand le bras est tendu). Donc, on pose simplement theta2 = 180° - gamma (ou pi - gamma en radians).
Trouver l'Angle de l'Épaule (theta1) : Là, il faut être un peu astucieux. On calcule d'abord alpha (α) : c'est l'angle que fait la ligne directe Base-Poignet (le segment D) avec l'axe X horizontal. La fonction atan2(Yw, Xw) est parfaite pour ça, car elle gère tous les quadrants et les signes de Xw/Yw pour donner le bon angle entre -180° et +180°. Ensuite, on calcule beta (β) : c'est l'angle entre le premier bras (L1) et cette même ligne Base-Poignet (D). On le trouve encore avec la Loi des Cosinus dans le même triangle (L1, L2, D). Finalement, l'angle de l'épaule theta1 est juste la différence entre l'angle total alpha et la partie beta : theta1 = alpha - beta.

Ça peut paraître un peu abstrait, mais une fois posé avec un dessin, c'est assez logique. J'ai codé tout ça dans une fonction calculate_ik(target_xw, target_yw) qui me renvoie les angles theta1, theta2 (en radians) et un petit booléen pour me dire si le calcul a réussi et si la cible était atteignable.

Étape 3 : Test Unitaire - La Vérification Indispensable

Ok, on a des fonctions FK et IK. Mais comment être sûr qu'elles ne racontent pas n'importe quoi avant de les envoyer au vrai robot ?

L'astuce classique : le "round-trip check". C'est simple et efficace :

Je prends des angles theta1, theta2 au pif (ou des angles connus).
J'utilise ma fonction FK pour savoir où le poignet devrait être : (X, Y) = calculate_fk_wrist(theta1, theta2).
Je donne cette position (X, Y) à ma fonction IK : (res_theta1, res_theta2, success) = calculate_ik(X, Y).
Je vérifie si res_theta1 est (presque) égal à theta1 et res_theta2 (presque) égal à theta2.

Si ça marche pour plein d'angles différents, ça veut dire que mes fonctions FK et IK sont cohérentes entre elles et calculent probablement juste. J'ai mis tous ces tests dans un fichier séparé kinematics.py, dans un bloc spécial if __name__ == "__main__": qui ne s'exécute que si je lance ce fichier directement. Super pratique pour tester cette logique indépendamment de tout le bazar ROS.

# L'idée du test dans kinematics.py
if __name__ == "__main__":
    print("Test IK et vérification FK...")
    # Définir quelques cas de test
    theta1_test_deg = 30.0
    theta2_test_deg = 60.0

    # 1. Calculer la position cible via FK
    target_x, target_y = calculate_fk_wrist(theta1_test_deg, theta2_test_deg)
    print(f"Angles testés: ({theta1_test_deg:.1f}°, {theta2_test_deg:.1f}°) -> Position FK: ({target_x:.4f}, {target_y:.4f})")

    # 2. Calculer les angles via IK à partir de la position FK
    ik_theta1_rad, ik_theta2_rad, ik_success = calculate_ik(target_x, target_y)

    # 3. Vérifier si IK a réussi et si les angles correspondent
    if ik_success:
        ik_theta1_deg = math.degrees(ik_theta1_rad)
        ik_theta2_deg = math.degrees(ik_theta2_rad)
        print(f"IK a réussi -> Angles trouvés: ({ik_theta1_deg:.1f}°, {ik_theta2_deg:.1f}°)")
        # Ajouter une petite vérification de proximité (tolérance)
        if abs(ik_theta1_deg - theta1_test_deg) < 0.1 and abs(ik_theta2_deg - theta2_test_deg) < 0.1:
            print("--> Vérification OK : Angles IK correspondent aux angles initiaux !")
        else:
            print("--> ATTENTION : Les angles IK diffèrent légèrement des angles initiaux !")
    else:
        print("ERREUR : IK a échoué pour une position qui aurait dû être atteignable !")

Conclusion (de cette étape !)

Voilà pour le petit tour dans le monde de la cinématique pour mon bras 2-DOF. Comprendre et réussir à coder la FK et l'IK, même si c'est "juste" en théorie et dans un script de test, c'était déjà une étape importante et vraiment intéressante. Ça donne les outils mathématiques pour espérer un contrôle plus intelligent.

Maintenant, la grande question : est-ce que cette belle théorie va survivre à la confrontation avec le monde réel ? C'est-à-dire, est-ce que l'intégration dans ROS 2 et la communication avec les moteurs vont bien se passer ? Spoiler : pas tout à fait du premier coup ! 😉 Mais ça, ce sera l'objet du prochain article de blog !

(N'hésitez pas à laisser un commentaire si vous avez des questions ou vos propres astuces sur la cinématique !)

Pourquoi Lever le Bras c'est Plus Compliqué que de Tourner la Tête (Plongée dans la Cinématique FK/IK)