gwen.works
+38
bib.yaml
+38
bib.yaml
···
779
value: https://github.com/unitreerobotics/unitree_mujoco
780
date: '2021-11-01'
781
782
+
unitree_ros2:
783
+
type: repository
784
+
title: unitree_ros2
785
+
publisher: GitHub
786
+
author: Unitree Robotics
787
+
url:
788
+
date: '2025-10-22'
789
+
value: https://github.com/unitreerobotics/unitree_ros2
790
+
date: '2023-10-20'
791
+
792
+
cyclonedds-helloworld:
793
+
type: web
794
+
title: HelloWorld source code — Eclipse Cyclone DDS, 0.11.0
795
+
url:
796
+
date: '2025-10-22'
797
+
value: https://cyclonedds.io/docs/cyclonedds/latest/getting_started/helloworld/helloworld_source_code.html
798
+
799
+
sdf-plugin:
800
+
type: web
801
+
title: SDFormat Specification, world.plugin element
802
+
url:
803
+
date: '2025-10-22'
804
+
value: 'http://sdformat.org/spec?ver=1.12&elem=world#world_plugin'
805
+
806
+
sdf-plugin-filename:
807
+
type: web
808
+
title: SDFormat Specification, world.plugin.filename element
809
+
url:
810
+
date: '2025-10-22'
811
+
value: 'http://sdformat.org/spec?ver=1.12&elem=world#plugin_filename'
812
+
813
+
gz-system-plugin-path:
814
+
type: web
815
+
title: 'Gazebo Sim: Finding resources'
816
+
url:
817
+
date: '2025-10-22'
818
+
value: https://gazebosim.org/api/sim/8/resources.html#:~:text=All%20paths%20on%20the%20GZ_SIM_SYSTEM_PLUGIN_PATH%20environment%20variable
819
+
gepetto.png
gepetto.png
This is a binary file and will not be displayed.
+398
-3
rapport/gz-unitree.typ
+398
-3
rapport/gz-unitree.typ
···
1
En se basant sur _unitree\_mujoco_, il a donc été possible de réaliser un bridge pour Gazebo.
2
3
== Établissement du contact
4
5
-
== Réception des commandes
6
7
-
== Émission de l'état
8
9
== Essai sur des politiques réelles
10
11
-
== Amélioration des performances
12
13
== Enregistrement de vidéos
14
···
1
+
#import "@preview/zebraw:0.5.5"
2
+
#import "@preview/fletcher:0.5.8": diagram, node, edge
3
+
#import "./utils.typ": dontbreak
4
+
#show figure: set block(spacing: 2em)
5
+
#let zebraw = (..args) => zebraw.zebraw(lang: false, background-color: luma(255).opacify(0%), ..args)
6
+
7
En se basant sur _unitree\_mujoco_, il a donc été possible de réaliser un bridge pour Gazebo.
8
9
== Établissement du contact
10
11
+
Une première tentative a été de suivre la documentation de CycloneDDS pour écouter sur le canal @cyclonedds-helloworld `rt/lowcmd`, en récupérant les définitions IDL des messages, disponibles sur le dépot `unitree_ros2`#footnote[`unitree_mujoco` n'avait pas encore été découvert] @unitree_ros2
12
+
13
+
On commence par importer la bibliothèque DDS et les définitions IDL de `rt/lowcmd`
14
+
15
+
```cpp
16
+
#include "messages/LowCmd_.hpp"
17
+
#include "dds/dds.h"
18
+
...
19
+
20
+
int main (int argc, char ** argv)
21
+
{
22
+
```
23
+
24
+
On initialise les différents objets permettant de lire sur un canal
25
+
26
+
```cpp
27
+
dds_entity_t participant, topic, reader;
28
+
LowCmd_ *msg;
29
+
void *samples[MAX_SAMPLES];
30
+
...
31
+
32
+
participant = dds_create_participant(DDS_DOMAIN_DEFAULT, NULL, NULL);
33
+
34
+
topic = dds_create_topic(participant, &LowCmd__desc, "HelloWorldData_Msg", NULL, NULL);
35
+
36
+
qos = dds_create_qos();
37
+
dds_qset_reliability(qos, DDS_RELIABILITY_RELIABLE, DDS_SECS (10));
38
+
39
+
reader = dds_create_reader(participant, topic, qos, NULL);
40
+
41
+
dds_delete_qos(qos);
42
+
43
+
samples[0] = LowCmd___alloc();
44
+
```
45
+
46
+
Et on attend qu'un message arrive sur le canal, pour l'afficher
47
+
48
+
```cpp
49
+
/* Poll until data has been read. */
50
+
while (true)
51
+
{
52
+
rc = dds_read(reader, samples, infos, MAX_SAMPLES, MAX_SAMPLES);
53
+
54
+
/* Check if we read some data and it is valid. */
55
+
if ((rc > 0) && (infos[0].valid_data))
56
+
{
57
+
/* Print Message. */
58
+
msg = (LowCmd_*) samples[0];
59
+
printf("=== [Subscriber] Received : ");
60
+
fflush(stdout);
61
+
break;
62
+
}
63
+
else
64
+
{
65
+
dds_sleepfor(DDS_MSECS(20));
66
+
}
67
+
}
68
+
```
69
+
70
+
Enfin, on libère les ressources avant la terminaison du programme
71
+
72
+
```cpp
73
+
LowCmd__free(samples[0], DDS_FREE_ALL);
74
+
dds_delete(participant);
75
+
return EXIT_SUCCESS;
76
+
}
77
+
```
78
+
79
+
Malheureusement, cette solution s'est avérée infructueuse, à cause de (ce qui sera compris bien plus tard) un problème de numéro de domaine DDS.
80
+
81
+
On change d'approche en préférant plutôt utiliser les abstractions fournies par le SDK de Unitree (cf @receive-lowcmd et @send-lowstate)
82
+
83
+
84
+
Enfin, si un pare-feu est actif, il faut autoriser le traffic udp l'intervalle d'addresses IP `224.0.0.0/4`. Par exemple, avec _ufw_
85
+
86
+
```bash
87
+
sudo ufw allow in proto udp from 224.0.0.0/4
88
+
sudo ufw allow in proto udp to 224.0.0.0/4
89
+
```
90
+
91
+
92
+
93
+
== Installation du plugin dans Gazebo
94
+
95
+
Un _system plugin_ Gazebo consiste en la définition d'une classe héritant de `gz::sim::System`, ainsi que d'autres interfaces permettant notamment d'exécuter notre code avant ou après une mise à jour de l'état du simulateur (avec `gz::sim::ISystem`{`Pre`,`Post`}`Update`)
96
+
97
+
#dontbreak(
98
+
99
+
```cpp
100
+
#include <gz/sim/System.hh>
101
+
namespace gz_unitree
102
+
{
103
+
class UnitreePlugin :
104
+
public gz::sim::System,
105
+
public gz::sim::ISystemPreUpdate
106
+
{
107
+
public:
108
+
UnitreePlugin();
109
+
public:
110
+
~UnitreePlugin() override;
111
+
public:
112
+
void PreUpdate(const gz::sim::UpdateInfo &_info,
113
+
gz::sim::EntityComponentManager &_ecm) override;
114
+
};
115
+
}
116
+
```
117
+
118
+
)
119
+
120
+
Il faut ensuite implémenter la classe puis appeler une macro ajoutant le plugin à Gazebo
121
+
122
+
```cpp
123
+
#include <gz/plugin/Register.hh>
124
+
125
+
... // implementation
126
+
127
+
GZ_ADD_PLUGIN(
128
+
UnitreePlugin,
129
+
gz::sim::System,
130
+
UnitreePlugin::ISystemPreUpdate)
131
+
```
132
+
133
+
Enfin, on active le plugin en le référançant dans le fichier SDF @sdf-plugin, qui décrit l'environnement du simulateurs (objets, éclairage, etc)
134
+
135
+
#zebraw(
136
+
numbering: false,
137
+
highlight-lines: (..range(3, 5)),
138
+
```xml
139
+
<sdf version='1.11'>
140
+
<world name="default">
141
+
<plugin filename="gz-unitree" name="gz_unitree::UnitreePlugin">
142
+
</plugin>
143
+
</world>
144
+
<model name='h1_description'>
145
+
<link name='pelvis'>
146
+
<inertial>
147
+
...
148
+
```
149
+
)
150
+
151
+
Avec `filename` le chemin vers le plugin compilé, qui sera cherché dans les répertoires spécifiés par `GZ_SIM_SYSTEM_PLUGIN_PATH` @gz-system-plugin-path @sdf-plugin-filename.
152
+
153
+
154
+
== Architecture du plugin
155
+
156
+
Le plugin consiste en trois parties distinctes:
157
+
158
+
1. Le "branchement" dans les phases de Gazebo, par l'implémentation de méthodes de `gz::sim::System`
159
+
2. L'interaction avec les canaux DDS du SDK d'Unitree
160
+
3. Les données et méthodes internes au plugin
161
+
162
+
En plus de cela, il y a bien évidemment la politique de contrôle $cal(P)$, qui interagit via les canaux DDS avec le robot (qu'il soit réel, ou simulé)
163
+
164
+
#let legend = (
165
+
..descriptions
166
+
) => grid(
167
+
columns: (1fr, 3fr),
168
+
align: left,
169
+
row-gutter: 0.5em,
170
+
..descriptions.pos().map(((arrow, desc)) => (
171
+
diagram(edge((0, 0), arrow, (0.75, 0))),
172
+
desc
173
+
)).flatten()
174
+
)
175
+
176
+
#let architecture = (
177
+
caption,
178
+
group-inset: 12pt,
179
+
group-color: luma(80),
180
+
show-legend: true,
181
+
..edges
182
+
) => figure(caption: caption,
183
+
pad(
184
+
y: 10pt + group-inset,
185
+
diagram(
186
+
debug: false,
187
+
node-stroke: 0.5pt,
188
+
edge-corner-radius: 6pt,
189
+
{
190
+
191
+
if show-legend {
192
+
node((2, 4.5), stroke: none, width: 15em, legend(("--", "Message DDS"), ("@->", "Désynchronisation")))
193
+
}
194
+
195
+
let group = (nodes, label, alignment: bottom + center, name: none) => node(
196
+
name: name,
197
+
enclose: nodes,
198
+
snap: false,
199
+
inset: group-inset,
200
+
stroke: group-color.lighten(75%) + 2pt,
201
+
align(alignment, move(dy: 2 * group-inset * if alignment.y == bottom { 1 } else { -1 }, text(fill: group-color, label)))
202
+
)
203
+
204
+
let subtitled = (title, subtitle) => [#title \ #text(size: 0.8em, subtitle)]
205
+
206
+
node(name: <configure>, (0, 1), `::Configure`)
207
+
node(name: <preupdate>, (0, 2), `::PreUpdate`)
208
+
group((<configure>, <preupdate>), `gz::sim::System`, alignment: top + center)
209
+
210
+
node(name: <channelfactory>, enclose: ((1, 0), (2, 0)), inset: 8pt, subtitled(`ChannelFactory`, [domaine 1, interface `lo`]))
211
+
node(name: <publisher>, (1, 1), inset: 8pt, subtitled(`ChannelPublisher` , [canal `rt/lowstate`]))
212
+
node(name: <subscriber>, (2, 1), inset: 8pt, subtitled(`ChannelSubscriber` , [canal `rt/lowcmd`]))
213
+
group(name: <dds>, (<channelfactory>, <publisher>, <subscriber>), alignment: top+center)[SDK d'Unitree]
214
+
215
+
216
+
node(name: <lowstate>, (1, 2), `::LowStateWriter`)
217
+
node(name: <lowcmd>, (2, 2), `::CmdHandler`)
218
+
node(name: <statebuf>, (1, 3))[State buffer]
219
+
node(name: <cmdbuf>, (2, 3))[Commands buffer]
220
+
group((<lowstate>, <lowcmd>, <statebuf>, <cmdbuf>))[Plugin internals]
221
222
+
node(name: <policy>, (0, -1), $cal(P)$)
223
+
224
+
for e in edges.pos() {
225
+
e
226
+
}
227
+
}
228
+
)))
229
+
230
+
#architecture([Phase d'initialisation du plugin], show-legend: false, {
231
+
edge(<configure>, "u", <channelfactory>, "->", label-side: left, label-pos: 50%)[appelle]
232
+
edge(<channelfactory>, "->", <publisher>)[initialise]
233
+
edge(<channelfactory>, "->", <subscriber>)[initialise]
234
+
edge(<publisher>, "<->", <lowstate>)[associés]
235
+
edge(<subscriber>, "<->", <lowcmd>)[associés]
236
+
})
237
+
238
+
On commence par instancier un contrôleur dans le domaine DDS n°1, sur l'interface réseau `lo`#footnote[interface dite "loopback", qui est locale à l'ordinateur: ici, le simulateur et la politique de contrôle tournent sur la même machine, donc les messages DDS n'ont pas besoin de "sortir" de celle-ci]
239
+
240
+
On lui associe:
241
+
242
+
- Un _publisher_, chargé d'envoyer périodiquement des messages sur `rt/lowstate` en appellant la méthode `LowStateWriter`
243
+
- Un _subscriber_, chargé d'appeller la méthode `CmdHandler` avec chaque message arrivant sur `rt/lowcmd`.
244
+
245
+
Cette initialisation est faite à l'initialisation du plugin par Gazebo, en la faisant dans la méhode `::Configure` du plugin.
246
+
247
+
== Réception des commandes <receive-lowcmd>
248
+
249
+
Lorsqu'un message, publié par $cal(P)$ (1A) et contenant des ordres pour les moteurs, arrive sur `rt/lowcmd`, `::CmdHandler` est appelé (2, 3), et modifie un _buffer_ (4) contenant la dernière commande reçue.
250
+
251
+
252
+
Ensuite, Gazebo démarre un nouveau pas de simulation. Avant de faire ce pas, il appelle la méthode `::PreUpdate` sur notre plugin, qui vient chercher la commande stockée dans le _buffer_ (1B), et applique cette commande sur le modèle du robot, animé par le simulateur.
253
+
254
+
#architecture([Phase de réception des commandes], {
255
+
edge(<policy>, (2, -1), (2, 0), "-->", label-pos: 10%)[(1A) publish]
256
+
edge(<policy>, (2, -1), (2, 0), stroke: none, label-pos: 60%, label-side: left)[(1A) subscription]
257
+
edge((2, 0), <subscriber>, "->")[(2)]
258
+
edge(<subscriber>, "->", <lowcmd>)[(3)]
259
+
edge(<lowcmd>, "->", <cmdbuf>)[(4)]
260
+
// edge(<lowcmd.east>, "r,d,d,l,l,l,l,l,l,u,u,u", <preupdate>, "->", label-side: left)[(5)]
261
+
edge(<preupdate>, "d,d,r,r", <cmdbuf>, "<-@")[(1B)]
262
+
})
263
+
264
+
On notera que (1B) s'exécute _parallèlement_ au reste des étapes: la boucle de simulation de Gazebo est indépendante de la boucle de mise à jour de la politique.
265
+
266
+
267
+
/ Si `::PreUpdate` est plus fréquente: Le simulateur appliquera simplement plusieurs fois la même commande, le buffer n'ayant pas été modifié.
268
+
269
+
/ Si `::PreUpdate` est moins fréquente: Certaines commandes seront simplement ignorées par Gazebo, qui ne vera pas la valeur du buffer avant qu'il change de nouveau.
270
+
271
+
// L'initialisation du subscriber se fait pendant l'initialisation du plugin, c'est à dire dans `UnitreePlugin::Configure`. On relie la réception d'un message à une fonction, qui est ici une méthode, `UnitreePlugin::CmdHandler`.
272
+
//
273
+
// #dontbreak[
274
+
//
275
+
// ```cpp
276
+
// ...
277
+
//
278
+
// void UnitreePlugin::Configure()
279
+
// {
280
+
// ```
281
+
//
282
+
// Instanciation d'un canal
283
+
//
284
+
// ```cpp
285
+
// ChannelFactory::Instance()->Init(1, "lo" /* loopback interface */);
286
+
// ```
287
+
//
288
+
// Création de $x |-> mono("CmdHandler")(mono("this"), x)$. L'utilitaire `std::bind` permet de passer à `InitChannel` une fonction simple
289
+
//
290
+
// ```cpp
291
+
// auto handler = std::bind(
292
+
// &UnitreePlugin::CmdHandler,
293
+
// this,
294
+
// std::placeholders::_1
295
+
// )
296
+
// ```
297
+
//
298
+
// Création du subscriber
299
+
//
300
+
// ```cpp
301
+
// auto subscriber = ChannelSubscriberPtr<LowCmd_>(
302
+
// new ChannelSubscriber<LowCmd_>("rt/lowcmd")
303
+
// );
304
+
//
305
+
//
306
+
// subscriber->InitChannel(handler, 1);
307
+
// }
308
+
// ```
309
+
//
310
+
// Définition du handler
311
+
//
312
+
// ```cpp
313
+
// void UnitreePlugin::CmdHandler(const void *msg)
314
+
// {
315
+
// LowCmd_ _cmd = *(const LowCmd_ *)msg;
316
+
//
317
+
// // Remplissage du buffer interne à la classe
318
+
// MotorCommand motor_command_tmp;
319
+
// for (size_t i = 0; i < H1_NUM_MOTOR; ++i)
320
+
// {
321
+
// motor_command_tau_ff[i] = _cmd.motor_cmd()[i].tau();
322
+
// ...
323
+
// }
324
+
//
325
+
// this->motor_command_buffer.SetData(motor_command_tmp);
326
+
// }
327
+
// ```
328
+
//
329
+
// ]
330
+
331
+
332
+
== Émission de l'état <send-lowstate>
333
+
334
+
Avant de démarrer un nouveau pas de simulation, la méthode `::PreUpdate` vient mettre à jour l'état du robot simulé en modifiant le _State buffer_ interne au plugin (1A).
335
+
336
+
Le `LowStateWriter` vient lire le _State buffer_ (1B) pour publier l'état sur le canal DDS (2, 3) qui est ensuite lu par $cal(P)$ (4), qui (on le suppose) poss-de une subscription sur `rt/lowstate`
337
+
338
+
#let transparent = luma(0).opacify(0%)
339
+
340
+
341
+
#architecture([Phase d'envoi de l'état], {
342
+
edge(<preupdate>, "d,d,r", <statebuf>, "->")[(1A)]
343
+
edge(<statebuf>, "@->", <lowstate>)[(1B)]
344
+
edge(<lowstate>, "->", <publisher>)[(2)]
345
+
edge(<publisher>, "->", (1, 0))[(3)]
346
+
edge(<policy>, (1, -1), (1, 0), "<--", label-pos: 20%)[(4) subscription]
347
+
edge(<policy>, (1, -1), (1, 0), stroke: none, label-pos: 60%, label-side: left)[(4) publish]
348
+
})
349
+
350
+
351
+
Ici également, `LowStateWriter` s'exécute _en parallèle_ du code de `::PreUpdate`: En effet, la création du `ChannelPublisher` démarre une boucle qui vient éxécuter `LowStateWriter` périodiquement, dans un autre _thread_: on a donc aucune garantie de synchronisation entre les deux.
352
+
353
+
Ici, il y a en plus non pas deux, mais _trois_ boucles indépendantes qui sont en jeux:
354
+
355
+
- La boucle de simulation de Gazebo (fréquence d'appel de `::PreUpdate`),
356
+
- La boucle du `ChannelPublisher` (fréquence d'appel de `::LowStateWriter`), et
357
+
- La boucle de réception de $cal(P)$ (à quelle fréquence $cal(P)$ est-elle capable de reçevoir des messages)
358
+
359
+
360
+
Similairement à la réception de commandes:
361
+
362
+
/ Si `::PreUpdate` est plus fréquente: On perdra des états intermédiaires, la résolution temporelle de l'évolution de l'état du robot disponible pour (ou acceptable par#footnote[
363
+
En fonction de si `::LowStateWriter` est plus fréquente que $cal(P)$ (dans ce cas là, c'est ce qui est acceptable par $cal(P)$ qui est limitant) ou inversement (dans ce cas, c'est ce que la boucle du publisher met à disposition de $cal(P)$ qui est limitant)
364
+
]) $cal(P)$ sera moins grande
365
+
/ Si `::PreUpdate` est moins fréquente: $cal(P)$ reçevra plusieurs fois le même état, ce qui sera représentatif du fait que la simulation n'a pas encore avancé.
366
+
367
+
== Désynchronisations
368
+
369
+
Dans un même appel de `::PreUpdate`, on effectue d'abord la mise à jour du _State buffer_, puis on lit dans le _Commands buffer_.
370
+
371
+
Un cycle correspond donc à trois boucles indépendantes, représentées ci-après:
372
+
373
+
- Celle de la simulation (en bleu), qui doit englober l'entièreté d'un cycle
374
+
- Celle du `ChannelPublisher` (en rouge)
375
+
- Celle de $cal(P)$ (en rose)
376
+
377
+
#architecture([Cycle complet. Un cycle commence avec la flèche "update" partant de `::PreUpdate`], {
378
+
let colored-edge = (color, label, ..args) => edge(stroke: color, label: text(fill: color, label), ..args)
379
+
let sim-edge = (label, ..args) => colored-edge(blue, label, ..args)
380
+
let publisher-edge = (label, ..args) => colored-edge(red, label, ..args)
381
+
let policy-edge = (label, ..args) => colored-edge(fuchsia, label, ..args)
382
+
383
+
// Simulation loop
384
+
sim-edge("read", <preupdate>, "d,d,r,r", <cmdbuf>, "<-@")
385
+
sim-edge("update", <preupdate.east>, "d", <statebuf>, "->", label-pos: 70%, label-side: right)
386
+
387
+
// lowstate publisher loop
388
+
publisher-edge("read", <statebuf>, "@->", <lowstate>)
389
+
publisher-edge("", <lowstate>, "-", <publisher>)
390
+
publisher-edge("", <publisher>, (1, 0), <channelfactory.west>, "->")
391
+
392
+
// policy loop
393
+
// dds part
394
+
policy-edge("commands", <policy>, (2.25, -1), (2.25, 0), <channelfactory.east>, "-->", label-pos: 10%)
395
+
policy-edge("state", <policy>, (0, 0), <channelfactory.west>, "<--@", label-pos: 80%)
396
+
// non-dds part
397
+
policy-edge("", <channelfactory.east>, (2, 0), <subscriber>, "->")
398
+
policy-edge("", <subscriber>, "-", <lowcmd>)
399
+
policy-edge("update", <lowcmd>, "->", <cmdbuf>)
400
+
})
401
+
402
+
Ces désynchronisations pourraient expliquer les problèmes de performance recontrés (cf @perf)
403
404
== Essai sur des politiques réelles
405
406
+
== Amélioration des performances <perf>
407
408
== Enregistrement de vidéos
409
+1
rapport/main.typ
+1
rapport/main.typ
+3
-3
rapport/proofs.typ
+3
-3
rapport/proofs.typ
+1
-1
rapport/sdk2-study.typ
+1
-1
rapport/sdk2-study.typ
···
233
234
Le bridge de Mujoco fonctionne en interceptant les messages sur le canal `rt/lowcmd` et en en envoyant dans le canal `rt/lowstate`, qui correspondent respectivement aux commandes envoyées au robot et à l'état (angles des joints, moteurs, valeurs des capteurs, etc) renvoyé par le robot.
235
236
-
Le `low` indique que ce sont des messages bas-niveau: par exemple, `rt/lowcmd` correspond directement à des ordres de tension pour les moteurs, et non pas à des messages plus avancés du type "avancer de $x$ mètres" #todo[ces messages plus haut-niveau = sport mode non? dire quand ils servent]
237
238
Les ordres dans `rt/lowcmd` sont ensuite traduits en appels de fonctions de Mujoco pour mettre à jour l'état du robot simulé, et de messages `rt/lowstate` sont créés à partir des données fournies par Mujoco
239
···
233
234
Le bridge de Mujoco fonctionne en interceptant les messages sur le canal `rt/lowcmd` et en en envoyant dans le canal `rt/lowstate`, qui correspondent respectivement aux commandes envoyées au robot et à l'état (angles des joints, moteurs, valeurs des capteurs, etc) renvoyé par le robot.
235
236
+
Le `low` indique que ce sont des messages bas-niveau: par exemple, `rt/lowcmd` correspond directement à des ordres de tension pour les moteurs, et non pas à des messages plus avancés tel que "avancer de $x$ mètres" #todo[ces messages plus haut-niveau = sport mode non? dire quand ils servent]
237
238
Les ordres dans `rt/lowcmd` sont ensuite traduits en appels de fonctions de Mujoco pour mettre à jour l'état du robot simulé, et de messages `rt/lowstate` sont créés à partir des données fournies par Mujoco
239
+1
rapport/utils.typ
+1
rapport/utils.typ