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
}
}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
handlerdu port avec les champs du message de mêmes noms, puis appelle sonon-receive. - un service par opération d'une interface que le port
provides, servi par la fonction de même nom. Un port quirequiresune interface reçoit un client. - un timer sur le temps ROS, qui appelle le
stepd'un thread périodique. Avecuse_sim_time, le temps ROS est l'horloge du simulateur. - des paramètres, issus des
propde 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 samplegarde 1 message ;depth nen garde n.reliabilityest reprise telle quelle.- La
perioddu message devient l'échéance (deadline) QoS des deux côtés, qui restent ainsi compatibles. - Le
deadlineou lestale-afterdu 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
solverdes 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 unemass. - 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 propchannelsdonne le nombre de nappes), unecamera, uneimuou unnavsat. Il tourne à la cadence de ce qu'il envoie.
- Il contient un monde : la physique au pas
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 topicgz.msgs.Doublede 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 avecuse_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, lengthoucylinder 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 90degdonne 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 :
hingeouslideselonx,y,z(ou-x, ou[x, y, z]),ball,free, oufixed(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 lequatdu 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
massou uneshapeest 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 sensorest un capteur du corps qui la contient, sur un site placé à sonjoint 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 actuatorentraî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) ouvelocity(rad/s, m/s). - Le
rangedu champ est sa plage de commande.prop kpetprop kvsont ses gains. - Sur un corps
joint free(un drone), c'est un propulseur : un moteur sur un site placé à sonjoint at. Il pousse selon z, ou selon l'axe de sonjoint slide. Il fait aussi tourner le corps en sens inverse autour de cet axe, deprop reactionN·m par N (la traînée d'une hélice).
- C'est un
Le pas de temps est le pas
solverdes 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 etimpratio="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é avecarchec 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
senddu 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_PYTHONdésigne le Python qui dispose demujoco. Sans lui,python3est utilisé.
- Le protocole est en lignes JSON, avec les événements
--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 avecmjpython.--ros2 [--namespace NS] [--rate 1] [--for T]: un nœud ROS 2 (rclpy), pour les nœuds dearchec gen --target ros2.- Le message de chaque capteur est publié sur le topic de son port
(
cart/encoder/position, ou letopicdu lien ou du port), à saperiod. 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. /clockest le temps du procédé, pour leuse_sim_timedes nœuds.- L'espace de noms est celui du déploiement (
packaging ros2). --rate 0tourne aussi vite que possible ;--rate 1suit 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>_msgscompilé.
- Le message de chaque capteur est publié sur le topic de son port
(
$ 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
noised'un capteur est son σ. La passerelle tire le bruit à partir de--seed, avec le même générateur quearchec sim. - le
rangede son champ est lecutoffde MuJoCo ; lalatencyde son port est ledelay. - la plage de commande d'un actionneur est
ctrlrange, saforce-rangeestforcerange, salatencyestdelay. - un actionneur qui a un
gainou unlagest 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, lashapeet lejointde 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 --lqrdonne 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
latencyd'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.