From 2a529ee8ed2f4380d44f3d78a4fd5ca9c3468ff8 Mon Sep 17 00:00:00 2001 From: usernamekkyy Date: Wed, 11 Nov 2026 21:19:28 +0800 Subject: [PATCH] =?UTF-8?q?feat(humanoid):=20G1=20=E5=AE=A4=E5=86=85?= =?UTF-8?q?=E8=B5=B0=E5=BB=8A=E9=94=AE=E7=9B=98=E9=81=A5=E6=8E=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 通过键盘复现 G1 预训练步态在室内走廊的前进、转向和停止。 --- docs/humanoid/g1_corridor_teleop/index.md | 33 ++++++ docs/img/g1_corridor_teleop/teleop.png | Bin 0 -> 31175 bytes docs/index.md | 1 + mkdocs.yml | 1 + .../g1_corridor_teleop/.gitattributes | 2 + .../g1_corridor_teleop/CMakeLists.txt | 6 + src/humanoid/g1_corridor_teleop/README.md | 29 +++++ .../g1_corridor_teleop/launch/main.launch | 17 +++ src/humanoid/g1_corridor_teleop/main.py | 38 ++++++ src/humanoid/g1_corridor_teleop/package.xml | 9 ++ .../g1_corridor_teleop/patrol/__init__.py | 1 + .../g1_corridor_teleop/patrol/locomotion.py | 85 +++++++++++++ .../g1_corridor_teleop/patrol/runtime.py | 46 +++++++ .../g1_corridor_teleop/patrol/scene.py | 112 ++++++++++++++++++ .../g1_corridor_teleop/requirements.txt | 6 + .../scripts/fetch_assets.py | 25 ++++ .../g1_corridor_teleop/scripts/task_runner.py | 26 ++++ .../g1_corridor_teleop/third_party_license.md | 23 ++++ .../g1_corridor_teleop/third_party_notices.md | 20 ++++ 19 files changed, 480 insertions(+) create mode 100644 docs/humanoid/g1_corridor_teleop/index.md create mode 100644 docs/img/g1_corridor_teleop/teleop.png create mode 100644 src/humanoid/g1_corridor_teleop/.gitattributes create mode 100644 src/humanoid/g1_corridor_teleop/CMakeLists.txt create mode 100644 src/humanoid/g1_corridor_teleop/README.md create mode 100644 src/humanoid/g1_corridor_teleop/launch/main.launch create mode 100644 src/humanoid/g1_corridor_teleop/main.py create mode 100644 src/humanoid/g1_corridor_teleop/package.xml create mode 100644 src/humanoid/g1_corridor_teleop/patrol/__init__.py create mode 100644 src/humanoid/g1_corridor_teleop/patrol/locomotion.py create mode 100644 src/humanoid/g1_corridor_teleop/patrol/runtime.py create mode 100644 src/humanoid/g1_corridor_teleop/patrol/scene.py create mode 100644 src/humanoid/g1_corridor_teleop/requirements.txt create mode 100755 src/humanoid/g1_corridor_teleop/scripts/fetch_assets.py create mode 100755 src/humanoid/g1_corridor_teleop/scripts/task_runner.py create mode 100644 src/humanoid/g1_corridor_teleop/third_party_license.md create mode 100644 src/humanoid/g1_corridor_teleop/third_party_notices.md diff --git a/docs/humanoid/g1_corridor_teleop/index.md b/docs/humanoid/g1_corridor_teleop/index.md new file mode 100644 index 0000000000..67ab19fad5 --- /dev/null +++ b/docs/humanoid/g1_corridor_teleop/index.md @@ -0,0 +1,33 @@ +# G1 室内走廊键盘遥控 + +通过键盘复现 G1 预训练步态在室内走廊的前进、转向和停止。 + +## 环境与运行 + +使用 Python 3.11。在模块目录运行 `python -m pip install -r requirements.txt`。 + +在模块目录运行 `python scripts/fetch_assets.py /path/to/unitree_rl_gym` 获取固定版本的 G1 资产。底层步态来自 Unitree 预训练策略。 + +```bash +python main.py --assets /path/to/unitree_rl_gym +``` + +使用 WASD 或方向键前进、后退和转向,空格停车,+/- 调速。持续按键维持命令,0.6 秒无按键后速度归零。无窗口时可运行 `--mode smoke` 验证步态。 + +## ROS launch + +在 ROS Noetic 源码空间运行,将路径替换为实际值。 + +```bash +source /opt/ros/noetic/setup.bash +export ROS_PACKAGE_PATH="$PWD:${ROS_PACKAGE_PATH:-}" +roslaunch g1_corridor_teleop main.launch sim_python:=/path/to/python sim_entry:=/path/to/main.py assets:=/path/to/unitree_rl_gym +``` + +## 运行效果 + +![G1 室内走廊键盘遥控](../../img/g1_corridor_teleop/teleop.png) + +## 大模型使用声明 + +使用 OpenAI Codex 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。 diff --git a/docs/img/g1_corridor_teleop/teleop.png b/docs/img/g1_corridor_teleop/teleop.png new file mode 100644 index 0000000000000000000000000000000000000000..644b0e10cddd680139ea0fc2b243b07d025f99af GIT binary patch literal 31175 zcmZU4bySq!_qB*p(%ncS9Yd$IbV)ZzNl7;d0wN_MHAqWIi8Mn92naaTNHcUwcf;@U z`L6e`mxaq==9zi!J@>@k`<#i<(on*|qQrXe-~rAHWqF+k4^S4sKL?D5;F$+6+kGEA z@K1Ol|6I@S%TDW5GE;BHBtwLZg)NHJ}^wHhA@FH7hBj zI(5@bJBc79XqO<#;KKeDGJ_U8Uc5cxY2SBOy4%gU4pxu(G?IFmFyfltH7*A9-I z0yDdIzI+VfM>ZJ_@SyMghljPF_&#c>dFwG3YwNWefEPazkUVx^E><1>PNJ3nh`U+F zlv#x(d{ADoOprlEQHwY?h0V%n9BWo3iVbU6<@0y>w1NJZv9 zsF05-e=43Dy;i9e@(GjlHSrdEx~W`CGXh#!}arRe+upJs++H} zQt)Vp>F2N1>IR;7bL73hzgJf>-`DzwXHV}}_KAS#m)+rhyxHU3xBo4Kktc`+FJ~HS z_W6I;E&z>jCyS#EuEV(xl|K`IVmP*}!Xi_k3A@@uDr2EW9!NujP z9!2hdMzqZ@sZhsnBmq06ut+O|6L}J zsi*PeN1b+FJveyGG~Je%o~O3_NfrLZ_3!XK48rvJG4t6Z#Q3X!kEj1toZEWw+YH6H z&rew-YC67;VzXFNkUmwhrcp{7h?#SlNlX;e5|ZyAJVjC0t zuqu9ZmVG(}mJ=8K&N|Phpd$Vo+ZsD&e%JkymuB2`@_$)iUVOE4}>u_ zgb;}zFxE`omM&^`;+hQ zB#vjysK0TD!y3VPXbWZ}HaU|?rVXxC8m7}e7$oLnHTr09WbmQ)VGM}nxR3Ab*R&W& zRK8A*aH>tP4nXyj-wvtUVbweG;$gJPn1Unz-NYZ3zdpJ-n{qj=AME1U+2p%@>xIT{ z72C@~xqvDQEdmCMVllH3%^+U=A-6oOH zK}kE0R0tL`vzYmJjNn}NQpAs0qyaa6;EMXINlYSo?IPSn4%ZvT^UB^~a zj_Qe3CG#l7N~UMP?0xw91CO~by~J*|S1WBUcgGi04f228=L~+kO6-jFXe@+Ek1p7t z8f6;h5T0k{dbFCh#$#qi2C#^p$rSpPgIv)fl7y7Fa^OK3kmQWm%^qbbX}zO&gbnx4 zxO0ok=V)v(#yK*pYy&5<^)RK#Pp_9Zv5hq+=CumbF(?sfu#)pUJ+=>6v$no-DTgFl?WA1k$uz_i(P>=ZQG zVN$IgSY{N2RWVl$e^4=9dZkO-cJn5@TI5kKA^pF~Q>p&EUJ4(=GkTiZ2(jY-423Qm z@#vyl>cx1xI{%lRic88zI;;(6wBtV z1BY28sYba>xx6WHF00mO4o0mQGs8GDLna$zZNr$V{0EYv`Hd3F;7RQD(RkR*4nfeJ zme$`|{vY5aOz~e)Ycc?HM%=HIBTPhh7W77;hXxyjsb6bqSJ8#XiAC z**{ag&SZ$LXA|Yhe?XJVX3(?n%CqJQ!`0F8xmYx;(?WzcBdBVZ-*L zy85P%)na>>cB7dVD{?zc4YBPdw&QbET1*?bmcc6D^hd6~_36XhCLuwbvkKYLw)!dd z&#Wkov`%3X*8U6*l@E3zz<$1)cD3K^6z7Bl=uVuti%?@lYH7~K38~jiIp5ChwOTb4 zO>wU+$8ttY`&PgH%l_HPxBe@y7qow{o)bzF zBi||6@w4t$0mDiO{nYQBY+laYHIg;E@$wik5vfug8Mw-SYMuOpd2%6uJNd$==q204 z?r$SRanc}E**J*fqrzI!^+nzTE>3V%mh{kODk>AOLIKj)DHr?LLGpgmf&=?^!)X?C z({pn-t@D9hu(XKM@B7R;BL8}idmKBCb00NHVH>F#&>x!d~+2JLtBo<62ZAHfxy=@QSvO`yPv6wA_JBc7g@zFtYrbPn|j9eJ2C z9RB%FL)GQ-fsCA*HdP@)fC6Ez0Fk8D$f5^I4?iPt%^a>D}aT za>R%OG5z0vFN#_7v`Ub!jPIs*e9Fd*4by1^x5&SC6p&HEc!kWY2cT*h*eXAF*k+7CTPUD z?`SCW3H|Y6bdFUZMOA(c)d<|K#*lLm4qrXG2xx6J z|JA*!%#8@FE?hD2SBc^%#u)CmrB!l6W1Mjdz1mz|jikFfw{NaA;lWw*rfeKGuBdNs zr^AXIpO`2cb83K{*}i#0@4yzw$p0z9{n}dfg;fB*?cv#ymT5M2at5u@)sCC=)keV( za__6dhk{bn*5264hkf0%}2 zaZWYnN-T9Yo2;sO)Jmgt_Wc>MTeqyx(HMpdx!l|ulDhg@JPFIpAc(Vl6XJ*8IZb{0+?Q_hRPmRdkWT%-w4XlLj&BVk6sb3TSUNO91 zq~aX*dk$0?Ir}*KoZv$ce_d7iUWvmA-GkX?J!w9^c<#AYfV)abo87r;aq)4N=u|AZ%U{KR#>$Hd)BjOO~-!_{^7`r_ymspM_u zR%hIb6kQS*k2frb<&FMYAm5Uc;ID?6y^tb;Ta?Ysl_2N#TLE2sJnU3A>n8?{Y}}4f z2jzg3ym!or#HL})X|cf|z+~RrKrUJ~PrvMB)^ljgsZN>c6Wp#kCfLs}eP~@B!ATkm zEh32>D=l5|t7u;S_cP&ROYN7I+MwfGgQ$Qz_hVZxlnPUd;-8s|s9)!Tj*4M0bU)7# zWu{TrhOHAZp)ZU{%EgF|?>)Eia5!8wjT_t|pmlDh>CV#1ik>u8*qez4bkj+l zOg+OQok=IL5OCk{cCp3g1j%Y+?h?p{~*uMXG=ISaA;;*W3)wYRg5Sd za{>qCwm82@{x&;*2|eM)SraDSn`h$1LX_oS9-t~I&4e|lK5RQn;yH^_(I_!8haDQS zOq50^5!85*oO3Oap=LcZm0wZ06(j$!eZ_^3vS4c0$lN^S*fp=nF;bR3N!fB;u}~)i z`T^mUR2hzrIm^4QsA_JmVQ#LU?t{5xJrx3P6)hL-zqiYa0MVHp+=Frw#-qxho0AAX ziG)pO6ftUC`WHAjK2`$*g9GN4)7^!IpS*G4cwL2) z`^tMiV2@hSYzWA~n0R=UnyX^m$kClL9Vzaj#fjse=8naN64eWJStMsfXDX$+&7Ru^ zx58GY0?r=AIzq>syj@(l2xBbCYLHGnIrDzY@dg&bbx7BlDKqwzZ)4xA$Tz*zY8fC@ zIhn4X4YB*DZ?CnijI|^#q#vT!dc2Bz233j}2v*Q5Q8KPYalLa z#jp;nMMrTsS(;al`6x#bX6M%S@>z)>WrZ@+#pPC&^uaG8Vb@th3yYl0%&Se!(92O( zo||oZ>6*cKW^UWtxwE$DO=pV1$?lO&FK*LDe5}aRvopbp<>w};Ym8SbAI%^;PE0u& z0oAdaOYxZ=tL$(6Ya>v=Ue0Y7trH&E3l8`Tgok)2#Q$~hI=qk

Fd#SgLzHAtWr^ z4!pm$wbi+POT?#9ynHB2V?F2YD73A)`GbXQhjqq~p9ynIfLId#%ap1bf$UIK-mq6M z{4He&`|R!R79#&uM30&L6Fhj(EetYT6dC>E5Bu2<@!W?-`Pu55Rvr|p1y)aqH41f# z9G5&F59LUQUL39#X9qZWdJ?%EJ9E|4=(`5pUCrO^`nJb5?Dv}1qc)E@xwyDwNry^h z!YQNj9LF_YNU?3>^tMecphzpr_YH6)1?l@sD3XCPvRHb;*7$i)v$i}hK;V%FH|guL zGO`cd)-1`-NMoxFIg^y-3r1qec+6V^yuH0oK%l;mpzWOu-mdPQ-Z?#MJCnQ~WBAb( z9&*0F*wfSV=Z~6DrVHd-YXgC3-GP=-RR*5z6xxVzf0DA4_=)wnh=@))EtgE$mBcw)rPnY-=mRg$=tThy?p^m{$_I>YjW(PR5vrutyalyz&3Wq_Z zdFux{TI;~3F`^dAX{+ZpVpUaD?H6mA6P0EIs~#ZTO+l24%dp{UwAX=iUq>J7DA@lP z$>1*Hm@iq6mU|3N^ij)DtBob^Bo=1^IX{|dJ^!Eeb;6i(P~^Oyzuu{FgrcLP@Aw4T z*`Xn(#FFtrJdC3c__#LAY;A3Y9*~8O42UL9coSoxXZ&gT#g1$F@_NvH<8c!6ry-5@ zzg=E30}^UGwV1@fwge@@U2YcW6&QCT+k0XHO0J9U45X^d6LmXPg_C?{oTO=~sra&f zZ);xhEbnZUzZ@cM+?#K|8x-GLco|SrQxmUf;W@@)x6E7-q2Do`xY z<|I8kJF~a5``$ZsXlKb_;?;9E2lL%ilD;I6-Dq0RhK1Z*E-o&PkL!!kBz?;=4DS6! z!BBvzt&?UjYM_tRemg26GRmSjoI9d<{QiCTKV%AmI^cv4;Y_YbQUXg;!C{hn8*Xdyk}TOliYoY7 z87~)S3Ee&L3Xkg{yYcf|?Z`jkF?qT=Mz0j+fd2?gqAKBDv9!;+&Do@PMx#wVKDO!(%XUU#zM8Atm z(??sY=t&p)TJEST-kM)VcBhyzf{%*c2>)1eo>`&gs;asUh1)TWdgCmoP@P*`=OJCA zyA};O^{XcMewb9~*G&D&si+$whqs@1^SF0{sXVhNO`F zg)NoBm4PJ+Mw8KwQgTPbeik|Yb!agWbV+(SYp#Q=ik zhklWD`hs4|-lb%KCaPFxEDWzSiyI>R1w$B79SLWu2hDNJoi*u{TIx1WIU8$Y2&xBe zRW$5R1hn2B4)A~`Kb`gtJ!IKGzkidMvH;0RdiP4qHhGG1i3+D+Ozam^JOGZ4QL=dskPniCppMh) zRZXZC>J*IpRQe@E84tLOv|cv^F7CVQ0QEWDo-#1^e=IFjOFH8%mL;19%0Oyrs{4u$ z4CySCn2>OK`awWIcg$(zYE-P9b5|w@2lvf$^M5L5f(O7F$!FL27ilREk1_ZuKc$M) zD$d%Xt?E`yyso2^(%_{{WKHgKfJ|?C<&UsQU4GRsOBs0O%rmidvbpJ4z&hDqf4XXD zW;O=d5ud+EsH;Uqo#u_p%A!F)*4Nj;y+=18LM2-#0cB$Vqa674@B%^zhs@-AAK<>q zc`vFQYhoU-M<+EmD-Bgvy8?n-0rPQjNe;0&aIWy6Lm#!gR9zrQr+By~eJpZp@y zgk-|!X6E3ltE*4Qxc_dQ1WC=CFx>DGI-)OVon~eKZZBVnO*O|S%*^@#*Er$5$!)6Q zMY3WIcM;?np!=K6&kK4we0WiqTd^sD<( z!yr>_)!Fy<8JA!YF)?;xtVyC#P?^n|KM$>Qq^Pu-^N_}Zl-{*B#kZ9dB%~lI{O`0tvYpHFcS^{mQLxr zGviJIKvY9m} zZqEr?{ZX69#TGMYOLn`zdN_d>b#?9T1?0ORJjW~7T5C#5=LCjiCM254w$U0q#i zX=s=UV~i`JCftCrBO{Y>AVBNbL3iP6M!J&4cI%Y>b*kI{AU1$JSm=mwfHXHW7_gHJ zWt+Y4U7kcox;7l7BAP3Ub)Gp7?CtHPsa)mZ|K^|x^NwRd>rKV&z2YQHm9VYMiic~Y zwCmI2J>Ed^R?$xO&mZPI@L-(l`pp}+xw(09abe2c(9jUw3Mk3R#s-E7Y@9)KfX-zn z=`Y~^k{w$`U5nrA9^%=EWx}QW_GJ)pK~fnS>%j5Nd+b9e-%HYBIxlC zqodoNPgU7Rpvrwe`$P^KULZkSq!mN2x^)vAUp-HR3fu5HoVxy#JxUsQBv1>YHntl` z4oKo6N6IL`JGXc3t|@ZJU(*P^Pp&oQiZgarpK;l77jY&(aUiz33u!VPJ#kD5VJ{cTUm(8Nw2IP01pXT-2l& zXLf$;{7Wpqgg8$rm&qQRRFKND5yk*o8~DT|Gs#$pKcwri$@qDfOZC3#loDfI#Vc-}<0xspDopyv z7?;?J#n@qyE3e51ZXKNZM#)p=i9lnYSt)G?ar<(b8jE%SnfR% zYNZkPo0PVBB9@~$r5e{Qi$^E5?b5HE4TY)zzM|Lv5K>?829W!c02-Ui{r&HTvx0Dz zCRs+yr@GEoIh)(dIsrW^1esY0aE-LKW{ca)w?0n5372T#j4u2LokB#PR!QI=5}}X- zF^4#_7Jf0>#Itooin~Z04Gn2-wdFEpPKtNSvd%p1Fapu%R96jyy&}&77&jv$qp*r_K`^vVYgRTtx(Od#N4~I;} z)0%u-@3Mv-6dfBn*LM&+-gqt!jY+%gT`N@-NR`b?ALs_iq+D2?*As+4e@6M3)9LS2 zIn=7*?`~8s7m}QJXy%<_(Zw+2|IFiuooNHzHnQ(NNk5E0w86xPhE?ok8h#ORW_0t_z@ z6#fF&bx@^_5$Z>2E|}VN#*z{eU!YJxO8ms{6M3I4$dcGy-%W;7_d0JUhXV|{2`bLU z8@oJ%tS{_pkdb2u#FSHAq$a8|OES8i1ZT$}0hQ zLNJY#sj2P7MeYX7%giV7l(Cz_ZywkQ<%7}f-uH` zfuKy!X=Wx2`P# z~k?%x(y6(P^lr5-8?AJB7ZM0ue;meclc6N-M2j-y3Fe{M-$?ci;4)HE6AUy zLex!6w+C?wb?T;^fgF%&hv7yTJ`~EQE|8=Q%1AUG-R{l~5P;x+{*+f%N?mQ&0CK#+ zHK8h$DX49Hahd#@mliM3sA$hlN{&CVLNT!H02-*^_kuCj%kLpA8zqK_V2_F{N9z2d`ZWkzvg`7TABH>Kzkm^ zC0~H~`euH1_DkhfxAg(AOmuYgsz(#j71XJg&dUYMDy>PU2At5C?Y#gIi(q(!PvS4Y zYI6QbN^e_JezLXIu6)4tYxTJVDz!^=l(cCSxy7Qp2w@C_5~szMfigZDnB3pr-_;dY zn$@|^SV>6<0E_l7E<1MiP4e6Io^D+Zo<2e|#o_4`stGi)o1UH-bK0oUb;eAYS-|(7 zJb99#W(N*(W1|GIxxl41A9{DQHBmJ*#KXSPpde5&} zFKgH4`ar+^vZ30J4_YrBAqHWmM-&U`#&AMa@rN=s6^j9MU5i=_NirCPI;CXhe)Jh? zq*#x;eo{Gj*#O=Ih)D23H`%#+rw=SkquAT5m#5xPlk5PO>LO^g?mD9|?kPmm+P-F_ zq9VS#`_*>321~LZPqD@I5*e_jL6x}+gtC<@!+MDaCGR<9x5p4+6w~{|izKEs5BuA+ ziA2{q4rEJJ{MX^>gId&pjRZSUsRVZD)Es>Xd*B}}Hh^*L?Ch+oC(|rKntuGS_7z?> z`-m?~#%=l&rU0lhC(+E0EP{W3hH|#IhQ{T86RE>K_icdWwZ2XUfUY@PFwjIyxeeHe z=R&S_0C2VC&rs<;Mxwji30W@xl)FVGMEw1cSmEP))aXB?l&Cc9p@Op00kNZsVtgf7 z!;ksXDfP?_pHZm*8NAJqgmLQZ?B-UC*i>V&nd*ky5z&Q4;<#Mb=hayoBWmqxM1_SX zoCJ60T7#E+-1gbx<(v0Vr0%W(WA``ftu&#>4QE(CI*5qidS~5dd)OKI1S3yV>SATe zKIaCAG=W#AJ6de-Lb1-pVCc8*4^uHAcrN(X*`K31dyf(EtxF?m%WpSz(H(jo99atH zctl4@Kq>gVCF6!@e-#~EUrWr15F;q|dfVIELvODy0?&3AcTNvHWm|#)w(M4_teu(O zs=C`9(Y`_Y&c}CK$Hop+wH)@(&YCuHAZyAQ2!6W}Ghy{R3u$v3s1{xhY8-AB=YaeM zIvJ*BW?U&MMUEg0oZaTVg)yFnhQ46es|=EoBb3JmAabZx3OOk*(d=JUEk2qqr4UAX zV)y8s-JTaN8r-~Tb#6`pNYlUJ$c2 zQ-BRr8F;nYh_iNUGh%e&+iZFjyxAYHxIF|~qM@njby@pmnOpFd$>bhNn-T4s?Ey`` z42f3)bRU1M0}*t>=gDf-?wJvypq4G*5Cy(mC3~bS0-5DQY_8l~`64zUb7XzS>2DK7 zfGnKGW1&%uICtstlL*-5mr-|2l++ABYte&P;H<*WGVCe%DnE`cQJdr?)%Mxj!yUWM7NHM^51M*8C5@l z2MGyE^S@H8%&@urbH9-ETXgR62_!!w#`0qgY0*FxPxEY=y?2#*y1j|nO|QW(qRAS? z!otFUk5yW<-`ajEtla!LXM(a$fT3fjAva6?iudmrCeAhg}XP4vf7-Z0~xN*0it zL7pd#1r0-xj(x?lLN9pkRxNHRBP*w?|kqX5VU5drkv~kh5HA{vmbV0ow{aUvHYI+!Mj^rnUgrA zP6ivo!z=V!BK2%WfeU1GZ>aribKo52)IKiBoBGt$RJybCa1OQEm$1J8zO$eCtj~6o z7rW)TKnNTN8`k#kE4MG^IY`~fTpRLdBdLq3IlDW5}Be`prevdgz|G$4a zQqrM*jqP>b2DWeGXCZ%f|HZAK=I%du9>Va^J3djUr)-b5-v;_J?mLv|p+hJQe4Kgn zM02!kUIPazYFaf*Y))T(%JQIXyqugFLkV;J3|&z|BX7C*1kvFeP9!qepzJVP*Atf$@ca(5*BHQt$8eah9N&A&)|8m)SW05kVB(BzV?`a z;*t2xS zF*4>jEiPgN7F<~}SXr4F9!+axd0Bj7dj~~Jm9p>=4N9~ zA3D*&p&`KIgFIgDne8iOsxZDA-8>%A)xG;>t=xIw6N{u3``O?=qi8m|Y-RFEQL>nJdiVhirithC|OAsq=*{Bog(S{T|W- zwLD;bFbnW|kESm}>i}V}r~KfJKYP}F-5r;mk+HcGp$!aj{@38D&3E!WZZx2_p1h_} zfyu%AC9MmFbM1>nSxFr4s>7y_$F4?j)c>p#9yLu5S{)43y|psVSy-jmrYDwwMq7&n zD%7;M+0l)}FU9hJ`1bbp z@bJfpS2gem#ABSU-?l}u-Rx#~vbw{|58kq^tm0=swelwbfggSq%o zl21A-J+!{rQ|t0DJJRv!??dCkOnO&A%_vQ|IVCNE3?hC6igp5ZK1oFEgBYp2TsBU?I=}ne z$r&G~xaWEByJE@(B85=J9IB1;3oGw+%d<2(g!2_9IwZb*)b#WToVH@E;~c{BGw=DA z%!vu%f|1>}tI37<}6W;2g!_evUM< zC7?r0ljhN6TBrlGgmanQxwlAUB;8JZ>HvJ@=tABa$VR6I?U%30UKW&08}Rfq%RZ9c zEyYiYEV4~3eL7`KZDa2=di;jNCgYm>TY%Zs;y8UBZ}Mfw*^`_ql6FHz)6)XKU~6i4 zft#Refe|Dxl#LDxTG|d367~g(Wm_Cx2Qbln_qiMN>I5>>vZX_B>n6-$UYny})qoAn z&CP9ZbDoM9BOHMuGqhN#U$)nFxw09$x^EX%!fF6~mg% zmNo#opqc7))PHSvR&%kP{Ce@btL^dS4pCCyK2lK_Hd!DQEn3fR{Ft62b^T>IJylNV z5+QvfUDp?kw!sLM^r<6o7gjZ%LX{H;R2@Rc6S_U*2~9{7UEUGgzLLr;qKJ_Py(pVR zNjn#^Xh~WCC;a^>+yc_zb_c0`d0HF$`|d6-I{@uZIfH;8p|1r}Ms4k9jbb3Tmg=dn zB!mBut}f2b@4B)&+^1^hjfkrz<4BrgSQFQxcFW0A4U2nbP`6-@xr_Rmrgkeu!$~25 zwkiew^4RMK-ojV!?J(V<%v@*B-F+J@x%z&->s}M^QA-2zFXeLZfuK4VRRLMh+nYY+ z!-NxhgX@kRKLWsJ;DzS|0MMA4nyM8euCA^?q^u9+SQr^SBW|ur0h+$EkoWpvvB;f! zx`O%d)CWIINo^WF#-bk99x-v(_=GbAvDglWOw!ZEijrc&Kq{_>**N}y>h!l4P+{hkM}aW1F!|*RL7t3r^2LRe0+RAFrv9K zv&bLh3uhs5$t+W|tZuFhx_CwrUT%a~vSBroc5)>@l3TQHw5)nzy z%98ZmHLWvNXGzXbE7C99-gRa8OF#F+*{4WvdavE$rx6j(WG?BWeECn>3RQIN$R>!d zo><*SO)kT9spAcl38fl^uMLu)?3Uh>59asd%_6rm$FTXz=%i+^4-)Ts0;KWLS(5uE z%ma5$CMtsre~PqSagp7VNT6SpPb9M6>rLBU&C!$G+}WU?U=jQhkkhp3 z6-67x{fP+rmC%p$jqTKH2@cY~s;!%qj1cxdizx~|YI`9bW!D?&#!7^q@+e$4pYi+{ zd#Bw=*$H+L`_GDg?m6r^^w3FNZairbqCimaM8H7HnC`lR4)85Q&T(+IW}#~t_LZhh zhkzLX8XvMQJ~VRqqHY?Da}+t=H(WzQ+x6IAvZq{J__zG*5*x$MA#DT#FdH$zbDq1i zO+bTehzG6VDbRd&DW8}R+~X3@LS5$nKJ|8C7tFhh&|(D9hf8Rx`dsVuyRVAeRd5{5 zH+$?)WpxeF3`oqr%9KN^trhN~g5DI?kU_4B?tUWSaxyhVj(BvgJG-=ogE-XbDuuUW z?Ux4Ubw2^1K+^aneg#1ujkhB|W(<1p85td#xc?Ig z*D1is0a5*)rjYdxWlP33d~AEO991OPP*2;lJYvhQTbAMgC*l3P>iR*tJ@}Flz0)E` z!3*!Y-VJXk(nugy}YrpvDg)TayhbS^(-f6*GtTP$h^_- zV7jL#;^buXK@)cE)%i#O4o8Iao=B9moBZ1hvx#m*~K+lwvNUp7gLnPAsY(wtOLg{KqVB!o(zeh$01|QM? zoe5`rMIK$t4G|VbI@Ffadl-ULd` z$cQ!*;fq3@^OS?C_S*oLoib%6YTmS9*b@yCsGR%abK(+VR1~2%r>$yb4tVFgFrL21 zpdX-H(SCk`q{K#CmKeeJb8PyHpF6X)XFs}p?ygz3_5C7lc>x{=hWIM``Nw6t0_|1wO?X}KjfkJYx$f!Ax_W-REU)IFL1Sn=U05`84UEH@I_W>UU zdQwr~;fNC5OLnF8Aq(~tTrM%#KNLs%L{qfznX;^3YO7U(NZ-}-2!BFh-iYS*r#Q-U zN_R=M|4vMtaFJ(tWOhDMw?)UV?k=KHAJnGJJ@uMMSyXdo8=9mchh9i$laku)&4<1h z7U-N>d_@Js@;$w@q4kN0iRzs=Z7}E?;O4vq9cQqE{op@?3uiW&?(Xm7+lPmSo45GA zadh15h(N#ZC>}4x@&Ms$!l_P;rRK+v@QFDPk)TC4X_MH!e4tWz*iA;)ZLK`0P7i;D zQF#5qlln-W?|^B=QAhq`bZs`4N8Lxv-m`<Hjox4qsno#lwb=DA-V7MRW6Ol=vm2=kygwiI z)~gUIknDiP8M8)GMI{ZbAN*_eo%6T%@j2~d%>hb7Yv7q{tGTp@2ru^X<;m97m3VKa z$9~gx-jop>lac?s+y~s&wuV-#7nkAm-PHo)D^;HepYB!S;kUhT)V7uiUGqJv%j)bX zZa}7|qSv0y?4d)M_Zqw=GDNzj=$5CSKq&~!{T5B`*DED~PY*g-K(T?<)`Gc1q8KpL z@-q>O&z{R7x^k<6^xuyEgn<$#XrjX5S71Oav<^WrvmoLQR4HHIEqm#i#*mx7($doZ zTC1R03vzE$TgXg=_8%8_P1931y&nZ{uJRHK__*W-Rv)p z_w)!!krWzN2vOo7AO{GE7K(Occ+YR3o&*NsEgcXk&y?{>OCz1U4XhWK{6wQAiKr)M zxSIX%ZppycYFuAqc;}C}Ynou5heM&`L#-mmkMlVB@WUXM3X46KAAFN%Qzu^7of|x) z>M%teh_~rs6GEs8ly+zIkKeA%-|C|cqo)6Sc7SORCaU&saW>8NqZE!?RE-3*Via^P z{M8Y2!DkUb2YE`2)p{~+G9-N$1SSs?a9{6fZDV%0zPc{qP5PZ~H}T8IfSy`0MFn+l z$t`O%T;1_YO1DGHLz9W5N=EgrJ!q^D@zy7@G=k)f;KG6Kr0+J@{9nJSInsTdKUZkU za|#!D!}L{DlGZ!rcyv2O`Xc8{x;1!z(*-}bX%gol{FZXiT!tsjM-Ruo9Oju`qYwb|aoh9Co?>L=v-S@%Kr^2j@7qpT<$BacF zAx0X^G~M&gV#@gDR-MyXQnI7-I^u1T4*8yNoPYXdW1H#Dayr91jC-cIOAEpV)buU= z8KWPcuM-?}cJlf;F8MI)9x^3Z5G8D{Sew%cLatQTEwSx$QI?7qu`}$Vwh$N(XMf#B zX!+V}U$|WZvw%#mbC#>6iXc+4<6uJyt30wkn@jYCeWFhlNt%{-i;*l^VaJ_p3H~q~ z^bg1Wg;=ZwRB=$q`Xq_8okgKEPD8}PHwxn5sw+<_1eSboVSZAZKxv!qvYEcK+`gPLA2hB9% zCJlc45nA7m^I_8qKcZxZE%McK6LEU{0h_=3=j;xGF#azUcK9x&ze^T;sTnyX52+2Bqd$Q{a$RoxYQ}7jW6QQPy=~qugOrY z_bV7EYh`Sm$-^JfCtTy-32#9AoP8Qb@Oxwf7UqEy8f6GaWkzf9IX9{nr{=U)D-KH^ zPTC0hCuQqtM*Wv%3(0zcR`*Q4bxBvBmKF1r2vKIN_|v3$husiZXlR*fC8!IF6{5UF zRPku$16DM~?*7eV<@Tpt4jl7aDXF zc|KZ?yxU6Nim;Q4D8cbp{wZu-p_A|n6P~D=*aAo*C))a1Oxo8Rp zc5^6x<7~sIjs@d5_mxjvab!M2hx|dg#3;qZr(bvtXv8qu5$dmt+ch}p@FFk=6nJW~ z=VPM@IrTU;f4Y5{RQ;R=D-yI__$fT}D#!lqlHSjfXn2oLrvWaI%^UvvR}X+pnEOKA7#K&kW9cqT{OHIDF+eenK(5Ldv|b>>vUJ7T#G5FF zlh}!Mp{sQZ8s5DZ>2sww8#G3h$A^UbS+UelS)4@>tTQMG3jaKTuM83{k|R^Mb2OXZ zu(J|5A|K`hrn5f8lcrK=S7XFR%$~A4zyjLhI;DW3WeU3)SGg|Z!PokTI<+=d1X6*n zr^ZsS6(E(Xw9u3@iv)#F|Gh6!}Npd4tSYgsdg68g!f$!EJ{^Q0RqxA*3JQXHl zN>wVqPr;E}ejNnZLxC@OKKYNJ75l5JQ!3pUk+LTNt)Q2RaBQ(SihM#=-n!xbN1h?i z@#UAkr6p(XhU<&FEi2Eg5Polwdlxt)Yhk&hUyqqKJE>hoEUC zjRr|ZK@N|gk$hi(x!A*2VL-&?ph#HMgp_P`$+fs_Eq3ThVWNtSWQRmnI}N9&V73>lF@ zn+D9{dMPm?vKp^{DPf{AOM4@UqFg3Vg?@u&_{%{REI!lLl9__KBRQBwd92Yl^Rv-uS#ecMu7r z)3~J?lzs5&XT10<79TchPDyy0%R=`+)W}KT0>*iyOp7HW7zKgx?u`b4f%;7NG)3$8 z?;kaaXLe4r#C^e+m^_<4D?w1dX_m8uHh=nF;ArqF-hnNlP+cq+Wrk>kA@=~KIp@m| z*Kb!MLOR>$S67d6Mq1OwK1QIbNcBev2oJJR3=YHazCY?q8v2rPcZ?ACtI~cA3-00# zkc!`(`awS1N|HbQ_wS$GUHZyWM`*V-0X;-e5DY8<1Pq4Trly*LuY_cg$+`fAyv@rP z=Mcd_gU~ea@HOKX`~D2;P8{dx$$`_Wl@x!r!4$w8`0%rwhoDn`Fc(7oB;ef$Hhd96 zF?i@j>)(|zE|1)cjRiupqf74Fc~&rgGcG4fUAeUZzL{ihWK<`8CZG-?YXQCwgw|?9 zFkbU7OIdu!I1fd(MqDdkH^~93pV?p?^cPbc`#x*;X0eklQ8Y5R}dt!ioWH&IA#RL3xO%|S zSsXS~ilvpkc=NQlr~Qcfg}YwxQwQ3|VxCryR0Jtpb}lleD`rRta8jLMn2Hn`&Do-(ac}J0bI^i4}T}8eh8pxS{3?fX`Zq?dBe}BMXD6C z$d^pl+NmDC;UA`oPtBBzekw0wlIgmFER3;l|MbpJfBPVcCpfi>adWrvH^O;#Gn9rFZZ<(3d&uiyPZjFbabI?-t93$(||n%Owzo5M@Lm}%oW5hTwh8Q<4`d^ zJJke4-Ez4n70A!~AusBTD-1c;JW8GGuSj&1SlmV5>KqZ-=#b4lBH5l@(3GfLdH3$K z$aT8w+s2m((m%71vU~GMGrc=G-5l)`(OCvG^|oQ0 z?(UA!N^b~^hS4b~N=OL|5XljWbVzp%7}AIe(v5T|Fc}Ea(hO;l7I@G9-N${{&dzzx zdG7nVe%HMKm2Zm>-LNzKaXxjphnQ9xS}~0^ymlcz1}(Bc`X%e<WgA!Sz3c?%w5_WymT*X75=^Z zT*6%80%~VGSclr39PAeWD>ye?VypBICHvhUBy@oQN3_lQUWV?Ui7`)NlDwNOY0k=9)=_u*rxU7z zTBYWOC!L}xO5#`VZjV!Tn;qs)ir}L?EephJUk-;q%C#C&H0KTHq19cuBvJBvRL)3n zshW#Y3WhJ7XZ@soTAEfGc;-;{2OhKmg9#wZ-rU3e{tz3crG=SSuRTAT+NiaASb>!N9>2_$GkT1xuZd{iCjPpT}r+MmiqFJ*SjSQqTN>RQd;ZAhux(t3Zq$e99orr6fL6P_Xc%vO6?;Xxargn$nLXA z`2NwP{;G|x#!VDd&bxRbU>Z;LkgLIEA7`5(T-(a3W9RRWD!_W!d4fLgz6*Zi&iLRR;82vCwgKYl)zuATrK=jmn0X9Rar3H}%||O0+XMELdUlDU-HE05EP}-p;%Jck*2V z;2fyWJen-0-F~ijHZ@JVHv^oFY#{Zdi7-PP?Dy%_^v5Sb2}eg)oytl}ya(+ZvO2$( z&ywfkqH3+eS_PWG6P&PLT!nQn6?fB>7QYO4R*R4bQB0I7eek@i!aIv;8g5JwJcoNe ze%n`b7JYZh0NNR=b`O;R4b(L@b?XX4aO>C1o(GM-MN4DWX1DN#fEATzFV0!%EO=gP zMpivzlO$3S4SFd@kMFHhZaR#Z@mrFlc=IweG}O;eCQawj$7#KC9{~S15cCC*neN-& zZuF*)oRctv+k+R(5;Zl|)r0F>KOV(AD&T1iDAD4UsJqD*q%H zQh;1dq731>1D4kok_%Kq7^#^Rjx;w5bypG$aT7-cBG z$~uUV^Pc^_)eJc}Xf)z`Fkk91F`3U{BticHjO4m_2|EU6`K-+>`&ZD`#iT6-}W>k=@%K zOtq_H$ai~HIMW1=tqEpel*O_PlIzlab8U4F>O)k9Z`Lk!Lk@V_J@eFj;pWL^4xEO+ zi&Kg_YKz2 z^jSfnx!%avCsqaYvL#&yi*C^QVV`F8&apGqHQXlHaQo=1`6ryi0`V!iOjJJN^qk+4 zV;r=#el((&SNFd!uQaMeOw>Dc)W*GUb&Z#cVA*4+#@oJQRl4ix69LyH@B(vw#{D2h~!j4|o+fEC3Ve*QfSj5uT zLT0}@b=TuOQR4_m>6(b={O51DQusPA0-{Gu@o{1CH`>*RUWsUi5t?@j@7DWj3AGA*BYo+fUj&yD(BaBw^ck8THh zx2PkD{oYi99T(gCl#*6~#~msevy*#Ku9koSxM-w-7AUQ>l&@d% z_i4NRkLrOyVu63wb^n2(ReQl?CQ(Ib1p%EtEs|_s;pF*zaM+|V_ zpvqC`kSQbvmgr@She z-cny5Rl3Sdax5N@rRM~MM%a?p#?m8*HrJvyb) zgAei+dy>d!w6Ro9K1J(X@_2Dv@RoV=){n`-)BH<%mgoMTJ(@pi$L`Vb{Ec&U+6Zi| zn;rBwu)1pXEKsUf%FD_MZs2!Niq?e6%9mORGn%yYP9U>#v(o3;5DR*s0z-IeFG+&d zNAbDVPGIeyIX}I>{|S|zH-13GdHVWhr&7Z0zm(ITHs8_nL(k6hWzWOKwx2*_Z*lM9 zAF!PVBM?z1(|*AK!~9m|-)CU6>pT31zRz&;m7ebAev{kcJoKQn(a)eWYtfddMzxX( zbawjnG1p^W?+s88o~rDgZ_SkuDg_8WdTe5U{n_2KMYa-97(x$AZs!hzlU6 z$IBDuv~jStp`?_))d_abPEoW21P4Mq^qO5+k2j7<)j$YmX~vwyrsjM(h}%JRlntGp6=5;15!EDQ zgKr`-?f~oFXyEur@-Pd>TfEh~IXNFo)Mzj+7=~74)rM+R8D5h*r>xlrY9KeM5N~X) zIy_lWkM1RLvdzY7k*oq~3Eq=Qf$dvu^I+Roo0kpn9Kfz^VP{{Ed)#G19jzXCP4%LS zSRH$NbGq)wX?=mmwL@BBk-a3uhKDy;@t7!+P|T%(@eg4y&B}_Bf-J*a3d33pZNASw|6TCx4qYG84%F1_uP6l&d15Zw!N@ z0Mnj;;ak$I!&K}rpJKrHKL$vYf773r0a81~aB3iEX{Viw>aBh`5c5{rUGgFlEzD4D z=-4F2c|jB@r<8M9#Qv(XRzn!|Z8y$>VYRh*$AE^Yh0uljBd%xg>PF(6m3ZiPskf@| zyunp+R507`M>M{rZ4Ut-qZ@z_+iL=#oWO#mR&~F0mPWqrQkKcGv?*lln|txO>uCOk zM9pW4_N>r8{)l{=YiWrYxg~@t-=Q#h2;#jKk4V3u&A_Y^!{CC^#M*EXa`KTE0qWA7 zT#B(q4=)7yqUhoXnqRRe4P_V?RzA+YebIQmvo}}X0Kenni-_giPueW3 zc3uvqZ7p8pCP+=7*ux9lf0Qpa5EsSVtIb=uZ0PQFul2eVb(GhXj|$LdDxk?L$R`L9 z;*n~bPo_f&yx+%1ZH_t09Rdm2^(OBad!8sJW@ezhboqhq?7BHi0xk)y z!?Sd^VNLM6uA1L%e65V@=}#aLEMIufV;_tKM@d&Fn3h=Jac-OUtyd$1Q*TebmW>Y& z5?mLaYduHl>d?0a-%DpwF0D`RIe9zqR~2r&$K(S|@DE z_bFwgOG>IOFK+`RV-I)t{0e|u1Cnuf{lLEXUvB_p|A6i>XY_*h*Y#oOwwUm3X+cN9x)uO znJWAb{}vx=?xhS_3PEIJUli-x-P@~OiJPmR+{lZ*{{X?UT zI+kW2`;pYjQidl03&TI1mLxBpa4Gtn-%hy4JdPDNkZ+_Vh8V&7RDto#M|U7XYk4To z)0iq{+RkxX@lX7ahiq$cScM^yYx0kk*B`9G9clzSd^?F`5yo+Fg(j$I1Z7_S{tZB@ z-+~Tr&bC!TiLaqR3k&3*5YK=#^?xHSTpjYZfFJ+K72wRfVeXV73VEO9zIzE~%m+ZO zW#aIv3AmG79Jm`;;>)nT>>tddGR_SZ$yfh+uK=irsrL0=-=VIil&8^Z>&X?<0Z57G(ohT zOTGY8=e=I4a@}y}z#ih?L>h!?YexCL@e|MYt=hTrSRxvzFJ=9W;Zr2@K9t|So#%e{ zR3cz{WV-Gb`NBh<;I(=7?_4edyzj((IE>^z5~9j{ZtK23t$XsW5Fbs@TFH5r@-bb3 z`+iW)pA?W6j+j!gt`M_ci91{k+L8am5bm&Gs%Aj^$QCjbAe-P?@wd$ve!Jg&qk={$ z0P)-7)u>IMODt9xX|UGsczjkI6cnv;cfyUKfD`ZR8TNt;mL8YSj8Zm*1pkA91@L&4 z@%y5RZs($5Zp6LUNAuKtLCQw2CuhsjT*T3~4|=cJGX7{!hc}N33(&`yKibs5#UR^W z?qVm9n!-Wmd-Z`T6|=@Yu8*HTvM5B~pz>KLad!-AlJ3VQ$mjHVJUx&8Hx+sn3Aii9 znixp6i7>W3Imt!z|1qmOF`Q zXHKgzfAprwi*63rNwzcg@5X#FTki(LnndJE>Zhx9DLA1OeH*w~hJkX?fP9`0#mcN5 zhfV=eq@V}9w6aK5iA@%iNHN$w65i&peC9_`vq!K0BK6@d$I%+Mb1K)rM73HL_Pe^2 z45yAdq65v^fx$R>+sMb0DM-iB zo#g0?l4{B}LK`UK4xJdcTlrPA6jNuc5_#vU(2L^&8lfF>#izt{ea5F}(P(wUSUinO z9_(1_n-AEr=oV(PD`10F3^*904!yX`tzb>ks`JG^VY8YkHW>+-W<=}sVvcRU;-7fmst5I?+e+Mzw7He*V1(@a2FngAj_Q`Y3G0% zX6|@l${5(yZ*t-K zzi5xU%>ysa*);_efKy!)&bTW+`3OOYG6VvM;6@{Mp`CqMnQQhJJs4~PrhJ`I@=ru5q z1P=EN{Udbu5#LQT3EPnHQQUEpRj*z5r9^CT*|mR9H<@AJEgR;G-SiHdfs@E3`51jTcbK>l{9bH0VyibNLzRI8XVUL3=z8B4 z7%X@0-#uk+o^2C+`pTl_;VJwPfexG~uPs1K4sxw#-)F5JKNL&d{lS}_J)-98CMqqo)0eMnu0{3-zl=MPUx{r`82S18eN+=KAgS!3gX5Bv1CwRMIi7x!=jx_?i88pT*O&`N~R)%EJxGtJ-_;WYY zrRPV5lr883d%l+!Uc>ovD8eI391aeDnwPG*8&ekX-J%@l!}RY--k^&@oe1czNRNeL z6%yY*GKOho9y1oCo@H&Jp^o}_Ph}K}l+4``?rRnBEh|r7*PhM`>m_)EE9!Jc2}IR2#h7guo5K{c~QW)sCzjcZ$c`q zju*G0OIQ`*fgANvwBzxSMKG$U!OVIt(y!IMJ!uQ{>hBwsO%X&)w2CPC@{#!w{E~)N zP8e5|_{C)A-!$|Wsbt+|o<%l}rJg)|T_qd{b;Rfw(I2XRy=7rGQ`Gw*44mpOm4HSg zKe%xx@n1POrSG{Kd}}kUqJaqq70f3?FdQj#YSKE^maFg~K42{7}_{@(>a8@JS&0sLz2m`es|&bH#j3}oWPsk{A9nP(1g>Skp?w~_%mPBSZ9GN@g7S5@?QYPL&yh44rXe&&aH{04 z3^ut^Nk%HR@yHeFl~+YH$*VuMgzTRVTdPCSzm<*DVa+E)-Z)~G^bhm!6T(zgOrYQ- zH~$wj*Sm3g_{}I?z7KcotO*)r;KItER z5Tl)lGSeuM1tb}xc-og<`cze4Yr>1-`CJtmKm^Yfruvj7k&olGp+Rr%={$56Mrf(m z;tS*R%0#vw5r-wllQhiYSK+ToQ_kc;k|P_61_$?=QX@xV*Tm#WCvQp`LQm)<`Ea2P ze3CvSiD+RKDE+D0@{R$Uvjc(&cI34qTFGMav1lvZZbEeZ@F;SW;^}kVEeXz-I1%K^ zL|)vPOQV&?u2#=5k(J;#&XJ2O$$|$ilPv?b(1piqL(G+6^10=mG4@6Z!UEbDZTR8d zbC7h~wBB97tFD)d;b%ugwF6<$}qd8!C$D2ltj#i$`~t)1J603zFrDpq1OISvc2NC z)Ib#hYW{}=<)Sd&)@uugbA|Jf`|yf&O}IbD+?>k~tih*GRvEM<;C`xTDJd7|wSimJ zChMPjQkJuZxYU3{l0z#{@Q}bNOp=yvqm(naehHg9F*ht{Kup9&fpX>jg0UEg`J)Gq zFN`b#_dgehX@v=zKo`Vu-ckPX7S(fGrgU1eRngfe&!H7D$*iX1$;N-MKE_~vo5|PJ zf%#ik0uCZ`$~yQF&81l)6WA8s{<(w1p}9wSobnijR>b#GMa0aZ9sk9-41#3N!M;^7 z{K-4n(%T0K{W+KwApxQuCj#!m5+0&q;!Q1vx2}shpYELJ)^{`X;97A8g?a)FS@*T! zg7wh~QpU%{Q^>Jl-bo9F&=SPi8bJvT#FPCHhYy3LIm)De^Om6Y6P z&l9nAC8_X64!$OckP+i9ni!IWFwZjx#QOv&|AEut(O>DnO^(ASEZ9j<^1<_ku_d|m zO$WE*r43-?$)8_~e8bAlG+E<@2n-0mlp;UTbv~!RmZbhAgxBaLP|7XTBlI#zUUnvX zwe${oSwK@j;__hQDKmaKST+xWdZogg8UUpykC-`Hj7X>)d)dY;gwx~^VViivD(stQThrS4=y^M=Xs?GQ zowMA#c^s%&N9#yzhOd)}Cx=sJ`)S!THce>%kH}Uzxo@eK#zezw?;=&fZL(zENJuP= zlVcp~V4olazM(fe!6Q>U?vaN!>(Wj_H(JT@Ku_k=%wIUjL8Jxpn4uXyPRXV%4g*pe z|MaSu!(j|Js3$uJrm!Mm6zgx&NhCdfNfxtdkYq`fOU3(f^EjrrwC^#m>lfES?}W$3 zh%UYMTC!->gDYQ;Vpw&xxon~2xH+syCLO{$1==vQ2M1WQrSVd7(iBR&XI`TsMo#l)@CD#%{M>1hEU2+Y@qA?jnZHKHT9P~I6xTQ~{}90)-H#DvF`#?8 zs!tVhGk)9_BNamk4PBDtj8ghLt|jQ{+d=Q%Z)8kp)lqyqO)Cd0^oL>hChTj0MPKG7 ztOie^CUUJ1j*N7Nf`!%0`j1EM%Rw`o$z$=^)Ey7+eN~}$G^mSI1GiY!tIMzr7{~iE zyv&nk5M(MLO}Or1a4T%&rG`!~;G+Z@PE&c`c6WZnFK^)E)YUj3IZi?^7cyMx!g<*x z?VtCyZ)Cj=jNHgUrbSh>dw&{>$&*mPmD(T zSQXS#cnPB7Q8;7YnO~}97%rPO?OrZ~2ihIjP`U~`%Rn1s6}-Ddk~ULx;T^^NVI4n6 z){1Ci$)SzzvNTioqOIdED_P zc26gwyZv-yk!?QEnZmW%949%bq~RGQ-OF%sU~ z_YmK3ugB05A2i=f&B>w{*slqMcnHmHb?-c4RLE28pb#hL?R zNT54j$H>x^zyu<;RSmSQ-Li<4U&y}V?GfkTCx6X?5Tuq5;5cX$O!Bl;x4ff4jUff- zKBk0q`fq(JmD+TrkjD%7ccBOWBftyF$aI{rn0T?4gti9t?@)kCF8BG$(q$pOx|D16 zTG1DA=4D>t`ZQ>rxQ@Za$^eB7xtux5NOY=8>fb*#@Ga5(sx=&%28a4#{4#rcTDf>y z3CvL2^?gnI01et=J~a-Ks;crtMieKQsocZFP2(}0dINI?a?T1eFR*OlQ}HK;GkHS# z5IHII)1{(uDb~3_SC~wMx44N%5f8ZCJ)z*&6jG@bI874q{ZEG4dRfG92zUgB}F>^=^~HeGpEI*8E)Es zzDW!zZ^@uXIN#?$M!QCg;}lY3^>)Mysz5|t$79RISU*JlT~+W=l=KCsl&kg8st89p zk21$KQisBEXA6nO`*Okaj(CzydknQoJKov*9=CY!;P$V+J8hyE-BR#MOO`da5|rE# z#I9C!93@QQDI5SHK@GmDrd;Xed8Xm^Rb6HFM$pHWgNvT9lLL3k!hoLF)XdfRc2A)y zZjnDZGpuOrpglaH14Kdo8jB#3VN$?bzn5g+$hu%kYD(bwy7ElkCYFelSJQ5Ztv6(k za3;|Lw+EjYR|F5&!Nj0ZnHq(*<}hO*T1r~M7zi|dHaSywG?5P4^2bM-zh7_kyq~8W z0~7{}G^Jp0!NUM16sJ6{dTcy-N1=UWc#-4|-Sz!dK{|Hge_;t{77VeD8-%)WA>6pM z$TINTA7ieOH3hPe4s8Na^&*qA2=`>w`lb)=e0~8_kCp)A!y%a+<$7G~7XV*7a2u0z zjKvQ$oG4cb{rx1}W8>r5=h_${l~>P=7&-pZG?dt(G~f;dpz00mSOu@9kT6Qqi-JI6 zWRO-kk2bV6Frr>GO~B-94p&~aX5dBl7ci;jAY~ly6cX3a0?K#XP9H>3+uz2HJ1Ghq zP>7+-s!gQTWEG6*AVb>rcgW5OCGfj{Do(x@*|?M=)kXpoe1=%kh$`|+Io#vK(rkl8pJGB zWE3EcA;ncN^|8@atF#~ylr7v`oH3zuk`#8;O;E7lYoF+I3(+ESI8z>xz+WKRLb`T@sckBZlykDnILi|VzSBrpB!(sn=2 zT1<02T|IrA@LQ_{YtcTv?Tg1zfKunq+hcSDjz#0cz&_Ec#T0*th}3MuT8m-I8L5?A zh_U`LxCw!c4^ebg9QK${JT8DQ;E>mChl=5g8GpLzFtx7zgHNQTB+`4AQU!`mQfa2p zh2(>e(IhAfOWlb%4+Y=+K_O_Q2Yuig@G02C8ru7KYZTl8mq+>&EHzUNR}5zwBinEE zFN$u;!7pkQLILtct+=JV+7N>FIxI5`nk*suxFE1f$DikQ>0^d0 zsJN-HjA8`T_w;z&p9nYG$F?kFO>P^<;rb%UnV-@rm($QpnS}QRf1`c3iGhx|@&xi> z>L=ewCiFX^ScJ2B!Ba?`g`&Ld3c~pw(Z6pjmynl)!Uk6c75HH>Mua@^ZF+kNs!`HU zxkIswPk5+nldpVq_kSxk9w|Es@0~G^$M%xhDzZaLT+%@yA7l$+RG~rn{;7xz>3&uO zlL>P=tYGkVYOa`nLA>Zu{85&4i7IUCHeM($Lv8Wf znn)yBN`48u)^LnhVMBBD%|$}OuZyBzZ6nctZ1}qi*k}CGE(Tsi6%rVYGBPD|J{D{W zlSFa4^7bR&i$#s_PbHcdwx>J3B}z+AL^8(maW8C(FOFe$*n}+QDOuQ864*x3j=?A8qr&iQBXzpBcoCdj-U=#Cay-A{#u7EY zg#25@=r1$|B2HMv+FZmIv!Q|Z_xNnSKly{AKjznXE}n3qST*Sxy+RlT zq{k@7C&J_TO`o}Ffq3C81Sr}?x74CNaSzoHxYzP0^1>=Z!KNVCQP0P^=aQ`)6ltqP zYM?o&Yc+KN+VO!D+3DuJ{o6RVwJ&p>R^**&dtWpYhGJy8YHuwAFMo?$d{d=oIUBG{ z#eoSYJr!RXRm1nTC3wK93#E96Rs-dpA?41}4$ESA7PLVpsUy(PR7V9qUhYOo)GNHW zAwjuo-Y^bS@k)%r;6Tv#-OK?xUZt#R{8zQ|i02>luJyXKVY9FJrQ>deR!S$3N%}3@ zkEmh^q|@2(e@A8mb=Lz21Xbn_wPMzXTEYawdmK6zYSNDCCm2npfnseN+?}yRqNH^R zloCldVW6o0TbisW7xd02#qF4_@izOE9TrPRyh)E(W>7!PP8h9Jt+BDTL7usXOl;&_ zaMq(56~w{)8GID$W165y8CwQ9>d0Pcc=XD@rtjmHgVEuLwIv zuZxcyMtnvz+wwDhBO|C~nG_lcEWb~5Lq^YbzX~d8C^-)Y-M5GWZE6Y1FdL{U%n$O2 zqpszQ60{4(V$|S6&rXm(zR2a#hA%ji!jfQ^O9Cx#}_ zy?sc-muhBPB|Hv?xlK#hamtbv&4m%qOk2SSk7DnW0G}O4xM`EsQUR5s}@N~4+lrX{R+~OkEr!eVA44q!hvvS zLJ=K^0MKW;zd{Hud-6xQt6Igejf&o;k8!)9GZ*>A(^P_r!%o8o_^fJH@&#O^&o|d& z_;iVDaw6Y<#lemhQcRfW819^jLNr<_Sh}P5GvzjNRj=ZPT6_zBucgLRC{(z}Ts5jH zPqEraq~U%a$#|SS<$#0W)WWMFr&t?5Qal=Yw?iZ3s(UY!bl~>KSP<+-yrG+{uGWk+ZNPcCf|Mwg{5`@u(;sw@j0e+Li@1M-mUYD#L75pV>4 z3D{4Vjy~;gu?$`bVf*Q3bK&SGJ3psp%DzuL5ARo^B|#cmLKeC?F$6Sn5dLgcQpDDn zfXq$EsgtGy=;KUDCdUI*IMD9pI?H9W8gw3Vto+duem;UzHI9zD$8a51(8jsej;KdMf^fv+{i8aEn8}~zO9n{!T9(O%060^YntFsg~Q+ ze&RwOOS)@3kP;a;(yHV}G~YMqAcP34F5&CAfhcJyTDkC1Il{E>rMgyYBs5A0#mtE$ zEQ`>TX9C|8b@1x^(z4&JED3C{Bo9@9d)(oMLYd0rcX}F;pJC2Y5kn#^F$0sr;YL_5 zT|@ZsS7J%-wjxAxOjdlRSm_l9bV1$aaYYmUeU1n_5z}+V>R$B~7eRlV21sh^-y~$=x@WKFiK7YzISNnwr5 zKGDhH^K+qMQW-j)HZak21d$O>>YuWmG4a^#T%z>8c;m6C&+K970g5iDY-}n}TKRjJ zntQoi?JgFDcYL1UDPSM41*~4{_YSJ`Zqfh`w})N9if%`48D|P7B{nD1?ZNcw2wchg{c0SJYc;OWCqDl zd2iWGR;YmD%zJb*Z^Lyo)Tf2ZUswxwtWT;?B%}YTI*5Hm5hVKh3{M0;>w8ub>f-Fj zPHRtT)l&OrA{B2I(>fT`({l89t@_lD%>ESLrWsu`YM#EoXm>6d-<()CoQr%9tFSWo zp+~z5`n|ZKmLVhC`kEv^@!;R4_4n8+YC4oyYj#i%g6To_vr~su%M*9)sRo(tXi4iy{%-uchs}7u-o>+r6|12ks;48M8zp?N*U24YZ_asocf%hA+_UFT&z#` z0P?bqFu2o#uGY>5;=$R9aAxNDsi*@jc3y)I5xS*TdY|ahG6f4?X(m>0>#O zm#0qdv%j8Dz}O{#?-&g9&!>jjCbO0Flq3TsB72SBq3|FMX%WVI>n3%Y{lm7aBqr)j~Z z(ux`r8tR^it*sSI8?Ui__c(lBX?x>P{T}&nVgy!`?@WN~mAtt8fk3>~45=+QiXF%K z65px0iX5TT@sT=Q+BXKMd0viF_SF$o>AK9YG)BwF$PnwDXY|$`3d3>OOo@b(8E~ry z_Vi82PJ~ym#(S#Sfd~(p49T?)Ej%%L))QpK1K+2U${lYB)uiY$<9^c?@1QeMQ%Rhj z7&w^G^uS8W8sr4Uc})a=B)lwc2v%^1cCdYWl>s!7zlq*(;OkaD+!2kj`*yL3K0h;2 zsncgDk^ToJZL@RGTA8w-`qg+=vN_aG|3qKPES9!UJSWNXsRVBT!nE0lQlvDo6ue27 zLyP{T6TU%`$(m^n>di(IU|bugovdd(GfX>f2*roNiA_VwCeDjO56 z!}ePCo&0R{YPzcK13(aRr!jHP+?@>9-~Qqr$0$Uo*CC^rX30*Jv%g(tGC*8p_MJbo zzSzAgt&*hiW8;~_kP!c|L$rJi(2AQUS)?vB4ZFQ^&}N8`8JApr{4Ov~@(B9@s;ic= z!*M&7>%kyRGWdXNNq{cixhO+U9wVKZJQU=P{nVi(Uo|s88u`rocQrF_lW;O^gSJ{C z|NMjQ)QLZ`nyd}K92N$ zs-8jar2(JJW87ck+qWg*9D5RPdqe`f_X@hkXwr#fa?s+OQCI^V4Q($kIjynJA?<~% zF;oJALQMR0m%#4&rk8D^A+=m(3KUtAtpzV~-6BfmMpGKG1q?nj;7`GAH!s5N`N$@t z!4(=;ZgyX1GRZF`Ht9f+IV_z3TyTS*y!wg-C-Zu`G(4AOWKiNzI=#r(Yh@<)1o|go zbkBxY%zykVobL6vf(-gHp1QAwBRgtF)Va)YyoxrF^>i}tbW!=KDxZF7obGWiQz_G> zsa=ej_eh*rL=A3yYsVu!2~SnXrGllJX@1+>_rSG$k;+~Wt8#&_o2DJj<(=q)%3zD5 zpRMHG{D1@29B#QuI5ALnwppyB45OEbpD?Dj^Z=?&h9yG$_uuF)oa9X-d9{GV5)pSeRU8mWOqsUP*P2;^&nfbY7B46IW|KA@p>uMUoYSir_{s%!n^ML>W literal 0 HcmV?d00001 diff --git a/docs/index.md b/docs/index.md index 9f6f16ff53..56129b6206 100644 --- a/docs/index.md +++ b/docs/index.md @@ -86,6 +86,7 @@ ROS 2 服务与参数基础实验(无人机模式切换与在线调参):[c ## 2. 人形机器人 * [人形机器人基础](./humanoid/humanoid_basics.md) +* [G1 室内走廊键盘遥控](./humanoid/g1_corridor_teleop/index.md) * [人形机器人双臂搬运负载的神经网络平衡控制](./humanoid/humanoid_carry_payload_nn.md) * [双足步态生成与 ZMP 稳定性](./humanoid/bipedal_gait_zmp.md) * [基于三次样条插值的双足多步态平滑切换](./humanoid/biped_cubic_spline_gait_switch.md) diff --git a/mkdocs.yml b/mkdocs.yml index edf15e5906..80398fc47a 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -80,6 +80,7 @@ nav: - 人形机器人: - 人形机器人基础: './humanoid/humanoid_basics.md' + - G1 室内走廊键盘遥控: './humanoid/g1_corridor_teleop/index.md' - 双臂搬运负载的神经网络平衡控制: './humanoid/humanoid_carry_payload_nn.md' - 双足步态生成与 ZMP 稳定性: './humanoid/bipedal_gait_zmp.md' - 基于三次样条插值的双足多步态平滑切换: './humanoid/biped_cubic_spline_gait_switch.md' diff --git a/src/humanoid/g1_corridor_teleop/.gitattributes b/src/humanoid/g1_corridor_teleop/.gitattributes new file mode 100644 index 0000000000..f13e51c0ec --- /dev/null +++ b/src/humanoid/g1_corridor_teleop/.gitattributes @@ -0,0 +1,2 @@ +*.py text eol=lf +*.sh text eol=lf diff --git a/src/humanoid/g1_corridor_teleop/CMakeLists.txt b/src/humanoid/g1_corridor_teleop/CMakeLists.txt new file mode 100644 index 0000000000..b84a1b1593 --- /dev/null +++ b/src/humanoid/g1_corridor_teleop/CMakeLists.txt @@ -0,0 +1,6 @@ +cmake_minimum_required(VERSION 3.0.2) +project(g1_corridor_teleop) +find_package(catkin REQUIRED) +catkin_package() +catkin_install_python(PROGRAMS scripts/task_runner.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/src/humanoid/g1_corridor_teleop/README.md b/src/humanoid/g1_corridor_teleop/README.md new file mode 100644 index 0000000000..20ed779402 --- /dev/null +++ b/src/humanoid/g1_corridor_teleop/README.md @@ -0,0 +1,29 @@ +# G1 室内走廊键盘遥控 + +通过键盘复现 G1 预训练步态在室内走廊的前进、转向和停止。 + +## 环境与运行 + +使用 Python 3.11。在模块目录运行 `python -m pip install -r requirements.txt`。 + +在模块目录运行 `python scripts/fetch_assets.py /path/to/unitree_rl_gym` 获取固定版本的 G1 资产。底层步态来自 Unitree 预训练策略。 + +```bash +python main.py --assets /path/to/unitree_rl_gym +``` + +使用 WASD 或方向键前进、后退和转向,空格停车,+/- 调速。持续按键维持命令,0.6 秒无按键后速度归零。无窗口时可运行 `--mode smoke` 验证步态。 + +## ROS launch + +在 ROS Noetic 源码空间运行,将路径替换为实际值。 + +```bash +source /opt/ros/noetic/setup.bash +export ROS_PACKAGE_PATH="$PWD:${ROS_PACKAGE_PATH:-}" +roslaunch g1_corridor_teleop main.launch sim_python:=/path/to/python sim_entry:=/path/to/main.py assets:=/path/to/unitree_rl_gym +``` + +## 大模型使用声明 + +使用 OpenAI Codex 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。 diff --git a/src/humanoid/g1_corridor_teleop/launch/main.launch b/src/humanoid/g1_corridor_teleop/launch/main.launch new file mode 100644 index 0000000000..ebb8a1f2cb --- /dev/null +++ b/src/humanoid/g1_corridor_teleop/launch/main.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/src/humanoid/g1_corridor_teleop/main.py b/src/humanoid/g1_corridor_teleop/main.py new file mode 100644 index 0000000000..a5b826dec9 --- /dev/null +++ b/src/humanoid/g1_corridor_teleop/main.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python3 +"""通过键盘复现 G1 预训练步态在室内走廊的前进、转向和停止。""" +import argparse +import json +from pathlib import Path + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--assets', required=True, help='Path to unitree_rl_gym checkout') + parser.add_argument('--mode', choices=['teleop', 'smoke'], default='teleop') + parser.add_argument('--output', type=Path) + parser.add_argument('--dataset', type=Path, default=Path('data/visual_patrol')) + parser.add_argument('--weights', type=Path, default=Path(__file__).resolve().parent / 'models') + parser.add_argument('--samples', type=int, default=100) + parser.add_argument('--epochs', type=int, default=15) + parser.add_argument('--seed', type=int, default=7) + parser.add_argument('--seconds', type=float, default=45) + parser.add_argument('--record', action='store_true') + parser.add_argument('--controller',choices=['navigation','perception','end2end'],default='navigation') + args = parser.parse_args() + if args.mode == 'teleop': + from patrol.runtime import teleop + teleop(args.assets,args.seed,args.seconds,args.output) + return + from patrol.locomotion import Walker + result = Walker(args.assets).smoke_test() + report = json.dumps(result, indent=2) + print(report) + if args.output: + args.output.parent.mkdir(parents=True, exist_ok=True) + args.output.write_text(report+'\n', encoding='utf-8') + if result['fallen'] or result['displacement_m'] < 0.5: + raise SystemExit(1) + + +if __name__ == '__main__': + main() diff --git a/src/humanoid/g1_corridor_teleop/package.xml b/src/humanoid/g1_corridor_teleop/package.xml new file mode 100644 index 0000000000..d0ddda5d2b --- /dev/null +++ b/src/humanoid/g1_corridor_teleop/package.xml @@ -0,0 +1,9 @@ + + + g1_corridor_teleop0.1.0 + MuJoCo G1 visual floor perception, image control and ROS indoor navigation. + OpenHUTB contributor + BSD-3-Clause + catkin + rospy + diff --git a/src/humanoid/g1_corridor_teleop/patrol/__init__.py b/src/humanoid/g1_corridor_teleop/patrol/__init__.py new file mode 100644 index 0000000000..bc4204b4fb --- /dev/null +++ b/src/humanoid/g1_corridor_teleop/patrol/__init__.py @@ -0,0 +1 @@ +"""Visual patrol on a physics simulated G1 humanoid.""" diff --git a/src/humanoid/g1_corridor_teleop/patrol/locomotion.py b/src/humanoid/g1_corridor_teleop/patrol/locomotion.py new file mode 100644 index 0000000000..d072059b3d --- /dev/null +++ b/src/humanoid/g1_corridor_teleop/patrol/locomotion.py @@ -0,0 +1,85 @@ +"""G1 velocity-command adapter using Unitree's pretrained locomotion policy. + +Observation ordering and PD gains follow unitree_rl_gym/deploy/deploy_mujoco +(BSD-3-Clause). This module does not train the underlying walking policy. +""" +from pathlib import Path +import math + +import mujoco +import numpy as np +import torch +import yaml + + +class Walker: + def __init__(self, assets, xml_path=None): + assets = Path(assets).resolve() + config_path = assets / 'deploy/deploy_mujoco/configs/g1.yaml' + self.config = yaml.safe_load(config_path.read_text(encoding='utf-8')) + def resolve(value): + return value.replace('{LEGGED_GYM_ROOT_DIR}', assets.as_posix()) + self.model = mujoco.MjModel.from_xml_path(str(xml_path or resolve(self.config['xml_path']))) + self.data = mujoco.MjData(self.model) + self.model.opt.timestep = self.config['simulation_dt'] + torch.set_num_threads(2) + self.policy = torch.jit.load(resolve(self.config['policy_path']), map_location='cpu').eval() + self.default = np.asarray(self.config['default_angles'], dtype=np.float32) + self.kp = np.asarray(self.config['kps']) + self.kd = np.asarray(self.config['kds']) + self.command = np.zeros(3, dtype=np.float32) + self.action = np.zeros(self.config['num_actions'], dtype=np.float32) + self.target = self.default.copy() + self.counter = 0 + mujoco.mj_forward(self.model, self.data) + + def set_command(self, vx=0.0, vy=0.0, wz=0.0): + value = np.array([vx, vy, wz], dtype=np.float32) + if not np.isfinite(value).all(): + raise ValueError('Command must be finite') + self.command[:] = np.clip(value, [-0.3, -0.2, -0.5], [0.5, 0.2, 0.5]) + + @property + def yaw(self): + w, x, y, z = self.data.qpos[3:7] + return math.atan2(2*(w*z+x*y), 1-2*(y*y+z*z)) + + @property + def fallen(self): + return self.data.qpos[2] < 0.40 or not np.isfinite(self.data.qpos).all() + + def step(self): + c, d = self.config, self.data + for _ in range(c['control_decimation']): + d.ctrl[:] = (self.target-d.qpos[7:])*self.kp-d.qvel[6:]*self.kd + mujoco.mj_step(self.model, d) + self.counter += 1 + w, x, y, z = d.qpos[3:7] + gravity = np.array([2*(-z*x+w*y), -2*(z*y+w*x), 1-2*(w*w+z*z)]) + phase = 2*np.pi*(d.time % 0.8)/0.8 + observation = np.concatenate([ + d.qvel[3:6]*c['ang_vel_scale'], gravity, + self.command*np.asarray(c['cmd_scale']), + (d.qpos[7:]-self.default)*c['dof_pos_scale'], + d.qvel[6:]*c['dof_vel_scale'], self.action, + [np.sin(phase), np.cos(phase)], + ]).astype(np.float32) + with torch.inference_mode(): + self.action = self.policy(torch.from_numpy(observation)[None]).numpy().reshape(-1) + self.target = self.action*c['action_scale']+self.default + + def smoke_test(self, seconds=12.0): + start = self.data.qpos[:3].copy() + min_height = float(start[2]) + while self.data.time < seconds: + t = self.data.time + self.set_command(0.0 if t < 2 or t >= 10 else 0.3, + wz=0.25 if 6 <= t < 10 else 0.0) + self.step() + min_height = min(min_height, float(self.data.qpos[2])) + if self.fallen: + break + return dict(simulated_seconds=float(self.data.time), fallen=bool(self.fallen), + displacement_m=float(np.linalg.norm(self.data.qpos[:2]-start[:2])), + min_height_m=min_height, final_yaw_rad=float(self.yaw), + final_position=self.data.qpos[:3].tolist()) diff --git a/src/humanoid/g1_corridor_teleop/patrol/runtime.py b/src/humanoid/g1_corridor_teleop/patrol/runtime.py new file mode 100644 index 0000000000..c848fe6d23 --- /dev/null +++ b/src/humanoid/g1_corridor_teleop/patrol/runtime.py @@ -0,0 +1,46 @@ +import json +from pathlib import Path +import time + +import numpy as np + +from .scene import IndoorSimulator + + +def teleop(assets,seed=0,seconds=45,output=None): + """Interactive MuJoCo viewer; arrow/WASD commands expire after 0.6s.""" + import mujoco.viewer + sim = IndoorSimulator(assets,seed) + command = np.zeros(2) + speed = [.25] + last_key = [0.] + events = [] + def key(code): + if code in (87,265): command[:] = [speed[0],0] + elif code in (83,264): command[:] = [-speed[0],0] + elif code in (65,263): command[:] = [0,.35] + elif code in (68,262): command[:] = [0,-.35] + elif code == 32: command[:] = 0 + elif code in (61,334): speed[0] = min(.4,speed[0]+.05) + elif code in (45,333): speed[0] = max(.1,speed[0]-.05) + last_key[0] = time.monotonic() + events.append(dict(key=code,command=command.tolist(),time=float(sim.data.time))) + print('WASD/arrows: move, space: stop, +/-: speed; repeat presses to keep moving.',flush=True) + try: + with mujoco.viewer.launch_passive(sim.model,sim.data,key_callback=key) as viewer: + while viewer.is_running() and not sim.fallen and sim.data.time.6: command[:] = 0 + sim.set_command(command[0],wz=command[1]) + sim.step() + viewer.sync() + time.sleep(max(0,.02-(time.monotonic()-start))) + finally: + sim.set_command() + if output: + output=Path(output) + output.mkdir(parents=True,exist_ok=True) + (output/'teleop_check.json').write_text(json.dumps(dict(events=events, + final_position=sim.data.qpos[:3].tolist(),fallen=bool(sim.fallen), + simulated_seconds=float(sim.data.time)),indent=2),encoding='utf-8') + sim.close() diff --git a/src/humanoid/g1_corridor_teleop/patrol/scene.py b/src/humanoid/g1_corridor_teleop/patrol/scene.py new file mode 100644 index 0000000000..df750e8a32 --- /dev/null +++ b/src/humanoid/g1_corridor_teleop/patrol/scene.py @@ -0,0 +1,112 @@ +"""Generate indoor geometry without changing the upstream robot assets.""" +from pathlib import Path +import tempfile +import xml.etree.ElementTree as ET + +import mujoco +import numpy as np + +from .locomotion import Walker + + +def build_scene(assets, seed=0): + asset_dir = Path(assets).resolve() / 'resources/robots/g1_description' + tree = ET.parse(asset_dir/'g1_12dof.xml') + root = tree.getroot() + root.find('compiler').set('meshdir', (asset_dir/'meshes').as_posix()) + world = root.find('worldbody') + pelvis = world.find("body[@name='pelvis']") + for geom in pelvis.iter('geom'): + geom.set('group', '1') + ET.SubElement(pelvis, 'camera', name='front', pos='0.16 0 0.35', + xyaxes='0 -1 0 0.35 0 0.94', fovy='75') + visual = ET.SubElement(root, 'visual') + ET.SubElement(visual, 'global', offwidth='640', offheight='480') + ET.SubElement(visual, 'headlight', ambient='0.4 0.4 0.4', diffuse='0.6 0.6 0.6') + rng = np.random.default_rng(seed) + brightness = float(rng.uniform(.6, .95)) + ET.SubElement(world, 'light', pos='3 0 4', diffuse=f'{brightness} {brightness} {brightness}') + ET.SubElement(world, 'geom', name='floor', type='plane', size='15 15 .1', rgba='.65 .65 .60 1') + boxes = [('left_wall', [4, 2, 1], [6, .12, 1]), + ('right_wall', [4, -2, 1], [6, .12, 1]), + ('back_wall', [-2, 0, 1], [.12, 2, 1]), + ('end_wall', [10, 0, 1], [.12, 2, 1]), + ('door_left', [5, 1.65, .9], [.12, .35, .9]), + ('door_right', [5, -1.65, .9], [.12, .35, .9]), + ('cabinet', [3, .95 + float(rng.uniform(-.15, .15)), .45], [.3, .3, .45])] + for name, pos, size in boxes: + rgba = [*rng.uniform(.25, .75, 3), 1] + ET.SubElement(world, 'geom', name=name, type='box', + pos=' '.join(map(str, pos)), size=' '.join(map(str, size)), + rgba=' '.join(map(str, rgba))) + ET.SubElement(world, 'geom', name='inspection_marker', type='box', + pos='9.84 0 1', size='.02 .35 .35', rgba='.1 .7 .8 1', + contype='0', conaffinity='0') + return ET.tostring(root, encoding='unicode') + + +class IndoorSimulator(Walker): + def __init__(self, assets, seed=0): + # MuJoCo 3.2 parses an XML file; generated files remain outside the repo. + with tempfile.TemporaryDirectory(prefix='humanoid_scene_') as folder: + path = Path(folder)/'scene.xml' + path.write_text(build_scene(assets, seed), encoding='utf-8') + super().__init__(assets, path) + self.renderer = None + self.floor_id = self.model.geom('floor').id + self.rng = np.random.default_rng(seed) + self.odom = np.zeros(3) + self._last_yaw = self.yaw + + def step(self): + super().step() + # Simulated velocity/gyro measurements, integrated independently of position. + dt = self.model.opt.timestep*self.config['control_decimation'] + yaw = self.yaw + rotation = np.array([[np.cos(yaw), np.sin(yaw)], [-np.sin(yaw), np.cos(yaw)]]) + body_velocity = rotation @ self.data.qvel[:2] + body_velocity += self.rng.normal(0, .003, 2) + dyaw = (yaw-self._last_yaw+np.pi)%(2*np.pi)-np.pi + self._last_yaw = yaw + self.odom[2] += dyaw + self.rng.normal(0, .0002) + c, s = np.cos(self.odom[2]), np.sin(self.odom[2]) + self.odom[:2] += np.array([[c, -s], [s, c]])@body_velocity*dt + + def image(self, labels=False, depth=False): + if self.renderer is None: + self.renderer = mujoco.Renderer(self.model, height=96, width=128) + r = self.renderer + r.disable_segmentation_rendering() + r.disable_depth_rendering() + r.update_scene(self.data, camera='front') + rgb = r.render().copy() + if labels: + r.enable_segmentation_rendering() + segmentation = r.render().copy() + r.disable_segmentation_rendering() + return rgb, (segmentation[:, :, 0] == self.floor_id).astype(np.uint8) + if depth: + r.enable_depth_rendering() + result = r.render().copy() + r.disable_depth_rendering() + return rgb, result + return rgb + + def scan(self, count=180): + origin = self.data.qpos[:3].copy() + origin[2] = .65 + ranges = [] + geom_id = np.zeros(1, dtype=np.int32) + # Only static geometry: no robot self returns. Angle frame is level base_link. + for angle in np.linspace(-np.pi, np.pi, count, endpoint=False): + a = angle + self.yaw + distance = mujoco.mj_ray(self.model, self.data, origin, + np.array([np.cos(a), np.sin(a), 0.]), np.array([1,0,0,0,0,0], dtype=np.uint8), 1, + self.model.body('pelvis').id, geom_id) + ranges.append(float(np.clip(distance if distance >= 0 else 8., .05, 8.))) + return ranges + + def close(self): + if self.renderer: + self.renderer.close() + self.renderer = None diff --git a/src/humanoid/g1_corridor_teleop/requirements.txt b/src/humanoid/g1_corridor_teleop/requirements.txt new file mode 100644 index 0000000000..f55b4da02e --- /dev/null +++ b/src/humanoid/g1_corridor_teleop/requirements.txt @@ -0,0 +1,6 @@ +mujoco==3.2.0 +numpy>=1.24 +torch>=2.0 +PyYAML>=6 +Pillow>=10 +imageio>=2.30 diff --git a/src/humanoid/g1_corridor_teleop/scripts/fetch_assets.py b/src/humanoid/g1_corridor_teleop/scripts/fetch_assets.py new file mode 100755 index 0000000000..71f10aa86b --- /dev/null +++ b/src/humanoid/g1_corridor_teleop/scripts/fetch_assets.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python3 +"""Fetch the exact upstream model/policy revision; never overwrite an existing path.""" +import argparse +from pathlib import Path +import subprocess + +REVISION = '276801e46c5d433564f24658bac64f254b7d2d4b' +URL = 'https://github.com/unitreerobotics/unitree_rl_gym.git' + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('destination',type=Path) + args = parser.parse_args() + target = args.destination.resolve() + if target.exists(): + raise SystemExit('Destination exists; use a new directory or validate the existing revision manually.') + subprocess.run(['git','clone','--filter=blob:none','--no-checkout',URL,str(target)],check=True) + subprocess.run(['git','-C',str(target),'sparse-checkout','set','deploy','resources/robots/g1_description'],check=True) + subprocess.run(['git','-C',str(target),'checkout','--detach',REVISION],check=True) + print(target) + + +if __name__ == '__main__': + main() diff --git a/src/humanoid/g1_corridor_teleop/scripts/task_runner.py b/src/humanoid/g1_corridor_teleop/scripts/task_runner.py new file mode 100755 index 0000000000..c1112df063 --- /dev/null +++ b/src/humanoid/g1_corridor_teleop/scripts/task_runner.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python3 +"""Launch standalone simulation/learning tasks through a ROS launch file.""" +import subprocess +import rospy + + +def main(): + rospy.init_node('humanoid_task_runner') + args = [rospy.get_param('~sim_python'),rospy.get_param('~sim_entry'), + '--assets',rospy.get_param('~assets'),'--mode',rospy.get_param('~task'), + '--weights',rospy.get_param('~weights'),'--output',rospy.get_param('~output'), + '--seconds',str(rospy.get_param('~seconds',45))] + child = subprocess.Popen(args) + try: + while child.poll() is None and not rospy.is_shutdown(): + rospy.rostime.wallsleep(.1) + if child.poll() not in (None,0): + raise RuntimeError('Simulation task failed with exit code %s'%child.returncode) + finally: + if child.poll() is None: + child.terminate() + child.wait(timeout=10) + + +if __name__ == '__main__': + main() diff --git a/src/humanoid/g1_corridor_teleop/third_party_license.md b/src/humanoid/g1_corridor_teleop/third_party_license.md new file mode 100644 index 0000000000..9ad8f6dbb0 --- /dev/null +++ b/src/humanoid/g1_corridor_teleop/third_party_license.md @@ -0,0 +1,23 @@ +BSD 3-Clause License +Copyright (c) 2016-2023 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics") +All rights reserved. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/src/humanoid/g1_corridor_teleop/third_party_notices.md b/src/humanoid/g1_corridor_teleop/third_party_notices.md new file mode 100644 index 0000000000..01dd8c97e2 --- /dev/null +++ b/src/humanoid/g1_corridor_teleop/third_party_notices.md @@ -0,0 +1,20 @@ +# Third-party sources + +The G1 MJCF/STL assets, pretrained `motion.pt`, policy observation convention, +PD gains and action scaling originate from +[Unitree RL Gym](https://github.com/unitreerobotics/unitree_rl_gym), revision +`276801e46c5d433564f24658bac64f254b7d2d4b`, BSD-3-Clause. +The upstream copyright and license are preserved in [third_party_license.md](third_party_license.md). +Assets are fetched separately and are not duplicated into this module. + +`patrol/locomotion.py` adapts the upstream deployment equations and observation +ordering. The pretrained walking policy is not an original training result. + +MuJoCo supplies physical simulation, rendering and ray queries. ROS Noetic +gmapping and move_base supply SLAM and classical navigation. These components +retain their respective upstream licenses; no claim of a new SLAM algorithm is made. + +FloorNet, MaskController, ImageController and CostNet, the training data generator, visual corridor +controller, indoor scene generator, ROS stdio bridge and evaluation code were +created for this project with AI assistance. Training weights under `models/` +were produced by this module's training script.