this repo has no description
Continue rapport
+96
-39
+29
-2
bib.yaml
+29
-2
bib.yaml
···
825
825
value: https://wiki.wireshark.org/Protocols/rtps
826
826
827
827
h1-rt-lowstate:
828
-
type: web
828
+
type: reference
829
829
title: rt/lowState topic Descirption - 宇树科技 文档中心
830
830
url:
831
831
date: '2025-10-23'
832
832
value: https://support.unitree.com/home/en/H1_developer/H1-2_Basic_Services_Interface#heading-4
833
833
834
834
h1-rt-lowcmd:
835
-
type: web
835
+
type: reference
836
836
title: rt/lowCmd topic Description - 宇树科技 文档中心
837
837
url:
838
838
date: '2025-10-23'
839
839
value: https://support.unitree.com/home/en/H1_developer/H1-2_Basic_Services_Interface#heading-1
840
840
841
+
control-pid:
842
+
type: anthos
843
+
title: PID Control
844
+
author: Araki, Mituhiko
845
+
date: 2009
846
+
editor:
847
+
name: Life Support Systems (EOLSS)
848
+
given-name: Encyclopedia
849
+
prefix: of
850
+
url:
851
+
value: http://www.eolss.net/ebooks/Sample%20Chapters/C18/E6-43-03-03.pdf
852
+
date: '2025-10-23'
853
+
note: 'Available online: http://www.eolss.net/ebooks/Sample Chapters/C18/E6-43-03-03.pdf
854
+
(accessed on December 2021)'
855
+
parent:
856
+
type: anthology
857
+
title: Control Systems, Robotics, and Automation
858
+
volume: 2
859
+
860
+
cpp-bind:
861
+
type: reference
862
+
author: cplusplus.com
863
+
title: std::bind
864
+
url:
865
+
date: '2025-10-23'
866
+
value: https://cplusplus.com/reference/functional/bind/
867
+
+66
-36
rapport/gz-unitree.typ
+66
-36
rapport/gz-unitree.typ
···
197
197
198
198
node(name: <lowstate>, (1, 2), `::LowStateWriter`)
199
199
node(name: <lowcmd>, (2, 2), `::CmdHandler`)
200
-
node(name: <statebuf>, (1, 3))[State buffer]
201
-
node(name: <cmdbuf>, (2, 3))[Commands buffer]
200
+
node(name: <statebuf>, (1, 3), subtitled("State buffer", `statebuf`))
201
+
node(name: <cmdbuf>, (2, 3), subtitled("Commands buffer", `cmdbuf`))
202
202
group((<lowstate>, <lowcmd>, <statebuf>, <cmdbuf>))[Plugin internals]
203
203
204
204
node(name: <policy>, (0, -1), $cal(P)$)
···
213
213
edge(<configure>, "u", <channelfactory>, "->", label-side: left, label-pos: 50%)[appelle]
214
214
edge(<channelfactory>, "->", <publisher>)[initialise]
215
215
edge(<channelfactory>, "->", <subscriber>)[initialise]
216
-
edge(<publisher>, "<->", <lowstate>)[associés]
217
-
edge(<subscriber>, "<->", <lowcmd>)[associés]
216
+
edge(<publisher>, "<->", <lowstate>)[`std::bind`]
217
+
edge(<subscriber>, "<->", <lowcmd>)[`std::bind`]
218
218
})
219
219
220
220
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]
···
226
226
227
227
Cette initialisation est faite à l'initialisation du plugin par Gazebo, en la faisant dans la méhode `::Configure` du plugin.
228
228
229
+
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
230
+
231
+
#figure(
232
+
caption: [Création d'un _subscriber_ à `rt/lowcmd` dans `UnitreePlugin::Configure`],
233
+
```cpp
234
+
auto subscriber = ChannelSubscriberPtr<LowCmd_>(
235
+
new ChannelSubscriber<LowCmd_>("rt/lowcmd")
236
+
);
237
+
238
+
auto handler = std::bind(
239
+
&UnitreePlugin::CmdHandler,
240
+
this,
241
+
std::placeholders::_1
242
+
)
243
+
244
+
subscriber->InitChannel(handler, 1);
245
+
```
246
+
)
247
+
229
248
== Réception des commandes <receive-lowcmd>
230
249
231
250
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.
···
235
254
236
255
Pour appliquer la commande, on calcule la force effective que le moteur doit appliquer:
237
256
238
-
#grid(
239
-
columns: 2,
240
-
gutter: 1em,
241
-
[
242
-
#math.equation(block: true, numbering: none, $
243
-
tau =
244
-
underbracket(K_p Delta q, "proportional") +
245
-
underbracket(tau_"ff", "integrative") +
246
-
underbracket(K_d Delta dot(q), "derivative")
247
-
$)
257
+
$
258
+
tau =
259
+
underbracket(K_p Delta q, "proportional") +
260
+
underbracket(tau_"ff", "integrative") +
261
+
underbracket(K_d Delta dot(q), "derivative")
262
+
$
248
263
249
-
Avec
264
+
Avec
250
265
251
-
/ $tau$: pour _torque_, la force à donner au moteur
252
-
/ $tau_"ff"$: le $tau$ "feed-forward", #todo[I de PID ou pas?]
253
-
/ $Delta q$: écart d'angle de rotation du moteur entre la consigne et l'état actuel
254
-
/ $Delta dot(q)$: écart de vitesse (instantanée) de rotation du moteur entre la consigne et l'état actuel
255
-
/ $K_p$, $K_d$: coefficients modulant la prépondérance de $Delta dot(q)$ et $Delta dot(q)$
256
-
],
257
-
text(size: 0.9em,
258
-
```cpp
266
+
/ $tau$: pour _torque_, la force à donner au moteur
267
+
/ $tau_"ff"$: le $tau$ "feed-forward", #todo[I de PID ou pas?]
268
+
/ $Delta q$: écart d'angle de rotation du moteur entre la consigne et l'état actuel
269
+
/ $Delta dot(q)$: vitesse de changement de la consigne#footnote[
270
+
271
+
#let ddt = derivee => $ ( op("d") #derivee ) / ( op("d") t ) $
272
+
273
+
On a bien $ddt(Delta q) = Delta dot(q)$ par linéarité de la dérivation temporelle:
274
+
275
+
$
276
+
ddt(Delta q) = ddt(q_"new" - q_"old") = ddt(q_"new") - ddt(q_"old") = Delta ddt(q) = Delta dot(q)
277
+
$
278
+
279
+
]
280
+
/ $K_p$: prépondérance de la partie proportionelle
281
+
/ $K_p$: prépondérance de la partie dérivée
282
+
283
+
Cette équation met à jour $tau$ pour rapprocher l'état actuel du moteur de la nouvelle consigne, en prenant en compte
284
+
285
+
- L'erreur sur l'angle $Delta q$ (partie "proportional")
286
+
- L'erreur sur la vitesse de changement de $Delta q$ (partie "derivative")
287
+
288
+
Cette prise en compte de la vitesse permet de lisser les changements appliqués aux moteurs @control-pid.
289
+
290
+
On contrôle la proportion de chaque terme dans le calcul de la nouvelle consigne grâce à deux coefficients, $K_p$ et $K_d$.
291
+
292
+
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
293
+
294
+
```cpp
259
295
// Avec i l'indice du moteur
260
-
auto force =
261
-
cmdbuf->tau_ff.at(i) +
262
-
cmdbuf->kp.at(i) * (
263
-
cmdbuf->q_target.at(i) -
264
-
lowstate.motor_state().at(i).q()
265
-
) +
266
-
cmdbuf->kd.at(i) * (
267
-
cmdbuf->dq_target.at(i) -
268
-
lowstate.motor_state().at(i).dq()
296
+
auto force = cmdbuf->tau_ff.at(i) + // tau_ff
297
+
cmdbuf->kp.at(i) * ( // K_p
298
+
cmdbuf->q_target.at(i) - lowstate.motor_state().at(i).q() // Delta q
299
+
) +
300
+
cmdbuf->kd.at(i) * ( // K_d
301
+
cmdbuf->dq_target.at(i) - lowstate.motor_state().at(i).dq() // Delta q.
269
302
);
270
303
271
304
std::vector<double> torque = {force};
272
305
joint.SetForce(ecm, torque);
273
-
```),
274
-
"",
275
-
align(center)[Implémentation]
276
-
)
306
+
```
277
307
278
308
#architecture([Phase de réception des commandes], {
279
309
edge(<policy>, (2, -1), (2, 0), "-->", label-pos: 10%)[(1A) publish]