this repo has no description
Continue rapport
+83
-88
+83
-88
rapport/gz-unitree.typ
+83
-88
rapport/gz-unitree.typ
···
87
87
88
88
== Installation du plugin dans Gazebo
89
89
90
-
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`)
90
+
Un _system plugin_ Gazebo consiste en la définition d'une classe héritant de `gz::sim::System` et d'interfaces permettant notamment d'exécuter notre code avant ou après un pas de temps du simulateur (avec `gz::sim::ISystem`{`Pre`,`Post`}`Update`)
91
91
92
-
#dontbreak(
92
+
#figure(
93
+
caption: [Fichier header pour le plugin],
93
94
```cpp
94
95
#include <gz/sim/System.hh>
95
96
namespace gz_unitree
···
115
116
```cpp
116
117
#include <gz/plugin/Register.hh>
117
118
118
-
... // implementation
119
+
... // class implementation
119
120
120
121
GZ_ADD_PLUGIN(
121
122
UnitreePlugin,
···
123
124
UnitreePlugin::ISystemPreUpdate)
124
125
```
125
126
126
-
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)
127
+
Enfin, on active le plugin en le référençant dans le fichier SDF @sdf-plugin, qui décrit l'environnement des simulations (objets, éclairage, etc)
127
128
128
129
#zebraw(
129
130
numbering: false,
···
152
153
- L'interaction avec les canaux DDS du SDK d'Unitree
153
154
- Les données et méthodes internes au plugin
154
155
155
-
En plus de cela, il y a bien évidemment la politique de contrôle $Pi$, qui interagit via les canaux DDS avec le robot (qu'il soit réel, ou simulé)
156
+
En plus de cela, il y a bien évidemment la politique de contrôle $Pi$, qui interagit avec le robot (qu'il soit réel, ou simulé) via le SDK, et donc via les canaux DDS.
156
157
157
158
#let legend = (
158
159
..descriptions,
···
232
233
name: <dds>,
233
234
(<channelfactory>, <publisher>, <subscriber>),
234
235
alignment: top + center,
235
-
)[SDK d'Unitree]
236
+
)[Unitree SDK]
236
237
237
238
238
239
node(name: <gzclock>, (1, 5), subtitled(
···
265
266
266
267
if show-legend {
267
268
node((0, 5), stroke: none, width: 15em, fill: white, legend(
268
-
("--", "Message DDS"),
269
-
("..", "Message Gazebo"),
269
+
("-->", "Message DDS"),
270
+
("..>", "Message Gazebo"),
270
271
("@->", "Désynchronisation"),
271
272
))
272
273
}
···
306
307
- Un _publisher_, chargé d'envoyer périodiquement des messages sur `rt/lowstate` en appellant la méthode `LowStateWriter`
307
308
- Un _subscriber_, chargé d'appeller la méthode `CmdHandler` avec chaque message arrivant sur `rt/lowcmd`.
308
309
309
-
On démarre aussi deux autres #emph[subscriber]s, qui sont eux chargés d'écouter des messages sur les topics Gazebo `/clock` et `/imu`, ce qui permet de récupérer le tick de simulation et les valeurs du capteur IMU#footnote[Inertial Measurement Unit, appelée "Centrale intertielle" en français], que l'on a préalablement fixé au modèle du robot en le déclarant au fichier SDF chargé par Gazebo. Le capteur IMU donne des informations importantes sur la position et la vitesse dans l'espace du robot.
310
+
On démarre aussi deux autres #emph[subscriber]s, qui sont eux chargés d'écouter des messages sur les topics Gazebo `/clock` et `/imu`, ce qui permet de récupérer le tick de simulation et les valeurs du capteur IMU#footnote[Inertial Measurement Unit, appelée "Centrale intertielle" en français], que l'on a préalablement fixé au modèle du robot en le déclarant dans le fichier SDF chargé par Gazebo. Le capteur IMU donne des informations spatiales importantes sur la position et la vitesse du robot.
310
311
311
-
Les topics Gazebo sont un autre moyen de communcation inter-processus asynchrone par pub/sub, similaire à DDS @gz-topics. Gazebo utilise Protobuf @protobuf pour définir les types des messages @gz-messages, qui joue ici le même role qu'IDL dans DDS. Les topics Gazebo sont basés sur un réseau de noeuds décentralisé, chaque noeud pouvant publier et/ou recevoir des messages.
312
+
Les topics Gazebo sont un autre moyen de communcation inter-processus asynchrone par pub/sub, similaire à DDS @gz-topics. Gazebo utilise Protobuf pour définir les types des messages @protobuf @gz-messages, qui joue ici le même role qu'IDL dans DDS. Les topics Gazebo sont basés sur un réseau décentralisé de nœuds, chaque nœud pouvant indépendamment publier et/ou recevoir des messages.
312
313
313
-
Cette initialisation est faite à l'initialisation du plugin par Gazebo, en la faisant dans la méhode `::Configure` du plugin.
314
+
Cette initialisation est faite à la phase de configuration du plugin par Gazebo, via l'implémentation de la méthode `::Configure` du plugin.
314
315
315
-
En pratique, on utilise `std::bind` @cpp-bind pour fixer l'instance d'`UnitreePlugin` et ainsi passer des méthodes de la classe comme des simples fonctions
316
+
En pratique, on utilise `std::bind` @cpp-bind pour fixer l'instance d'`UnitreePlugin` et ainsi passer des méthodes de la classe comme des simples fonctions.
316
317
317
-
#grid(
318
-
columns: 2,
319
-
gutter: 1em,
320
-
figure(
321
-
caption: [Création d'un _subscriber_ à `rt/lowcmd` dans `UnitreePlugin::Configure`],
322
-
text(size: 0.8em, ```cpp
323
-
auto subscriber = ChannelSubscriberPtr<LowCmd_>(
324
-
new ChannelSubscriber<LowCmd_>("rt/lowcmd")
325
-
);
326
-
327
-
auto handler = std::bind(
328
-
&UnitreePlugin::CmdHandler,
329
-
this,
330
-
std::placeholders::_1
331
-
)
318
+
#figure(
319
+
caption: [Création d'un _subscriber_ à `rt/lowcmd` et d'un _publisher_ sur `rt/lowstate` dans `UnitreePlugin::Configure`],
320
+
text(size: 0.8em,
321
+
grid(
322
+
columns: 2,
323
+
gutter: 1em,
324
+
```cpp
325
+
auto subscriber = ChannelSubscriberPtr<LowCmd_>(
326
+
new ChannelSubscriber<LowCmd_>("rt/lowcmd")
327
+
);
332
328
333
-
subscriber->InitChannel(handler, 1);
329
+
auto handler = std::bind(
330
+
&UnitreePlugin::CmdHandler,
331
+
this,
332
+
std::placeholders::_1
333
+
)
334
334
335
-
.
336
-
```),
337
-
),
335
+
subscriber->InitChannel(handler, 1);
336
+
```,
337
+
```cpp
338
+
auto publisher = ChannelPublisherPtr<LowState_>(
339
+
new ChannelPublisher<LowState_>("rt/lowstate")
340
+
);
338
341
339
-
figure(
340
-
caption: [Création du _publisher_ pour `rt/lowstate` dans `UnitreePlugin::Configure`],
341
-
text(size: 0.8em, ```cpp
342
-
auto publisher = ChannelPublisherPtr<LowState_>(
343
-
new ChannelPublisher<LowState_>("rt/lowstate")
344
-
);
342
+
publisher->InitChannel();
345
343
346
-
publisher->InitChannel();
347
-
348
-
this->publisher_thread = CreateRecurrentThreadEx(
349
-
"low_state_writer",
350
-
UT_CPU_ID_NONE,
351
-
500,
352
-
&UnitreePlugin::LowStateWriter,
353
-
this
354
-
);
355
-
```),
356
-
),
357
-
)
344
+
this->publisher_thread = CreateRecurrentThreadEx(
345
+
"low_state_writer",
346
+
UT_CPU_ID_NONE,
347
+
500,
348
+
&UnitreePlugin::LowStateWriter,
349
+
this
350
+
);
351
+
```
352
+
)))
358
353
359
354
== Calcul des nouveaux couples des moteurs
360
355
···
387
382
388
383
Cette équation met à jour $tau$ pour rapprocher l'état actuel du moteur de la nouvelle consigne, en prenant en compte
389
384
390
-
- L'erreur sur l'angle $Delta q$ (partie "proportional")
391
-
- L'erreur sur la vitesse de changement de $Delta q$ (partie "derivative")
385
+
- L'erreur sur l'angle $Delta q$ (partie proportionelle).
386
+
- L'erreur sur la vitesse de changement de $Delta q$ (partie dérivative). Cette prise en compte de la vitesse permet de lisser les changements appliqués aux moteurs.
392
387
- Un couple dit de _feed-forward_, $tau_"ff"$, qui permet le maintient du robot à un état stable. On pourrait le déterminer en lançant une première simulation, avec pour objectif le maintient debout. Une fois la stabilité atteinte, on relève les couples des moteurs. Intuitivement, on peut voir $tau_"ff"$ comme un manière de s'affranchir de la partie "maintient debout" dans l'expression de la commande, similairement à la mise à zéro ("tarer") d'une balance.
393
388
394
-
Cette prise en compte de la vitesse permet de lisser les changements appliqués aux moteurs
395
-
396
-
On contrôle la proportion de chaque terme dans le calcul de la nouvelle consigne grâce à deux coefficients, $K_p$ et $K_d$.
389
+
On contrôle la prépondérance des deux erreurs dans le calcul de la nouvelle consigne grâce à deux coefficients, $K_p$ et $K_d$.
397
390
398
391
399
-
400
392
== `rt/lowcmd` <receive-lowcmd>
401
393
402
-
En pratique, les valeurs actuelles pour le calcul de $Delta q$ et $Delta dot(q)$ proviennent de l'état du moteur, accessible dans `rt/lowstate` avec les champs `q` et `dq` du moteur en question @h1-rt-lowstate
403
394
404
-
#figure(
405
-
caption: [Implémentation de la mise à jour de $tau$],
406
-
```cpp
407
-
// Avec i l'indice du moteur
408
-
auto force = cmdbuf->tau_ff.at(i) + // tau_ff
409
-
cmdbuf->kp.at(i) * ( // K_p
410
-
cmdbuf->q_target.at(i) - lowstate.motor_state().at(i).q() // Delta q
411
-
) +
412
-
cmdbuf->kd.at(i) * ( // K_d
413
-
cmdbuf->dq_target.at(i) - lowstate.motor_state().at(i).dq() // Delta q.
414
-
);
415
-
416
-
std::vector<double> torque = {force};
417
-
joint.SetForce(ecm, torque);
418
-
```,
419
-
)
420
-
421
-
Cette équation se rapproche des modèles de type PID (_proportional-integrative-derivative_) @control-pid, avec le terme intégratif remplacé par $tau_"ff"$, ce qui en fait une expression plus adaptée pour les politiques avec des mouvements non-brusques: le terme intégratif apporte une capacité d'instabilité qui complexifie l'entraînement #refneeded
422
-
423
-
// === Réception des commandes <receive-lowcmd>
424
-
425
-
Lorsqu'un message, publié par $Pi$ (1A) et contenant des ordres pour les moteurs, arrive sur `rt/lowcmd`, `ChannelSubscriber` appelle `::CmdHandler` (2, 3), et modifie un _buffer_ (4) contenant la dernière commande reçue.
426
-
427
-
On trouve dans ces messages les champs nécéssaires à au calcul de $tau$ @h1-rt-lowcmd comme décrit précédemment:
395
+
On trouve dans les messages `rt/lowcmd` les champs nécéssaires à au calcul de $tau$ @h1-rt-lowcmd comme décrit précédemment:
428
396
429
397
#let greyedout = content => text(fill: luma(120), emph(content))
430
398
#let undocumented = greyedout[Non documenté]
···
452
420
[Respectivement $q$, $dot(q)$, $tau_"ff"$, $K_p$ et $K_d$],
453
421
)
454
422
423
+
Cette équation se rapproche des modèles de type PID (_proportional-integrative-derivative_) @control-pid, avec le terme intégratif remplacé par $tau_"ff"$, ce qui en fait une expression plus adaptée pour les politiques avec des mouvements non-brusques: le terme intégratif apporte une capacité d'instabilité qui complexifie l'entraînement #refneeded
455
424
456
-
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.
425
+
426
+
#figure(
427
+
caption: [Implémentation de la mise à jour de $tau$],
428
+
```cpp
429
+
// Avec i l'indice du moteur
430
+
auto force = cmdbuf->tau_ff.at(i) + // tau_ff
431
+
cmdbuf->kp.at(i) * ( // K_p
432
+
cmdbuf->q_target.at(i) - lowstate.motor_state().at(i).q() // Delta q
433
+
) +
434
+
cmdbuf->kd.at(i) * ( // K_d
435
+
cmdbuf->dq_target.at(i) - lowstate.motor_state().at(i).dq() // Delta q.
436
+
);
437
+
438
+
std::vector<double> torque = {force};
439
+
joint.SetForce(ecm, torque);
440
+
```,
441
+
)
442
+
443
+
// === Réception des commandes <receive-lowcmd>
444
+
445
+
Lorsqu'un message, publié par $Pi$ (1A) et contenant des ordres pour les moteurs, arrive sur `rt/lowcmd`, `ChannelSubscriber` appelle `::CmdHandler` (2, 3), et modifie un _buffer_ (4) contenant la dernière commande reçue. 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.
457
446
458
447
459
448
#architecture([Phase de réception des commandes], {
···
484
473
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.
485
474
486
475
487
-
/ Si `::PreUpdate` est plus fréquente: Le simulateur appliquera simplement plusieurs fois la même commande, le buffer n'ayant pas été modifié.
476
+
/ Si `::PreUpdate` est plus fréquente: Le simulateur appliquera plusieurs fois la même commande, le buffer `cmdbuf` n'ayant pas été modifié.
488
477
489
-
/ 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.
478
+
/ Si `::PreUpdate` est moins fréquente: Certaines commandes seront ignorées par Gazebo, qui ne vera pas la valeur du buffer `statebuf` avant qu'il change de nouveau.
490
479
491
480
// 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`.
492
481
//
···
594
583
`imu_state…`,
595
584
"struct.",
596
585
[Valeurs des capteurs intertiels du robot],
597
-
[Messages `gz::msgs::IMU` sur le topic Gazebo `/imu` sur le modèle],
586
+
[Messages `gz::msgs::IMU` sur le topic Gazebo `/imu`],
598
587
599
588
` .quaternion`,
600
589
$RR^4$,
601
590
[Posture dans l'espace du robot, dans l'ordre $(w, x, y, z)$],
602
-
[$w$, $x$, $y$ et $z$ sur `.orientation()`],
591
+
[`w`, `x`, `y` et `z` sur `.orientation()`],
603
592
604
593
` .rpy`,
605
594
$RR^3$,
···
610
599
$RR^3$,
611
600
[Gyroscope],
612
601
[
613
-
En utilisant les valeurs de `.orientation()`: \
614
-
$"atan"_2(2(w x + y z), 1 - 2 (x^2 + y^2) )) \ "asin"(2 (w y - z x)) \ "atan"_2(2(w z + x y), 1 - 2(y^2 + z^2))$
602
+
En utilisant les valeurs de `.orientation()`:
603
+
#math.equation(numbering: none, block: true, $ vec(
604
+
delim: #("[", "]"),
605
+
gap: #0.5em,
606
+
"atan"_2(2(w x + y z), 1 - 2 (x^2 + y^2) ) ,
607
+
"asin"(2 (w y - z x)) ,
608
+
"atan"_2(2(w z + x y), 1 - 2(y^2 + z^2))
609
+
) $)
615
610
],
616
611
617
612
` .accelerometer`, $RR^3$, [Accéléromètre], `.angular_velocity()`,
···
623
618
624
619
` .mode`,
625
620
${0, 1}$,
626
-
[$0$ pour "Brake" et $1$ pour "FOC#footnote[Field-Oriented Control]", deux modes de contrôle pour le moteur électrique],
621
+
[Modes de contrôle pour le moteur électrique. $0$ pour "Brake" et $1$ pour "FOC#footnote[Field-Oriented Control]"],
627
622
[0],
628
623
629
624
` .q`,
rapport/wireshark-message-type.png
rapport/wireshark-message-type.png
This is a binary file and will not be displayed.