Guides pratiques › Construire des robots

Guides pratiques

Robots : ROS 2, Gazebo, MuJoCo et procédés

archec transforme le modèle d'un robot en logiciel qui tourne et en monde simulé. Le logiciel tourne sous forme de nœuds ROS 2. Le monde tourne dans Gazebo ou MuJoCo. Le procédé (plant) est l'objet physique piloté : un bras, un chariot, un drone. La simulation le fait tourner à côté des contrôleurs du modèle.

Un premier robot#

Un seul segment de bras, articulé à 1,5 m du sol :

component Link "Arm link" {
  physical {
    mass 1kg
    shape capsule 0.04m, 0.5m at 0, 0, -0.25m
  }
}

system Arm {
  part upper: Link {
    joint hinge y at 0, 0, 1.5m range -2..2 = 0.3
  }
}
Diagramme : Arm
Ce qu’archec dessine depuis ce code : Arm
Essayer

Générez son monde MuJoCo :

archec gen main.arche --target mujoco -o out
  out/mujoco/arm.xml
  out/mujoco/plant_bridge.py

arm.xml est le MJCF, le fichier de modèle de MuJoCo :

<mujoco model="Arm">
  <compiler angle="radian" autolimits="true"/>
  <option timestep="0.001"/>
  <worldbody>
    <light pos="0 0 4" dir="0 0 -1"/>
    <!-- upper: Link -->
    <body name="upper" pos="0 0 1.5">
      <joint name="upper" type="hinge" axis="0 1 0" range="-2 2"/>
      <geom type="capsule" size="0.04 0.25" pos="0 0 -0.25" mass="1"/>
    </body>
  </worldbody>
  <keyframe>
    <key name="initial" qpos="0.3"/>
  </keyframe>
</mujoco>

plant_bridge.py le fait tourner. Il faut Python avec le paquet mujoco. La section MuJoCo indique ce que devient chaque mot, et Procédés propose quatre robots prêts à l'emploi.

ROS 2#

Un déploiement qui déclare packaging ros2 fait tourner ses binaires Linux comme des nœuds ROS 2 :

archec gen main.arche --target ros2 --deployment VehicleRos2

La commande écrit deux choses :

  • ros2/ : les messages, sous forme du paquet d'interfaces <project>_msgs ;
  • ros2_nodes/ : un paquet ament_cmake par binaire placé sur un processeur Linux. Il contient un exécutable rclcpp par processus (ou un seul pour le binaire), et un nœud par part que le binaire exécute.

Chaque nœud enveloppe le module C de la part (--target c : sa machine à états et ses fonctions). Il donne à la part :

  • un publisher par port et par message envoyé. Sa fonction d'envoi C convertit le message et le publie.
  • une souscription par lien qui arrive à elle. Un message déclenche la machine à états, appelle le handler du port avec les champs du message de mêmes noms, puis appelle son on-receive.
  • un service par opération d'une interface que le port provides, servi par la fonction de même nom. Un port qui requires une interface reçoit un client.
  • un timer sur le temps ROS, qui appelle le step d'un thread périodique. Avec use_sim_time, le temps ROS est l'horloge du simulateur.
  • des paramètres, issus des prop de la définition et de la part.

Threads. Chaque thread du modèle est un std::thread avec son propre exécuteur. Sous Linux, il reçoit sa policy, sa priority et son core.

Topics. Un topic s'appelle <part>/<port>. Quand le port envoie plusieurs messages, /<message> s'ajoute à la suite. Un topic sur le lien ou le port remplace ce nom.

QoS (la qualité de service de ROS 2) : elle vient du lien et du port qui reçoit.

  • buffer sample garde 1 message ; depth n en garde n.
  • reliability est reprise telle quelle.
  • La period du message devient l'échéance (deadline) QoS des deux côtés, qui restent ainsi compatibles.
  • Le deadline ou le stale-after du lien devient la durée de vie (lifespan).

Lancement. ros2_nodes/<deployment>_bringup lance tous les exécutables dans l'espace de noms du déploiement.

Votre code. Il se place dans les régions USER CODE des nœuds, que archec gen conserve (régénération). Ces régions sont : les includes, la fin du constructeur de chaque nœud, ses membres, main avant le démarrage des threads, et le CMakeLists.

Les binaires sur RTOS restent du firmware. Le générateur liste les topics qui ont besoin d'une passerelle (CAN), et ceux qu'aucun nœud ne publie (capteurs, ou simulateur).

Gazebo#

archec gen main.arche --target gazebo place le procédé du déploiement dans Gazebo (gz sim, Harmonic). Ses nœuds tournent alors face à un véhicule simulé. Le dossier de sortie gazebo/ est un paquet ament. Il contient :

  • worlds/<system>.sdf : un squelette, à compléter par vous. Il n'est écrit qu'une fois.
    • Il contient un monde : la physique au pas solver des parts, un sol et un soleil.
    • Il contient un modèle du système : un base_link, et un link par part qui est un capteur, un actionneur commandé par les nœuds, ou qui a une mass.
    • Chaque link porte le nom de sa part et est fixé à la base. Sa pose est un TODO.
    • Un capteur devient le capteur Gazebo correspondant, s'il existe : un gpu_lidar (sa prop channels donne le nombre de nappes), une camera, une imu ou un navsat. Il tourne à la cadence de ce qu'il envoie.
  • config/bridge.yaml, pour ros_gz_bridge. Il relaie /clock, et chaque capteur vers son type ROS standard (PointCloud2, Image, Imu, NavSatFix). Chaque champ flottant d'une commande part vers un topic gz.msgs.Double de son link, qu'un plugin applique.
  • scripts/sim_adapter.py (rclpy, écrit une seule fois). Il traduit les messages de Gazebo en messages du modèle, sur les topics auxquels les nœuds sont abonnés, avec l'échéance qu'ils demandent.
    • Il remplit ce qu'il peut : horodatages, numéros de séquence, tailles d'image et de nuage, retour lidar le plus proche, latitude et longitude.
    • Il laisse un TODO pour le reste.
    • Il transmet aussi à Gazebo les commandes des nœuds.
  • launch/<system>_sim.launch.py. Il démarre gz sim avec le monde, la passerelle, l'adaptateur, et le fichier de lancement du déploiement avec use_sim_time.

MuJoCo#

archec gen main.arche --target mujoco place le procédé d'un système dans MuJoCo. Il lit ce que les parts disent de leur corps, comme dans Un premier robot.

shape, dans physical { } :

  • box x, y, z (dimensions complètes), sphere r, capsule r, length ou cylinder r, length (selon z) ;
  • at x, y, z : la position de son centre, dans le repère de la part ;
  • rot : son orientation à cet endroit. rot y 90deg donne une capsule selon x.

joint, sur une part : comment la part bouge dans la part qui la contient. Pour une part de premier niveau, c'est le monde.

  • Le type : hinge ou slide selon x, y, z (ou -x, ou [x, y, z]), ball, free, ou fixed (par défaut).
  • at x, y, z : la position de son repère, dans le repère du parent.
  • rot : son orientation à cet endroit. Écrivez roulis, tangage, lacet (rot 0, 0, 90deg) ou un seul axe (rot z 90deg). Elle devient le quat du corps. L'axe de l'articulation, la forme et les parts contenues sont exprimés dans le repère tourné.
  • range lo..hi : ses limites, en rad ou en m.
  • = value : sa position de départ.

Ce qui est écrit#

Le dossier de sortie mujoco/ est réécrit à chaque fois. Il contient <system>.xml et plant_bridge.py.

<system>.xml, le MJCF :

  • Une part qui a une mass ou une shape est un corps, placé dans le corps le plus proche qui l'entoure. Sa geom porte la masse de la part ; MuJoCo en déduit l'inertie. Une part avec une masse et sans forme est une boîte de 0,1 m.

  • Une part kind sensor est un capteur du corps qui la contient, sur un site placé à son joint at. Son nom indique le capteur :

    • imu : un accéléromètre et un gyromètre ;
    • encoder ou angle : jointpos ; velocity : jointvel ;
    • force, torque, touch, range, gps ;
    • attitude ou ahrs : son orientation, un quaternion w, x, y, z.

    Avec un autre nom, c'est l'unité de ce qu'il envoie qui décide : rad, rad/s, m/s², N, N·m.

  • Une part kind actuator entraîne la charnière ou la glissière du corps qui la contient.

    • C'est un motor, ou un servo choisi selon l'unité du champ qui le commande : position (rad, m) ou velocity (rad/s, m/s).
    • Le range du champ est sa plage de commande. prop kp et prop kv sont ses gains.
    • Sur un corps joint free (un drone), c'est un propulseur : un moteur sur un site placé à son joint at. Il pousse selon z, ou selon l'axe de son joint slide. Il fait aussi tourner le corps en sens inverse autour de cet axe, de prop reaction N·m par N (la traînée d'une hélice).
  • Le pas de temps est le pas solver des parts, 1 ms par défaut.

  • Une keyframe contient les valeurs de départ des articulations.

  • Les props d'une partie corps disent comment elle touche et comment son articulation bouge :

    • prop friction = 1.2 : le frottement de glissement de sa geom. Un modèle qui en a un utilise des cônes de frottement elliptiques et impratio="10", comme MuJoCo le conseille pour les préhensions.
    • prop damping = 0.02, prop armature = 0.002 (l'inertie du moteur vue à l'articulation), prop frictionloss = 0.05 (frottement sec, N·m ou N) : ceux de son articulation.
    • prop couple = {joint: cap, ratio: 0.0005} : une égalité. La position de cette articulation vaut ratio × celle de l'autre : un filetage, un bouchon qui monte de 0,5 mm par rad qu'il tourne. L'autre articulation est nommée par son chemin, ou depuis cette partie. Un ratio inférieur à 1 est écrit dans l'autre sens, pour que MuJoCo le tienne avec raideur.

plant_bridge.py fait tourner le procédé. Un message de commande fixe le data.ctrl de son actionneur dès son arrivée. Le message de chaque capteur est rempli depuis data.sensordata à la period du message : ses champs flottants dans l'ordre, et le temps dans un champ stamp. Il fonctionne de quatre façons :

  • --stdio : en pas à pas synchronisé avec archec sim --plant mujoco/plant_bridge.py, qui fait tourner le reste du modèle (ses contrôleurs) face au procédé.
    • Le protocole est en lignes JSON, avec les événements send du simulateur.
    • À chaque period, les messages du procédé vont aux parts reliées aux capteurs. Le simulateur exécute une période. Ce qu'il a envoyé aux actionneurs repart vers le procédé.
    • MUJOCO_PYTHON désigne le Python qui dispose de mujoco. Sans lui, python3 est utilisé.
  • --csv out.csv [--control seq.csv] [--for 2s] : une séquence de commandes fixe en entrée, les mesures en sortie, en CSV. Le fichier de commandes a une ligne par instant en secondes et une colonne par actionneur.
  • --viewer : le visualiseur de MuJoCo, s'il est disponible. Sous macOS, lancez-le avec mjpython.
  • --ros2 [--namespace NS] [--rate 1] [--for T] : un nœud ROS 2 (rclpy), pour les nœuds de archec gen --target ros2.
    • Le message de chaque capteur est publié sur le topic de son port (cart/encoder/position, ou le topic du lien ou du port), à sa period. C'est aussi son échéance QoS, comme le demandent les nœuds.
    • La commande de chaque actionneur est lue sur le topic de son émetteur, au fil de l'eau, avec les types de <project>_msgs.
    • /clock est le temps du procédé, pour le use_sim_time des nœuds.
    • L'espace de noms est celui du déploiement (packaging ros2).
    • --rate 0 tourne aussi vite que possible ; --rate 1 suit l'horloge murale.
    • Sans --for, il tourne jusqu'à ce qu'on l'arrête.
    • Il faut un ROS 2 sourcé (rclpy, rosgraph_msgs) et le paquet <project>_msgs compilé.
$ archec gen examples/arm/main.arche --target mujoco -o out
$ archec sim examples/arm/main.arche --for 3s --plant out/mujoco/plant_bridge.py

Vérifications#

archec check signale ce qui ne se lit pas (body). Dans un système dont les parts déclarent leur shape ou leur joint, il signale aussi ce que MuJoCo ne peut pas placer (body-unmapped) :

  • une masse sans forme ;
  • un capteur dont il n'a pas de modèle ;
  • un actionneur sans articulation à entraîner.

Capteurs, actionneurs et keyframes#

La façon dont les capteurs mesurent et dont les actionneurs agissent est décrite dans le modèle (spécification). Dans le MJCF :

  • le noise d'un capteur est son σ. La passerelle tire le bruit à partir de --seed, avec le même générateur que archec sim.
  • le range de son champ est le cutoff de MuJoCo ; la latency de son port est le delay.
  • la plage de commande d'un actionneur est ctrlrange, sa force-range est forcerange, sa latency est delay.
  • un actionneur qui a un gain ou un lag est un actionneur <general> (gainprm, dyntype filterexact).

Chaque keyframe du système devient une <key> (voir keyframes). Elle contient les positions de ses charnières, glissières et rotules, les positions et orientations de ses corps libres, et leurs vitesses (qpos, qvel). plant_bridge.py --keyframe K part de là, tout comme archec sim --plant … --keyframe K.

Procédés#

Quatre procédés sont prêts à l'emploi. Chacun est un projet de départ :

archec new my-arm --template two-link-arm

Dans un store, ils forment la collection plants. Chacun contient :

  • un procédé dans MuJoCo : la mass, la shape et le joint de ses parts, ses capteurs et actionneurs ;
  • un contrôleur écrit dans le langage d'action ;
  • le système qui les relie ;
  • des exigences, avec une phrase EARS ou une propriété formal ;
  • un plan de test et une fiche.

Le plan de test s'exécute face au procédé :

archec gen main.arche --target mujoco -o out
archec sim main.arche --plant out/mujoco/plant_bridge.py --test
Modèle Le procédé Le contrôleur Mesuré dans MuJoCo 4.0
two-link-arm deux segments articulés, un moteur en couple et un codeur sur chacun compensation de gravité et PD à moins de 0,02 rad de sa pose à 0,78 s
cart-pole un chariot sur un rail, un pendule lesté, écarté de 0,1 rad de la verticale LQR, son gain issu de archec physics --linearize --lqr le pendule à moins de 0,1 rad, vertical à 0,02 rad près à 1,08 s
diff-drive-rover un châssis libre, deux roues, une roulette, des codeurs, une IMU, un GPS odométrie, boucles de cap et de vitesse via trois points de passage au dernier à 10,4 s, à 0,073 m au plus
quadrotor un corps libre, quatre propulseurs avec leur couple de traînée, IMU, attitude, position boucles de position, d'attitude et d'altitude, le mélangeur une montée de 0,5 m à 0,05 m près en 0,4 s

examples/cart-pole-mpc équilibre le même pendule sur chariot avec une commande prédictive (mpc.ts).

  • archec physics --linearize --lqr donne le pendule sur chariot linéarisé (A, B), et le coût à venir P du LQR, utilisé comme fin d'horizon.
  • Toutes les 5 ms, le contrôleur planifie les forces des 0,1 s suivantes, dans ±3 N, et envoie la première.
  • Son plan de test passe face à MuJoCo. Avec une simple limite de force, un LQR saturé y fait à peu près aussi bien.

Limites#

  • La QoS de ROS 2 n'a pas de budget de latence : la latency d'un lien n'est donc pas transmise aux nœuds.
  • Les binaires sur RTOS ne sont pas des nœuds ROS 2. Les topics qui leur demandent une passerelle (CAN) sont seulement listés.
  • Dans Gazebo, les formes, les poses, les articulations et la dynamique du véhicule ne sont pas tirées du modèle : c'est à vous de les écrire dans le fichier du monde.
  • Un capteur que Gazebo ne simule pas (un radar) n'est qu'une note.
  • CARLA n'est pas généré. Sa passerelle ROS 2 publie les mêmes types standard : l'adaptateur peut donc lire ses topics à la place de ceux de Gazebo.
  • Dans MuJoCo, le sol et les couleurs ne figurent pas dans le modèle. Des contacts, seul le frottement de glissement d'une geom y est (prop friction) : chaque geom touche toutes les autres sauf celle de son parent, comme MuJoCo le fait par défaut.