From 221a636c8dd86546a952a86a7211aae88bad1b34 Mon Sep 17 00:00:00 2001 From: Tyler Date: Fri, 20 Sep 2024 15:32:11 -0700 Subject: [PATCH] adding wheeled files; TODO fix racetrack terrain C++ error --- .../data/Props/terrain/racetrack-terrain.usd | Bin 0 -> 114057 bytes .../data/Robots/UWRLL/mitcar.usd | Bin 0 -> 7157 bytes .../isaac/groundcontrol_assets/__init__.py | 8 +- .../omni/isaac/groundcontrol_assets/mitcar.py | 57 +++++ .../isaac/groundcontrol_tasks/__init__.py | 63 ++++++ .../manager_based/wheeled/mitcar/__init__.py | 0 .../wheeled/mitcar/agents/rsl_rl_ppo_cfg.py | 42 ++++ .../wheeled/mitcar/common/__init__.py | 15 ++ .../wheeled/mitcar/common/actions.py | 14 ++ .../wheeled/mitcar/common/commands.py | 9 + .../wheeled/mitcar/common/events.py | 42 ++++ .../wheeled/mitcar/common/observations.py | 30 +++ .../wheeled/mitcar/common/rl_env.py | 33 +++ .../wheeled/mitcar/common/scenes.py | 64 ++++++ .../wheeled/mitcar/mitcar_manager_env_cfg.py | 206 ++++++++++++++++++ .../mitcar_manager_racetrack_env_cfg.py | 183 ++++++++++++++++ .../manager_based/wheeled/mitcar/utils.py | 118 ++++++++++ .../wheeled/terrains/racetrack.py | 56 +++++ .../manager_based/wheeled/terrains/rough.py | 45 ++++ .../utils/runners/common/rl_runner.py | 20 ++ .../utils/runners/rslrl_runner.py | 200 +++++++++++++++++ .../utils/wrappers/torch_clip_action.py | 37 ++++ source/standalone/train/train_rsl_rl.py | 145 ++++++++++++ source/standalone/train/train_sb3_rl.py | 148 +++++++++++++ source/standalone/train/utils/__init__.py | 10 + source/standalone/train/utils/app_startup.py | 150 +++++++++++++ source/standalone/train/utils/args.py | 65 ++++++ 27 files changed, 1756 insertions(+), 4 deletions(-) create mode 100644 source/extensions/omni.isaac.groundcontrol_assets/data/Props/terrain/racetrack-terrain.usd create mode 100644 source/extensions/omni.isaac.groundcontrol_assets/data/Robots/UWRLL/mitcar.usd create mode 100644 source/extensions/omni.isaac.groundcontrol_assets/omni/isaac/groundcontrol_assets/mitcar.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/__init__.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/agents/rsl_rl_ppo_cfg.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/__init__.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/actions.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/commands.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/events.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/observations.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/rl_env.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/scenes.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/mitcar_manager_env_cfg.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/mitcar_manager_racetrack_env_cfg.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/utils.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/terrains/racetrack.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/terrains/rough.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/utils/runners/common/rl_runner.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/utils/runners/rslrl_runner.py create mode 100644 source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/utils/wrappers/torch_clip_action.py create mode 100644 source/standalone/train/train_rsl_rl.py create mode 100644 source/standalone/train/train_sb3_rl.py create mode 100644 source/standalone/train/utils/__init__.py create mode 100644 source/standalone/train/utils/app_startup.py create mode 100644 source/standalone/train/utils/args.py diff --git a/source/extensions/omni.isaac.groundcontrol_assets/data/Props/terrain/racetrack-terrain.usd b/source/extensions/omni.isaac.groundcontrol_assets/data/Props/terrain/racetrack-terrain.usd new file mode 100644 index 0000000000000000000000000000000000000000..bdda1f19b14a443a036baa9cdbdb500dd59a689f GIT binary patch literal 114057 zcmeFa2b>f|(?8xlb9ZoXzySvw@s=Pt=XAF_yW|`NBz1{A zBCX(`f1gyy|D*3nA$b4o8>bt0Yvy)DhC2x0J2s<3quXY7xS>IpjXx1g-{#rM0Xc*T zXlDi3DvuD6p5+`CC&B>xFfPQCWRKn2Zo}!u)jV(ds;(k<$G(qZ9diiM(PhWK(yK^P zwIz*{k6vi>KEW;aeN~(HH|C2I5rF?NKhw7*+hS9}1Y6FN^7A~0#fdckjBH0D%E&># z_&c(X*s`e%opOxii0M4j8ODipfK?d}m1BC`JNzmr&J(54SgbHT9xnJekpW>t6(g4` zp&8*qGAjD$*=2f+w#p(}v_?~Mr6=>>&`eJjjV8noEN`-%epZAvf}1Ys4K|tSGC;Nk=Uw)>BTS83t9BSPw~a(o1La zRnTY+8#ZLViW;qad8I=ojpmePWzZbGdiCn%cHDQiURAVkpwu_B3!@GTb%5Hp6aU2V zkZcjt}k3axc+by z-;w(~aJ%V>m?leuxD{72R|o?4K{GFK-*S_vLQwy~KknRRxX5IPV~m?;=qcm(kc%3~ zi*%Q*b#qfh*Nzm`Th(rDiWbeY)ovXVA&lapTLNz# zCL%JEkmvx=4oV4gq!0~-QC5Uy6Gm>4K0?R^LR1jxV}+4Pq9?o0E5@lYBvow&W*l+SLI ztv9uLN4DJDVoURPn{Aa%-)r)|Y`m?}2MxE&20QBSl=XJi-7V|vsl8X$+UMIZy@^(m z^d!6eG9eHjlr<03I4G+hs&-gbJyIn_Rz6zkSjFSA!in-H%Y7)zo+|TE>C>e?E_p_l zI9vQ&vGcO%Cq+Ij{F!{_^Fm+9f?pQ+O6I@d`dY?)lkeNS7iFIBa(^##{UCE*iv3aM zxSahbneAsOvtG^ei_CmY%1k$8#+w;#$*9{>ru#MWu1xz|ntL+hcNzZ2DRIXrEgu@? zPs(y1mOUjYSvf{@m&MK({X~ux+hn0H3VtaIe3k!#94ETUychF)C&!~qa$b_LKjyeB zIjxv06U2?Q(bthu#+x$3EhLowPP$)Z#3$UQFaRqecGeDfMLqS?zGR=o0_Hl#jx~#ho~?=mr`4VBzuA6Whx;4au5WjLQ|pb--rv$4_x^t0zFPOUi$}5dgYkic`-iF@tZ_f3%8{y9KY!-4!f8G( zb-J|qVY!ppWarggkb3z2a8cHCCFbg*82Qc5$Z)4Mvg6uz>t)*wzZt8Wz9*Z!-(ZKV zzw@He+>n<08c9%WpejX%$2OUq>Piu zP?7HrJ$xh|gvkdM9&P$NKN&@2ns$a4-D~(Uvs9&%_06zjM*8|@rlUr5OGR{474e3d`@1~yVqW>MoKY%EX>WL@I-+EB^g^_AV~CmQdbGTjd0lwb zxNl^Eh={e}@1hzb!&haNUy7qY=qTbYG&a(Uf+D(jxR6ssM7X$})vR#5qC8gVe%K%3 zzen6l^IO`xk-w(9lm2$ptqeCa-hj*pk>Yx;Yngw^ay9Ffn4h!#l>Ks!A7d}<1M9 z9@<>Y`)$5&;=XoW$p2M=FAIKA=vvsRPm9P;ik>fauK3v!XG;DN_It;V%A6{TM_c~H zQ2$F)%#d=}EID+xd~r_IBNA;GPfwx^%kn4tEtY*>>+^c=C9>Djp38bH z@4lkj%C2v8S%tdMX-&tqvctOeZ$9^yY`4Dc2HECq^d+q~$yV=BS0Znu$u4iEnJ>|E zNOT>zFDI>}?BSVirjhcQaD?TMx6;UwY08#3D{rSUZlwvonI__tEFk5JX(pfxsPl2D z2m6PJ^0oHKA!uQPMROS+ta&hVnrQi}D2qDwtB4pNlBGmTFZHoR2P2&Ur5eV7v@{T; z{?dA?1E+zOehyy^;32*V7?jg`YCc9I?zEm6Jd(zx#?$4eJ6{k{lzc~)!~bHh9}*nr z415&V8SJk#ubSYv%sjHjiuelK5*(+;ab%N5G>NgtMxpB>L^9{$ALW%Mp?{UvVXQ}- zh({`azr0p?{#J%6!`-$FS_IPXNjfHgjv1iBIGU7xn_)hwmC*YUPV>*SX5?|!#z`^8ER-NB} z3;%DWL-~4|mU_BR%i(D`Jgoyy>%h}G@K<%N--o zGj;zd;bEB?N#8$APRM7)y3Ze~lvH*0goK+vRdJuqTsY~`u#5>qO0;wjtQD2CaaXJO z%@6vxM~^(6NagfK)sgO7lSVT>X!;m;sXIj&-zUUrZ zWhKq>#s1t)-La#xb6p@v_e7^~;v-!BCb++!VRD=d-;8zt^ui>@E6f?;o_#l&@p^Fs z-0SP#XM9$$ll#PuoE-n+jz(_(zVeJ;s9nW9^LRbRhaSu4UO2ry&9 zO8V+rImSnoE}XQtLk{LS+95+y#T%b<-0po(Bp&njW|?#TXp@-l1A}>t4X&h~SN1c{ z_!llGzWjb&<}rp3OYB?qd;*no?47bn-*#NiJSFFgq|1E^G0&aS(-ONK8^rYcX1b)w z1@6bwb8zRIpE$C60j971E;{K<;zp*AZ})oQk#;2+ua>wxaq#HbEN8^R%!y5xhV%36 zKXhi|kc(AVw~X@>6DO2x%lWDk@pa-SF^ibL$hD96UpbP2`FGB(pE#hfkNJn~n3=e~ zZ%^ir%DgtQ>5og9|AWAU{XJS{W&V_(Ztp*Gr~&iupItODrOQC(?^dWzVunvwF#k8n z@9b+Br|MUSMuqklT-Aj6^A+s2zu=`8n1A-qllN=z3p0($Y=R^hwh6OhPi87UCso z`AX9Ao1~Qk@!P)%kKrXwrsFcnw{~n&`KdD)U-R1Vr2e^&G5*QJeo6gChH+Z6Gqq33 zbHl}WRJOWFo*~s4_YWwORQld`iKXwUIp2Q@zufQL#5ofOa~dc24oTb-9Ladu!Z-HE zR#juO%2cSVowks1b>75pZmMxTNAj}70$-`QhOV0)7yJnte}cxJpyewShs;zSJ+3bE zU&WzYs{DTj*XQs(XV*%i^iD<;N}ALp7vrmIHA?z5=cz=ZPyeBBQrdYI<5Ro6lyq=P z9lSy=h5TpGWE1{9dzAvo%OuvdDPa7v_571?X~I?anwWBo7RJfO^O{wGq@qPK(+O>LKIQrv6GFokx$W%hc1-bEf?kyh_XZ-y`3GQoS8!+B# zbsqPDIo%omWZq8+ahZlOel2EULg@kH7+;|dAc=JCzcn%8pTs$D#Q*9*RpuSn#JHnp z7h!sbS1k9sVLx#>99r1j?eYv^yv8v#M^WW6f#`{Z6CM1F6F=mzJ|qY787KLFrJK&9 z$JJ%(X{q$dXB|>?({+)Y-hby}_6_1y&qRpY!&8dui+g10&2V@70kYyJ6Swo3Dm z-M)&%xNp>|%KVAj%egPERBMFOwHmq?-T0N$on&=!&&cr}PEZkpA?H@a!0;XNPCXI6YH5Yd7)Gvql|Yoa%tq9v{PX z9zCuuQ%_6RLDx;!S7LS{aN;8zza%?8t(R&2%XCVI(jx|yen=;cPU~xjKS7g| zpvk9oI`h!_Sks5r^-QGfp<@ zKR=SWU`BK@cACc>me+AP&pmHZu7hRLZsrLDO{Hs!dC@C@>d0B zPcFo1{n%zjVDp^vj8`1KD3E!boAK{gOb-li+mP{2BL@cB*KN)C+bO<4<2)T1pSCnh zV9&yCjF-r@+n;YzPsV#}%iv#fOyM2Fc6xRV?9TMd=T=z@#&=@;g9d55xr?@8{N$!; z-p{5rW&G5P_TE`#>M*{1hx*t|B^~m&+nL_!%dD$N`_rA~}%HILSx4j8nSMhjG#edNNLWLVw0d zf8>jC%GaLfYA`-L&t2<$R1wDS{5;=Env;d`_IEQ_cbEUnIvl?@%DwZmb&P*eXjO8K z)x{WJv^PB<{iuGr{Gb!Ghz@?ni63$pCpnPsB_5KGbQz~~p%3GvPmfopG9LZ)E8diM zUuV2V^k(mO;qNkjs?HJbrkp8^|8Vt!cUb!$7%%(sEpJpdlb>Vn#?n{n=d6sM930{6 z-0~U5$7PE2C0?z}cx3KKU-NeL7%#gZ!k52tJI24AB7F^t^kw`?-rL^a&x~L^%e!BD zON^Mrc&*|o-X+AU@LFVKNc{at$sRh((8*DPo6i@ zx_3|E$0iR>E}mE6>3`bk|22>(%t}YI=G#{k>Ygyjp&JS~>WpUJbMLF?{FpT;_6H zo#Bqxb?+$Sd%Jw&eIf5w#+UR<@eV1pgz@s*H+xaf8K-)V{)gpI|AT&saq5@QUolSo z75Xv8sUJiC#yItF==T_>eh>X2BjW40{ zTovf!?Gq^3_D6T#Wv#r=ujmm8bUb9~#4@!*a6-dyorm?vRdEpMdjD&rHa zIo7lneqo+jiLY7ZPhDfY^1?ON)&e(}$CG)prSt1@^!`xqCv|;vIr{VZtLLcm>-y+= z>iX#EDjeS(KwquW5`yZ6uBWcQUOsxc>H6z>DqIMvdrGDd)c2xK!*jWAWkH`dCF}Iy z%sbbteR;Nc-(55*IOFqk)}4(IW<28g2bnDZ)47P}N}9$hOvpchdT5wk{HE6{u;|E+6Tm=U#nxF>CI3 z5sVLOUC26GH=OZv+45U0-=w}gwa@YCZBTEUdYjeTvfjq^wy*aIdS9XUA$s5P7k!Rj z1N>e|PJ=iL5M{Br01LMta&j&pV z;~QT|wEBi;W89hhq0h(tb&L9Zj0KF-Sb#BtaT+5qb}&w32gVe}X-vUb!#IsK7=sw6 zF$iN5<1{v5%wn9zER1E0(^!Tvj&T~}F!nJ{V;{yu#%WB%Sjjkzl^8=Ar!f>`E8{e_ zzO!mlkn+AhdWCn}rOBN4bK}-}d+eARq`X^8Hh70$m=>hGr}=u5_obl|85aYOdH403 zz&thQ9QMAsWGu)1ZIa(RZR#kNIq!$v-YuRNnCJDBZ@uS-4Pu@KS3dW~-ssOfw`+gw zy>qK8^9((H&-?uP&dhVZ=1uR%{tnFZ@%A6R!BI_^o-5q&xvWM^zxm-k@8N`+Oiv!4 z*0*hcb*6XAZ~8LND$4k<%Mrd4L#HWx*=XHb&1e2|E9!d3Z=A#YZPtHjRp~j8 z`A6;f!P-6dRpyWT<*K#+)&k}~HRq=F;xDf-zx7^>HE_p#=HGfamo?(&1>}o$x}$*3@7MVK8oyuT_iOt2HGTY=K7LIfzn0&CmfwJu-+-3ifL5LXtvmx-c?Pud z3}|&Mpw+Q}R>uNb9SdmnC!p1I0j(Ye)5bUB=M@he54=2D=W9Xa1pq zsNmv?3CzD|T&AE_SA+c;7vuUcY*18i{iR~ezv83l;N}L!nLpc_tlE6i2o}iSH?ZYs z5$i^`2(;;Xfo<#ER?BzN1uqsa9;lqs-73;FV-V}&0IiGjOwAm;{LU5s?6+52Z3brz zewKc(KkMi{R@zT#zN)WnX^e2zx!E~gr{B~2L%pBW`%h<1tB*(eyiV(TeT3^{lQU+i zaZ?ByH}x@8@4NIqP4DaUK2Yx)^*&SYOZ7fh?|bz=S?{a$K3wnH^*&!83-mEUlprnX1Do$71A5WXq^J5@m0sa6GG zzX~Y(RY*Ri3x3iYYsTzB-L9qEyP!|19V~>iOxq^LY1uY0T_)!XvYqW6t;>W6t;>W6t;>W6 zt;=x!7V9#7ji#^N^fjHn*3;L3`r1%mGwN$eeT}KFWl#lSkD{MrkHSYGQteSfgzQoH zc2uf8N{Em>N{Em>N{Em>3UO2IQ9^|5QEH8&uYGizm2S(@ZCtwTOSg&Xwldv@rrXw# zxm24Q(nbALIw)J8(oNa=l+Mc5r}Cm~eJYR2)~E8WY<;Rsl&w#dm9q7zGF0hOYE&mf zIw+o0yC3SIvJ>j|L*1^Z+Z%N|q;8+o?UpK!F3Vm^`bdM2oinxkxt)f1To&;t8?366 z$_A_Is7aOAmfh~a z0_DFU+Jm(URCDGKBk_9vX7}}r0iqr*(v*&dZx-grk=I3 zkEuGK>|?5KDEpYIGs-@u>XNdLsXC_YW2){wUbZ~8ahdBjG~KqQ+uU?poNlAjZFjm& zPq+2yHbC7rsM`$naZ1_qg$UX6l}$;v?dvvwweO(#)xLuep?wD-Li-Lvg!UcKKd0_H zP^b6SVhN;^&b(^QYj)VSQ zdPbo=DV)bn-IKzYn7Sv0uh*vTNeL0!lfpMGQ}?9Mx1{b#2@%?p!WVN>_oNUvbx%r& z(4G{&bD6p)g*uSBCnZE^PbzgC>ZAHbYf^pRO5e}Y_qp`_FMVH3-!Iem(e(W_ecw&r zkJI<*^!+=1Ur&{-%d&0JT+X3=Kvm{{%cI(Y8awF8(0JmEK@Og}7DJfA@k~?8MB%Ee zu+9Qq2wG=h{lz$~zpyT2oYrMnuQ5*RHLT+pr*$0GcZ}2e?oayQUZsn5BI8&u)B2Hd zte0tB$vD=_fL$5KdfA6{DC1Z!)B2Qgte0us$~e}`w4P-g>tI^vGLH4K7wccfv0nCK zUCcPv%NR2j+x)yr**d`pVsM2r**ofPe9WX?LJk1vI8(b*#WftlHGvm zWH(S{#pPq;YMrR>OX~ZT`aY(iePkKB>Ncs_(1n`>pyutiC_1@7wD8x%xh@ zzW@8D@6jS}{B2sFqN{JyYPkK)+SKpYI{ZWj&iwj!ZFPPhczm|}C-CTLrRw1D+i`Us zh3n5ne_r}?)Ss{Z-1YjP*Au<|==DmkZ+bn{>Mh34C)QiC^B^y57kaGzYw{m!FPc7N z=i#{2zBK(GYmZuf$NUxXu5VO^n*O!%uCTal4J%{EMlLGj6wW#>qy9vSOTUcPJ~y$)=~- z#2&K&qO6$5ZsUwoIcfFz&vnnI)pO>jZ#k>7rP|GFG2ExF#Td8OVvO5sF~;q+7~}R@ zjB$G{#<;x}W87YgF>bHL7`NAAjN5B5#_jV+jN9jt7`M+OF>aqnV%$ED#JGJPiE;Zp z665xHB*yLYNQ~R-bd-Clje>FeJQCyfc_hZ|^GJ-_=aCq<&m%EzpGQKwO0`ii9y*Ui zA#DG_n2~D#VcfR=c!_S?e;BvzKaAVm;qY^fjZtmekjnYCZVGH7U0#`z##eq4i+unlwPAZ=Z!@JhawR`an;n+v^U-?R5v^ z_PT>{d)>jfz3vE-e0$x&xV`RR++KGuZm&BSx7Qtv+v^U-?R5v^_PT>{T6bvk5bE0% zKfb}Q;ne^8NFUX=qs&aY!}s&6R`bz*U)CBvAMGKWNowq)eT6nt+k&6wgf#v;kmif@ zb0FF8qYzHe#gJ@B89&J6Bbd7noz2YdBS^L@GkKnu>{(5}n(ieVmy8mF4l1y=_e8kNwnlj2-O?P=E6J=Pd%% z$1K@BG(i1Mh2GNw)E6zkFc)W!%KG}YUJ#%@>-DAc1JsWdJ2W#uecRcd;{w#*O~2EL z_8NTszpNS{du`1Q+mp$LJ2*D0MRwg+7VWag)?4{xMlacaGq$ebC7bX*+0{#S;*{9& zUa~vaoHWx*_T(>0%;$2VeB<6v_hb*JaI%pTKiSPGKH1Vq9@*DPKH1zUJ+i}7`ed6Y zedwG}cw$U|>=M%&9`KXRX2Oma{Ae5g$@_my-i0%wyL!Hvgf`*#-^lHur_|pZ;Gs4` z_XxKroZ1rcQ~RR$WP>4j)DB5LwawcjmIbK2x;E?%P#fO<)tLab>tj={1gNdg%pii) z{@X1{6Qn*Nd0$kJ`ia5qGX<&d_@h^*AoV8&E<^>Xk2(8MnjrN%KRzdd)EE6)=W2lZ zr=o$g0qV1U-LNM>bNDl7mIP>CU$*6d0L}F;j4T+S`Two;Tj<=1FJ(jlKCd}y<`j$8 z4&Rr^=%w|?4`BHgHlJmv_2WoEZ$+id1)QfZC#j; z);1~0>3p=_`FU7I)ZOepyWR%qxsiP`cb~gde}e?+JkT3~+k7tZ*9%wptYYQM7x?_* zjr=G1Oylt%cJevLwVPJ(*~b<)$9wDIT&3?!f|t%KU0QY1qO(nJ*Ycsg%nmjfmy%5P zqq`pB^Gp$2O9Y7jY=t3wHuGlDC47eI`OkOo8P3JoKjO1XdzxQTXFYS?<1^20+mJ>WJf@I%W_1o0|*?h(s=K^F0%HDVnJ?{WLZ`y~V`j7Sy zr1n7jRn$JvUV_wKXkUxk5AAhPd!qd>YG1S`M(vIE$*BF2O_JIp*(s@glI@b(E7>!t z{gRE7+B4ZbseO|zl-fJlN2&dj&6N5BvZGS}K(vbT)L7(kad`$Oj9<nM5Nm}{SD}v8-Twi1D*%iSD z_v(8-i+d}$sK8Or;elI%EsMokZ=Ku`eCKpSE0~ZJ%;TP59XoM2c&Yp{%RBy5@U!1` zSZz9d8eDPYW9wnRZ-ZfXzh>C)nO_)w)bfv1*wpn&knUHXJ;CtS%pk*Bp4|)=-`T?O z(#CZRt94n*aAvl748J%!g<*R!g5kq{-59Pu+n8Z3Pc?>@HWp$yGg~yn_Tp~9PM0C% zGlU%bzK}!r6Cahy=b2I%es^rKeP7?9dnjE0y*^{1pYC6u5fdQT<76j>pfh~9X}3-P zieb;+B-$7AGdz~72*Zb)sxd4VPv@^$4#U?v)7lOHtMi*WNASo)XDlfG!XW9HZPJP$ z=}GYOO!aM;;!*c0p32XyXF5URQ}-!0tZ%g`{~pr`k~{_LTcgtLWjaBkJNHY!J;3(_ zN#5Q)C;8riU%vVY-`{$B0f!SLy%bFG*w8xt(F_hJNZ|^mcx-5OyEUG}2~xO%DIObI zalYp{oFIiOnBuXa_2sK=Ih-JcE12T3p|$keS{zQ0!WB&M*bwDVj>8F3xPmDj8={=^ zayUT>SCGn`?o*JTNDe1R;R;gz=spGdUI|e75u|VhNq@Refj$Q~oFIiONczz|KX(qt zbLMab`MGmA%E6ZZJcrXg;!*igMEyTvEm?FvIqzBNJ-$6p^3VC=yjOFDd1k*|$!k=6*OPU06K|bT8PSF& zc@YnAG1{Dh-yO@&_ejG-biibSphGV65#&B5ID8JbX}VW!b#ajT8oH;xhuTHHn(KTt zmvrFuS82|A-SS^1yXbZ6RnIhr2TQ-iu=f|e8Ai@*#c)#e4Ab6k!w_;9=C{T(JU3(p!@NFaV}pFYfBXBl7+%}4m0_jn zdl({JhJoTI8K%8|o?(*{-!NRb@@GJtfwJL^{zS+Bj8!8(AvQ&vV3_tyfML(oyBG!r zZed6~Uw0T}^w>YbZ{;SbE=XwzpmEW`-1>q&ZO0@uyhKMdEBJVL(i}SXCB(%YI~YcM{RYG0kuw-B>(RpwuWZvJ?fV@p^Wu)m3`vGs zLn)}}E(>xI?y`zUQarjRIW9}dB|4?!vQ)av&-adA3F|8I^U1!I>XQ&x_fk6$;^5&P z43~dAl;NI1lNi1^Y$n6`FTKie>_oLD>^p4@?p=NdD!R+B_*{M!&*fKgTz)0j0SU+LlUJ9@qQLp1cof7xYk&T#vh=?r`9*}yRC=_3pSmw#lqYeE?7HE(td!``nK zVyNUfa$SB!cli~c%dg_O{7R0?ujIP?Djk+Wi}jQ zc*=zT-Yy!C zI6cyx)?pz?&%wDTI|TidJ;Je1IQ9z1e&N_Npf~ub4_EeyVROIdvxlF~_=wLnww=2!DbjDNVNd@HEvE=%#bEEUgXDLE)7mg}-qIxb74>#~#{ zE=%dOzj=u#@xiwYV;8G_?}gI4`2P5zH4F=P zp2P6bFM}9vi>uABcQYC{g{W*?;51ftp8@Ey9H{6nOYylZ70+cUIW9}dMgBP*m!;Bm zSxOI_J!8FGmZK-7_4o2a;q)7GYc}gM=2jHzS8YH`hM+TiuIyHu{xQSYjlbIARA+?v zd44{I&y}Td8{b7bN%IT*CDwqJ4Bt-QlVR+};eal`1J}Mq{jbYkdTk4ayWj9I^er#V z@Z{@xd^ktpuXZ|}5AS37&2=}uct^|s!LARzcyG)9I1 z8vAX4LMyEc4-$MSG8e@^$Ifj_uF~oe2A?Aw=F&|`z zc^J~c`TceW7-D|M5c2H%ybc|C4e9(pJO6Z9Y98vcDmPNL-%sw<@=-aBUSE!3p4QYJ zTvn2io}s_XH80(V?>oYvqv|v*KqB_7IFVMtLDt~ z0fO5;Zp(0SMtY}S{jOx*hBU}tO)yjjb};^THW&RHnhpOp{NInC9qDD~J!g9_+2@=U zan6Z2XGEOyA~?EPP#bH0P#{h&Q^d(W5j4?2$agNcs)^dRxkzA?q4y=0O@`^zL3`}09chxVZ< zUF_q7N4*O|deI&>=}GT;klwUzqw=@!={x|1(^-HZ<=c)=X8=eZzvn^aWT%IE!RgbU zKj}l~a!5Zachc8q>(6@_&VLJQ(4i9_+$!E*z<{w( z0`?5Ow7-CTMBZP(-Xgto?ZbX!GVL#557JNj3)pw&{RQkf^Zo+%n|Xf$d(FJRfPH4( zU%(zS?=N6~nfDj4x6Jzs*jMKL1?(yF{sQ)sd4B12U(%QCzob9eeMpl)g6!x$ zA)?beLc~Y!2T?qFH;ClWdqE_Z-U*^~Xn&Z}r8A(|gA364PwdMD=H>7`6Z?0x z4-&+l9_@<+vCl{QBth)`(Y{F#`+>BN62u;1kj{5v-;nlM0@zEWeU||C7ik|RfIUaH z(_I*ApGW&P0qj@uJ`naWc^?SpT$8Xr>c?Iu?fdw#|A~H=elCmlf#{z0Pbr-C zfry{>fha!h1Cc!12O{~j4@BwFJ`kl(`#_`*?E{g1v=2o3(moLBPy0ZWAKC|^{LwxT z<(Kw>DF3t%MCC#IKvX`o4@Bif`#@BFv{$LMZ{DxOeQG;jx`cjjvz=e%>B;_oim!L> z&IL#9ksg)Gb+>6JR~OLv-*zHNn@<15?%)KS|1_uIckBR%Mz{PI=zs<>;78C!$Y1@Y zZ}U7qnF4bN(#823Q1k&F?9_YOa#)-QLs%a;`aMbZ*sbk0oNipr^QN!rDuQ?H`y;k& z($Phc@w?KiNK&;WjgyaFX!Jh8E%yCwoA(1dP5giQKhDqeZOJ54|J9sAcTdhwQ4Wg} zX~6MMmlev0B+~Eozax9HEt|^FDaS~TmCjSk&Qm(v|M@R;(`)?9F5)~<8cjWB({taJ zPyakTR57O32l{tiCr!L<;G<_p|7aVct+I$l|M=?Alw9e_{5LeylSQLpWyU^q^juGf zlV79Z56p2F>nZRATEQpK3TZT;_{Dmjf$!+=$SbVTh>Q4&!2dH}(I?P~J%LtSqXES) z)>8uhpXHU*Xv9T)rQrXWue3%>myiC7Io4AKzEhS$z;-<0D(czqzO*wCfM;74z9T1H zI%+wwo^l$^FsQP`dPv%z`6_6%h7B7sUqy{pzP!?*l16jNvNC9nUcGwtayxF7tydK- z94L()?2_$pYTr)$6UUFoJML>cJeKQ9Tx#2;_EHzF9$bC625?ThqIT98t_d9f<2WGo zTg~BGz_o;H1=kvm;?sE12Cgk!JGkfINWN<8>aVmNoEj4UD>q2&hI}pPQ}b3iBIm=1&96n^_?w;p0!hdg+1<`vSp|FXJmtif7&?ZnN5@@msLp)i{mt% zbR$_nI?t7vgv1GIZU62#wa%9Rzco(%Xs7?59;f=uQDyV5j8kVg&R>sHx9#x%);QJU z70N>_*Kz0B6NUQG{O}!`(-%G+r~Z1J+B%oZ?f=|3Rd1fs>#xSCK6dzDjZ@R@@MOEb zJAL^-J5IS&X980W^mq;Kb+{#POW~HmEr(kHw-PQ39Q~*R{Z|LIRfw}1ZVlX8xOH$6 zj(#2B9Q;2$&|y}3$Ce$LuHVStCnha>r$xlP@bBN7SfxqZmeTiZ+s4koTHjY&`Wn@1 zL}fyaD{6ueG+Xt6hKSNcAi}~-Q1ApLA{dws znNWw6Hz98 z;h8vmV2`U_x_9IuzV&$cfXBieeVq7^$NJWsi5G&Qmy^CD5Bz^hZvgWTe$C|co$$BE z%nMNbc}MCqzoY*%6Q=Y1Gk23Xd|S!F9N!6d;ydX<{^q{(+m#lzBS*6^xseVr0R!olINxRP4QLzj<={i(7k&8@s^Wc^}OO!^XuGa zY@Lqz?fBkZVdYq!dfxF~s(&QUzE6#RpwmL=x8HK&+xl4dmwe3f@VrPrtJQ>3oPY51 z^G?op$WQT5K9C!4MKtKk{1mRsqi0LuPWe0eqyB;TP~I%xd0viu=iUi-Z9{Kut~=J)ILam@FTtb zsn64{Z>jlp)Gei6;lkLo|gcj~X|KSIynu0N^u1Nr6jo#)|{hojHH9s{@?IrSIv zp=W&R^QCaQcj~ixUZMKrggf<>+AH|I&hw)7K>9oKo#*4+tMZ7qobto-;PjpNPI*%Q zK>0>`KGMfYPo=-#qW;AWNBsR-c~XBIpPIfZzj&|1@6<1?evtmD^%MF%c2BZNp7T7Q zAD53K5B&qBm->9|`o-gqimTeQQ~#Xy;>36An^S+(^Flkc+oO}7Q$Hb-%gd<`C=b4O z@?-0pT3&d5T%VlsR{g<#Yy?Z_z29>5b;2EaPWy8D7v~=JoAc|`C+B%N&lhDu;i={A zvjN32hGM@PK z4+HS*)rV>5B{0H6AMU{+s;7S#!1W~baTb>q`Z$aFAdCLN2>nA6`UfWT4^QYHq|iT9 zp?|rN|ny-K0SpV>`{y}8t1M&I?fAtRm>mLZ#KOBsH17&jEdiqiLryqrX`ce4Q zKOT7cQF#0@!?4Q{|I|m}^$$#wZTa8*5Ih}%qkfn^a;JZIhz@g-&D%L}O1N|AlyK+Z zDdEoHQ^K7CsDy_Oq0-^rrynJ!a<)JC!eve$e9=F&it*g9UU>Pt9|yNT!2Q=B_*U~K zdU0kI9|om#Y0X6kLJ6m}7##{FoYrV`FqCjwyV2oL!f8!M2Sf>{wH_T3C7jlPbWoIV zS{u?~QNn4>2(XcGN|!#GLD>J`$FC_po@3BKT*B>-Ix%j4)QNHXqfU(5A9Z5f{-_h< z_D7viW~m=_Vx06>A5&Iy82VTV@r2gM|8pO7R`vl(&9M)pe!z=$2-yezJ0DY4b4aBR z9S$ap>;`l|m~i{UPmJ3ieq!AI@Dtcga8T{x zrpD#GQ`~oCirkDVMQ*{BBHzW8BDdm7k?-M3k?(`D9%1W%Z^D%#-;r<24e|)C4G1{` z_ZF@}zHElm6z@ZBGqFuJ z7uz9uhioab;lN-E_}SrS$61T!2>k%zA4sBqfYe)nwqv*vK5Lk`f^fM)=>2FT}Sb};~V&&&R}&jviZ z*n+%mK)G(<^51|`+6?y&m+nUR8&THv#d6sIha#8D6@V*HvJK&{l*?r&P*%WS30iJ& zG=N`UY(lKHh_@C`tG-wUzB*!wtcxR-OXO0(rLrE5N-mYl0GEMdDI_ez)d`$S;4cL& zH#q9Sr&zf~9{721jB+JH-@w%gW%CByDm<55(54~$+$fuzfQ{iA0c($P$|*X?T)6K5 z-dOP*$}AT2oG7D#G6za$AWAR?=&{hOGo-!_e+jhBBWmNyBl03{N0}S2i_8bVvn1G6 z#v!$i;LjsEAx0k1^Ww-}SJ@4rU1V3pps*wI5T1Mr{KI(uDezn3`BF{TAzR_O974Tp z1$-y`ow7BKkY>eE(pKF2_TrH2A-hBC-mFP)=zIv; z^nu??_Qcf#SO*;QZI3GhG-?m`oaiK?aMZRV{0_LH5YicbCy`!smr=mFBVLr~fwIT| zzo!gCNM~H>5!O*8L7$EyS$0C)MCjK6l#U4XL+8$dPSPBLPIVEUjGS}=y|bt#UXxS< zUqg+oEnY{BtOI|EER9;UPWr?mxfuRpSqq$tCGqA3<^|n{IGquz7AQVkc||_>`4HxX z9|w-EsKblk7USxIxUa$QEEl5gc7@*+VP5cAxZ;Ehu@-`|P_{$31>7sz$!5}pkY+MJ zIJz<34dG)zC%+xIJ>ay22VD8Z7{0fFdvN8)N$r=wIYv&E%>k#vy(HbBC*aC23cxj! z1pu2NJOMBsR{>EFenC+Pej!m)OqMO=6kIL9GX-uc`kFE*nIKx!0bED8oXViQO5+Z1 zz#;sIT!nLRuZdM~i{UR8Z-|PZtb#scpn)5jdLVxaQcZwA6>uukr$R$YlQfx(^s0+V zvWBPu|7F0J5mpoaWY8zdNwSIIRu+MNtq}e)++^tc479HSzcBp5q6n^{Oerd=<0>kOiMFy0 z@-hkjv+$pV%t>%B<7y2_MIf^c{5GfBvNWCi$OvCFs@R3+P;oC6oX*N!CMangrJtegjz_S3O|WMF~+7;U)1r;$;=YtPa02{K}A6 z1AaB=(iR#NhhGBh-X4IHp=$RFz#ItsZph0-42u*Ow(huO_PkHjt$tu>mBM5)EZ(q){Jf z#mls!3d$la^gaa50=Ulx-K)rK(7LM3ETf@o7Qkw7RYA)pW8lZgthllOs|>#y`kTY@ zAmoJO)ZQUTIgBd+%4ji9z9e3PKObIC>>&_)-FMer9v+9L3c1HKY%b{XhPaVjX)6D*-3KO++pdpxX+h zwgjBZ5wa5gGQ?kk*egL_0e>0bMwHbk_@l%oT%*K0xJHSQVg%qw@wS*FHzIV7d_|5B zAA_T;EQOLT1HZJ~1PXMBh@V_i9IT^feP$vWT%BfrMDG08?;34f*lOX>ikV`9PnB7(h5Rf;<+* zbpRcX`;5AE~dk;BWH*qz^2200sadpvFUKraPP<65IGq9+2IrCU{JFIs}0Hv5@F&D zC_~`;5sNrxz#mMUa;BIGe+b|ZIScPL4MB|T@u#V$Ko+Kszsz zk%wf?NnzwM8M#ar&*1x#0dW>(a#ozfb)L)TJU9bL`z$U$v`7N?dGR^mdBjKtd`2dT zLh>AF=fQJEe2VLg_ynnbf;0!iKLcrJK|Kfe8LrQPB_ihM;sW63h@AvjNG75j2Z}Ec z^DBgW2KWVHe+K+>#Qa2jiR%;4KEvIY@V^A#KCxe1#I+w>`yu6mC?NNPS`gCq!{3kl zuf#XvTkw7fI6!=j68I8Qz5)Ci@LR+tjxTWc74Qp~;gm;TTp4}^;AKTQ_~mdnPOcD5 zF%N2r`OkRxO=LOI1h5>ad5M_gpbt&J*+Cj{qDY=ZpF9M*@zNuOfV;90j}=IC^5z)C-gz zz+QzT&YlR_D&7;BpwCv3QD%frnPd+69`X?ZdKz5sAtas5B-6`S`M$`3(Dy-mPrMJ# zOu*A4RwkJaB{v8&g3Tg|OD0NohrA4&-wept7Lh?l0Y-r%os2}BjPNtyN(*`<{0xW@ z1=wArl|4iR(n|yB+aN!u+$Lg?`ZlDQ9)236mR?4HD@^tTXE@?Vz@-Bvtt46)UOm9% z4k6+2!{CP@JQ99dw80nTY%v@Di-0f6Ibsg{p@2i>E8-RS!$5x?HF7rGEQIz4HXHu) z2yzvdg}>?}zI;Li)k=N8R5i z^2_}2_X0nN{^@66m*DonzXbmx;sz0N5dH!AgZLTLAK<>lbqS$Af${@d$JgR}Twjat zK>r?;0l>Zn{tfN|NIQtQm%)7*^dCX{4%iQXmyw$varZpj0O+z;xTFjI9$b4N`x@ZS z;wpHqAoexjB;zXltKt`2*MRj$*pKjkgp|whuZlRiTjY~*a*rUmTkL|=J)qvg{WWn@ z+~T;mfL}%28@Rg)e>c*)2HxEqegk~-P%nGK-G@I9j>@(-%KMJEE$-pEEq=rOefEC? zy$`V4@NeU3{EqMkaKFKS0RO(&A$E#CaP1U#;da5#CwGawGB5m{fICGVnFsz3l*2)c z7I(pO3z{Fq6S{$?b_dsW$i4|(@4&w!enmRBLHiZ5ZsNKN{@;y1xC|bi% zJpW(8e-HF~;K(Jniw{tLKM*F|cK9YJk3??xxnwTTsIC)7PT%hsdi0ln4u;C60za8g2gp@{fq%69_fsBebN0ke(BMF64R# z(#`=qR^|r29i?$l=EHRmPcaU@3p@u=Qn}#g#&r<)k5E#7AjL<>(eLp8fd2sZ+d+SX zo@f}-4xkSjf%Hc*J_7AxnJGjXGp!kErZdx9_^Gha8Cm`lxN<{D#>x!7E5 zyls4BWH2+DnamBwY2%D>)>vn}X}o2uH$FDD81EWejrWZAjcAkB`vtMX_<^zA*kKGe zN0=i`TFH$vN1HF1MX@fWm2F`$#vE&wF~*stjq}DQ#;3+-#^=Tt#!BN2W0kSm7-GI) z((39<<11r@@w&0ZSZXXYmK%f2!RA1dMi-jhzi19Mhnciqqjg|lRgvPL;0 ztuerS-mHXiDWef%WHquInT*Uv79)p|&4@NG8s8b2&7HFp3(*jKW3{BfBVMJY$e0wt!L4$Y;bEE+fB@#ms8@ zjfv(YGhmR7FRKU|2aFgqo0;9rVaA#{%{=BMBbS-mJZu~>4jPAy>E>i}iutlR)tqLM z6)p>`O=Lfd61j}rMjj)tL96a##&ILXIBI-ooHAyZCybNELSvD!*mw=Edd)XpH5M3E z#5|+FSrzNm>R9vlHv5=;&31mO`~ z&CX^Q(}GRtw!8yNQ!QAi>WB_zN3)apj`6+mgR$8#&4nh->DFNe)XsR$=x;o43@`>7 zeT{xbPotO7+vsETFghDujIKsEqq|YxY$Mtm9gL1fCu5K?*!Wf6mA}b*@^|@%tSj1^ z_vHimP(G6F%;(H{uq`)$t+x?uysgaE=CfuSvn|XE(x_@wGty$@CQB<>8_-aMX;d;Q z8&&X)f)C|M*bm4ibW|RbO<*N&VYW1zna#~+u#~$Ek5S#IVT2jsMor^ec~Leoo0=_P zdu;_PYGbp3+0blcJ}VN8?_@>edl_$Bl0VAJ@&|cB9+xvsvRDRTxeLI`NcO#C`J)jo zzA?TvrW(_X>BbCWfjPx^*{EhtHf9>LjM>HeW(H*1(R%~=?G;>`qeHbxv; zb1W5JbAedM>sDIX&KIx3c39hdUHHseW=C;KdQ8jwNS>A-%Qvv%T#i*Ntz{<}G>e}h z4~q%LL}MpggsDe9#5UvY=%L3M;|&_0c8DFAy(PjLy$!RA(Q=IOff#F4GAo-^%&O*e z%x0%y6*5JfmFMJn`H4IuKa~@Cy*F7@G|QRg%?joh@=N)ZydXc5pG$g|koGd>o3EOq z4VqUZVfK?OUNS}*@5_Uj-O`F+r12i+CP#REL9?gf#t7qpC~cN8%bMe`rk;QqRw*;9 zyl*@(9vYAE(%~P*J>z#{wmHXq#hh!-GvCF2%P?azW+JpUZijsftg?*bu-9aUJ?bP@ z7?tG-UdPee_yyxdW1|>iY!JU0ca4(f*YX?KEy$`c7PGZtW^r?rC}9>gY3^egUc+b9 zGHM%jj3OqjPibX46n3ulyi%u?Vg;<1p2HpjS-{99QXV@D9WnFCAZaCC4z{%xuxz}F zIZPSMZOTg88~7MA^rozeh<>+*)Y zDR0SN5|1qq)(-Xlb-EdSeu&Sz|BEgtK8z@ecNRs$j&R{heyCf#r~SO&YanW^fIo z;?Ee{smH&CF)|lyVYD+;6C=*Am?iv#(eN(My=b0Q*QjUIHyRiXjYbBozi5>;5Hkvz zeKj_k7=7g+QP`vb%z(xDHTkR22y^x(@`CZTLF@FASbuhd4e=9?Dt>7PF5FdkvKcyEC{u+PeuE8tK}V|j=XKquEzz;v>u6qSYhVJtf7$kj7jU6H(>!HYsNb4 z?ZwOW@+Tv=yoA|N9(fO=E6vgJnQ0Akp@<;ii^6Cmlc_+T^a24Q) zw-;PRxV~`J;fPLo>kUWos=<*g$}8#A2afVcx>FeGPWS!bC?3hG30DJ-iN4_r66E^t&P)Sd^yQNGK;^?;-B zu5c8V8?HAT)gcP&4oCHn!s(vsBE|0qR{@TAd%+PO)hBx9ec`AcQJtWAL%c*IKB`;K z!wrR_^7s~x@=xVTc^e4#864$r7~I!zls@IF3)~R65pdtZQTR`AgW-ra9Pa<4>ns4L zD!PZgKR`ky6i`Y55h)2J1PKWNl@Juc07Otsuth=TN-H1|BB6+4i-p|?Dq(kbcfAG{ z-}B7Om3?2o|Ha3dGv~~ix_9^9-|n9I6zrelcfLgb+S;b|ZR0ff3jPDu{|qd%9qpXs z^I<$x(uS#g1?lcdK9BTVm0BvW&wXhU2xCUl`zAprAm2ZWMz%uJxoaEa`rzLqIsrIwLK3VVbBwM#L zTnEm_e3%RN&wh0TUjr@$b@ttHSV#S3piaN`T|erVfI8c>{p&&fO>hM`u4SN|eOU~T zg0_pmGW)NcI>&W2SVz4+9tL$!!*W;)SHfzz3YLR)ZUp5=z!eIj>Z063oF5K+zk3PZyWZ@x{oLMmL%UnYTkMefc0$OHFDi--*#O;%e1#0 z%T|H)EV~n|Z`nO?AKVLf!>e!?%q=PKxlpuK_X23E?m2i6^m9L0U;P@efBIE#8|oa# zW3Ulk0q5R1bRL|0=XNzXPp+e7>X(D{T$hJnHLL^I$98Scb<)3O&WG);hbQ5Acmk~R zB3SkmSnpX-_ZswJtoF^a4PaZ^S!UbTSGF&<*AIJbUxsJEI<{#W&Xw&tN6w{f*jMLH zz4N5bzSy>Nt{>-mAb#~{`v;Jp4*GJ=wbh??w)Gyo0q?`>@FBbjmc0dIl6(^B0H}gE%QutNS9S7R zL+MoZH)Xb03t3+!q&2~M_OlGwj`r#-vrfw-Z$-KzXx9qLf@OO`xg@tGtqtv<0@Q)> zpbz_EnLcZfE(Xt+{>a|j`~KFjH227)=TPQ3<-O0V@HX-`rJ{{_>sBV!wi?_6+rxHn z7x;U7<=bEt+zaZOgLB|KISe1>p91^p+D^{(??g>A6y^HmAixUZ`mLi0f)lAFbaBr?b;8^?3?SVY(HFEW&3SkY)3oW zv7Wwc=M~N!-wzIf1HjjIU+?>a=>X^p+B}2K{DCkSv^fHX!oex8-q-heqzA#_pl)E2 zk0jNn?VkjW({}YY2JDA^hk#}J9t-x{F*_IfoCe2%Z5#%<-(lqKpP1K=KJ4dkuuR(% zl5F4fXFu%EQE)Ph2Iu@3I0c;ZkzgO~$7wJH)Sn0Hjt1*bgkwP;r^9YA6^;k{YQHZ8 z`(W9LVBeH40mth&ZOcB`FWYx+w9UYAZRW6Vd7tfreHjP#uRfd#lfb^&rwiZ=uwRqm zOt_!x>?~5tl_$WiFdpthr;kRkiu^gGXM?(ipnMnQwyBT1$u}moj}MYRpVU4&PtMD| zl-cid$=g@w(0Ovqv%qr8?6ZBezmCPeT4sOjr@kDM`e`r&oYytzoonaWbNg%L`Q+D< zI_K)Noej>tZ9aw0zB>0$kXkkqE&|74y^WM#OlrOL^lET?H$Wd=2UCa1&@}xplO?16INsu+EQQoA%$nTo3l$ z_J0HWZJBM^zWuPjw}4~Q?_;2ye(j64zkvPFuXay>b(|afRTUlp`*|DKFZ**J*k8-n z!XuE|-bMatxD&RAyTP)1U|o{8BfS@@!NZ_#J?Q^_(7)|&NV09~?*`UoHE9!g6mCXt zN~$l*H^M4-3e?-LhoCt;2)5M%Z0m7wjE?_Fa2(FJ<@#{GEOSmQ)5f_`*6w$(9mi+8 z_SybgPydd?d2r0Odk1~ZW&5SAW75Y<;5qaxJOj_cYe{~d^o>;Z1L+G%w){Wv9$5c% zcoDt@eS8S|uVnZ&i{|#np_K> z53YyuukbTChuS(v>NbIPD?uBtzu!O`Wv^jvEVC_rc;0DeTk7mjdHUNN9CsPgKS>?i z-(Z>TX`}2|^-~(zu{jq_$yb0HEN4k5GhmsBh`)eKB zv~B%b&-L_tdy8_+3n^`MMbFpZ)S2s0*&O>%Sd1 z_IjkQcXiS#;5w^QwvP6-K%L`t{dXdD4lLUlT7!MDT-!Qe{n}6yEZY?j>J~hh4z+st@+rzBT~ME$a=Asa2Ajk#+~$u)HhShV3;3*UvuMk8WUDOK`oK zfNQ0V^^{!;>yHH6b?t1!KDnmK^}x06Lux;cf}xO z{Fb@)uCZmVz1MAfQtKQHhlBDI*bDSK9$NCgJ&<&7=m1^7`E-psf#u4smFJ0No;#i| z_Q}3zV?Fz1z5T$saIW?R=h8WL{#>X1VF+laz6Yo~5Doy>$a%5v`n7+qpL1ee=g9fC zp0)!(|9w+lyHQDYE+>M1oo{U?Kz}$NY_ngI?UVhn4ec#=P7VUcYCi^p^J&>R;5;4% zhk*6%+aS=+GIiQoW`AwZajWw@IvccAe+0NTmq0z%<#5tvP!6;`HsyztdVUQ{Whau3 zh7n-?NO1g~SC%;k+AB8$>zx43hx)N_3>*hX!%1LUXTtHIKlQfjd3q|G0@_{(`m%j( zGq7!K^b*b+jHKp18o<9bH5O5e;(-1w(N`Lww?F&YVx+N zY+ZHB!Frd&5@^6$Ehe>3wqyJHRJRP&?Tn56)5bDwtbYXPuLb&RNNvM5uY~55IR?)i z`*kg7>v>_`aWuxpHnq2&_Dz#7+qa)L!41%i*S_mXZ-)oTH-PP6&j^_mRrX%edg!};MxunW3fIiggPn|pp>Mef?zJWFHF+2`) zsXv~%oJT6FDVs*Rk#q*BHkN$?YvE~_N%@ncPrw}V>qwQ?!}p+kG36UlnZ7;%<=K?$ z$9_2m{n;;l{RnTuv+y!J15@bRbE(YvdJ&W-Q|@>j>$hOJbMO+VSN;w3W7#{Pza6Rf zD(L{yS4jOHO2;)3YhtMC$lV?Th|g zC)eaW(z0NA0qmdc*tUIg{bVchuGutF^XEbt(2w;k(_SCSw&D0r0pAmSfAc-^ROGEm zE!zgFfbz+(Em&3&womd{(j8zsXbbwO1ZUw#e>Fj!KJ?WU)K>>}`Wy(=KtEMs2&lKL zHrSSJdOfJuZV`A*>`z(;jsfj<0^6~_j-@5(@t}=9^;ZM5vuq!*AD3fWmvkif-mnX_ zOL9ZfBha@`<-3t;+W_jPvc>3q?YBMc7jS<1cdTtm>yb7AZ5x60wcS0*`niVs9ZA)- zf@_hRk+uNeKYZ`mGs&$<_2YbN2_14#$KL>L7969Y*-SJfQ?p2MTzU^oR1g28YY=;LtEP9K(A zUs?MhVA=6d4Ne8?Ed=$}Jrpdn{!};$hQfR}0*1jDI1(I(I>)6v8=O0BMu273za+`l zzX&`J>cP&St@G==I@ex9?ZCNooX*iPV4oeUbEXJzr~LV(r@@(UdMdk&bUMreb#qgC z4(Wwps;#f}xi4SiAA^~c`@ZJuy1vhYNl+%`&m}FugoJ)F$X~}j7p!Od)^m+rPxBXm z^|kk2%=?Wg*7LlyZRi8UoYwnom!FW*TdzNeO zoH&1;6JCGAVK%t--N7=?kDg$CbtAxbl&xz!*3bQUUb(i8)zq=tKhIg$%XM=cmf0`M z9m9^`dM^dX?0u1W?}HYTIwz*DB72T>AKa!MNjlbdGc)YfRXxg(4mhw@r=cb5_dc@f zogK{+5hCPy2@l_|;hmN*Ps?8XynC-M%Ed(8=7~3Nigr^P(SC2P!(*8DjQhrx8%MY6 zvUy^q1OB^S?xT5)_9y0b`I2^F(6y}bYU=ali86(f!iLA9UtZa0|KC1-|6ub(S-)V+ zpC#FtKL<@MDNMS)q%dV2oA|t6Qn1f`uUxUAB-{Ad{K?PW!i?3-+curWu9$l_YJZ~n zn>X#bKwj?X_PMinY{j79hO1UzH zK@ExA(6vnAkY2|PAJ*=@J`MKYIQof}&-dF>-+nBq*{D^&6E1Ao{>0`rE*(0g!I+_c zmE+#70`G0xaP3s&wPN_@iOyf|p_?Z@*YEhVx72qomel;a;^v7<+s1jhc~idu!&vhv zJRH?#am#cqDQp_HdE#A1$bm;zI-nC@LCNPZwjcZ5uFEr@99ij&_0N9NkuNU1Qib=P z?K#h?@|xD=`s1xL;lS9B|DC_}3z|PylD+t3oTFzyIid#lYm97MHQFChQW(%b*84i8 znvHoZ*oybOvfO*abmnp(<9Ghv9oqH;)BNA!n)btW?e=j= zw%dw3uWof$N%rg~&s67{-GO_-9l3_V^Y8Q8WA{C1yD{x&mOs3CoWCvY*J!?Z;*2`w z2h7@BrS+^oL!{&)^{#Jd5-#@5M zt6bmpd9K`r_u^e?iDPE@x0+JFf#*M0Gv4#M`gx}Bm(Ps(^NMw2{#?J{#V6M`!o}5GF zIRERFDh%7SqU`1TA-?lBYqyuY{T+9S^3O#XWbW{eBsGu>niNd{6Pg~Y9<`P z2OQ_TzVwgd_)z2FPv7BdMS1^gFv=x0H*Xm8XW2K${Q1I@g^imx^m(ae3+lF{E;OMp zUcd7mtnqQ9$7_7tY~!=vZ>b;sT-eh2|9YtNFL%=SK~Iz_bZF1{_kO8D{kv9d=zHbP z4;+5~d+Tf8c=Cf=>N|g%C;r^HWaQqBN=9xvqWy`}2K77P(a!u`WFMa8JNWujn)Ww7 zw)31lR+VJ8-%*k^To&i<(@rtvd>!cP$38L#}dUQD_z)m&a=0? zJM;1D?wES*{P~kBUCcW@XUCG7o`14KnZ0{?-Zrn*uFHVGzG!%5AKqX4F@|fYzq)i` z{9AGC^B%nWt?H|XyuYRYar~8A|J8d&oxghj-1zIEGwRG9u%R{g3~l(E2lQ#nJ@3Z( zU(0nbKKp&Mjkof(tR*#9md@8;NzH0IRB63z+rJym+PrneS(`7c^96NIZO6UJUi@tY zs@`$){a@d4@`JT+9De7pn|8S?j{7LiN5@;Haj#?hozVEO?M8k#bnda!29=EL*^&1| zIG!2$2An_XyyEkfE0=tE?Uw8RFjji=#7mCdJn>OppPC)K=ZJ~}-x}Jp8|M#H?#?qP z+;;iVH*Z?hWyQMrd#rLU7uU3H=if2ZuG68{{K1R+?=!?T__*1mK40~q{=VE7_2hK~ z;<}xDNl8}ij&<{IzdXPG(QvhGp4hV+?Xz~i{tWD0<&b8*s0Yn>4|@v6Uea(`pE=ts z{(R=L1tr<4sn%Is^AFmOo_cos^T1u&-@V_`w&VA=?BxBppC#A&0PY1?%L8Zho%6*- z*UrCV>X#SIn*Qh|n>uZtSgXz-olNypu1UKt>vtd9`O(@F_H16O)E-~-=U(qX&iw)W z4GU^dSv0-Z)Jhl6pWJ(D-6Z==<`5OM?mA?++HFGfct3zn} zP+oCXZG7yMYks`D^NPh>1I}5T_xSp`d1AE{3pc)YU72?dTJ*|FUrX*djK5bL&NIxB zzW$dfygRe+HSxW(?ws0FJpZj9&mVm}^U027O0st0z5R{#>wc_$6tA5lIsd2f{<*wN z;jy_l-CUCC!}I9o`xn=|JDfMpwT!;Q_g-Je7jE?R`z_wBrmQPdSotCK3&ah(f8%3y z?s)osZN}cQ^8O9kT(A%Yc9Q5|OAKzHt?c+Dr*Zgn;bp{^GbJ%hGT?am&dH6!c&p9-f^RMqU z*3W%#2ivLhoyRcllES<-?O!U%R&F|(JDJ`maoxb|G5pPMNz)shyOmcBzsWSOKca!> z@E@J}E*o)+?|*%ljm2)^#y(^DI}{v$GVkNyoR?&`U46xZ+b`$*xZ8gFZ$txM|DAW= zH@kh@8emd`O{g<@=pO~0jp)n5T<)sUSdihblN|o|e8ds^( zs!Dl^8rQ2=rE#mqtqh)5rBE;YpR@`p?UXsnQ{X=>^;4xD8UC3^2>%6(kWU3T@IQR( zguk5l|L_l?K%G>Bq9oLdEn*ZLhyR5N^tnoTigluGk*CllyX8o1FVd3kMGF)sL zw<_2~sDy|SvyHnNtsIi}jvq7sRcS@v$*>}+D<(>&bdkO@8k9C4JJGm29%zM`pl#-b z=A4`0xjg-%Zh@A9L;e@6EQ|#XrExHB)rxtgI<-B*#At1WbfhL3CjQHZ-)9Be^7e*- z(mnfV70(wt5t@lbn4(I@mRxbE%6~MDlIk^>k1${SpPu}eiRPG4Cm&F%ubG73XPKs> zUY;?HoztgoaT5;KkCYkBk9skak|2;T5{t5E@fQA(4Tsa3HA8(ZGHYdQL+m$6pS`e9DgFlnTXLN@<h|x7-ERApZ?)R368vgn=#?pv!HDU~s7>6T9*Qi%w9F7=$B*ykAj^0rm3na$; zi19p%qlSD^c%Sd`$M_#H7D$XN5~GR4h#+~S5Mz@R$1I64MT%pb)GIO8NQ^s@$3ZbB zN^#Va7#Sr-Ly1vOVvLg**W?5G^9kSP+7jy`k2YcCi8XK*|6(i(qj3~R(-y z2p`4qMPf9O7+EAn6Y+P@$MM@WZ&1Y7h;d3{ieJ$fQzyp$DURzCV~`X_{fUugisRE1 z#|Qd%teP0lCdR5Mj(<}e^QJgPPH|kE;y60R@pVd-Y>87+9GfS`EQzs9VsxJv#V1BX ziBV5tbe|aYC&m_vF;QYPl;Su*#nFjkyrTbK9HT9fiel`d7^x^mGb)Zw6k`#^xJ1Q~ zieg-%7^5ggK#6fvVhoiSHzmesiIH7mOqUqVWh>Uc40BN+`cRDWQi{|_EkKZ zmKb>@Mo)>6T4L0e7@g(+inbEt+{6esF?vmma}(p)#Ar9gF==81ni!4d!-BE3VpOdU z3&sk{V{sYZD@OK;ak*kNuNZ|ZM(2uAzGBR<822m20E>}_VjQm+!7D}=i;=@(%&-`N zEJhW3%NE;EWYl9MP>f(pzx^xZ((N#c>CBqbChZ zVzjmxH*QVA_-!$++uDLL-ePpO7zb`Gzw&yVZ^vI%Sj|79+chF@+`Fp^dBnRI4=={I zi?QyCWBA3Wele0?jQ`FGkmk5eNSrq43`^_F~M!7-=uY|BEpTjrOsgf5ruju>)hgz!+07#u|)~ z1!D}t7|IdW`WM*OqLFjJQY1mdKIC5haVGEfzDN+|0@R ze@Y=n=!~&BW0cJptuw~;j4?iAJkc1JG)5bZkuqa+%os^D#wd-^F=Hgr7&kOV8;$Ww zV?@$C3TccvYE;n}$Es3>N@TPo*?$EF>7NK+Ze0Q zzoqjRCGPD?LOg-^bXhT?%!@IPWBlV0*c??jsxY!Jf`7j27snscU9XF$1ui7&f^msIXFf{j?s~04B!|KI7UZ~k&CUK0y7$Y*qD2y>8V~o5QqcFxu z#xbgKjDnmI|K$Sy85g-YYH|x=Sr}K@n8z^|a*VJ1@3_k`YI2OD9AiAkxXv-gbBy;K z<3Jl>*;3;-JKxQSU0@W+7*R4tmyD4nV+6_=nKH(qjIkzpuk-uYf@EBn_#;cAos*B@ocjSrB2-Zg3j63$B&P(qBHu? zr6fkAUQUF9D+?L&1%((ZI>wNWQLAGV%JLju$GZ__%6Qu` z26G;V+t}MNo^*^Y9V2t+QLDE^%Z?GaGh$Or1REu3zA>u^CySN`t~1Kp_6m3TQvmDD9#&`~8LDXB|Y+^Jxv z!ibWc(T*r_PlJ*CKkjJ|_dtld9K@Xv;@${xpM<#ALEPow(vlHGtBSiQEF_=rlMr`W zhb`);%*ypCyltn zM%-;9?w+xlcj(7>XLLV}xaUUPeSX8d1!i4^ZQ68GPT`*OrRI`W-E z+(jhrC=&N7iF=sDT|we*A#vxBxQj^KQ6%mz688g%`;FxLfw)Ua+=nFYVG?&HiF=sD zy-ngCCUI|*xHn6_2TR&iuVip z_kK`uzp1z@)bbMdjLP?+G8cEUiu+u}{jC1IZ&lpks(5d!+e+N+D(L)&Ul2`s2gsy!cY26+Ri zI{ug)TFkiEmGUc``e5>U)=adzZ-e`q@DKaO29rVGq0gN`>AYy>Vs`6^Rawp z8S@?v>+S;QfCcuo4YW#fJyP3jkYsIlh4YeZIiI^^4U??yO~4ud@x5D8>)20aZS}W% zlADlPM?3XRNwsN_PPsvq)bbgMC(Z9QNnw zBtJ{)oaN)gRkkyA@f(hpkzEt*EVvI{AJTo3+=H|qa?d26M7|gKTyOcwl=V(}b!Q;& zm*jEe_a~q0EuTOcN9O<;Fm}gLM*U3L z>tqZJNb-TC1L3J8TYfMMO0s$FPfPM4q}m^pWb*GX?Ihg05(~t5or205E$w|R(hwGqDKNcKMs*nCjzMfPc zcct~UTpz4^I5?Lw*PQmlLBBH2v9RtIxGt{gDWJ@`=J}T~-f%Dv8Rwue3o{E`TP~&S z%p^P4uJu_eIb0`w=6cRo&)ld+^x(4>-t)Jz5<8!8bAihlS%bKc{s4k%;?9|x((ri zBvY9E43kuHl&}u?;8{B!lzTs5tisHaRism4Mv|wH9tAff`CigHVS19Mk!m|L$(CE^ z!X&G^7G@_|yNluOB+nwf2y%bgT>`nktH^7U`nP79)y6u?3rW?@ zOY$;OZS<@DHd1}utiUm!y!9^yWp(;kslZ{m^{)iw#iWbi#&mvPMt%w0l;kVOTXw6) z9NOB4%R%`T?8x)^LzI`2T7P+xZAW|C!OVDzVJzAx+c(Q@QQ*+8V{^`xmyyv&BslM|5ZX`bl?5FnT z2ZMc9ev(xG_FMUJ(siIu<*i9AE2F@n?KZFhlvl^RF(ZRaKlnUr%l-IM$o47kUlsBL z0Q+$4L;e|f4lH;YJ|q8pk}dBJU6cGG>9c@chN-d0vKQc`BwOz*_%6wozXJFP$E)P^ zk^4ty48PEZ2CtKUnN%C)*GS)h-2Xe2=_B{A?me&#?cXQWhc?O$Nt?o3Nq&>m_I^&X z<+PW*o8(HI?;nu2UF&PB|6h`c3B(eg@gP>VHeJ_0<0-$)Auq z-(8aYDe1?scapy#)rR}gaH#tnoOcV21fp zluG`sw-xyoX?*J1kS~+;_|w1rjR)oUE=YroxoNW)< zcO+FN{!WlhJ(u(KZHBB5uM_P#7qab(WY@kLvSsS2XMD}9X_Bjxs`orlZ~t}xZIo+} zTDDW_ueLjqZJDJ5vMKq6g&b;W}|Hxc=d=-u}=NEO72QCyeJAWa|z9*Fn8~ zv46S!LFDagu6LZu&WZN@NcC|*YR|qq*R&Z9%UO>Q!_or##<`er{)Xc~^7d~qSYW>z zpc|ItLrLwQ?Wnh3))|;&bti-URDUq3W61Sm$=lal?>uYc8f$M~hXCz|;}BBY%-d5w z5vC^nY3OV}@2}S(eKnS7rUS3NMaV}ac?tQU6ArH}d^jgTuidF|LXx#TAI?ek$B{n?oMR8`je!f3tnLDspJdx}eNF-GUB}al zWY^mE?4x?^&H&q0o<^$up-DcI)H<%4dh1#?DaqE=erA%#k)Ft#`h+B(z+9e1{@m0* z#^vjYF-U{I@ubcv;}3`RjLV>%1=iK(yriE|C8r{dvjWxy<0QBs-qzpkM1xAthEtXhV4%dHu~ude_CZVBW%U5#`QRe!kn6iy^ny z_iWIw6|`dwL+l#mOUOHyu7mO{Qs&$EGC`h0s$M$}{VsrY^h?>iZD^-#+xl3T?8%2% zHFJ|}-{(W_?^5#2M~Hu8eeIZ|5SLWhzUxyv59iIfq}_1n%QD&zhwU=wA#SM!T!&tJ zUeDoJM4oHVc&nKO>bMrOBa+^7u1VuF2E972&1`7WTh8@s+?}Am61gLOmM8taq{}F4 zTAD(R_pw7Z-f53fjtZz2B%*Fihw z+ex)qk@RN?n^c?Qlm0>S+B^j6KVUxAFgE5r z$oG)m2dqmt9wqHT-CWN+XKN`VK2ETI68U~$9>cMYylv+G%|8sbtAXYH@%Kow??aw8 zv^*n%K|jLrZp>%Y3kP-? zKL6>j=ap@EPHMopYfR#B4l2J*-t#xta~*}4qPD5tet50C10L@od;ZgII6fe?4{s$I zy9}Sk7tJ*AI({!{W_ z19qAApOIn{;>s$2PX1fKE>r%36q^wDRrv?<{{ieW6fs5(rc^#pOW7G>GQ`V|APD*&~B*zJt=((v1D!kXJq@VUuD~&e_6HUU%%Sp zBj`7gI)>c;ugLfe@o=sGH~Bxnc0Kf^p0Z5&cT(G|n*9GoUR(XD$Di>_v%Ed)>62?P z=)Dg0u|tuJeW+*s$H`-#;Wr#5*p;EJKFPk6?ULUrwa?mT*o2t3R&d?RBL_R=0oFq< zo9gq^|6o5b$@tE&&nhK<70BZ|o1J9qd46p}s(;S2OyBxZcHM2$ddm9p9H^Y?ThH^) z^FY08ua7E8PdgcH82dKpO{rsi+91=f;EQ&`VL85xr5p6LgXrPw4K>U^x@bI zxjFgVzkYW|Zj|gBlj^fevbU`k#eX=g*NS|Hq_-|U zLp=>>JB*Ju!?6b`eKCe>W&v%7{?V^+v>|N<*ku;rJM^z@(mM|H**;0`Oxhl(7wlV; z+Mm6Wo;0&f$E82~WX{39N#C8kZCbA0ap1%Fv_ZBl{AWFrz2_Qhld*>3;5^8fV`K9M z`^BX6DeIQ>oyd13uTKx_()Wz{4TtrZLu2h`7TB(1x+KYSN%3J!-=N2D=9n%{_3az} zvpGrDH+{_*Z#Wz$V={hds6UK+Z}LYaxfkgXl{zRY}t|I>33$qP*QD}hj1K5egt{#EMPt} zZJaj`<}w>kUOQ#x*Lc0oagd!4{paJgKlCNU9@ZY6?HLO=*t;XKMbT(@J%ACvUt zU7HO1aL_l~&sc|WSa&o`PBQgEY~SN4)8HIZY{I%Jdw!e?6Tri|Cy*bLWa~{NKQ6s~ zo{W4JubN0|%g#=+zOfDQceS@%Tlx@=aoF0Y$*KSA$lKnOB(KMgHbeYmD`e=H17kpE z%B~CR{m=Y3*Q{^h0`NE=c@lZcJfU4tb*_r5eJ>)voYaD=NEaj9jxx3(zVa1G#@5&048P%^ zzA=|WA1tulD`WkjUx94hYm)utK9}Te0ZbzowaL|tszd7%(eZ@Zb)Br!Woh?iD z*ORA>5F1(j)yOv>+m7;$q~yc6lpQDiF@AEUyhmxCoyp%yna6dcD?vZXwypo0lYA4Y zeX!3Sw~)s+w4Swnlz4h>K%;roz?PU76FX>m2w@v-1$8ScR5PMnKapEgu9O2OS zop5iG?clsC(#^rcJ{6!1Y z(RN0E!{IpSLx}5af&HQ{wh<2cVShu6XAA5nZDsT`9QK30g;>ZItS3JX{p|GmH;>oa zvng8#9`$moMi{fsz@;X9Jn ze+HQtiWxq`K|Nz2hwn^U@I3h@w2fUjUO~PVpD!i(e)2C<_F|GL_wOObOU$hB8tMDU z=!2~78(_Km*Gcu6+rLHr6YO)nFJB(w~yiY zlKl7N^=rZBr270N$>ms!&FJ;3p1wIB_BkA-(f>$3_rI2Y(LdK99L#-2`^KiuEKon5aRoZhLfGXKg$m zT(fX6x30Hy9uMki!#aec1X-I3>3S=rUobYTM;Y?kR8BHuH%521m3AEENv%^g>C2L@ zM&5N(zcuL&$hkf3*mjr`^~9acY@=bSAI42SpN|^k^|=dZ-C6< zU%#x6v9yDpb#iU9ynX!|YdCM8*t6lY2+xB82kjV_JG_P~(^mLfIqM#d-I0frZ;|YW zkT!!6NpJa3@-35IU2_ZGiV`-`~AY2{P}kzQ6mP8xHHXfsU!Z z_1cqfN2-D8L)hv^SzCQ{N%m)xr%hvL2YqK!Y>by3WPD}kz?5W9KeJ9SImzZ}(^%iZ zUO)6Do1XOhkk`kIBzGrIpWG)R*!M)X&4ZKPy1k%xk{`jY2l9SN-j_Um&o-p-cO$Ql zK1pv~+vuBQ>)M9_Nw%N*I55eU({@IAIP{Cp5D(je1JLn)?t64N`XOIPet%L6XvhAA zxZle5lfKwSIQo;<$IK)TBtMJ1el)P(^uaNQgMK-l5G&h)gOK$xJLw0JznHv!)H6?x zuaM98A;{L5lkBIFzl6N?v_F*8bM%;`A58v8@}9@)4fp;0te%CZnBe;T%VLN z8lybuk0;gU)Fh81Jqhyu+RsJU=l<1=g}i;s7o*Spt2+hq_AOt6KKHNgG|1bxd@1_V zN$sz?Mqodc&mnDv-gQtugVZ^nK$(Ygc{ZGz$De4O!^Mw^^qm{Jo1h? z_lKX1wnB_?{cDFV!%sLClAe!jI~G{~0=P8kr;t*|K81sJeZ2{>(Jip<0^~`g%GR9< z^OOAyQha24CwVe?`W#}VYj3@I$X*A^);$E~CVO9Z@ZoD(IHr^DOddbs;C|fe*O=v* z1>VQcB+vC9j*H08hJH!rwZZGv80wh@+Hx&s1CyR>)41ur1_Zrz=}X4-84l~wKjW+i zy>%IDM*qWMUB+M>^`N&dbD1&Da4ZZh*1-3*%mV9jzPhI2&~`C! z-iAXR=cDIvIMi|edCrDIU1K;c$(+AoUyfR&^dZEEXN)1HJYxy>4kJmqj>EnKT=!vb z0M5U#4*=JJ*I?LRU|-U{&>ecge$X47YuCgzr~y5o5A=kc!1dn;x;>(hF6<1uLPMwr%;!IQg0v*x6g-DKznP;D z!=8ByG3=d7&Z7|X-{(5cv9RyWXvz-)&&eS$7!Cu^2hR`Bp#y{X~v zeh1hQwuhRqEmVXmupMj-+dw6#4CSE$l!bEO>y77K0ZPF0aRP9k9QMOGoxHCBJR5|4 zb$rGdhkOojuN?N{@wv%oQ=c7tru8|C=f1G-5YHGsvxR+%e17to#^*er4}2c+nZ@Te zo^Qh5KU_CqKZ6;hGhsZO24}!ouoaYnGlAEq@cuf9)a#3DH|(J>jnr$A*MP8}!2;4t zVIdp}hr>~D1dM>8;B_?yP6e-_Yv>R6c442GD@o_U0$2`<;99sEE`yn{5H5x0)V+#y z1zZGkVLnWUX)psWgvD?X#bKq<^7!HHMa1a~; zhr?db9@;?%*aP;4J)tAChC^Tw90~(r2pkCgp&#^u0dOn~g_h75nm|(+0Y|||Xa=L8 z543>3Z~*j#{h$YQg?*t5bc21MJM0g=p%pZOhOis#40WIe)P`N5F6;vJp#kg!HDO1n z1)ZQXRD#N|EmVXy&=$6aZD1=X1G__W@N@SPQa@|=Gx}2GOG5$b!FI4c`2B?Hq}9OB zp8dXo-%0SZ{tA?p2fs7mXU=}s%=ZKS*=yJDQux{Q+ralw!uLt4U|$vd+??;4gwLY= zj)dP+@OvMx!8`Cid<=YM9`*tDdmFC<-!%#Q1^Yb^zc1nE>wNDdd=G{1k%WD17m&_} zx$qcm4In)j4uXMj1o*eTHRx8u)=Uv0-lDa;8A!4{9Vm->VHkXA8B8B zfc(So5ZnXz!ugaR0tY}J7y}1Fe|Ql6U{XHg3E%M>LOK>Q7zX_9@Si=}k0d`FE`bx^ zM3@5?1AnXZ`?!7=H+;A8IMU-`226vga3^i8g1g}^xF7BVKKCh1f(dXA_}P-5U-5ZX z*!Pdmfx`Cz&mcV&PJ`27BAf?D!?7?LjsgC@NgST=J=Ma=q<7H9?Qk313OB;yYQDJTtfp&s~KlM>Q0ur<_% zIj42s{j{;Zb-99)uRq9GXHCxEJmQ{toJQ01J1){qO+X2i0IZ*d88( z$6*yzg(u)iSPOT+@9+y;4>!V(a4lR1KY-u=xg5TOC2$2i56{6;SOd4g?XUuFf~#RA zTmx6Za<~OcDM~5hFjrL;QOjXLyr3}tt7n;R=^E#Elh)%a1mS#m&1jy1Qx?(un;bV z1u!4x!7P{!Q{e)b1e0M7oDVZ#I%F^rCcrr`9!A3nFba-^qv04h70!f{;dD3y&W594 z7z~GzFanN*p>Q}H0Da&PI1C2Ep|C&nh2C&5^oM?MAPjps0C%A5^M*Rp&XQlD&SE8N?>a!1=~Pr*cQq_ zMc4|eL3P*x8p3Rhe6I9b$n)S4;`QrbcDTNZ}6F;&7vF5oko&u6uv7Sx0qup{gMf6|BQq}AXL^4pXC zMEW224!(yU;78a7U3sVoTSHmc7Jf&!34VhL$mO66l!6i{z-DxR!zb`Q`~_dYNAMwh z4xhmXP#XI$Nw*^Xl=NeG2Y!WL;0<^aeuh`!EAYPc8~7Gpf|ucKcnjWxci|Oy5xmEE zo%A*EUfO$R@0Go0eh$3P^}cvDcz@}ArT3TKZ+g#{-{X1D=l!7fqTYjgAL#v{_kP!a z_lVbm_lt{QFS zRDy%yAZQJ3pcU)^EubYdhGwukG>0b86dJ*vup8_O^xP1bKqDQ3CF-_H~@M*h1!bxy6jDqq|0nUN5;V2jh!(ccZ0YhOb zoD8SHG&mhjg9~8@^oPN4C>#j=;1K8w17R#23;=cbXxJNC z!G2H|>O%uK5=OvIP#fyN&QK3_fy3Z%=nnfrSLgvfVISxQond$A4PBrYbb=1h5t>0$ zXadcl5!8U{&=7Wo9icJo20K74s0l5hER=)Iymhr9Z42$7J^Vr0-|#22K>nTdKd>$N zGO!htg3?fc64*rcSNH{fgTLTs*bZGas0v#{WvB#IV0)+t+rW?5{s2G0x9~lD0Pn+x z@DaQNufcclHGBo%z?bkYJP+@|3-AWK4zI#X@G*P_pTj5c1$+w6!He(~yaI2-)9^Ap z1LZhpw}P#q0+fNWP##J{DJX#ge8f0DgiqjOcmy7W&)`#d5Z1xN@DSV$cfpNt72E(f z!%c7p+zz+Ft#Avhggaps^rgvukUy{ZJaPgY3!~w5I0MGQ$#5c^1jAt{jDnGHBrJiY za5*f7%U}^)1y{lqungwGa=4H7eU9;Y=4x00*TMC0EnEXP!VPc?91XKzE?f!=;S!h) zBVYzx2uHy%xD!^v6qpK=U^1K!6Jb0|fDA5xbKyKV8%~AO;7m9R#z7r82Uw!;Hw3z0 zs0VvP2j~oKp&fLAj?fAAgx0VR>;c`ODRhN5;aTVg&7mDn^UW$Hm=%3E=6Jx9JhbOkzb*(mm{7K)W5pNP5)p0dGAeM zd({msyInP{)Ia`}xA>3s+uc>s=_~xSxq1)#Bg*>@zBIHyX??Arw?Ci%G`HLKGfr8p ze$M%ahxXgv5|*+sq|xy~zWl-?x~tE}m;1}*B{z5YFMZyhiiI6QeN*fIbnBaItXr$F zJd7{zTQ2ARe%f5pL;Lev_6q&~XFRjF5A|p6RNvF2(BaTl!GA9I?|n$H&)cu}Rn;I@ zJ|d3i_(S{rmp-4L|FzG@bIXE}YkS_*vfJ+4-5SO>YQU*$+pymx{qA7<-w(Sm$opSW zH=Kv_s?P{A{zCtk&)zfCFO5Z=F&$^k*jP+k5b?|H%Jaf9bMOp?*G|T%WH;E}yxfN9bR{U|@)^^^_0hgX`5-$V_Ag(reEo9&rIWvHc)`BD5AOrrqtB-| z_jZkfj}>jYgljgx2HqPpD#*KBUG<+fiq||}gG1_<4*KN>G!1L?aHk)`nDRC&JY7<_ z^6`>Fvj<8FH~uuUhi!U2ISzb;^Ze7zm(nBd6_$1k@^Q<~401kC`MUhCd@ug&)0ofV zbyx}?&fE38DeS}x`~SW!oV&E+TH78p^xWXTF4tx8dFFZNb@adMPt+$MSoE{(sNgd`>Rq&1SDFc|E^u%i8AG{ympY3O>*J^#0&u zIoGQG^J_hy_k2#OwL2`dna=@p7_Qy2JO3xtFO}wG$9H(`|IqiI|9-7ox9!w5zSigK z?m3%1)##sV2Yq+1rTp5;{dNAq`-_r7$3>6)*K1||+S&T=Mr&xgK;Of)*P+4k)x~mt zE$03nZX2-9%L*LYn)P1Jdrj{xcLDETn?oDu2;IT^r2~PNr|{e`lJqzj3ugegK!psZ z0heK69xMVbq3}%LJ)!q--n;o?z# z(|CCde;<2|^d0co?o0R{et|#1=RcnX{hMGl@b7r_U^npZfBsEyFX#l_pf~t8AOCjf z--`UZjHh4G+R&unwMqSKv+f2tJ2z;Xkkm z{s#Zv?BBTk8@PWX_wVexf`6O$Z~p$R-p>bmfS)l80RQ&y=NU)C32+LW1%9?N31+~> zumBdr6|e$sgj-=1JOFFqDR>FqfOp^{_!7Q{U*Rt(%?VloDnm7>0XxC2&=mHB_Rt0P zg#%y!42GdF5{`qha0ZNr45q;>mwg z7zgLUB$y7fU@k0#rLY{XgPY-YxEmgTN8xeU0MEcn@CLjGpTJk}BW#AU6?kue?V$$j z3=NQD>nK|^Q?Eujr`fG*Gj_Je+K5DbCQa5_wZ zg>Wt018ZOfukAdVhu8l%Nv{UWu7hXcMR+O6Po%QvNVU_3Tk3zlvy<1=-?eam(!a>- z@k_8A+4mxKz8_ic>-hV?ZD{zLoPRsX->>{Tp?@<}=X=O*uyegJvf z`q$1ey#?y?{yav04XlMHVLhl*wx2JCVWSoS0Q1in{U_GglP|N0}zzHe0rKl?TBXS&LM*6ZiOe%@=|&w%sKiG7dr zz0NW}BUa8oZ}#(I%j$ujJ^NX*pFba#@_w#-Y%24;Z!jDR>W=|+$AO=XtMk2b1gIMg z%|KlX@I6%BQQ+t7mibxxUZBqR)UzS~{N2yi)$I)(p%Ylv2HJsTU7#y;hkcn*Y-NfZiQ9g{e!lCcCX#l z6=;*+@608?0JPPvHMD`fK%36seNlI41$%({wxCU8XaOxjo#pLdSMVN7{qE2lw5bp3 znm|*q+|O4n-vt_gkax`Acp=fGT;SCq~tzW|oN(vw>Qz z=JVH){gC@Y{+iM!$p@19dgJR%Z&3CbA%A^RXZgV}9enNbb!&2xeOAa{(|oO)m~>M} z^Vh#~Q+Y<}>!PohCxZHMFdim=b|XN&ueI8o1Y_X;HFYNNQB`LjK6jGI1Oj9N1j1qn zVMm3;poO?FEJ09#KoU39FqyeYMkX_176`aS0TEYRa9>cXZC$EVTX)6QeW}{keTfyd zYOM`LyP%Tq`G0G zy5bb1!QLm7p9qsY{Q#`JkBrBk0LlkJ_og8*8dOgC?>V=Fu!UgD=>Da9opL|G0Al^_ zNag-ZZ0~~sV*Tbw`M;s-x$fVfGRl33{RDP_^2+@O-UH=6f-X>*@8Dff?tS zr^8AJLj;s>2IWtIQ=t)7LKCPAODVnUNGq@^a~i0O-dP+EEZ6+BPNp(i6Vuw5^2)6S zmT!7>t#h?|aTd$&nlg~4xC7Qf8I;2~P<$qw<;CN%6TG<7^JUM4_uzf_&a)q2Kl0*# zWB=pDUD!`Rn%;@X-v!@#@ekM^y;yz^3}9z@1=hQUFMFux&%#!F@nP7XdGSJQy%+O3 zuGe?5&5-bH3pVD(?by{`e7fgnuq%0hPHJq-}tWa3Nd-($9jkLHY%-1(ZJ%q@M?yLAh2q1C%=#lv@C& zLKu`+&ekcd30zOJ?JI7E7-+xjF)6>+J8tDp_j0|*j;`DpUS8);=SJss9jpiC>^Y6% z+iRgR9iVdg5CP@wwNtq~P_7@?Yp%TdNiJ6T0N8$_GPX~tjE`95{9yZ~`l0%q28HrD za4z9_p#H4BtDFO8;%Ypc4V_@iZNRr~}Eb>^2MW7kPPUwvD6ll~Zi@ zNBvj2V(puOT(H}#{^MXN)PgN<+fdH-^9X(m$m%%cuLFCmo%k9zw!S)idn`IWyWJ4_ z!%!%OVK5v@`uOAVCxEpR@h8JkFuRWp;m?72un3OnWA*-bF*LyOee4PNdMBKMO>ll6 zy9NJ3*b0}!#c)}l_)7e%;Zje(3cJnIuEAacSAf-@=a^rBjcA(hl-FD)U2~pvJHP3E z_zUKkNXNxdHt)P?>v)e+?>gE%EiB{EcuInx6gd0hPN3O=YeF zmAeUUhRZ?k>aPZ?-GYA^Yz5`7f^DEOmx6Lvf^@ytzrT;&f&UX28!pdnooWFP@2=<;Akqa1?wB zpY^eyAr?!||DaUu3V zIM~yUzzzcGK^P2EL1k2@VvQq>A&nP}ktv`u*4KE^7_qvo-;SFaatlChs=Xt<_-L&C zo}m4+xrO)|Z`w{{ZZRx{TCjC#TgA&@IjBx)+E#HL)Pvnt<5OeO)}!qdCm{uCuytu$ zeHT%#9c(^}ud#d~Tm&}18eiLIKxJ%RV|!~K{XG25ply}2V_ez=aEYf~h5eZ8`&ZZn z-$DVJuIY!Ku6Q8WYxg#~a{q*GkRAm2w$CUog2C_(`n%8tAHyf0w%!8OrJU;f82$>c z!yE8tuw~SiG}~8g+iLGG@Fu9858xwETYm!8p`7Zm`|%3?tMD4AEn7~SZQG6&wKo;! zg4%fx-UqcK?T?^(REOP{^{RQ zmEo&BwKEb%dA{1R^$y1`ff1lG($&r|P_7izmTg=04~EgO6xyI2ia@%pM{TLhASi?& zsEloEAii=#!M3HG>a2xjupH_@ZP@MAhAs0lzODNO{1@RRkX`_`4V8Hwlvll~vmP4Y zc=!X@Hl_a_Y&}om?}Vp8y4q3hN!SUx&!}$OzUn&>PJ+kaaZvrL_hB!#bxHp%{0<%h z<<*|@4}$bZL2cRnP`#(Xsqhc0(c2h}UxwtEBqjj$b5#{q&+^BB&hbH> zKi2c-V&{Y6ah`u1c8M3uo(iWyV;{Q`|5~^X6Jdd9Z#zBPw;0@8}GsUVC#DsU)%i=UISZB z+dT`cQDPIKl@B7-@aqfGwM0e32P|77WD3Am8WmQp6}(>lRE>H zOTubML8~-~LmPC!I#BseI2Tkl4VCzZKovwF3ff-VEO<6UJ{CJzFjoShFeXXFz5 zy`1#l!9(ycJOw-9Y4|-n13$n%=z-zv%YU%ngVnq7{|(ar3(9>7(s#qRApLLf1t|9o z>;dIIgU>1lP36z@#%2&c6FaeZ12$Y)+W1tL_n*z!m45L7~a*#e1 zMu7AKK>9e)xi7}*oGU*Xl(+kS6aIF%1MC{`2~5tXK_f`BYrv;?y5i|j0n##%^ayNpGQ#l={a_X<@n}2}668{~(2Kk?Q{y(tac=6|+|4(c;DE`XxcVqwM z#j<Z4Imi=|_X~G-N>f0+617BuK9T>8l_P(&vNp<6tpJ*YQa|26UX#HD0d- zjoIrxtNt(l2Dk+@29`oCEQdO%hX%0oz#x3(rC$PeZYaQ4t`LHtxklOmPb_H;Wp zY2K1P1crj0$L!pvc~QC@w|2a1E~tjXLB5?c4)L_3u(RPnkf!-Y@qDNOI|pfgQhX^~ z1|g6pU+3!z&^gn0L4E(T_l~3RLtyVC@{fUILH8Be1~>uiy-EHm&^>AsZ1H^EujF42 zSAt^QujF41*FYyoy8^a>)fA`MzYcf-RznJ;WgrX6B|*Bhi{N5d1=7!lEpQh4VmVl)o8lzqte7&Nuhr-w$_#<{rg6;30U}^B?fyr?5Lg@q?f_Pw~_6 zdoR`;DE|-e3@Em9sp4nhIndlH`yyyA)w;NyQ{^8ES|_)2tm36m3z~m5?;h>NS`)YP zvi!}U_3)XXIbFF8uo0w7TL2lifK=OxWon%i_AvFqk%;a?3e zaZInkA7K&kF|ftcUq*Y)(=SBRHfO`Ra205K`RBnVxB{d%c=7qztss3l)Pr)Dg7hV@ z43xVVq^teKpq%zYy4^47+K-FCmeKZ#F9BPZ+EIKN*fzB@Q@joA z?+om5OS=YELEOtLzSfJ|un8}|&hwMlc2Io1=dZ@5y!Zxe0Zf9)AUzGz^Fg@@FcDNH z7o-n?@t~X!q!&UElygA(Ko|td>A_HXKNtYYeZlqZ0p$Xq+%Dq3fpYsm`7Zbbl=~5+ ze*hnWa$kbUebx8I3Ar+$6s$)OQHFQ<8jD9$Zo)*} z=5RKisYNqwZpKZeYu!{sB9`HGUbcNsM=Wj9*{0*d>)h1R=H|4UNt;Nx%}s^t+)O5x zXi1v|sbnH!+LP&6CYDUlR5X@N8)`|nyOB(6jcYRDREwK2%VRCA8B?EZH%-ZGB08_P z8Z&)qTY^HVw0mqU)7ZP&7JibEibc|M(rLG?Dek7s^ruZ=Apa}dZYmXyB}~ZtR!t?s zYuem&>y2i0xW!Fm7P{+#ho<;J)U&4ja=)?^AXXogMOV7?2Mt*M-rO-I+8crvoejheP>#!a~CwBe*h-PDDq z+n>sGR7c&WY|HU6cWrfhjGE^cnp>+gZbxQbGM-G0Gi#cYE!9wDcK465T}O4wZDH4! zHZMwWFm5`tINUMa6qcIY@>rtObmynS>#E(vniwawjbmzzCfnS`mekqKu*Mxu;Z!ql zPRebLzvRr0W!6nI`Jq%Y6ZZMa8pDVBSX8)z%aEyV;zFm*H7C2NVITPmE+beMwCW?W173p239%x-H7R~n}%`LwTaq;Zy|!XEJV2&Zg=#Lfow@ zGT}M#CO4W~Y6`22|1Hi{BHb3tWWtGv`-QLDNk`}q^P8%}ZA~$*)N!}x`bteWlI57% zQvUAY^TU}iHyj2*tT{#(JFz*LYBQO2?d}qKzF8AX$C_gCYjT1eBTf0z_Jo-q4`DEO*1O1D%|jl8M9_Z zxadvM%H|m}D`!?kXEx6;3mPk%4n3@@>aZCzE21-I9Ck>wY1XV6uG=l5?#A`-4L z42*S^W-(*P&}ZGTdEEuE1S34bo`&OT)6BGSJT1B%^OD&SCa=h>UX+N&B5wMZdD%7L zRJuByxz9Ks_paYwomUo(`JDTciMmLu%Mm71$2h*{a<)d3Sq5=kEPACs7=F@e567#U z7!2?Ehb%5Kg)f?5s4?9dp2L>eOnWv%rPPBsL|l~HaDAg>o}iT zy2S~})T-uqa_uM+iN>0nvuVedGrjk{@j=G-Uni&7tasaD>2z$(RI`zZjjj`mEHT4k z6=qo~nQduJ<>kyVS4HA(I2B2TGY1=gkr}=GKod%~ha*S%a;i;Eq1ja5N(XFB#@{xz z$p|w_%qcv;7tF1621noZKfpwmj2liwEPR)5PZ9eQ&Qz^9mA%dIHRV&!vi5K)oYSw{SB9TWaDt-K`kQDnl07ouJJHAa z%b2yXDAR9?*}!FqteTtb*p)xOHk@hQc9vt2UR_wbQn{AXaS(i_=s2 zsuOOH73aJ zot~Z^9;PdX7A*^y-DM5SYWQz{%}}4?_?&8=FYlpwC*QO8iA8(!9$GbZ-()@o&bI}x zPA1Qy zRQ5A|{uYGd{H-+aOg2Sh25FshYG{4{TM+vB)_$gHirJZOf=df~H~UM_gvyL_d6B7@ zVpatBwpmUVbD#);*8qN=e2lh9)ud`Mb>i_mQzA@*Ao-Q%L^<#SXVRz6}mY5&&P1P84 zMu{n#FsPpimiSGW<0}bO2F=PbX3D?boxRWuSnD~{T9c6qEq37?%2PZ4|h5tU+2SD7V%U4&Ug4Haz!u9)G4^M zu=m`)&OcPE7}|MRfYthf(9e3$kpBv%R3RWTP%x7rIoO7V z;`(C#HZ;^U6xU$t`BQB62L2t$-4h6O2l{^#2>5^O9{SbrFGhSms^|03djq>lyGnES zb&cQKH9mJAfBZl0?J6DkT~}%0p03iO-O!zHy5|l3rmJ*V&o^D;i@u}4VPAI5`tYmj zXO8UJ`_<_3-;e%u^th)-@144{_@JleJ~7w#_}rYw7Cf?W&oiM9$WQy-F@O5>*awgG zJ+NfQvHts)J$uBj>-IhSm7bUMu~q}L_tQPWhb>;l{uej+8AwbaA>&MQoLBvhDH`f zibA>$^>){KoFV_%H{_o^&OXQCk3AkucDCV@a(=%}QY%ksWg2Qkv znJc=BYnJ&#PRIoOet+%{{PW~o)=T|)c_z=_HPLVK=nRK3wzvD)(|!EO&DDI``&p^K z{!kth#r~O>$H$)U1OLmD&ZmCqu{BHVEJb|peL4!>^#yv3sjFYUXvxw4Um#Zx!+gf? zH)g@2n&amGf7(pegEF7xx>|hp=WFV1k@b)6&v)%lo7y?`3vEu)NssN%zvJcWYHQ~8 O%~w9YKd*Iuef|%$%NO+k literal 0 HcmV?d00001 diff --git a/source/extensions/omni.isaac.groundcontrol_assets/data/Robots/UWRLL/mitcar.usd b/source/extensions/omni.isaac.groundcontrol_assets/data/Robots/UWRLL/mitcar.usd new file mode 100644 index 0000000000000000000000000000000000000000..4dd33fcad580bc838f2b320b7383167eb3fa92a6 GIT binary patch literal 7157 zcmbtZdw5humOpiGKNFJfPSVgk(*cn@fFvYh46hp!UXwsNB+Y=YaMOJ|xzgRYxqTZF zMiX&Evft=_u8)x&6A&F?N$;<2y0Oi& z?jKwI{ccsAbL!NoQ&p$x-VUr^J15xL(o7CA)11kp`TzIHG75NZ$tkyG7}g9;K;Dp3 z=0y?6m_ws-6C|!9OC#DV5jmxupb?dtG6Azp#P#v?9UWG#WrAeG(+_0QpSmwoj-dQ% zCPzKB_-`h20=;48rDR9`efH5AE_iREZB`eyF0g747wHgQ^E4$bU}nB@8&G*NQr(1<=Z^}Ug?%>!N= zz!Ud7Penks|6-GpCza&TOcIq!-t*7`<|LNVckg{41~E<-?|& zk9;V9%a@iAjpA5_KN0+_UA!HUPnv}1SQ_63V`)>)Lz@a(w!d_G+qNv)Oh@==+IiZP z=Fk}9PV`Cs^U%u9Lz|vMLlMa*&A>BjYgXR{IW#WCZ2 z;5k-bWe$x?ak(ly$I8vlq2*1RXUfgNGwVyD51VQ5Ke5hEH}5tBh>zyxw#nnWz$aDb z&;)@`vQMhXq2*=dYIA6W8 zYizcS0$v0Vv%U49k{yYP@>RelAPS5F#wt3WZ0vuL6)j-!l(jUDvIgXHB2og#wR$t^ zI*qpz3<2c@N@Gx?$?Y2cIFU;~IkqeJty_+&9FCJa{ZHuz}FVDVxm;-d4 zFXTWOXoKu1?%uO!@%GO>*jIx~&MhZC3Fj)-+zKDrIZimYQjlBWJNld$KNika_%c6N z$;++qXnu~vkEe4LeqQkLAtQ``(M#b+8`rLyx`5~VUzv4r7M#EG!_%gpGG*?YMUluu zi?j4h*`&XIbf>BFrBmf*&fAQpLHOhQ(h%owyl?Nv=ab9QUpRX7ZO-4jXU}Vy{Lpvg zD=vRNLw{r}{rS;$CLMZS&gBm-%6ppEv-)X_o?Lpa9@z0_uAYqtr@dmRmkgQW!iO8v_5zy5b#S}x)lL18!##IABFO_KOSCfNFajtjl3Fl_VIOOei5k-J*~o>%EFWYt`O9gSxZ?^=PShxOoK) z6IHjz;*imkN~>ygUyMi-m$$@}WM7Cj!|=GVQjv&_qoTD64Np?~L(x=NfhIM)R12$0 zlF*j6X#GmGSy7{~hcqbcK12IRP8ofg4w9Qn=YB!LDF;Qh&?Y5Jv6KOCC6fflg%$l) z8nWs}e`8qbN%gK%m932l6$)Ei^vgy=={K6SsHV@LEfKA^5%AD(o{Dz;jk?kcf3!zd z#W6oh(pW9`SCeZRS*EM;X>>rB2O5?578U-vnGJS_;q>laeI*;;{Uhe8q=F`0iA3*Y zOH^ZEE;**_+FzKzTfR!5*Xa(p(`f9GLz|Lx*QJWCHANGBatRf+JSEtMh}DxjQ5d&A#9Xt; zS-N2s30H?w21tP}W?w;<^vTf}xt5V&ok`+*qV-v3Uq!po*wD3v8hVy&Yv>w7iz_iX zQOSlZ@HX5K!hAQX@nlFg3`MYPZ%l#^RXRN+H${7tu+~nlIqlT+OB%0>~r(XL~qVasX>MK{h`K4{Z4%x$PoPA0LH4~S28 zN0o@ty|qtKqHc2Q_mk)$cjOtXFrC)-VGxRkggO%NtS83|5{gK;01G!NCl$RrriGO# zb%vmwVzR<&2x*%%8)8^dZcfyq~nS)3DTAequ5sFcv& zEm{=&oI3mDG~KSoBU+;xOGFnDJ1zEfYAHRW;H~3I@+95lZ%Wo#TwY>#G$vC$tHSWv zt*lIq@22a+Y9x|MD&31MTf4D1lZJdBJ%#QmfPkWgl7qIEfu_K!5|UP9^e#FeC)CbR zA0`rU!|qo@8)#7O)#7q=Rs3n@m3=fHteJEG0WTg>6LPev{~g(Ry2nzL`pzClkZ<=aBUVY+%?%eGT@SL`wHMthcd1V-GfPnO%H^HaF>p8cIc* zn6wrFas|<3@?kE?OP1f$AQlC-c6;P6#9k(6CB1?TE%!u~aHkTD+(W{jDX*N8NiB*n z6j>xKd+Vn6k!qVij8 zRg11}@sT_1AlrWw606jH>XO-9evz3KcFR7kfn%ht;C9G z!CK9@fEvu__N|KEreb5=Kn^9#m z_?4gExA?7{pXvO*RtUHNe!=ztML;pYuW$V00l$Uuj{^L~pWov6%RIk~@t1Y}a?Y=8 zrGOtO1EvErfD3?`z=gmppd6?GDuF6sHZTX63t-x)2B-!2YhoQx5AYYi`2Zh718@n@ z2rK{=0*e5CYvXSXJY71PjlQi>kkN@UZ!qX>3QA2)9W5mk2=W)Q0F~e&F#IkSnf1uR zmlxWOEVNDhilsnn}E`fr1>{Q+Zg|u;h7yoU|-mCo#xAz1262CL^ z?>rV4HBr8_-a(!U8-H8Olfrh8N~sC292lHHQl+%h>Y%y`deA}cwTO6V@CGI<2z&s1 z>7e2bK6ujRq}mFa;-FeT%`sbaphYiqp@r<`;_hRmyej8 zhVbSj@oz6YC%U=?9q*ni~E7Q{UjalXjrtH)#Bz2!aeBwUWoky zmX4F|L%;db{SGSHy%H-0zVI9XX*xXz(LVt10uMA$@kcNh<`s{?#C#`AoK8MeX9Dwp zB`|g0EOVf3ZuCzNx=AQ!bxisd8m{+~pW)=M^GRV;6KHS^umiXg*y|v+0&RWLL-2LJ zwBJV?`RX4`_pzl!LoYh8kkNzRkLi>0-kq@!CW5`2d4}K+-jqrIU^ewbo%jLUC`upB zRhBl8=V9o|m-vg;Zf0o^iy!fk=kN}~Y#f8=qdpQorP>M7e}Oy!y9;}|B+mg)%mx>J zExsHufSnlNudC?*=Hg+#Ybk$l8sj-pQzoaO3B1GtQVI!yv zgI;ig*r5W|c$an9$sNRzJf4cW%tbjg(@Ll9U*(^*d|7bDHthbJ^9y_aSNX@iCtRO- zPr5&~7kyFmA5%Ub|L>)r`93K7WYWKZJLO-k1uriUUaIi^wqpE?<-QlnCq7^Pr}BwMDkeQw zG5K)ClxHiZK2tI6P(|s%3jfm;Wxr{CYW0)t2ik>yY!@GIbv)MU+~1n_>(=~7TgN@p zTJZ3OhpK9RRaN_7)x3RGbq`e4|FY_$y;bw?U+ujwbZ!*YB>s=&q}N zewA=%?2edldraIFv-~t>y)9R8ur7z`HB@QjIrLAIW)L-1rD(kCNW z8N$5*d^>aO+b9UXXh+Gp?I8Jlu^WU|d>tG3zgb`M~8Hl;n}FYZ}_{TWWrn^@iD93Hv0vSxV1 zhfs)McPR}EHfKxvno4`ciIIZo&YQ^Qanduc>pUk$uCAPSVx(jaeYi3`P-&ZeawKoM z^G32epsaA)$&sy<^(RN1)18IeN)GWg$J3WW8p*ry2BExsaOt36E$_0^aK5c+kjF*7 zNO_pw3xIH`wH$uA!cra>TnfJkUXosXb{FOSF#QCrPCrTO9CXI^2z93Yk5O%UV*p6moXow$c#P@Ij3yhIPYOzRU zDG89|>F5XqJs#E-$Kx|TyU(?B8q3vIxXT^n<$DF3#$1vcvd_sioy%{r(%f2uSKUQ zJ{|;H#L-wz{3|eGAy(dSQ623PzjO^3U6bBHOF5?qx58bgohI?GUViYKDZrmrE5arW zwvkn!>C6oq?c5jR6ZZ5O`#1J)vfs|S+FO^c=`G$&s5Ym;I T_ML%c&AI%u{9=SulWG6oX|iM9 literal 0 HcmV?d00001 diff --git a/source/extensions/omni.isaac.groundcontrol_assets/omni/isaac/groundcontrol_assets/__init__.py b/source/extensions/omni.isaac.groundcontrol_assets/omni/isaac/groundcontrol_assets/__init__.py index 58473d1..5f17878 100644 --- a/source/extensions/omni.isaac.groundcontrol_assets/omni/isaac/groundcontrol_assets/__init__.py +++ b/source/extensions/omni.isaac.groundcontrol_assets/omni/isaac/groundcontrol_assets/__init__.py @@ -9,17 +9,17 @@ import toml # Conveniences to other module directories via relative paths -ISAACLAB_ASSETS_EXT_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "../../../")) +GROUNDCONTROL_ASSETS_EXT_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "../../../")) """Path to the extension source directory.""" -ISAACLAB_ASSETS_DATA_DIR = os.path.join(ISAACLAB_ASSETS_EXT_DIR, "data") +GROUNDCONTROL_ASSETS_DATA_DIR = os.path.join(GROUNDCONTROL_ASSETS_EXT_DIR, "data") """Path to the extension data directory.""" -ISAACLAB_ASSETS_METADATA = toml.load(os.path.join(ISAACLAB_ASSETS_EXT_DIR, "config", "extension.toml")) +GROUNDCONTROL_ASSETS_METADATA = toml.load(os.path.join(GROUNDCONTROL_ASSETS_EXT_DIR, "config", "extension.toml")) """Extension metadata dictionary parsed from the extension.toml file.""" # Configure the module-level variables -__version__ = ISAACLAB_ASSETS_METADATA["package"]["version"] +__version__ = GROUNDCONTROL_ASSETS_METADATA["package"]["version"] ## diff --git a/source/extensions/omni.isaac.groundcontrol_assets/omni/isaac/groundcontrol_assets/mitcar.py b/source/extensions/omni.isaac.groundcontrol_assets/omni/isaac/groundcontrol_assets/mitcar.py new file mode 100644 index 0000000..9390fa6 --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_assets/omni/isaac/groundcontrol_assets/mitcar.py @@ -0,0 +1,57 @@ +from omni.isaac.lab.assets import ArticulationCfg +import omni.isaac.lab.sim as sim_utils +from omni.isaac.lab.actuators import ImplicitActuatorCfg + +from omni.isaac.groundcontrol_assets import GROUNDCONTROL_ASSETS_DATA_DIR + +JOINT_NAMES = [ + 'chassis_to_back_left_wheel', + 'chassis_to_back_right_wheel', + 'chassis_to_front_left_hinge', + 'chassis_to_front_right_hinge', + 'front_left_hinge_to_wheel', + 'front_right_hinge_to_wheel', +] + +MITCAR_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{GROUNDCONTROL_ASSETS_DATA_DIR}/Robots/UWRLL/mitcar.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + rigid_body_enabled=True, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=100.0, + enable_gyroscopic_forces=True, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.001, + ), + collision_props=sim_utils.CollisionPropertiesCfg( + collision_enabled=True + ) + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.2), + joint_pos={ + 'front_left_hinge_to_wheel': 0.0, + 'front_right_hinge_to_wheel': 0.0, + 'chassis_to_back_left_wheel': 0.0, + 'chassis_to_back_right_wheel': 0.0, + 'chassis_to_front_left_hinge': 0.0, + 'chassis_to_front_right_hinge': 0.0, + }, + ), + actuators={ + f"{k}_actuator": ImplicitActuatorCfg( + joint_names_expr=[k], + effort_limit=400.0, + velocity_limit=100.0, + stiffness=0.0, + damping=10.0, + ) for k in JOINT_NAMES + }, +) \ No newline at end of file diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/__init__.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/__init__.py index 5dd3986..204fe64 100644 --- a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/__init__.py +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/__init__.py @@ -22,6 +22,69 @@ # Register Gym environments. ## +import gymnasium as gym + +###### WHEELED ###### + +from .manager_based.wheeled.mitcar.mitcar_manager_env_cfg import ( + MITCarRLEnvCfg, MITCarPlayEnvCfg, MITCarIRLEnvCfg +) + +import omni.isaac.groundcontrol_tasks.manager_based.wheeled.mitcar.agents as agents + +gym.register( + id='Isaac-MITCar-v0', + entry_point='omni.isaac.lab.envs:ManagerBasedRLEnv', + kwargs={ + "env_cfg_entry_point":MITCarRLEnvCfg, + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:MITCarPPORunnerCfg", + } +) + +gym.register( + id='Isaac-MITCarPlay-v0', + entry_point='omni.isaac.lab.envs:ManagerBasedRLEnv', + kwargs={ + "env_cfg_entry_point":MITCarPlayEnvCfg, + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:MITCarPPORunnerCfg", + } +) + +gym.register( + id='Isaac-MITCarIRL-v0', + entry_point='omni.isaac.lab.envs:ManagerBasedRLEnv', + kwargs={ + "env_cfg_entry_point":MITCarIRLEnvCfg, + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:MITCarPPORunnerCfg", + } +) + +######################################## +############ RACETRACK ENVS ############ +######################################## + +from .manager_based.wheeled.mitcar.mitcar_manager_racetrack_env_cfg import ( + MITCarRacetrackRLEnvCfg, MITCarRacetrackPlayEnvCfg +) + +gym.register( + id='Isaac-MITCarRacetrack-v0', + entry_point='omni.isaac.lab.envs:ManagerBasedRLEnv', + kwargs={ + "env_cfg_entry_point":MITCarRacetrackRLEnvCfg, + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:MITCarPPORunnerCfg", + } +) + +gym.register( + id='Isaac-MITCarRacetrackPlay-v0', + entry_point='omni.isaac.lab.envs:ManagerBasedRLEnv', + kwargs={ + "env_cfg_entry_point":MITCarRacetrackPlayEnvCfg, + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:MITCarPPORunnerCfg", + } +) + from .utils import import_packages # The blacklist is used to prevent importing configs from sub-packages diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/__init__.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/agents/rsl_rl_ppo_cfg.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 0000000..d92afd9 --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. + +from omni.isaac.lab.utils import configclass + +from omni.isaac.lab_tasks.utils.wrappers.rsl_rl import ( + RslRlOnPolicyRunnerCfg, + RslRlPpoActorCriticCfg, + RslRlPpoAlgorithmCfg, +) + + +# Adapted from lab_tasks/direct/cartpole/agents/rsl_rl_ppo_cfg.py +# +@configclass +class MITCarPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 128 + max_iterations = 150 + save_interval = 50 + experiment_name = "ppo_mitcar" + + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[128, 128], + critic_hidden_dims=[128, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/__init__.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/__init__.py new file mode 100644 index 0000000..3107d53 --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/__init__.py @@ -0,0 +1,15 @@ +# This is the observation ordering (DO NOT CHANGE) +# TODO: allow arbitrary re-ordering using this list +JOINT_NAMES = [ + 'chassis_to_back_left_wheel', + 'chassis_to_back_right_wheel', + 'chassis_to_front_left_hinge', + 'chassis_to_front_right_hinge', + 'front_left_hinge_to_wheel', + 'front_right_hinge_to_wheel', +] + +from .actions import ActionsCfg +from .observations import ObservationsCfg +from .events import EventCfg +from .commands import NoCommandsCfg diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/actions.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/actions.py new file mode 100644 index 0000000..5115b17 --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/actions.py @@ -0,0 +1,14 @@ +import omni.isaac.lab.envs.mdp as mdp +from omni.isaac.lab.utils import configclass + +from . import JOINT_NAMES + +@configclass +class ActionsCfg: + """Action specifications for the environment.""" + + joint_efforts = mdp.JointEffortActionCfg( + asset_name="robot", + joint_names=JOINT_NAMES, + scale=250. + ) \ No newline at end of file diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/commands.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/commands.py new file mode 100644 index 0000000..df27f12 --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/commands.py @@ -0,0 +1,9 @@ +from omni.isaac.lab.envs import mdp +from omni.isaac.lab.utils import configclass + +@configclass +class NoCommandsCfg: + """Command terms for the MDP.""" + + # no commands for this MDP + null = mdp.NullCommandCfg() diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/events.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/events.py new file mode 100644 index 0000000..ef8fe4d --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/events.py @@ -0,0 +1,42 @@ +from omni.isaac.lab.envs import mdp +from omni.isaac.lab.managers import EventTermCfg as EventTerm +from omni.isaac.lab.managers import SceneEntityCfg +from omni.isaac.lab.utils import configclass + +from . import JOINT_NAMES + +@configclass +class EventCfg: + """Configuration for events.""" + + # on startup + # add_pole_mass = EventTerm( + # func=mdp.randomize_rigid_body_mass, + # mode="startup", + # params={ + # "asset_cfg": SceneEntityCfg("chassis"), + # "mass_distribution_params": (0.1, 0.5), + # "operation": "add", + # }, + # ) + + # on reset + reset_car_joints = EventTerm( + func=mdp.reset_joints_by_offset, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot", joint_names=JOINT_NAMES), + "position_range": (-0.0, 0.0), + "velocity_range": (-0.0, 0.0), + }, + ) + + reset_car_pos = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=['chassis']), + "pose_range": {}, + "velocity_range": {}, + }, + ) diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/observations.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/observations.py new file mode 100644 index 0000000..fdb4779 --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/observations.py @@ -0,0 +1,30 @@ + +import omni.isaac.lab.envs.mdp as mdp +from omni.isaac.lab.utils import configclass +from omni.isaac.lab.managers import ObservationGroupCfg as ObsGroup +from omni.isaac.lab.managers import ObservationTermCfg as ObsTerm + +from .. import utils + +@configclass +class ObservationsCfg: + """Observation specifications for the environment.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # observation terms (order preserved) + joint_vel_rel = ObsTerm(func=mdp.joint_vel_rel) + root_pos_w = ObsTerm(func=utils.root_pos_w) # position in simulation world frame + root_quat_w = ObsTerm(func=mdp.root_quat_w) + base_lin_vel = ObsTerm(func=mdp.base_lin_vel) + base_ang_vel = ObsTerm(func=mdp.base_ang_vel) + + def __post_init__(self) -> None: + self.enable_corruption = False + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/rl_env.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/rl_env.py new file mode 100644 index 0000000..84dd709 --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/rl_env.py @@ -0,0 +1,33 @@ +from dataclasses import field + +from omni.isaac.lab.utils import configclass +from omni.isaac.lab.envs import ManagerBasedRLEnvCfg + +from . import ObservationsCfg, ActionsCfg, EventCfg + +@configclass +class MITCarRLCommonCfg(ManagerBasedRLEnvCfg): + """ + Common configuration for the MIT Car environment. + Includes the basic settings: + - Observations + - Actions + - Events + as well as the number of environments and the spacing between them. + + Also sets sim dt and decimation. + """ + + num_envs: int = field(default=5) + env_spacing: float = field(default=0.05) + + # Basic settings + observations = ObservationsCfg() + actions = ActionsCfg() + events = EventCfg() + + def __post_init__(self): + + self.sim.dt = 0.025 # sim step every 25ms = 40Hz + self.decimation = 4 # env step every 4 sim steps: 40Hz / 4 = 10Hz + self.sim.render_interval = self.decimation diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/scenes.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/scenes.py new file mode 100644 index 0000000..7007743 --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/common/scenes.py @@ -0,0 +1,64 @@ +import torch + +import omni.isaac.lab.sim as sim_utils +import omni.isaac.lab.utils.math as math_utils + +from omni.isaac.lab.assets import AssetBaseCfg, ArticulationCfg +from omni.isaac.lab.scene import InteractiveSceneCfg +from omni.isaac.lab.terrains import TerrainImporterCfg +from omni.isaac.lab.utils import configclass + +from omni.isaac.groundcontrol_tasks.manager_based.wheeled.terrains import rough, racetrack +from omni.isaac.groundcontrol_assets.mitcar import MITCAR_CFG + +################ +#### SCENES #### +################ + +FLAT_TERRAIN_CFG = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="plane", + debug_vis=False, + ) + + +@configclass +class MITCarBaseSceneCfg(InteractiveSceneCfg): + + """Configuration for a MIT car Scene""" + + # Distant Light + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DistantLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + # Mesh + robot: ArticulationCfg = MITCAR_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + +@configclass +class MITCarFlatSceneCfg(MITCarBaseSceneCfg): + """Configuration for a MIT car Scene with flat terrain""" + terrain = FLAT_TERRAIN_CFG + + +@configclass +class MITCarRoughSceneCfg(MITCarBaseSceneCfg): + """Configuration for a MIT car Scene with rough terrain""" + terrain = rough.ROUGH_TERRAIN_CFG + + +@configclass +class MITCarRacetrackSceneCfg(MITCarBaseSceneCfg): + """Configuration for a MIT car Scene with racetrack terrain""" + + terrain = racetrack.RacetrackTerrainImporterCfg() + + def __post_init__(self): + """Post initialization.""" + super().__post_init__() + # Set initial state of the robot + self.robot.init_state = self.robot.init_state.replace( + pos=(0.0, 0.0, 0.5) + ) diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/mitcar_manager_env_cfg.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/mitcar_manager_env_cfg.py new file mode 100644 index 0000000..13759bb --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/mitcar_manager_env_cfg.py @@ -0,0 +1,206 @@ +import torch +import torch.nn as nn + +import omni.isaac.lab.envs.mdp as mdp +import omni.isaac.lab.utils.math as math_utils + +from omni.isaac.lab.managers import TerminationTermCfg as DoneTerm +from omni.isaac.lab.managers import RewardTermCfg as RewTerm +from omni.isaac.lab.utils import configclass + +################ +# REWARDS +################ + +def dist2goal(env, target): + root_pos = mdp.root_pos_w(env) + # target = env.scene.env_origins + torch.tensor(target, dtype=root_pos.dtype, device=root_pos.device) + target = torch.tensor(target, dtype=root_pos.dtype, device=root_pos.device) + dist = torch.norm(target - root_pos, dim=-1) + return dist + + +def upright_penalty(env, thresh_deg): + rot_mat = math_utils.matrix_from_quat(mdp.root_quat_w(env)) + up_dot = rot_mat[:, 2, 2] + up_dot = torch.rad2deg(torch.arccos(up_dot)) + penalty = torch.where(up_dot > thresh_deg, up_dot - thresh_deg, 0.) + return penalty + + +@configclass +class RewardsCfg: + + """Reward terms for the MDP.""" + # (1) Primary task: Distance to target + dist2goal = RewTerm( + func=dist2goal, + weight=-1.0, + params={"target": [3., 2., 0.]}, + ) + + # (2) Upright + upright = RewTerm( + func=upright_penalty, + weight=-1.0, + params={"thresh_deg": 30.}, + ) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + # (1) Time out + time_out = DoneTerm(func=mdp.time_out, time_out=True) + # (2) Cart out of bounds + # cart_out_of_bounds = DoneTerm( + # func=mdp.joint_pos_out_of_manual_limit, + # params={"asset_cfg": SceneEntityCfg("robot", joint_names=["slider_to_cart"]), "bounds": (-3.0, 3.0)}, + # ) + +@configclass +class CurriculumCfg: + """Configuration for the curriculum.""" + pass + +################################################## +############## ENVIRONMENT CONFIGS ############### +################################################## + +from . import common +from .common.rl_env import MITCarRLCommonCfg +from .common.scenes import MITCarRoughSceneCfg, MITCarFlatSceneCfg, MITCarBaseSceneCfg + +####### RL Environment ####### + +@configclass +class MITCarRLEnvCfg(MITCarRLCommonCfg): + """Configuration for the cartpole environment.""" + + seed: int = 42 + + # # MDP settings + curriculum: CurriculumCfg = CurriculumCfg() + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + # No command generator + commands: common.NoCommandsCfg = common.NoCommandsCfg() + + + def __post_init__(self): + """Post initialization.""" + super().__post_init__() + # viewer settings + self.viewer.eye = [11., 0.0, 14.0] + self.viewer.lookat = [2.0, 0.0, 0.] + + # termination settings + self.episode_length_s = 5 + # self.max_episode_length = 512 + + # Scene settings + self.scene = MITCarRoughSceneCfg( + num_envs=self.num_envs, env_spacing=self.env_spacing, + ) + + # Set seed for terrain generator + self.scene.terrain.terrain_generator = self.scene.terrain.terrain_generator.replace(seed=self.seed) + + # HACK to gain control of ordering of joints through JOINT_NAMES + # observation terms (order preserved) + # self.observations.joint_pos_rel.func = lambda x : self.observations.joint_pos_rel.func(x, joint_order_asset_cfg) + # self.observations.joint_vel_rel.func = lambda x : self.observations.joint_vel_rel.func(x, joint_order_asset_cfg) + # self.observations.pos_w.func = lambda x : self.observations.pos_w.func(x, joint_order_asset_cfg) + # self.observations.base_lin_vel.func = lambda x : self.observations.base_lin_vel.func(x, joint_order_asset_cfg) + # self.observations.lin_vel_w.func = lambda x : self.observations.lin_vel_w.func(x, joint_order_asset_cfg) + # self.observations.base_ang_vel.func = lambda x : self.observations.base_ang_vel.func(x, joint_order_asset_cfg) + + +####### Base Environment ####### + +@configclass +class NoTerminationsCfg: + """No terminations for the MDP.""" + pass + +@configclass +class MITCarPlayEnvCfg(MITCarRLEnvCfg): + """Configuration for the cartpole environment.""" + + terminations = NoTerminationsCfg() + + def __post_init__(self): + """Post initialization.""" + super().__post_init__() + # viewer settings + self.viewer.eye = [11., 0.0, 14.0] + self.viewer.lookat = [2.0, 0.0, 0.] + + # Scene settings + self.scene:MITCarBaseSceneCfg = MITCarFlatSceneCfg( + num_envs=self.num_envs, env_spacing=self.env_spacing + ) + + +####### IRL ####### + +def _compute_learned_reward(env, model): + obs = env.observation_manager.compute()['reward'] + rew = model(obs).squeeze(dim=-1) + return rew + +@configclass +class LearnedRewardsCfg: + """Learned reward terms for the MDP.""" + reward:RewTerm = None + + +def root_2dpos_w(env): + return mdp.root_pos_w(env)[..., :2] + + +@configclass +class MITCarIRLEnvCfg(MITCarRLEnvCfg): + + rew_model: nn.Module = None + + def __post_init__(self): + super().__post_init__() + self.rewards = LearnedRewardsCfg() + rewterm = lambda env: _compute_learned_reward(env, self.rew_model) + self.rewards.reward = RewTerm( + func=rewterm, + weight=1.0, + ) + + +########################################### +############## ENVIRONMENTS ############### +########################################### + +# TODO +# from wheeled_gym.tasks.wrappers.irl_wrapper import IRLWrapper +# class MITCarIRLWrapper(IRLWrapper): +# """MIT Car environment for IRL.""" + +# # def __init__(self, cfg: ManagerBasedRLEnvCfg, render_mode: str | None = None, **kwargs): +# def __init__(self, env): +# super().__init__(env) +# self._episode_length_s_persistent = self.cfg.episode_length_s + +# def irl_mode(self, ep_steps): +# super().irl_mode() +# self.env.cfg.episode_length_s = ep_steps + +# def rl_mode(self): +# super().rl_mode() +# self.env.cfg.episode_length_s = self._episode_length_s_persistent + +# # TODO: Use ClipActionWrapper to clip actions (which uses env.action_space.lower/upper +# # which should be set in the config) +# def step(self, action): +# action = torch.clip(action, -1., 1.) +# obs, rew, term, trunc, info = super().step(action) + +# return obs, rew, term, trunc, info diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/mitcar_manager_racetrack_env_cfg.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/mitcar_manager_racetrack_env_cfg.py new file mode 100644 index 0000000..0ef3c49 --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/mitcar_manager_racetrack_env_cfg.py @@ -0,0 +1,183 @@ +import torch + +import omni.isaac.lab.envs.mdp as mdp +import omni.isaac.lab.utils.math as math_utils + +from omni.isaac.lab.managers import TerminationTermCfg as DoneTerm +from omni.isaac.lab.managers import RewardTermCfg as RewTerm +from omni.isaac.lab.utils import configclass + +################ +# REWARDS +################ + +def track_progress_rate(env): + '''Estimate track progress by positive z-axis angular velocity around the environment''' + root_ang_vel = mdp.root_ang_vel_w(env) + progress_rate = root_ang_vel[..., 2] + return progress_rate + + +def upright_penalty(env, thresh_deg): + rot_mat = math_utils.matrix_from_quat(mdp.root_quat_w(env)) + up_dot = rot_mat[:, 2, 2] + up_dot = torch.rad2deg(torch.arccos(up_dot)) + penalty = torch.where(up_dot > thresh_deg, up_dot - thresh_deg, 0.) + return penalty + +def forward_vel_rew(env): + lin_vel = mdp.base_lin_vel(env) + return lin_vel[..., 0] + +def falling_penalty(env): + pos = mdp.root_pos_w(env) + return torch.where(pos[..., 2] < 0.1, 100.0, 0.0) + +def forward_wheel_spin(env): + joint_vel = mdp.joint_vel(env) + return torch.sum(joint_vel[..., [0,1,4,5]], dim=-1) + +@configclass +class RacetrackRewardsCfg: + + """Reward terms for the MDP.""" + # (1) Progress around track + # progress = RewTerm( + # func=track_progress_rate, + # weight=1.0, + # ) + + # (2) Upright + upright = RewTerm( + func=upright_penalty, + weight=-1.0, + params={"thresh_deg": 30.}, + ) + + # (3) Forward velocity + forward_vel = RewTerm( + func=forward_vel_rew, + weight=20., + ) + + # (4) Falling penalty + falling_penalty = RewTerm( + func=falling_penalty, + weight=-10.0, + ) + + # (5) Forward wheel spin + forward_wheel_spin = RewTerm( + func=forward_wheel_spin, + weight=1., + ) + + +@configclass +class RacetrackTerminationsCfg: + """Termination terms for the MDP.""" + + # (1) Time out + time_out = DoneTerm(func=mdp.time_out, time_out=True) + # (2) Cart out of bounds + cart_out_of_bounds = DoneTerm( + func=mdp.root_height_below_minimum, + params={"minimum_height": 0.}, + ) + # (3) Stuck TODO + # stuck = DoneTerm( + # func=mdp.root_lin_vel_below_threshold, + # params={"threshold": 0.01}, + # ) + +@configclass +class CurriculumCfg: + """Configuration for the curriculum.""" + pass + +##################################### +############## EVENTS ############### +##################################### + +from omni.isaac.lab.managers import EventTermCfg as EventTerm +from .utils import reset_root_state_from_terrain_points + +@configclass +class RacetrackEventsCfg: + """Configuration for the events.""" + reset_root_state_from_terrain_points = EventTerm( + func=reset_root_state_from_terrain_points, + mode="reset", + ) + + +################################################## +############## ENVIRONMENT CONFIGS ############### +################################################## + +from . import common +from .common.rl_env import MITCarRLCommonCfg +from .common.scenes import MITCarRacetrackSceneCfg + +####### RL Environment ####### + +@configclass +class MITCarRacetrackRLEnvCfg(MITCarRLCommonCfg): + """Configuration for the cartpole environment.""" + + seed: int = 42 + + # Reset config + events: RacetrackEventsCfg = RacetrackEventsCfg() + + # MDP settings + curriculum: CurriculumCfg = CurriculumCfg() + rewards: RacetrackRewardsCfg = RacetrackRewardsCfg() + terminations: RacetrackTerminationsCfg = RacetrackTerminationsCfg() + # No command generator + commands: common.NoCommandsCfg = common.NoCommandsCfg() + + + def __post_init__(self): + """Post initialization.""" + super().__post_init__() + # viewer settings + self.viewer.eye = [11., -20.0, 14.0] + self.viewer.lookat = [2.0, 0.0, 0.] + + # Terminations config + self.episode_length_s = 30 + # self.max_episode_length = 512 + + # Scene settings + self.scene:MITCarRacetrackSceneCfg = MITCarRacetrackSceneCfg( + num_envs=self.num_envs, env_spacing=self.env_spacing, + ) + + # HACK to gain control of ordering of joints through JOINT_NAMES + # observation terms (order preserved) + # self.observations.joint_pos_rel.func = lambda x : self.observations.joint_pos_rel.func(x, joint_order_asset_cfg) + # self.observations.joint_vel_rel.func = lambda x : self.observations.joint_vel_rel.func(x, joint_order_asset_cfg) + # self.observations.pos_w.func = lambda x : self.observations.pos_w.func(x, joint_order_asset_cfg) + # self.observations.base_lin_vel.func = lambda x : self.observations.base_lin_vel.func(x, joint_order_asset_cfg) + # self.observations.lin_vel_w.func = lambda x : self.observations.lin_vel_w.func(x, joint_order_asset_cfg) + # self.observations.base_ang_vel.func = lambda x : self.observations.base_ang_vel.func(x, joint_order_asset_cfg) + + +####### Base Environment ####### + +@configclass +class NoTerminationsCfg: + pass + +@configclass +class MITCarRacetrackPlayEnvCfg(MITCarRacetrackRLEnvCfg): + + # play_duration_s: float = 60. + terminations = NoTerminationsCfg() + + def __post_init__(self): + """Post initialization.""" + super().__post_init__() + # self.episode_length_s = self.play_duration_s + diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/utils.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/utils.py new file mode 100644 index 0000000..4d00ec8 --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/mitcar/utils.py @@ -0,0 +1,118 @@ +import torch + +import omni.isaac.lab.envs.mdp as mdp +import omni.isaac.lab.utils.math as math_utils + +from omni.isaac.lab.envs import ManagerBasedEnv +from omni.isaac.lab.managers import SceneEntityCfg +from omni.isaac.lab.assets import Articulation, RigidObject +from omni.isaac.lab.terrains import TerrainImporter + + +def root_pos_w(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Root position in the simulation world frame.""" + asset: Articulation = env.scene[asset_cfg.name] + return asset.data.root_pos_w + + +def root_lin_vel_below_threshold(env: ManagerBasedEnv, threshold: float) -> torch.Tensor: + """Check if the root linear velocity is below the threshold. + + Args: + env: The environment. + threshold: The threshold value. + + Returns: + A boolean tensor indicating if the root linear velocity is below the threshold. + """ + return mdp.root_lin_vel_w(env) < threshold + + +@torch.jit.script +def _f(euler_xyz: torch.Tensor) -> torch.Tensor: + return math_utils.quat_from_euler_xyz(euler_xyz[0], euler_xyz[1], euler_xyz[2]) + +__f = torch.vmap(_f) +def quat_from_euler_xyz_vect(euler_xyz: torch.Tensor) -> torch.Tensor: + """Convert Euler XYZ angles to quaternions. + + Args: + euler_xyz: A tensor of Euler XYZ angles in radians with shape ``(N, 3)``. + + Returns: + A tensor of quaternions with shape ``(N, 4)``. + """ + return __f(euler_xyz) + + +def reset_root_state_from_terrain_points( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + # valid_posns_and_rots: dict[str, tuple[float, float]], + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +): + """Reset the asset root state by sampling a random valid point from the config. + + This function samples a random valid pose(based on flat patches) from the terrain and sets the root state + of the asset to this position. The function also samples random velocities from the given ranges and sets them + into the physics simulation. + + The function takes a dictionary of position and velocity ranges for each axis and rotation: + + * :attr:`pose_range` - a dictionary of pose ranges for each axis. The keys of the dictionary are ``roll``, + ``pitch``, and ``yaw``. The position is sampled from the flat patches of the terrain. + * :attr:`velocity_range` - a dictionary of velocity ranges for each axis and rotation. The keys of the dictionary + are ``x``, ``y``, ``z``, ``roll``, ``pitch``, and ``yaw``. + + The values are tuples of the form ``(min, max)``. If the dictionary does not contain a particular key, + the position is set to zero for that axis. + + Note: + The function expects the terrain to have valid flat patches under the key "init_pos". The flat patches + are used to sample the random pose for the robot. + + Raises: + ValueError: If the terrain does not have valid flat patches under the key "init_pos". + """ + # access the used quantities (to enable type-hinting) + asset: RigidObject | Articulation = env.scene[asset_cfg.name] + terrain: TerrainImporter = env.scene.terrain + + # obtain all flat patches corresponding to the valid poses + # valid_positions: torch.Tensor = terrain.flat_patches.get("init_pos") + valid_poses = terrain.cfg.valid_init_poses + if valid_poses is None: + raise ValueError( + "The event term 'reset_root_state_from_terrain_points' requires 'valid_init_poses' in the TerrainImporterCfg." + ) + # Tensorizes the valid poses + # TODO move to constructor of terrain importer + posns = torch.stack(list(map(lambda x: torch.tensor(x.pos, device=env.device), valid_poses))) + oris = list(map(lambda x: torch.deg2rad(torch.tensor(x.rot_euler_xyz_deg, device=env.device)), valid_poses)) + oris = torch.stack([math_utils.quat_from_euler_xyz(*ori) for ori in oris]) + + # sample random valid poses + ids = torch.randint(0, len(valid_poses), size=(len(env_ids),), device=env.device) + + positions = posns[ids] + positions += asset.data.default_root_state[env_ids, :3] + orientations = oris[ids] + + # sample random orientations (TODO) + # range_list = [pose_range.get(key, (0.0, 0.0)) for key in ["roll", "pitch", "yaw"]] + # ranges = torch.tensor(range_list, device=asset.device) + # rand_samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 3), device=asset.device) + + # convert to quaternions + # orientations = math_utils.quat_from_euler_xyz(rand_samples[:, 0], rand_samples[:, 1], rand_samples[:, 2]) + + # sample random velocities (TODO) + # range_list = [velocity_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]] + # ranges = torch.tensor(range_list, device=asset.device) + # rand_samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 6), device=asset.device) + + # velocities = asset.data.default_root_state[:, 7:13] + rand_samples + + # set into the physics simulation + asset.write_root_pose_to_sim(torch.cat([positions, orientations], dim=-1), env_ids=env_ids) + # asset.write_root_velocity_to_sim(velocities, env_ids=env_ids) TODO diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/terrains/racetrack.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/terrains/racetrack.py new file mode 100644 index 0000000..77050e7 --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/terrains/racetrack.py @@ -0,0 +1,56 @@ +import omni.isaac.lab.sim as sim_utils +from omni.isaac.lab.utils import configclass +from omni.isaac.lab.terrains import TerrainImporterCfg +from omni.isaac.groundcontrol_assets import GROUNDCONTROL_ASSETS_DATA_DIR + +RACETRACK_TERRAIN_CFG = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="usd", + usd_path=f"{GROUNDCONTROL_ASSETS_DATA_DIR}/Props/terrain/racetrack-terrain.usd", + collision_group=-1, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.5, + dynamic_friction=1.5, + ), + debug_vis=False, + ) + +@configclass +class RacetrackTerrainImporterCfg(TerrainImporterCfg): + + @configclass + class InitialPoseCfg: + pos: tuple[float, float, float] = (0.0, 0.0, 0.0) + rot_euler_xyz_deg: tuple[float, float, float] = (0.0, 0.0, 0.0) + + height = 0.0 + valid_init_poses = [ + InitialPoseCfg( + pos=(12.0, 1.27, height), + rot_euler_xyz_deg=(0., 0., 135.0) + ), + InitialPoseCfg( + pos=(-5.33, 3.3, height), + rot_euler_xyz_deg=(0., 0., 180.0), + ), + InitialPoseCfg( + pos=(-8.7, -7.27, height), + ), + InitialPoseCfg( + pos=(0., 0., height), + ), + ] + prim_path="/World/ground" + terrain_type="usd" + usd_path=f"{GROUNDCONTROL_ASSETS_DATA_DIR}/Props/terrain/racetrack-terrain.usd", + # usd_path=f"/home/tyler/Research/GroundControl/source/extensions/omni.isaac.groundcontrol_assets/data/Props/terrain/racetrack-terrain.usd", + collision_group=-1 + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.5, + dynamic_friction=1.5, + ) + debug_vis=False \ No newline at end of file diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/terrains/rough.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/terrains/rough.py new file mode 100644 index 0000000..cccdad3 --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/manager_based/wheeled/terrains/rough.py @@ -0,0 +1,45 @@ +import omni.isaac.lab.sim as sim_utils +import omni.isaac.lab.terrains as terrain_gen +from omni.isaac.lab.terrains import TerrainImporterCfg +from omni.isaac.lab.terrains.terrain_generator_cfg import TerrainGeneratorCfg +from omni.isaac.lab_assets import ISAACLAB_NUCLEUS_DIR + +ROUGH_TERRAINS_GEN_CFG = TerrainGeneratorCfg( + seed=42, + size=(8.0, 8.0), + border_width=20.0, + num_rows=2, + num_cols=3, + horizontal_scale=0.1, + vertical_scale=0.005, + slope_threshold=0.75, + use_cache=False, + sub_terrains={ + "random_rough": terrain_gen.HfRandomUniformTerrainCfg( + proportion=0.5, noise_range=(0.02, 0.10), noise_step=0.02, border_width=0.25 + ), + "flat": terrain_gen.MeshPlaneTerrainCfg( + proportion=0.5 + ) + }, +) + +ROUGH_TERRAIN_CFG = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="generator", + terrain_generator=ROUGH_TERRAINS_GEN_CFG, + max_init_terrain_level=5, + collision_group=-1, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + ), + visual_material=sim_utils.MdlFileCfg( + mdl_path=f"{ISAACLAB_NUCLEUS_DIR}/Materials/TilesMarbleSpiderWhiteBrickBondHoned/TilesMarbleSpiderWhiteBrickBondHoned.mdl", + project_uvw=True, + texture_scale=(0.25, 0.25), + ), + debug_vis=False, + ) diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/utils/runners/common/rl_runner.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/utils/runners/common/rl_runner.py new file mode 100644 index 0000000..9cfe744 --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/utils/runners/common/rl_runner.py @@ -0,0 +1,20 @@ +import torch +from abc import ABC, abstractmethod +from typing import Callable + +class RLRunner(ABC): + + @abstractmethod + def learn(self, num_learning_iterations:int): + '''Train the RL algorithm''' + pass + + @abstractmethod + def reset(self): + '''Reset the RL runner / optimizer''' + pass + + @abstractmethod + def collect_rollouts(self, max_steps:int): + '''Collect rollouts from the environment''' + pass diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/utils/runners/rslrl_runner.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/utils/runners/rslrl_runner.py new file mode 100644 index 0000000..a696e1b --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/utils/runners/rslrl_runner.py @@ -0,0 +1,200 @@ +import warnings +import rsl_rl +from rsl_rl import runners +import os + +try: + from tqdm import TqdmExperimentalWarning + + # Remove experimental warning + warnings.filterwarnings("ignore", category=TqdmExperimentalWarning) + from tqdm.rich import tqdm +except ImportError: + # Rich not installed, we only throw an error + # if the progress bar is used + tqdm = None + +import time +import torch +from collections import deque +from typing import Dict + +from .common.rl_runner import RLRunner + + +class ExtraRslRlRolloutStorage(rsl_rl.storage.rollout_storage.RolloutStorage): + def __init__(self, extras_shape: Dict[str, torch.Size], + *args, **kwargs): + ''' + example of extras_info: + { + "reward": (torch.Size([64, 2]), lambda infos: infos["observations"]["reward"]), + ... + } + The name is the name of the extras tensor and the size is the size of this tensor. + The callable is the mapping from the infos dict to the tensor. + ''' + super().__init__(*args, **kwargs) + self.extras = {name: torch.empty(self.num_transitions_per_env, self.num_envs, *shape) for name, shape in extras_shape.items()} + + def add_transitions(self, transition, infos): + super().add_transitions(transition) + self.step -= 1 # Undo super step + for name in self.extras.keys(): + self.extras[name][self.step] = infos['observations'][name] + self.step += 1 # Redo super step + + +class OnPolicyRunner(runners.OnPolicyRunner, RLRunner): + ''' Override for logging purposes ''' + + def __init__(self, env, cfg, log_dir=None, device="cpu"): + super().__init__(env, cfg, log_dir, device) + self.no_log = self.cfg.get("rl_no_log", True) + self.no_wandb = self.cfg.get("no_wandb", False) + self.logger_type = None + if not self.no_wandb: + self.logger_type = "wandb" + # self.pbar = tqdm(total=self.cfg.get("rl_max_iterations", 0)) + + def learn(self, num_learning_iterations, init_at_random_ep_len=False): + # initialize writer + if not self.no_log and self.logger_type == "wandb": + from rsl_rl.utils.wandb_utils import WandbSummaryWriter + + self.writer = WandbSummaryWriter(log_dir=self.log_dir, flush_secs=10, cfg=self.cfg) + self.writer.log_config(self.env.cfg, self.cfg, self.alg_cfg, self.policy_cfg) + + if init_at_random_ep_len: + self.env.episode_length_buf = torch.randint_like( + self.env.episode_length_buf, high=int(self.env.max_episode_length) + ) + obs, extras = self.env.get_observations() + critic_obs = extras["observations"].get("critic", obs) + obs, critic_obs = obs.to(self.device), critic_obs.to(self.device) + self.train_mode() # switch to train mode (for dropout for example) + + ep_infos = [] + rewbuffer = deque(maxlen=100) + lenbuffer = deque(maxlen=100) + cur_reward_sum = torch.zeros(self.env.num_envs, dtype=torch.float, device=self.device) + cur_episode_length = torch.zeros(self.env.num_envs, dtype=torch.float, device=self.device) + + start_iter = self.current_learning_iteration + tot_iter = start_iter + num_learning_iterations + for it in tqdm(range(start_iter, tot_iter)): + start = time.time() + # Rollout + with torch.inference_mode(): + for i in range(self.num_steps_per_env): + actions = self.alg.act(obs, critic_obs) + obs, rewards, dones, infos = self.env.step(actions) + if actions.isnan().any(): + raise ValueError("NaN in actions") + obs = self.obs_normalizer(obs) + if "critic" in infos["observations"]: + critic_obs = self.critic_obs_normalizer(infos["observations"]["critic"]) + else: + critic_obs = obs + obs, critic_obs, rewards, dones = ( + obs.to(self.device), + critic_obs.to(self.device), + rewards.to(self.device), + dones.to(self.device), + ) + self.alg.process_env_step(rewards, dones, infos) + + if not self.no_log: + # Book keeping + # note: we changed logging to use "log" instead of "episode" to avoid confusion with + # different types of logging data (rewards, curriculum, etc.) + if "episode" in infos: + ep_infos.append(infos["episode"]) + elif "log" in infos: + ep_infos.append(infos["log"]) + cur_reward_sum += rewards + cur_episode_length += 1 + new_ids = (dones > 0).nonzero(as_tuple=False) + rewbuffer.extend(cur_reward_sum[new_ids][:, 0].cpu().numpy().tolist()) + lenbuffer.extend(cur_episode_length[new_ids][:, 0].cpu().numpy().tolist()) + cur_reward_sum[new_ids] = 0 + cur_episode_length[new_ids] = 0 + + stop = time.time() + collection_time = stop - start + + # Learning step + start = stop + self.alg.compute_returns(critic_obs) + + mean_value_loss, mean_surrogate_loss = self.alg.update() + stop = time.time() + learn_time = stop - start + self.current_learning_iteration = it + if not self.no_log: + self.log(locals()) + if it % self.save_interval == 0: + self.save(os.path.join(self.log_dir, "models", f"model_{it}.pt")) + ep_infos.clear() + + # self.save(os.path.join(self.log_dir, f"model_{self.current_learning_iteration}.pt")) + + def collect_rollouts(self, max_steps: int) -> ExtraRslRlRolloutStorage: + alg = self.alg + + rollouts_storage = ExtraRslRlRolloutStorage( + extras_shape={ + "reward": torch.Size([self.env.num_envs, 1]), + }, + num_envs=self.storage.num_envs, + num_transitions_per_env=self.storage.num_transitions_per_env, + actor_obs_shape=self.storage.actor_obs_shape, + critic_obs_shape=self.storage.critic_obs_shape, + action_shape=self.storage.action_shape, + device=self.device, + ) + self.storage.clear() + obs, extras = self.env.get_observations() + critic_obs = extras["observations"].get("critic", obs) + obs, critic_obs = obs.to(self.device), critic_obs.to(self.device) + + with torch.inference_mode(): + self.env.reset() + for i in range(max_steps): + actions = alg.act(obs, critic_obs) + obs, rewards, dones, infos = self.env.step(actions) + if actions.isnan().any(): + raise ValueError("NaN in actions") + obs = self.obs_normalizer(obs) + if "critic" in infos["observations"]: + critic_obs = self.critic_obs_normalizer(infos["observations"]["critic"]) + else: + critic_obs = obs + obs, critic_obs, rewards, dones = ( + obs.to(self.device), + critic_obs.to(self.device), + rewards.to(self.device), + dones.to(self.device), + ) + + ## on_policy_runner.OnPolicyRunner.process_env_step: + alg.transition.rewards = rewards.clone() + alg.transition.dones = dones + # Bootstrapping on time outs + if "time_outs" in infos: + alg.transition.rewards += alg.gamma * torch.squeeze( + alg.transition.values * infos["time_outs"].unsqueeze(1).to(alg.device), 1 + ) + + # Record the transitions to both buffers + # alg.storage.add_transitions(alg.transition) + rollouts_storage.add_transitions(alg.transition, infos) + + # Reset + alg.transition.clear() + alg.actor_critic.reset(dones) + + return rollouts_storage + + def reset(self): + self.__init__(self.env, self.cfg, self.log_dir, self.device) diff --git a/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/utils/wrappers/torch_clip_action.py b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/utils/wrappers/torch_clip_action.py new file mode 100644 index 0000000..3e96cc5 --- /dev/null +++ b/source/extensions/omni.isaac.groundcontrol_tasks/omni/isaac/groundcontrol_tasks/utils/wrappers/torch_clip_action.py @@ -0,0 +1,37 @@ +import torch + +import gymnasium as gym + +class ClipAction(gym.ActionWrapper): + """ Adapted from https://github.com/openai/gym/blob/master/gym/wrappers/clip_action.py + Clip the continuous action within the valid :class:`Box` observation space bound. + + Example: + >>> import gym + >>> env = gym.make('Bipedal-Walker-v3') + >>> env = ClipAction(env) + >>> env.action_space + Box(-1.0, 1.0, (4,), float32) + >>> env.step(np.array([5.0, 2.0, -10.0, 0.0])) + # Executes the action np.array([1.0, 1.0, -1.0, 0]) in the base environment + """ + + def __init__(self, env: gym.Env): + """A wrapper for clipping continuous actions within the valid bound. + + Args: + env: The environment to apply the wrapper + """ + # assert isinstance(env.action_space, Box) + super().__init__(env) + + def action(self, action): + """Clips the action within the valid bounds. + + Args: + action: The action to clip + + Returns: + The clipped action + """ + return torch.clip(action, min=self.action_space.low, max=self.action_space.high) \ No newline at end of file diff --git a/source/standalone/train/train_rsl_rl.py b/source/standalone/train/train_rsl_rl.py new file mode 100644 index 0000000..d7f0544 --- /dev/null +++ b/source/standalone/train/train_rsl_rl.py @@ -0,0 +1,145 @@ +""" +This script tests the OffroadCarEnv in envs. +It also finds the joint_ids order returned by ObsTerm funcs + +example usage: + +python train/train_rsl_rl.py --headless + +loading a previous run: + +python train/train_rsl_rl.py --load-run +""" +################################### +###### BEGIN ISAACLAB SPINUP ###### +################################### + +import argparse +from utils.app_startup import startup, add_all_wheeled_gym_args, add_rsl_rl_args + +parser = argparse.ArgumentParser(description="Random agent for Isaac Lab environments.") + +overrides = { + "rl_max_iterations": 4096, + "env_name": "Isaac-MITCar-v0", + "num_envs": 1024, + "video": True, + "log_every": 5, + "video_interval": 5000, + "video_length": 1000, + # "no_wandb": True, +} +add_all_wheeled_gym_args(parser, overrides) +add_rsl_rl_args(parser) + +def _args_cb(args): + args.save_interval = args.log_every + args.rl_no_log = args.no_log + +simulation_app, args_cli = startup(parser=parser, prelaunch_callback=_args_cb) + +##################### +###### LOGGING ###### +##################### + +import gymnasium as gym +import os +from datetime import datetime +import torch +from omni.isaac.lab.utils.io import dump_pickle, dump_yaml + +if not args_cli.no_wandb: + import wandb + run = wandb.init( + project="IRL", + ) + wandb_name = wandb.run.name + run_name = wandb_name +else: + import random + run_name = f"bfirl-local-{random.randint(0, 1e7)}" + +log_dir = os.path.join(args_cli.log_dir, f'{run_name}_{datetime.now().strftime("%Y-%m-%d_%H-%M-%S")}') +model_save_path = os.path.join(log_dir, "models") + +if not args_cli.no_log: + paths = [model_save_path] + for path in paths: + if not os.path.exists(path): + os.makedirs(path) + +############################ +#### CREATE ENVIRONMENT #### +############################ + +from omni.isaac.lab.utils.dict import print_dict +from omni.isaac.lab.utils import update_class_from_dict +from omni.isaac.lab_tasks.utils.wrappers.rsl_rl import RslRlVecEnvWrapper +from omni.isaac.lab_tasks.utils import parse_env_cfg +from omni.isaac.lab_tasks.utils import get_checkpoint_path + +import omni.isaac.groundcontrol_tasks +from omni.isaac.groundcontrol_tasks.utils.wrappers.torch_clip_action import ClipAction +from omni.isaac.groundcontrol_tasks.utils.runners.rslrl_runner import OnPolicyRunner + +from utils import WHEELED_LAB_LOGS_DIR +from utils.args import parse_rsl_rl_cfg, default_isaac_cfg + +####### FETCH CONFIGS ####### +isaac_cfg = default_isaac_cfg( + device=args_cli.device, + num_envs=args_cli.num_envs, + use_fabric=not args_cli.disable_fabric +) +env_cfg = parse_env_cfg( + args_cli.env_name, device=args_cli.device, num_envs=args_cli.num_envs, use_fabric=not args_cli.disable_fabric +) +update_class_from_dict(env_cfg, isaac_cfg) + +env = gym.make(args_cli.env_name, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) + +####### INSTANTIATE ENV ####### +env.action_space.low = -1. +env.action_space.high = 1. +env = ClipAction(env) + +if args_cli.video: + video_kwargs = { + "video_folder": os.path.join(log_dir, "videos"), + "step_trigger": lambda step: step % args_cli.video_interval*args_cli.num_envs == 0, + "video_length": args_cli.video_length, + "disable_logger": True, + } + print("[INFO] Recording videos during training.") + print_dict(video_kwargs, nesting=4) + env = gym.wrappers.RecordVideo(env, **video_kwargs) + +env = RslRlVecEnvWrapper(env) + +#### CREATE AGENT (FACTORY) #### +agent_cfg = parse_rsl_rl_cfg(args_cli.env_name, args_cli) + +# dump the configuration into log-directory +if not args_cli.no_log: + dump_yaml(os.path.join(log_dir, "params", "env.yaml"), env_cfg) + dump_yaml(os.path.join(log_dir, "params", "agent.yaml"), agent_cfg) + dump_pickle(os.path.join(log_dir, "params", "env.pkl"), env_cfg) + dump_pickle(os.path.join(log_dir, "params", "agent.pkl"), agent_cfg) + +runner_log_dir = None if args_cli.test_mode else log_dir +runner = OnPolicyRunner(env, agent_cfg.to_dict(), log_dir=runner_log_dir, device=args_cli.device) + +if args_cli.load_run: + # get path to previous checkpoint + resume_path = get_checkpoint_path(WHEELED_LAB_LOGS_DIR, run_dir=args_cli.load_run, + other_dirs=["models"], checkpoint="model_.*") + print(f"[INFO]: Loading model checkpoint from: {resume_path}") + # load previously trained model + runner.load(resume_path) + +env.seed(agent_cfg.seed) + +runner.learn(num_learning_iterations=args_cli.rl_max_iterations) + +if not args_cli.no_wandb: + run.finish() diff --git a/source/standalone/train/train_sb3_rl.py b/source/standalone/train/train_sb3_rl.py new file mode 100644 index 0000000..f5a85b4 --- /dev/null +++ b/source/standalone/train/train_sb3_rl.py @@ -0,0 +1,148 @@ +""" +This script tests the OffroadCarEnv in envs. +It also finds the joint_ids order returned by ObsTerm funcs + +example usage: + +python test/test_rl.py --headless +""" +################################### +###### BEGIN ISAACLAB SPINUP ###### +################################### + +import argparse +from ..utils.app_startup import startup, add_all_wheeled_gym_args, add_rsl_rl_args + +parser = argparse.ArgumentParser(description="Random agent for Isaac Lab environments.") + +overrides = { + "rl_max_iterations": 1024, + "env_name": "Isaac-MITCarRacetrack-v0", + "num_envs": 1024, + "video": True, + "log_every": 5, + "video_interval": 1000, + "video_length": 1200, + # "no_wandb": True, +} +add_all_wheeled_gym_args(parser, overrides) +add_rsl_rl_args(parser) + +def _args_cb(args): + args.save_interval = args.log_every + args.rl_no_log = args.no_log + +simulation_app, args_cli = startup(parser=parser, prelaunch_callback=_args_cb) + +##################### +###### LOGGING ###### +##################### + +import gymnasium as gym +import os +from datetime import datetime +import torch + +if not args_cli.no_wandb: + import wandb + run = wandb.init( + project="IRL", + ) + wandb_name = wandb.run.name + run_name = wandb_name +else: + import random + run_name = f"bfirl-local-{random.randint(0, 1e7)}" + +log_dir = os.path.join(args_cli.log_dir, f'{run_name}_{datetime.now().strftime("%Y-%m-%d_%H-%M-%S")}') +# dump the configuration into log-directory +# dump_yaml(os.path.join(log_dir, "params", "env.yaml"), env_cfg) +# dump_pickle(os.path.join(log_dir, "params", "env.pkl"), env_cfg) +model_save_path = os.path.join(log_dir, "models") + +############################ +#### CREATE ENVIRONMENT #### +############################ + +import wheeled_gym.tasks # register envs to gym +from wheeled_gym.train.utils.utils import default_isaac_cfg +from wheeled_gym.utils.data_processing import load_from_sb3 +from wheeled_gym.tasks.wrappers.clip_action import ClipAction +from wheeled_gym import WHEELED_GYM_LOGS_DIR + +from omni.isaac.lab.utils.dict import print_dict +from omni.isaac.lab.utils import update_class_from_dict +from omni.isaac.lab_tasks.utils import parse_env_cfg +from omni.isaac.lab_tasks.utils.wrappers.sb3 import Sb3VecEnvWrapper +from omni.isaac.lab_tasks.utils import get_checkpoint_path + +from wandb.integration.sb3 import WandbCallback +from stable_baselines3 import PPO +from stable_baselines3.common.callbacks import CheckpointCallback, CallbackList + +####### FETCH CONFIGS ####### +isaac_cfg = default_isaac_cfg( + device=args_cli.device, + num_envs=args_cli.num_envs, + use_fabric=not args_cli.disable_fabric +) +env_cfg = parse_env_cfg( + args_cli.env_name, device=args_cli.device, num_envs=args_cli.num_envs, use_fabric=not args_cli.disable_fabric +) +update_class_from_dict(env_cfg, isaac_cfg) + +env = gym.make(args_cli.env_name, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) +n_timesteps = args_cli.rl_max_iterations * args_cli.agent_n_steps * env.scene.num_envs + +####### INSTANTIATE ENV ####### +# env.action_space.low = torch.tensor(-1., device=args_cli.device) +# env.action_space.high = torch.tensor(1., device=args_cli.device) +env.action_space.low = -1. +env.action_space.high = 1. +env = ClipAction(env) + +if args_cli.video: + video_kwargs = { + "video_folder": os.path.join(log_dir, "videos"), + "step_trigger": lambda step: step % args_cli.video_interval*args_cli.num_envs == 0, + "video_length": args_cli.video_length, + "disable_logger": True, + } + print("[INFO] Recording videos during training.") + print_dict(video_kwargs, nesting=4) + env = gym.wrappers.RecordVideo(env, **video_kwargs) + +env = Sb3VecEnvWrapper(env) + +#### CREATE AGENT (FACTORY) #### +env.seed(args_cli.seed) + +policy_kwargs = dict(net_arch=dict(pi=[32, 32], vf=[32, 32])) +checkpoint_callback = CheckpointCallback(save_freq=args_cli.checkpoint_every, + save_path=os.path.join(model_save_path), + name_prefix="model") +callbacklist = [checkpoint_callback] +if not args_cli.no_wandb: + callbacklist.append(WandbCallback()) +callbacklist = CallbackList(callbacklist) + +if args_cli.load_run: + run_path = os.path.join(WHEELED_GYM_LOGS_DIR, args_cli.load_run) + resume_path = get_checkpoint_path(WHEELED_GYM_LOGS_DIR, run_dir=args_cli.load_run, + other_dirs=["models"], checkpoint="model_.*") + print(f"[INFO]: Loading model checkpoint from: {resume_path}") + runner = load_from_sb3(resume_path, policy_type=args_cli.rl_algo_type) + runner.env = env +else: + runner = PPO("MlpPolicy", env, + device=args_cli.device, + policy_kwargs=policy_kwargs) + +try: + runner.learn(total_timesteps=n_timesteps, callback=callbacklist, progress_bar=True) +except KeyboardInterrupt: + runner.save(os.path.join(model_save_path, f"{run_name}_interrupted")) + +runner.save(os.path.join(model_save_path, f"{run_name}_done")) + +run.finish() diff --git a/source/standalone/train/utils/__init__.py b/source/standalone/train/utils/__init__.py new file mode 100644 index 0000000..74fdc22 --- /dev/null +++ b/source/standalone/train/utils/__init__.py @@ -0,0 +1,10 @@ +import os + +# DEFAULT_RUN_DIRNAME = "graceful-frost-97_2024-08-12_19-29-14" + +WHEELED_LAB_ROOT_DIR = os.path.dirname(os.path.dirname(os.path.realpath(__file__))) +WHEELED_LAB_RESOURCES_DIR = os.path.join(WHEELED_LAB_ROOT_DIR, 'resources') +WHEELED_LAB_LOGS_DIR = os.path.join(WHEELED_LAB_ROOT_DIR, 'logs') +WHEELED_LAB_CORE_DATA_DIR = os.path.join(WHEELED_LAB_ROOT_DIR, 'core_data') + +# WHEELED_LAB_DEFAULT_RUN_DIR = os.path.join(WHEELED_LAB_CORE_DATA_DIR, DEFAULT_RUN_DIRNAME) \ No newline at end of file diff --git a/source/standalone/train/utils/app_startup.py b/source/standalone/train/utils/app_startup.py new file mode 100644 index 0000000..95f391d --- /dev/null +++ b/source/standalone/train/utils/app_startup.py @@ -0,0 +1,150 @@ +""" +Boilerplate code for starting up IsaacLab backend +""" + +import argparse +from . import WHEELED_LAB_LOGS_DIR + + +defaults = { + "no_log": False, + "log_dir": WHEELED_LAB_LOGS_DIR, + "log_every": 10, + "video": False, + "video_length": 500, + "video_interval": 1e4, + "no_checkpoints": False, + "checkpoint_every": 1e4, + "no_wandb": False, + "rl_max_iterations": 1024, + "irl_max_iterations": 1024, + "agent_n_steps": 256, + "cpu": False, + "device": "cuda:0", + "disable_fabric": False, + "num_envs": 256, + "env_name": "Isaac-MITCar-v0", + "rl_algo_type": "PPO", + "rl_algo_lib": "sb3", + "seed": 42, + "rl_no_log": True, + "logger": "wandb", + "load-run": None, + + "test_mode": False, +} + + +def add_logging_args(parser, default_overrides={}): + defaults.update(default_overrides) + parser.add_argument("--no-log", action="store_true", default=defaults["no_log"] , help="Disable logging") + parser.add_argument("--rl-no-log", action="store_true", default=defaults["rl_no_log"] , help="Disable logging for RL algorithm") + parser.add_argument("--log-dir", type=str, default=defaults["log_dir"] , help="Directory for logging.") + parser.add_argument("--log-every", type=int, default=defaults["log_every"] , help="Log every n updates.") + parser.add_argument("--video", action="store_true", default=defaults["video"], help="Record videos during training.") + parser.add_argument("--video-length", type=int, default=defaults["video_length"], help="Length of the recorded video (in steps).") + parser.add_argument("--video-interval", type=int, default=defaults["video_interval"], help="Interval between video recordings (in steps).") + parser.add_argument("--no-checkpoints", action="store_true", default=defaults["no_checkpoints"], help="Save model checkpoints.") + parser.add_argument("--checkpoint-every", type=int, default=defaults["checkpoint_every"], help="Save model checkpoints every n steps.") + parser.add_argument("--no-wandb", action="store_true", default=defaults["no_wandb"], help="Disable wandb logging.") + parser.add_argument("--test-mode", action="store_true", default=defaults["test_mode"] , help="Disable logging; Disable wandb; Disable video recording; Disable checkpoints.") + + +def add_train_args(parser, default_overrides={}): + defaults.update(default_overrides) + parser.add_argument("--seed", type=int, default=defaults["seed"], help="Seed for training") + parser.add_argument("--rl-max-iterations", type=int, default=defaults["rl_max_iterations"], help="RL rl_algo training iterations.") + parser.add_argument("--irl-max-iterations", type=int, default=defaults["irl_max_iterations"], help="IRL rl_algo training iterations.") + parser.add_argument("--agent-n-steps", type=int, default=defaults["agent_n_steps"], help="Agent max steps") + parser.add_argument("--rl-algo-lib", type=str, default=defaults["rl_algo_lib"], help="library for rl_algo [sb3|rsl]") + parser.add_argument("--rl-algo-type", type=str, default=defaults["rl_algo_type"], help="type of rl_algo [SAC|PPO|manual]") + parser.add_argument("--load-run", type=str, default=defaults['load-run'], help="Name of run to load from logs dir.") + + +def add_env_args(parser, default_overrides={}): + ''' + Add standard environment arguments to the parser. + parser: argparse.ArgumentParser + Argument parser to add arguments to. + ''' + defaults.update(default_overrides) + # parser.add_argument("--cpu", action="store_true", default=defaults["cpu"], help="Use CPU pipeline.") # Deprecated due to support in IsaacLab v1.0.0 + # parser.add_argument("--device", default="cuda:0", help="Device [cpu|cuda:0].") # Deprecated due to support in IsaacLab v1.0.0 + parser.add_argument( + "--disable_fabric", action="store_true", default=defaults["disable_fabric"], help="Disable fabric and use USD I/O operations." + ) + parser.add_argument('-ne', "--num-envs", type=int, default=defaults["num_envs"], help="Number of environments to simulate.") + parser.add_argument('-en', "--env-name", type=str, default=defaults["env_name"], help="Name of the task.") + + +def add_rsl_rl_args(parser: argparse.ArgumentParser, default_overrides={}): + """Add RSL-RL arguments to the parser. + + Args: + parser: The parser to add the arguments to. + """ + defaults.update(default_overrides) + # create a new argument group + arg_group = parser.add_argument_group("rsl_rl", description="Arguments for RSL-RL agent.") + # -- experiment arguments + arg_group.add_argument( + "--experiment_name", type=str, default=None, help="Name of the experiment folder where logs will be stored." + ) + arg_group.add_argument("--run_name", type=str, default=None, help="Run name suffix to the log directory.") + # -- load arguments + arg_group.add_argument("--resume", type=bool, default=None, help="Whether to resume from a checkpoint.") + arg_group.add_argument("--load_run", type=str, default=None, help="Name of the run folder to resume from.") + arg_group.add_argument("--checkpoint", type=str, default=None, help="Checkpoint file to resume from.") + # -- logger arguments + arg_group.add_argument( + "--logger", type=str, default=defaults["logger"], choices={"wandb", "tensorboard", "neptune"}, help="Logger module to use." + ) + arg_group.add_argument( + "--log_project_name", type=str, default=None, help="Name of the logging project when using wandb or neptune." + ) + + +def add_all_wheeled_gym_args(parser, default_overrides={}): + add_logging_args(parser, default_overrides) + add_train_args(parser, default_overrides) + add_env_args(parser, default_overrides) + +def startup(parser=None, prelaunch_callback=None, import_gym_envs=True): + from omni.isaac.lab.app import AppLauncher + ''' + Startup IsaacLab backend. Imports wheeled_gym environments optionally. + Args: + parser: argparse.ArgumentParser, optional, default=None + Argument parser to add arguments to. + prelaunch(args): function to be executed right before launching the app, optional, default=None + Returns: + simulation_app: omni.isaac.dynamic_control.DynamicControl, omni.isaac.dynamic_control._dynamic_control.DynamicControl + Simulation app instance. + args_cli: argparse.Namespace + Parsed command line arguments. + ''' + + if parser is None: + parser = argparse.ArgumentParser(description="Used Boilerplate Starter.") + + AppLauncher.add_app_launcher_args(parser) + args_cli = parser.parse_args() + + if prelaunch_callback is not None: + prelaunch_callback(args_cli) + + if "test_mode" in args_cli and args_cli.test_mode: + args_cli.no_log = True + args_cli.rl_no_log = True + args_cli.no_wandb = True + args_cli.video = False + args_cli.no_checkpoints = True + + if args_cli.video: + args_cli.enable_cameras = True + + # launch omniverse app + app_launcher = AppLauncher(args_cli) + simulation_app = app_launcher.app + + return simulation_app, args_cli diff --git a/source/standalone/train/utils/args.py b/source/standalone/train/utils/args.py new file mode 100644 index 0000000..e242803 --- /dev/null +++ b/source/standalone/train/utils/args.py @@ -0,0 +1,65 @@ +import argparse + +def parse_rsl_rl_cfg(task_name: str, args_cli: argparse.Namespace): + """ + Parse configuration for RSL-RL agent based on inputs. + Needs rsl_rl_cfg_entry_point to load Agent Config + + Args: + task_name: The name of the environment. + args_cli: The command line arguments. + + Returns: + The parsed configuration for RSL-RL agent based on inputs. + """ + from omni.isaac.lab_tasks.utils.parse_cfg import load_cfg_from_registry + + # load the default configuration + rslrl_cfg = load_cfg_from_registry(task_name, "rsl_rl_cfg_entry_point") + + # override the default configuration with CLI arguments + if args_cli.seed is not None: + rslrl_cfg.seed = args_cli.seed + if args_cli.resume is not None: + rslrl_cfg.resume = args_cli.resume + if args_cli.load_run is not None: + rslrl_cfg.load_run = args_cli.load_run + if args_cli.checkpoint is not None: + rslrl_cfg.load_checkpoint = args_cli.checkpoint + if args_cli.run_name is not None: + rslrl_cfg.run_name = args_cli.run_name + if args_cli.logger is not None: + rslrl_cfg.logger = args_cli.logger + # set the project name for wandb and neptune + if rslrl_cfg.logger in {"wandb", "neptune"} and args_cli.log_project_name: + rslrl_cfg.wandb_project = args_cli.log_project_name + rslrl_cfg.neptune_project = args_cli.log_project_name + + # wheeled_gym naming convention overrides + rslrl_cfg.max_iterations = args_cli.rl_max_iterations + rslrl_cfg.num_steps_per_env = args_cli.agent_n_steps + if args_cli.no_log: + rslrl_cfg.log_dir = None + + rslrl_cfg.rl_no_log = args_cli.rl_no_log + + return rslrl_cfg + + +def default_isaac_cfg( + device: str = "cuda:0", num_envs: int | None = None, use_fabric: bool | None = None + ): + default_cfg = {"sim": {"physx": dict()}, "scene": dict()} + + # simulation device + default_cfg["sim"]["device"] = device + + # disable fabric to read/write through USD + if use_fabric is not None: + default_cfg["sim"]["use_fabric"] = use_fabric + + # number of environments + if num_envs is not None: + default_cfg["scene"]["num_envs"] = num_envs + + return default_cfg \ No newline at end of file