From 2af3f866f7fe25f97f06439c312ab1e0e5754c4d Mon Sep 17 00:00:00 2001 From: matafela Date: Mon, 9 Nov 2026 18:34:49 +0800 Subject: [PATCH 01/45] feat(toolkits): add atomic skill trajectory annotation --- docs/architecture/README.md | 1 + .../images/trajectory-annotation.png | Bin 0 -> 375312 bytes .../teleop-trajectory-annotation-design.md | 403 ++++++++++++ .../api_reference/trajectory_annotator.rst | 86 +++ embodichain/cli/main.py | 5 + .../toolkits/trajectory_annotator/README.md | 162 +++++ .../toolkits/trajectory_annotator/__init__.py | 26 + .../toolkits/trajectory_annotator/__main__.py | 26 + .../toolkits/trajectory_annotator/_cfg.py | 62 ++ .../toolkits/trajectory_annotator/_cli.py | 128 ++++ .../toolkits/trajectory_annotator/_file.py | 61 ++ .../toolkits/trajectory_annotator/_frames.py | 259 ++++++++ .../toolkits/trajectory_annotator/_models.py | 98 +++ .../toolkits/trajectory_annotator/_server.py | 185 ++++++ .../toolkits/trajectory_annotator/_storage.py | 222 +++++++ .../toolkits/trajectory_annotator/app.py | 110 ++++ .../toolkits/trajectory_annotator/frames.py | 244 +++++++ .../toolkits/trajectory_annotator/hdf5.py | 233 +++++++ .../toolkits/trajectory_annotator/video.py | 272 ++++++++ .../toolkits/trajectory_annotator/web/app.js | 600 ++++++++++++++++++ .../trajectory_annotator/web/index.html | 85 +++ .../trajectory_annotator/web/styles.css | 156 +++++ .../toolkits/trajectory_annotation/README.md | 76 +++ .../trajectory_annotation/__main__.py | 90 +++ .../toolkits/trajectory_annotation/images.py | 59 ++ pyproject.toml | 5 + .../toolkits/trajectory_annotation.py | 270 ++++++++ setup.py | 6 + tests/test_main.py | 1 + .../toolkits/trajectory_annotator/conftest.py | 126 ++++ .../trajectory_annotator/test_annotation.py | 342 ++++++++++ .../trajectory_annotator/test_browser.py | 365 +++++++++++ .../trajectory_annotator/test_frames.py | 271 ++++++++ 33 files changed, 5035 insertions(+) create mode 100644 docs/architecture/images/trajectory-annotation.png create mode 100644 docs/architecture/teleop-trajectory-annotation-design.md create mode 100644 docs/source/api_reference/trajectory_annotator.rst create mode 100644 embodichain/toolkits/trajectory_annotator/README.md create mode 100644 embodichain/toolkits/trajectory_annotator/__init__.py create mode 100644 embodichain/toolkits/trajectory_annotator/__main__.py create mode 100644 embodichain/toolkits/trajectory_annotator/_cfg.py create mode 100644 embodichain/toolkits/trajectory_annotator/_cli.py create mode 100644 embodichain/toolkits/trajectory_annotator/_file.py create mode 100644 embodichain/toolkits/trajectory_annotator/_frames.py create mode 100644 embodichain/toolkits/trajectory_annotator/_models.py create mode 100644 embodichain/toolkits/trajectory_annotator/_server.py create mode 100644 embodichain/toolkits/trajectory_annotator/_storage.py create mode 100644 embodichain/toolkits/trajectory_annotator/app.py create mode 100644 embodichain/toolkits/trajectory_annotator/frames.py create mode 100644 embodichain/toolkits/trajectory_annotator/hdf5.py create mode 100644 embodichain/toolkits/trajectory_annotator/video.py create mode 100644 embodichain/toolkits/trajectory_annotator/web/app.js create mode 100644 embodichain/toolkits/trajectory_annotator/web/index.html create mode 100644 embodichain/toolkits/trajectory_annotator/web/styles.css create mode 100644 examples/toolkits/trajectory_annotation/README.md create mode 100644 examples/toolkits/trajectory_annotation/__main__.py create mode 100644 examples/toolkits/trajectory_annotation/images.py create mode 100644 scripts/benchmark/toolkits/trajectory_annotation.py create mode 100644 tests/toolkits/trajectory_annotator/conftest.py create mode 100644 tests/toolkits/trajectory_annotator/test_annotation.py create mode 100644 tests/toolkits/trajectory_annotator/test_browser.py create mode 100644 tests/toolkits/trajectory_annotator/test_frames.py diff --git a/docs/architecture/README.md b/docs/architecture/README.md index 48c2870309..a02553208a 100644 --- a/docs/architecture/README.md +++ b/docs/architecture/README.md @@ -13,6 +13,7 @@ Local and CI Sphinx builds regenerate their own version-specific assets. - [Earlier frontend snapshot](preview.snapshot.json): retained as a reference, no longer consumed by the app. - [GenSim / TaskSpec design](gen-sim-taskspec-design.md): current implementation status, ownership boundaries, and staged target architecture. - [Task Program scene expansion design](task-program-scene-expansion-design.md): workspace integration, shared expert generation workflow, and three-developer implementation plan. +- [Teleoperation trajectory annotation design](teleop-trajectory-annotation-design.md): proposed local video annotation tool, frame interval semantics, keyboard controls, and persistence. `agent_context/MAP.yaml` remains the sole topic inventory. Groups are presentation choices. Neither import declarations nor adjacency establish runtime order. Every diff --git a/docs/architecture/images/trajectory-annotation.png b/docs/architecture/images/trajectory-annotation.png new file mode 100644 index 0000000000000000000000000000000000000000..e5e79afa6378cb03cca3b838021efc6acee5446f GIT binary patch literal 375312 zcmd?RRa9Kv7BvVVKthm^KyXNcLvW{%Bm@ZV5InfMTM>cK=Wb_Bi9z9(DFvGS^&l?+`_KNgOOvEEE(J9I5Z$lu=O7Gf_~U%f5Jq zytBrYGl7Eg8b#{c7ghJvqh<7;Z_UUs&txX1mIJYB2&~4RbuTY3ldxNNc>6~jgP)P1 z^#nB|F914IoO0I53f(G52b^yan{P2mxBWzY$hzCutaIX{H(BinGepk+pUasf5%u5U zn8OSEe}}(D#D1auJ9eggM*i|RfAb0Aq2m5~ zd(7kiXv2z6LB6cqI1I&F+eDG{8a zq8jme=DSwzQ^S4V^Ny~Wmf%cE?p^K@J#^XB2?Z1Qle(mM_DRzYJH%()6w5!)hvfp3 z8zJS{12u8}ph1{`{`VD1K+EpRa5488+^ua)+TztAFM1`S4X7KPlCo52 z5tdL27?aE=TxkLBFZFl+*926bW4-2Y^vuh*%oT!qHt3{7l1;W2GnG}2YAz$o^pK|T z?2E`HrOLw}Db?@EiPEfJv*#rw;O~08nL$H)?)l1F`MD&3xwGL2CzXLE%HGLdJjxY9 zaaKd46{)BjtNEa3E>@NF`J_9Z{Ik(KV5r@M0ny6%cCmsV&vW{#?cUjK6=iS0lZIo&=PzY zx8|#S2es9#43VWB2$#!^;53m$&5O=Vgh|Uunf5Af2nsTFObpBnx2l779+mLdWeq;+ ze5Bc%UL2d{j*eA5;{ey0r@f|fgZ+){y*VWZG=!Cf(+z7&?Tn6$c*5^r%HepQPYpJw z=f%tQ3tNmg`8X<1oRnH+2zz`Uk1i$}WzVx1D`ZqqI9-3x_VjM`ghmh!CxAd{85z5) zWp7nn`$Muf-%`+8ZqsmxBm`|5isSRQCD`nrLUqCEB2D#fy*(vEL#8YXRbdRG1hfwP z;qkS{e=4b3UgE4&TGTF)nVWxqZ<7Kw<&GEDhlTKy<3{}s_4Gz;D4~cKy?FVbxD*$b z7lB6Wybl47bJoZtlUO9o;7ad|FQk>fDI({{uNabXR3=B~b9Ag4(98*bR}gP(mRCU2 z4QTeXXhJ2xNmygA;2IqrDc$0yvSAhFOXSs^ff_B=oPK2^oohI-tD{5Ds|TsXPE05_ zUsEb}PH)b&FZ<{O4c44;Jo#p0QFAL;TYvE&Q{i%w4yf)73CYf8PDq%cI?EYRFyz;b02Z5`5~7%|#kEwwwEXqX;mZ@61L56{lvEWsM<9eKXc*T}Z>4Hm{%Bb7sK^w_*lID+{-R-=*&`4eiZ!A11EX6{1hTP<& zq>0t$`m%(5$~Z9C>vTp<^vN}cPBy2WuS|#EY2*EWvSuc2gAN*(7&O@r3dw>(lz_I- znA(>)sL3z)g+$osS-9wVnE6-$03Hzji{uLqp4y?25#yRSw4Y^%KE+M>jO`%TG>?)) z!pKB#KsDf-4Jo!g(wEU2)y4us-qS0L4i%Q5;z|+5@42DtRw`a(P!otWI2rBjNv+$7 z8RcwqA6MFJwq4`OFMHw~&E?Iuw`yBp(_Rg;l*~cYH=dC)_B#$#%q=W%DsPMADuw?Y z7)Xer=iOO`tW4~4@D`|8tcTO-2d%zwBVL^7BW|$YVM+lO|0Jl-WW)e1;>BbqkEPU_qmtaAxU+9q z!tA7-d?uX?K{ZMSPHqRKtW(Aq24)7<~3#M&nB_6v*(_dF1kQ;XX*FUKKE~sZCzgtzDr0DM&RZP zFm|6zRzQrEU0vU4vr#iwGhpv8&P8>uj}0NMuQ{j~GjY)BeeHMrECy3hBG_y5!o!M8 zTbybaY1`IL=}d;Ax}!ibb4-RKLv!P8ORcxUFoOOmY8@?yV0sUy(YUa&#Pm+}46 zSRT^l_cg!;ESs=8;>)eltUO3*RIy?qPyjc*fsQ{X-n=(X!{dMErlKMfq2BQ`ESjA( zr~eIkevkkJBAb^}7@8G9t0EzS10D;*V9fw{oG-oXcNav}o+-^l<#Q9*Chd{EkH${~ zG^dI;sr-X6ut84$ua4!r`K0pz1tc&4X$u& zTRq$Aol@{!{qW!*z|)>DN7ZMwhv0Uu-U;+xG^2NtjMp?a$_~`bSKc?$%g9jWaJRF~ zOVRXwgMXWV^un^YM=i|@Mj>FUW%zaJT&#INrxFxLg_VD{_wqUX>Fz|O|GQTIPAxN? z{FD3ARI1IHC@hb%qyLq9=b~Ht+?-cu;{(#WkzxXBo6NcQ_Vy^wVbH)zF{7Kgg7xHc3)$%qUwji|tAiQH zWAj~BBO}s1^TwQ$6P3fF<3JeAAufAI;0wXu5Ud>@-Rx0O0~8eJ*Lem)tz`K$^oj@? z=IS_+I-CeuhFT+A1B&~D%IgB-v+=1+?cS`^qH{uJAU{7!Q(!jPjxBGcLI+8y!LBy- zww#Xqyb*cr;azvjE2EnD)*S7%%jVF0OZbRJ2$QJcfLmrN_hOiI@*WA(OWBR00s)TSf9Z+6TTY8rZ48c)-%Qt+~~J71nzpRAUhX2w9} z?rAQ^dZ#9t3*#2t#u(^{H1a>C+}2u-H=zNBb}y}FEfnj&bUunnPU0I$l7f>a#5-`2 z8Dk<#MnQsw5O{t=$sRsBTi|D4M5mDYh<``Q0~7?rvyz>9io_;cv_O&@he-FD#UzvP zC7;t+5d%Ev0Mi`OfxrX`$|{@O zg4M4O2nnEClI8To2P1xtYB5L-EVJTzG*JxsJ+ijwyeI9J6}w51s?&7Es>Q;x(xgj- zk*QU2=@8+gwXzqaMsc&FFMsxAE4{m&A){C7oAN;zFSz%x4xv45^sYw3RfH5T@s7ue z({b+S^sau>?d(kHJxLnRgTK^fZ=~U8fO!QDE@3kNd?UA=x!U0Lh~(5u_{Al~Y3aHoB*mfb2!7t3G-sj(OM92A zpP~zXy4oPnhMIoGmgM2G@7gbe&N&0&^mIvaG*Db*0-J%0#w!U3!i}{jnp$=T4?GM19s)>n7*l2G2ow>#iSy!n5BKBo3AyPVRNB z`;ci6!m>ncu6K`qm6rN^Rk~D8xPY#|V+?@eLSrv~iDBaIkRoIjZJhpO-ZHebq|7i> zzFP7A?%)_ai!6!~n&UEj$xaa>n#m*pzH$Q;=Y-0f!1sLm9?fKF2_u_l#!e|LmV#7d zT%z=iKid&To+69RCEXft<$cepjngR)iN^HrOs~A@LOURyL5Q9@b=?GC*4Ff0a8zsa zrOt^f*XBg{@lZI$C(dl1xxS7})Q7#WmZS1j?OPsDvhz{beg+UO<{4fZ5usn5FOyhm zT$BVEP7*c^OAe8R`ie&o}&YRJtaT*^ozrwGEPtT5cR%x|i$ zt4rYrI)j3stgIsfjg95c{9TV0+%|V?@{81besxZbM}8!lFxm?{ST3(VvX;-Dk6o4c zhlI%dI1p~Lq7~;L-5>jsYREuxXJ;FBM9;)gAdLu}s7DKmsH}A%<}-2q3&P|p*Ci-r zp_h!EDAqDlAO~Iw{!x16Wq))`!C33D4&!hYwp$H4SyJV|qpiEXCK^dyY4iX!=9zd} zS^|O4)*P`RfRds6?D!@p7oc%!kI%iYLPU5{S@}_~l~-if@y5nyHkow<6Fez_TQ@$C5WuT52Pyz|@D*;(Xn8xTZ0| z%*|C#EMj`lY3|AW?@#b6gmVaoQhX@rc=%EH{dOApR@f4^93KAqk{MX9$!k9+FkR8EM0 zPA9^fuLMfqSrRVO~5oQ706Xq)@J!>@mSfiQTE1>X%dTi zV9M!mNYb zBvt8vE`U?4@kl{0F_p!_=6od;pUm<05q zMhlkq_X;N#{bu;sU*LGgC&vI0bM5Lj09-Pz}Q9G(U<24bVX-96@?#SN$xt*R0539ZV zs_PRkMTDFj4mP%+%b}6Av7y!J=a;8v^;vHVMujV^4#&{|3oa-4RBs=2$aBq?%VfTf zg;9WKYpf?@3|TqHalYb*YOsZE?=H>*l3NKZ%g#G%ruRJK~I$;`ufISUdiUi|roHttPL_&|)wqdhCMhkwpL9 z%(0-I<`W(|&+lhwL6NEUhX?3#zCP|kEA^s}r@`fIEpB%+^`Z|#SDl)^57%K#uWa?2 zvq%4~Kl3~5S{HUS%e2~`xsoB-x4?9@Gw2<9@NO?MX(N)~g;BD@)?2l4z}yA5q-5p$ zb9oxl(r>)3HK}`jf0`Q)5wNw#fYMrhHhsB&8kxD7uU-eqX(7oQLBU3BN`ViIQd4$X z)d{7nX@INSb_$Hod?ZOEjvje@&(?%Xm=!XpDvkq|AmqZWbk)ITw10r{4MKWmkY}WE zZ$FXLLG(dp#)|oS672{NWI3F3WE=r2Yr$^70KPhfZeM9A*%WhypjJ=ADQ(HaBo0BD88h6~XwP4o4BN z6A$>nRqGw@_SRON#q!0b0F^Ss7Cq!64Xjzz<>FTV={5}VkvGYe#4Gvg_nw|5Z3l_eGe6PErgd%} zDxDMu=1uog--U;VUtQratj|KcsIEeptaa-r>Mfk48>D9?3*QLgp5A6H+pVe&DWK&o zy4h$hR)pxg|0wirmX~cjUTHEJ%9T2FJrMz(y%*;*9}0fk1H7N+1K&{osQa!ecylTW zJOr4ZO=(t(f`y&V{wOW-+1^@BlRnki_}(`S*>HDte^dD#t(;5UBQN*;PhRi`E|qFw z0I%Y+__QpsSv{5o=gclwBrdo>hldwl$<10NZV7cxLr!I8L$+8lMtup-=2phLJ30jU z_~H9LvBzmHn%~RpTA?J5qeRWCu2W^fB_osE;r_L zkW@}`isl@EpA)4i+|Du@q+T`@txhSvMa9iPTzOva)ww>H67HVPw=xF~ zDx?XInq!L7^8azODX+|tv|fgWL0$~sr*w7oNMU}GPh(|hn;B?B7w1E!^9JL6ELZGq zb&+I%Q&&a18JkXi#c#!>*v|gRT?Fi)(Q5Qd=sOOz_5Ml2>CBHGvxyyeWdDPP#6Ip{ zY2$Y028a**!wY0et^!9`d3Z|@iHrbg6{QnTBvmUTe=wG(QJr8zz;e4b5@Du5`GRCz zbl`}P6TMZ5+hU>WtK*J|_tW7ifY;8O@O9?Ky{EUZbU$;7pyvj*q*>r`A;n6w9L)Ls zE{L3HmqVC$&2H=WtX}=FMd*zsSae0||fB%+N`jFS${&+&1bh63? z+;X%ekl_pOOw6c1oSL5*i7YRet&#)JelY2KVCUhwv0Dt8NTsZ`)EDuc$P4mXE8b0O zzDm1<|Bfh55SI+RqXlP>6aXmdPpzy2$ z1}yCsEcc#udj^K+xTLYx(Z4M%LjBkX?}uu7+&c5+_F{P-tdD&qV+#XyvNrarpF($# zvubN;Dw&&Z7PSfJoP;;(OKafGx0p71$+3xtiMF_@C`8@txUi$Q%)Jo?vMfrojqD!o zZike8#v;`da@)Ay&xw^nc1e1A=!ArYBY-|fpQxL))Hq$DJSOL+L7X(#!(hNP7l?G=j^$mo5k_BrU}(N{MrF5S2` zK`BGZ>eQpHUNVaqwPnLZ!o&gkO{44Q-gH+HO;yv!<;qjhD<4-oKC#UwtOl; zf07;aK2C4qk{sZm#cBOBQ`BXmLAUv)vAMB->POaHXA9e|ewzab^6flC3gHdh41(fO ziN+)w+t}~`kG^YbgF~_GX=v%RHMO`4HEuNjzDkyQWVv^Abet(!r4e?#T8KcqyRq3` z)tTt)Mn-gR(kZZj&xM)(uvrml8X~3>lb(i-QpEEQi34M}Y^-rsBcJak5t>pfSz z76I(+Ocxhn0(#_=`xDk);DI8}^#$F2>q#f%k=qU2qs* zAFMWjwKpX zvEfO8gZD}sac9RR3U{6g2nmvqsN=0$-#8#tiisf@$wbAJlvLrWz71%&O-k~ev2%2A zdN@7JV@nqX-wfA3VBkHRFP7_x`05>~OGG~Xo$G@2#9y-V2*J7@z+-(9a#~%z-8A&H z6<5yTadl-d<+$Hpm*JzA3Hnef`|ogjwFq+ zu_KUlR^PW4B~O|0=!Utyccp?!DJjTktKAgzjP@=#GQCR`pG@0JBtro;(g@QH$)<9E zM2r_nO1IT!`}2ZwVOV+Va*+KRt&>fC$~ zeSEA$c_e#Dhx_~Egx|+$y(D*sO0d5G9}CYXEohIhJsKOeXUnHxvywJ8g05cPPmz9p zU;bA25+uh4Jd+FuH@V&Q+U7)s>`V|1c*-QCqeK;+{thD5m7B@TGEnPtTXgIDFEId4Hc$rro+BH@Dc8n|sJ- zOCjX_iB?>7xpTgs@j#aw@F^xe2A>jtUHP`J>raZr8|A6ZU8-F^n}yp);N=_3|8W5i zoZOna>#1pJkD7H|OH(k=Xd94B&-2Pmt{8z59G~cw_9a`0id92ZcesDFcWGG4lrr59 z>T;a&n4#d~u_kLcMuvg1=JPcR$8)*a^&g}N1d_o5r)XAmBKsgO9UO{2nb=yG7}!dAo$PNv?6@dC zW#ur=t?%nb(Y1z!7w%5p?&mazH|VF22qTav{BptN8bk7b)vB@Zfap&f-96U6F5>P zaG*^-&NMY6o|@#+IjvBi{`5oIEu94H_=NzF1r|`~Bi$TS%jOFm@@-e`-MKo9 zBFZa<_edai=&Y?M>_%5{gzUfc2btqtI$b7(*4Y&7C7L|)WvI|3zIl=(g-g>RUBniF ziT>fV+m~e(`I+`!?-<=3&sV>bH09#LYeO(TQ8ePch*wy?*p4~C#K2$r8pUOPq zdTE!~l|Uv|iFED}K1tXat_I0U}mMP4wg7u+1ou+n)o)fB&MQ1;<&mzufE{_rFVTy)%Ix zbOz&)aGQ@k+~K`r(d$4_bz_v0${0yiYV$iiCYw#FQc?b$qlzRIrKCb|M_5H7QqVD5 zw=SnA!4B@<}P;`;AumENi+C zk|aWslXj~C#=FLCSndPn=}_one`hemUwpE+mz-Nd%H_Vr%l6E zf#wEriGXCVc1lVLEft6O1v+R(f{d zIiFEsx8wVu8FjL3J`|hBd`$PutG+*7&;!ph zf4Xb*x|48S+w{<1Zt=M(9u?)_;ensrJ@uz0Z&392L&6?EwRV=cjdRe;r{yKa$4~n$ zYv0}M*B)g&tw?;kt%q^(@qt*o3=FMxI=i=Lj=eaoZoRsFtsZqHdO?tnP$-V&bV;Mv zDk@8eE&IajweQ%TPEO-mq+?vXM~&8Brj`pv-6%lG1-(`9mwx4^kJS<5Nmd%HTH1+O zx$1Y3X+}n`s-#_ymuALhHQ@J}n)8ECa5LmPOi=GH+F4jw{QTJ$7Xt~|@5Ar32!?yN z`aV7+D&Osn=!Zu|)mPT#3x`C$6NX*F@7Ftn1O){@!JQz+?RRc=WF+whPC32dUT0_c zIPNn=wZ`Kg#l+A&k8ZE(MS+j29qs-YjGFBH{JC~de$u85j{7a8EejooGj=*HTMDjo zqvi803ZnUVoCf!|gij?x4i|C8bqlpte6~{+wrPTkbv9w)Ok(Q1zaph&q?Fj2+L0W` z`B{3Q@K9NpgD;jvuEmMKRKzP3H)vj1itg3CW3b+w0Ku?&@2 z3o|>aaE74Pg@!U&MZ?&f+)rJ7e9(}C#(DoMN-Gf zg>6n+{%^}&LQn-Xb@jTqid<}0dZO7{=BFc3X9BrcTeBMV<0b>Ut@?*AB(W1XvK7T9 zOkug;IbF#}$ckKv^DF9^8D3;f7Tbd34PROrPku(O>j`VM-$}~mX=rF#X>_PfNOS`N zfk@pH0lOi%IC`eWqxi4JsFRb^q0ChjbJl)2-Z8PRIF;`?t+2v3NpV)~i(n=hB~nHa z-XNqWeL9LP{PH@>IM_o-sVB3xGz}8cm_3Hq-H~!vU%Ic^JKajxO_wejKdGjIY~Yac z**KgGRTki#PE3F0h|$%a#UU2*-059iYW2D5*f(9d$GN4nbL0^ne@80e~;wOjI} zmoNAxA^vdIwrX8cqE&A@U!V~MUnO9^qTBB^hKm+UClriDYxsS!=1G=2e0;FGLXg#m z{=P*vMjq`)3P_JDcsV#YxR^n5`Dwy#22*DrROq9goc2XyN%G~=`Zmwy(-Ukc?vXV& z5vy*ph>M~3*RSzY7hGI~!u>RKG#pxJwERpj12IHWMUDryi~YV3o-HQ<+}@EzYF^B) z9?q0m%+yF#=MO{g5oGYl5 zkNar%@aA7ur4iyhCQMu^$^E&2h99nKE@ZbDZE^Z+66N;8Z+3PP5=RI7S58;V6p^xr zOpUyH%51?p@Z7~*txGAg6^d+VJP#3lzZgfi&1!!I>*)N-crE$TrGLo9=UY~2;%LSX zt@#$u^8gV#-2;|IS7YW0X(11LdI15*m(F|uZ@H=V)Zs*N+ZKaVlV;4=*zE33ZE=ws zlaBJ*D8X1x05f-a7-Q^77Mqi{R z4OXKXs$X+F*J=?q!`(kLbdT(5h8N+dIULd5G3G|JMMq})!8UcjRY*3 zIP}E}lp%gu;dCtb=LKoap&bEFpY(UOx25(E;Hyv3(b1Xa5b?058g@Q5ft8L9$CFe| zT-$nk<<0HJ+d8+7ESY4J1=H~BH#zOsP)O&RZ0@!PHDtph z82^qJD3tS9%~m>1yz{-)bJqVZCj)TU*t$C8Gv@u~B;pDCF>X6IKQE0{5YU1fAHM3l z_V?GLk8rEdrr~Ct?V`uyy?A379+_Z+STL@v1r+WIg|^rRtbAXV&R!9JsyHh^BEH9~ z+1Sh7(McoE#1U~HJ`4!HEH#cJI=iG?_T3+zQ}+$bnMzVU)h zDY=#}D=T9Zah5kA-1mK{)g@ln!Qwtztc<}o8}8Rl02-Q8lJa|PL@$xRZtvGLP-ANl zC#8qkA6Ke6k>HEA+G2e+7(bYIZV!WT9w#*S*kB*SHurc{bE@TOviF8+rCB@%cc0{U zo>p-@D+2-SLIC+mkW3br5~guoY;4@0J&e7Yr73G8?aK)>nZX9AeMBq?XZA*!H_HKS zT!d}QlG+&jMK^z}7}kP=kia?S_|x-_unZQHf!|-*lR5gs>3HmBoY#Ms&z3B^Kebb1 z*|To|KR-TR*3-5<-5=kG(iShRsBbS;TG{zyTtVTED>cc4V;S|_!orjf!^1?iKV2?o z(nMN4Wiv$PyZZ%rcxV}DV8wR64bFL)FAmCm?+M?$5&Zc%;~j9ApL|DGc1Yj%soJXi zUVpBlemyuUotQh5O@$Cy)l&(%6Xa-7E5?g0qLI_dyUG=Wkm_<(R@(~tbj^F;L)@!XZX zH0hS$i|b=vk%!VWwf~WR50%8prl+T4M^R(|@TerQ{j4E7HmIX3E>!H;0<($gHUlRg z#l)w{7|HLq>qS55!VdfJ#nemZ-@AQbO=Nzp{xE)LwtevU;3=~*p%-sWTx_))35%tq z-WLx%0=%aW?(84;dxXBFf0-fVBhMzmfn~-JY97!6WZ8EPy1LuQgBcb*GpQMU+PNIo zb}i9qjU2~Sqgt~!S8jusRL0iVHq~Y&PQkLn`uiq2s?F}AL5U=CRrS^J7K)uyQ#Dg` zytyPKBrK1cHzR3)Dm%C1mCH-Ew(WPm*i7>h{K1_TjMK5nu@VxeE3KHL~?KWcaQ0RmOWR^PRh(Em~Ujp;oz z@U#w<(=E6qQd89bOvrncI++6^jtIohM#d=dUfC300EC?f|B+XSjx`;FlZ$iL=iH{O zMNgIS{Y0DHX+T#OlZAmQhKY>I3)7{fm}*nm;r?Nti(8BD=Oufq?;i4UN8_GY?&VFx z9MxYRQiusaJwpO~6ma-&_nnLGN~osU@T4=l+o5Zk>l7yqtxo zc#Zw_J3l|?M8);{{JEZ35;M3h@v(7hw2jxAU|AYjODOd8^l(Frzez0YDN*nG-pKWz zTg{Z*IayRzl@REc%w}fA*Ivbk?K0SYbqIxDwv8fVaGi-0GNurg`ab!gtpaa^Ge-4W z589TNQsbY>1fA5K1HJG2h5Ci^>GIud!?fN;L)4xt;uxFel8`1FnfXV;u#K~Y+}o-Od|;%?Ef?K6DOvY%$S;B!sE_D zdAag*sX9aN=!~uHNs3JZY&~BG(vzK!Khn*Uw;5gaec)hb_F8`es^lbc+t4isgeDXs zP$WH7zx)y#ol_)OdjTATnm9BdRnsolmszSe#hqrL-`vwPuV~vyl%@|uu}vRqos*aL zBs7tht*NOkF7d6(P6edW6Hck-Sof!|(Ago7Ahe=_=L=1lUNb8fS0H6ict*qhX7u~}}K zz7f6WH$rgdb9V;ep2JGS(FPwM4>!8QGKTI_ZLzTbdcFT3varzefm7()m3WEu(c8_z zX*0|1zmh88GV)-@q8^R!@bxv6z390DyJf+dSIvduENoTQWDNnj*kjJ|zR?bs*Sg8l zw3z}6D;U9w?;D<0U7NRGz0qsM74(c5CobbhPX~cJp`}VWL9*eW%Or5f78Vxz!fP0; z0n3ZcuC8^~DMBAUd{Msm+LIyk?&@G3DObvkQBwMQ+M9t(`GzN;2dOgFHtCt`+goY9 z8`LNv=dNh0uI5@?@$&xKakA{SRre+~DVp72W9yX2TQVyE6%A2IeY)&>ws;ywOeo}e z|7~KeyXZ;1&TdwHJT)y(3xh($dM}KK?eWJ&L1P06zDUN?Y(|*4)sNPle$elkkvU67 zfa}S`jSG`$7FL>G`{jc?$najKvo`icwnI188d~s z0nz`Yo22?5wuAlAGQ}8vpPc{;)9fPV=-0z%kk!}ba;xGuzD&=_Y!pd7iJ5W8YG0bO zpgchm`aGlj zW_4iO+vj1+I54jp8wbajwEO)rz%?==0%Y4aIo}Q69!v~9=#Goa$Y@);3*?>QO#`?Y z_kPlA2{=9}Q;i@+oGfULo&IVDgR|#9v9K)GSq$S!O7{Nv@x%LJI?+D}yUj^^s<8$V z!U))(F8(Z~^8QnnVuQ!vjMZ$J@7=IQv+Jn=c-?$4AEz5Y>|h*_R;pgAWzEI}8!9L? zB0^b?1%t&b-T;D0(^sJX$a&31hQ8-a;SOhL6K71D-kMvEm2N7^Pv2rOijtH6^LT9X z1>H!g(K1rdtEQ*XJvTB`=l4pS&yi=oj^P*Y*w0^L(cs>72j(^sg9QegCfP9J5aNl? z%8pfGVO-R>_Hm#3D)w&lO`efkf;l~u%hQFC9Y7MLs4t!rg2;|MGL-lOYV6Fct?$q2 zFXhujI!EAxqoSdtyY+U{KAHUMzmw6P2eBmz*pv$uB?)zYPJw zNOD%ipYBGhkZ9&;r8z1sqI2Dw_Y}#+3r0}_^qM4B)}o`4=-{CG{S9nxesVx5_1gs84KVn^=lbptK8V*bs73Qn7Y%5t> zYJ%OM(s7<$Rn=TqFqo6$KtiTEMX3-##-BEXn|Q5HW_T@`L>2hUKbxtMk;Fu+YozY| zU;L{RU0Lg;O?m|ykT)rJX4rdvej4GNOsJKj;%gOK0*Z>tx`BZqN*>E^uC5~m8Ni-M z{Z>z#482xluR=ECy8ZeX@Ob;6A(BBwM%HX`ITOpws3{(3Vr=Zab%x63aQ=EC$7?ng zH4^pV&IegtCM0C(R+K>pNeCP^>#!z2^G*SLL$5esey#m&`20@YyPID1(heCmOAQ)h_dBb2M;YSf**Ujx{QsD z$%Q?Ky5{jCwrW5xPyP=d8+ zN-9!>=|fNiSH8lX`N>hbaMN_DcA~^1Vv(x9zn@k^xhfPZ%rz;S3=W4!QHsttIJ{l# zE#eL>9W%XbMsn!jwM@_j?XcmVP09nnQvJ%}!{zM_Jd*SYWlC78*^K2^jAQSCu> zDr(=?H>PNL6E5g-@*T}A>~yYMCNW<&{d+qFv(;f&hZYkC2I5th?u3Zv?e~)}|12NM znCmk-fB;A@9CyMwv|dPBj>YN{xfwt?>WA{zaO6HVprb7iIG^I9h@MzrW^7IR_9|$c zm^FuEzLHT0%?DS*AIuQaxp@JD&pVhf2dWiBGW8J9#FaU}imBgUo+8(3Z@?&r3=@}9$Uzyn+94u>gS54oVhsgTZ&TBm-={>b zds8k$bLZ{tAFOXBw7kD=q&8BS8JpTJ)tF2rIaoXUU-f2phb7*-9Ke$8px{x4?z;AN zD8UszI3=MDX``ju^-@vfobIo?)i>V08A%mp7Jfu#U+&o6&04EPElx*74W5pUj``1R z-(uqbEJvYE+y)OX&*phe*^?Y^t!#6FF=gp8oGEJnpL#uuO-`1D=|1xucAKC7uEtRU z#O;+2$_PlFb?W~UX~q;5Q>r!r;=~L{;kNzes}IHI=H=C^+T)W3BGc48;?Qp5@*W9W z+f0WoXJ9`n@9mO<*qL9`o+4XG=63p1k#y+Px86Y8veZ!J(4sy~PuKcejk zad}Mv@O{ji>uq#7bS>BOF)}ogDRW&$;DjsJ@Op9-!R@9k_v>MrB1wYD7_P?)X=!Pl zvy+MGh_m(_R$W0J+o{{L^&HqtPmgIy1M0ZdZ292v{=5IYGk3(BybE}UEw0nfhZ>B)PLa;!TxdU_&sSVzbDXoO&a z`J=y)4&2yJBJgtGn+vow24eMC4pqzSrQTwI9U zZ44H=jfdZQvv06|N9xz154Mj87c1oM^n_0cp9z9_)}IgBb=({r`KZkuwjz#>()-}y zrJ-GAOt@2+=N z9`0(byx=@mO|Hv9Fc|i}p6Z(o@t8nNJSvt4uXzo9;K=4V5+i|PL)G*S7HTh(#&c-w z7&RY-J{#D{>NY#;L9H19Ksl|8W5iX0HGQ-=m&7k7ri9Lgg(c*xRgy7_88xt| zQ^bG%lRlomsg>W%1 zR)56AqriIcA`=DkzefqEWj#Y?y(JtTr?c&#-z_3Jf z;g=yHaY3uJC(n)76`r)45p9=5tS9r;Y!wwb33z71d5Yh6S<~*d5q7IkF+n7qF&K}G z&8`z&=MW1Edihji07Ikwx?zDlMV?ts+G?c{5#YYRWfbNvBw=Dw6ykJq(%M((HXQnjlRd29nyV0vRrUim+2L{WN$Xb>m^#5|-SI@iuiH!wX* z^ss$MD~vlVBfcv7pP!?koI(a(*;im64Z(MM2#(gkq>k`|L@NBPo$Y*+?L6b1d}HI@ zgwj!he;+S2Wdi(P2RJCMH~f1f0m`q)|F0bg!T!%1Wgh-NKCr?wlC}1~i3B;1ZVyMT zv+=)cgyKw6RQT`s{~rfs2$64uMKQJ_ujh&R3 z_<=QB_~(D`oABnE{l7&AzP5XE?RnR*!_BKjEGyP|CWNFqyeJOMO~JC)oNIddlBAHP z_jzDL@jr7glKFG2U@Ozlsp;_v_(XlGWC9P!z8IK`HXGKGPXCcF_IFrbJgs7%_@9q$ zR4_=qAnxL8ly~QqZb#^)3aPcj}J35TG!%+uu*>#M>s!!ga<9bo_6#YSzll8aHrO+AsR3dsK6Vk6~$|JEFV zn}TjX!`B3kmgBz{n02nKvSv*VNpk!N{%ldsEsT_!6L;kudnNGlij;`{_aHtIx)&SU zTQ8H7I4u7iT~SdQjC2z`Y?S+5|GKj30+^R|AX@& zk4{X@wfi(!B(^;I^4|k;{(FF=uYc74z*&6dv(a3=T+URl5kao^Xh;O{+qSuB6uFNC zGBOl>(Ov)NIQ%JAcG)tNp8tE*E}M<>bcNCJ{{S@L%XbbA$2^B$2~b3JD%Gw9D$@P4 z&JGOp4fLs-+)K}Vr2fD61z{a~9x^Jtvz8X!M`F0Aj&wy?h!|b0#DPuXR83>!JUXok zTrMT*|9e4>qT{>2roUp0-X>a4LXUU=jf9>XyH=pxXSXCRNs&JgG8WsF_IA*=D@mi*Nue-S0!trPN z!CRXr%cuPa;x5su0tD53Jf4U?iIS-w&#|yfdnZd(wtEAgv*%+v%8~Co8;|LKeVsUnMx0lk#`{b0m6?C9}-yZ*~ z(y)WEC=)Vldf*mYTc-Woh|8+Y*;(FUdHMC;sJpwUZc*m_`Nyhn_sVJ?kEAPVRy|CE z&z!)W5}6Eiv$e18JV{=18oU16cjAagkev)o8;lEsCthyDWSyvTMstS2guE^mTHG=d z`OemWxnV5V!`1a;x!~PS>6b6}ux0Ow8Yv?U4GpYdj`dEh>9XZdoC>D3E;NlmZO@xIDbOD;cHX>Qzqm2eYuE>M%yJ7q+^*BdXs_ zndhf2Z3eM2`KnlH_(I9SjK99B@^dh+uI*tH=INH;wa#KI7od zZ$63?s!L}uFU0tei_|+)2^2RJ@u{ho8z<}3GK78UD&4`KGoBeN_XOnjuJ6&yw4t9q z`PFipb_KtM>!LCQMn@}^rcg{xsngKV0JYDF!?TX^4^EC%$&M2f>(k$NZLO@VY)uoQ zlcST8@K5=`}$6nZn#1#Q={DzZPP~dVrm%q_^0A?qGZpxo2x-%B} z$0x>Tm;Ei*yDF*PTQ-pKL)S{|+DsTUMlv9TclV4asW0`;4!I33kLL@G5^a-{lQEEd zduI5X;73B?xbka{N8H9vYt`e4+Xl#+@~!Pc=f#tVFezwZy@ zei`={ZqPKrcQi_%dO)G_ck~+vEf#eb(OV^LFo$SS<7bJwygl&;gq)%V@8urvxKH4O z$v^)X6s!bwS2q>!n{B!vQ7reS;&>hgQ^HA|W*Lct$SA zA0pY~S9Yd6KE;}rHVDXD!<(}-9wiQ^f9jov|CneUdBumbYA1SmJeQOjOI7^}aC=Fh zd0y7l%B5UOw0M08)^=CEk$3c4r}S$4*7^b>~C3^U8GF{eddZcL7h@JWp=f za;Zc`{m$VVA2E3_rh5z9Vo;8STx(4BxpvuNbbHzpOTUJNW%4)(q|-xX)zvSqpA*ri ztsWfQZVizptI6Gc>M5PCxA-|c484rBxuk^LA5^nSC-}Oa!il3an2Hq&e^#qAlT_5z zv2n&ZRaT;KzqM1yVQZf*_>I}31FUuq-h&J(;jFp_yb z@bx#Hjt*&G6;P=A2V*5_wKq1p0g1!xtEX0DYeKCp0v_-k=<1qS^7;qu+SAKZxUn>q zaR4e;fu|K!-y>B~{QJ>$5PPl^B%E>w9}UDmyAGS5;Oj4QW%@ z+PNGdhF5SM6)dK3wy1fyZa zgt~fGqsw4?sAPohtw>JZ6la1Go|?*t618jQTYSWD*aEX&&ag!XqNf(rT6VH`Y_>M^`RgI3qA-CPf@Iw+{H##I0w1HR=_;f!ne`*(-~ z8(W;W)zbI7zicXrVbTW!d3-zo-9ksHf4cOiljBzIYtGs%duowF@nC|D0@>0H4QIoL z#0;t)xjWcyIcJ;tAz$e{Y_`$F#ZV3 zK>R>mFerfk$f0bt&Td;=dVf49k5&oZ+uy^&%B`SzYm^|sj_ZXQSy2tk@#l5fG6PH& zrF&BH7ySGZbx(SWbJ2jc`5nX8$k3Telk?`U^%s%yKsH`D`0$fx1bFVzqEhVdH&!(s zDcoR?{4x9a)!(D;H`7d|6)P$K%^yZWUOrxxk5mk6HoDfFxxX4Y*xqA5b62Vy!?bY5 zoU|{II(fPq4M0}c>%FX2)92YUy#(E#QAD4oBqg1|T?}NxZ6l zs`IparYr8^HS_!KpQGO*`~b*8M@sB1x|^c0@sZgwEj2X;xr*Yqj?QYXJDbr8t*d!O z?Qdf!LQJ=r3ny{E+8rB{x%sNwLhqWFfZ3hQT?xZ+4V?VSe zE)M6|)*jCt+7}LR3U@#yUNUXBwMjr`X~kG#I+Vg`v5_i0yJ%q%t_^&cJMMVsDD}q+VG&_Q63SoO zT?PnU_T52qGl9ch*`kK=r&5W5N0gwAT$(j|4xXsO($l5#9~C*E@`&q;ZF1Sy?#ve# zHx(ED8Meu&F5p4Y4+3y%t(j7?jH}OK&suU!8QmQA{7F}9a?P%nbX#^;2H5|ep(a(FYqDrmJ(q%@R5fnTQ} zCB^G=GJgB3hFk93?$7B~giZ}bawx8Kc1-p5aA857^U*@c@bOmZynGtZNnbDJ`OXqE zaLP{~wWikN$Gy3~DHpoC(-}qV_R_y$UmH&MD4G5dLO@B^^w#@!6%=FhRfz!+wehm0 z)MCreskZ~Kt~j$6=PMkjFefJ2jKt6P=X+=S`X(niLASEQFEFlptkr|Nu{RB;cP^5W zK#frEdYPfgl!UYB;g;7cUVZfLWI{p6_D|JjD);AyAGeqNf2?FJ1Rz#-SDlye7cVGq z+CW1a)X!|Emg%zT>~8SiI5Dg%5>8?x)~CMtELIhKpmBlcCXaqX)i#k4nmDM~BHx&jX#C+wMXLaAbP8O+R-( z*LA0tVJykcrAZg4i*h{JE!9XzAP0rdw%EE^*QEV=PR;#UWH!%kS-_i)LCkR#Udfxu zn)Mc9Qpat(<@0dl0>v&j5S*`&k|97Qi!(MsQ|=CNdoE*@Obo8RR8G5EX!qJi-V^tm z1Bc2C3IJD(1L=Qz6h|r(-FN+p3Q$ zU^7{RLcJ&hf#GwV7OBlQ+xlmC1^>`#%#-W2y>`0>)?)+$RZA<``9)WANA^w#3__`` z1C_sMJ0unT@R&G$EXvV!X8#|5F`1sh;Bfx;$n2lP$+0_5Yg5zD)`p_Uu?0S9tiKBl zY*;k#1!8P_1KyCtaPe3+xQ!+Or=7L?o2Eg!$1VpyZ3~#_cAO5!XB|{bf#gCXN_!+z z;FE&a%>F0$zCllbhI1dv;C}ddGz3(Z@#*nuj)9_Id?7fW?fgvqyzA;N!jH@sidGYb zNOu9n=<<2|AawiU5a>+UawRIwq=6x=0ESRP`AY{k<;kgn~7GZaY z^XyeBiO9p*>L@SQd$2d$oi#n2a~Zd>*KKm<6?pT8mX;fkl)zn9eJVjbNSyP2DxDv0 z{lTNzp|eToLA%L8rDK8w^8LNVLIA}r@SG7bU1evNAAjrcH$Xu>TB=S=7g31fwmo%=Dr)#ftah&Eq_9|_e?~t%AUHLZA``ZMfh^_hzjFe&U2a`2{-C2> z&GWNkYIa!!P@>Qk(hTrwJyBZk0nbl{kg+Puwq$q+t1gH(+C9 zJy&Dv?(dyn!G8D2#zyCIn3D_XOuNCIyGt+@DgCwX6qow4_Z>lg>#n}f?)V>v^&aKN%?ig$CF_Ooc2T(eQ5=SON&uo=xN_XArQV&98nCV&cC;rd&|PY!b2AzK6UY3?lYjJaP4HI zGv64$bD#WL-j0ZY_=A0%OG|4z;?&8VFxLR5Pas!3@^^oKXjqgJ3|43mM)Zgkn-aoj zc=Zn9kl*m()3+VsFhNyNrBjF1*lyYx*?66tnuwvLiJl`6IgxR6Q}^rG&w+GFiVYt+ zzMbjietZw%dO6nQQj(SRiH|7~>jGafD={o=Vo=M;VjQ;YC8N={AmgWmXjOF9pF!E3 z8F%CLkXPNjxxQ+45?QnHf9!D76o{nz|&d)IWr8t(hnsOAFPEJi^=t;zc zM&Jpe6hbVwZ5|_n;{*nqIbSJm^s>l;u9v-?0LJ4t?xS@Pf7B`*%tixYLJ0NkZ9C;` zPCPz(n$W{M*m|%bDgM!25KxYZrF&#&YjJ+zGMXGKf!j^}KtHvRZzd#-t+HmoUJamB zo}xeUJOQg z+*8RbEJP9T`G|}q|32r}a|+xYStay|j=s%ORX1V)=`=Em{afLusb<_YKj*T?#*oI3 z_}JMi|ID4h{^7y}iVlp_mCpS_U)Y1L| z{IkORx4}b}D8~ z1io%-X*ok?wp(uY6vNy&b- zCnB}|_I9GzX`4dOapQQCaR!4yJT7tl!^m47<~x4==AsmWRB8OL*fbC(P>`|qtPQ<2Zi3k0cY$##C=_;qq0v)T+!_pm^ zx_Uy~riJ;ZnHIU&=1a<+UZMqI+*nC;Ig?2f6Px&igyF5iDyBhZ?K-%})sBO+b2yLM zZ;Q#%VD@#eHD!LGs!7X4ARqRa?EOtmEb;nSeR>WO8jAWX=h8^~eVx@bg>;OFAdN!G-)@cN-#_}7h7)kO`3F%A=+&%EEV{pvvYEzS zo^5%_*Q**(X4>d*E9E$kcRg!=s7$h%_&N?&wTPjCk@+?kCr~iPxX7n6FeF3Qx}=~^ z_={Nq)XX_=e|Bs-w%tKHcB{>x#|t7Q- zR{Zrb@Hy`WiqUHJYVjyy*4Da~mW74}SLsA$Ym*bob}en~Q6+{(C7?A5!t|n+(zWOp zYBR2r(I^|m;B*<-!gSu5p^~LHOL5+tOg8YjNlzaFoxOWy%d{L){77OhLuY$?dpYc= z&ftrQ;)CfzY!s{+-d#4^p*fduif!@K`(cgqscFi$?%{4Jw45F!=l6) zY60s9x;PInpYq)Nukp3Qz~}UOi~a4?)OEY&6(ako+_qA4E9UwFn6kt-;a6=!HVE0d z(abO+m!k!TgCr&Ew}IzZC$YPf!yArnM8lbETQ1aedjbAUq>auFcl^}F>fo%K+qISY!vJ{iG?aczw`FIp!5RoRXXza%#k)rQqs%rLpWeozu8%b|#qpoJK&vuqU|Mh_8!9UGkIF^GpV zD^=@E;^glx)usxhZj>2a7%||i!R&PS9Tzy=H~1<%!kk?QEF8I`Q)2LOZ$p1Q;!KN< zZgV=bgbGm4QFl0?yZv<)s51_RSKZt2O9Zsj6R%|b{(aVq4Y71{n<^tEr`LgWO{OVx zCX`p}m+IdKt6)w)`p^p~CeNRFO6^~s+11}K3#rW>p4#U_&dO-He>$xFT>=afkLk1Q zQDOJ*MP$LzmCtmt{KXu6@bFZ2#lq!2Lii=+Rn*n_n12;{9_7JWNBIA@7Xb0{WYyFC zyMfrO20fZw%x9-4fbMca7+75FSQSWm#pOjrpwIr(VRPJ~4ik%{&2trwRxRjpzwssK zu&NT-ZSygYsVQMb?Y`^urQXemA$sq@ax4AL=2_3sZMv@IXeC1ivpwf)aoM}6vs~t-VvZ11vR-Dv9cHnONU6@^xQ5buI&W6FiQ-q(TxKJ|;IT<-S5@tKqR35~LEMco;4nW)9%(ytM> zfnnpRS@3PN3A3_WGh!3S_&m^N`rHfnD!rm-3(@lwez=#_vfr!?H8eI-GgG|$rlKLh zcYTHdS2K9g&&p3*D2{1nkcsN)AZQr2B`4O+8#;ab+zZdtI?pn?H6|xFmtN`(B#28S zY8F2)_rL}#aeX2yqoib@8O9b4<*v?o{|Jkvw`*42G%dOmCBp29y3i#lmuXK8&0f(?G@YdwlytOm~iHj$w|G-9i&si*iEkt$98B zeF`=pRJNqaM~cqtxq0OIt+QQ1-5+COV$D~8cZ5TT$$T|A2lWCg=f5rpT*OwVmQ%TY zYd=fdY33%^)D$3%Mw71W%;n=zgPBs|gnUCx6f0z2-l21n^Sr}9IL^pGhb*nl?S{+b zAd2F>pFTZ3J*$3;?p16Nl$w;cz^u#vChSnQ72!fHgzD@$^WuY{;hQ^%Bb*L}XK%V( z;n_`_m&ZiXPSVS|6n~Awxre)(sKI+Me`9B;sHw>7sx#NHTgq8}DUNU3KG*0Z!AIh< z=PDsa__nZoxk0KOl9Mha$q8l4X`sP0X!SPg-;e@U>Z2t>IZO}II_F(>Df`w*Qzi-_ zch8HoqVG!X?tCbVwu@~8Q^8&}GB9c_a-7wg{lbbo<4#62LXjzWDFZCb(zw%Q^Pd*`so6PHEVT^2M@u4q$i+Vy zv6bTGZG>0Q;-gCw&KiNy%D`d(Aak6Pa4yt48EwqJ3uVQicDbgYa}o9w8%znsEJDy=b|8;H;t@YIT=rRH6(Pl# zw}XQN`(0r_C%ZJZ1*AJLg?Mbjam6gt8*%rGlD=Gt9ofVPBzII`qiU4tHhSMR#m4U7 zY!(JC%LL2sTt`Ij*##yTb}5*I7d1vCb_PYo4Dvz;cUar6!pZAuE2ASnO8yQPF!T9x zdPWmY*}K)Oh)Q7cq%AD6;2fLmEkn^#n^&_>!h_dg59YIGowt1n~U|*bFZa2x#TS}o=Mru(03WwWqOk~acGGX z_Wc#W$+u!eMEA2pI3HKg6VAiUU2iHk=CZkBo*AF)wLiQ#IjOpvOqUo&iE&&WR@ZTvKU)x$KC^!sknGS$gVn9fz8me}m`gei>}+rD zTq2?r6xpj6#Jz;lFS`Ryj7Z;Gnp+v|?`nmemhD~|fpRvkwD8?-d}d}ZmLw5I?gA=w z1&mo)4G0F5vtCsKJANXK0BPW~i<6K8+>I#@B``R52;Shx?Ms5Vmf9;2)*9pg9Pp zB-GT>K6PWz8`sT{_IfYV^wCh}OI6|eQF^nAP?E^a`s=kVz2*x89~NSE>nHhr`KgtP zZyW#0$1w^T7Sp3#D;@OD%IDE4WOUu*W(xV7%!J@Er*KoU>B4g*U+eTyN2jXT_w}{D z-SB3OOC#DX(0rTvdzeMNUB25WBQ5RCwewp28aCZ7nNFa7m64%X;GZ+}^6Qu(7!i5E zujBNEm}L4s`s=ly(6%+TN5Y#j(s3%U+M($PkQrdW%Ea*cs@re*;p$|dtAWS;XxqNg z`k5!cM{wfE1{_mF$v4`lkV2@2pO)4NL=Nao-NnA7ELo!=)6+%Hfcy)yh-kFmGRJFA zh-7(Bw;+6DvAxm-6B74mWa_`IwyH=v1BJH0+76wiNl$B zpS~PDcf zOaVb_?O+-{A=s7WU5PnLpC0g1wtHSzeTf>`kitWSAN$;Ph48MC9Hnv-#AiUEjxj7; zqRS-cDKnb$qwZjOdI#^D=u;__DsOkl4Fams)lYT`jm~juQ&}Q+?;q~?4F{7NGL z+2-rc1IzYxy}7NaErJh@sWYxSV4*}1U%|n>A92VGp*clRiE)54-H-h1*YG$_Dr;NY zBc+xkL{T&q{eoEEu=rXJ5Avo7xsJAW#UTM6`o#ER0Iqj9-B)V#K;=3&fc*ePhV(O0 zu8AbuA92!oFFEpYXSI>@Ou};Zu%-H>*v2Yz3fP`p$7uV+RawsFpm@B#2gVm?kJg8k zj(#7RKB%j@=EmUia%Kzy{>Wj;2>%4Zd}ENm^Lp&MWD&Y z;OTKczpnl$xBTo0iCJ>E@T=>`<0ocF!V(J&?%uzPq`1rVC8gx8|CBkoYSQJuJzi>( zW`zcEkvBJbxK^cSw_$pa6X7l9B^P}e(XQS9>*_v|*{{x-_4e|&17(_^w=bqR*LQ^I8#aX-4dT1zR zh06^Kpz9T^_$4=*>_gK#MV~vW-(Zl5C?WTMiwcQEcR$au={<^buCJ^uZ0MnI;3xNa z(4W-67h{83-$otMs2|zN@I&p1_$xe0%poBs4wzu#5EAKJyz0G=$anrDT{edE*J}1+ z>Cg(`(#t3^&OJ#od6*EbuJ+sF|E9af*^32_q?ZaqkauE&1fE=9gY>79|H71LOt&)h zU7$8N@m;E$3w=e?Sq_q;GJI43C7jz1+QxE<6v*U*S2HkT3yXZyGsy}Y`*(p6_M66L z5<%^t7zRUbviI3p7@uTMBCYcoyIaCA45}R+3T?f5g#m#rAFC>KD9!x06 z%F59-c1#VJeoVH>Nl>ImK1c-+FEb1Gxy-Y%-U3ro9WL9fg<7%pJ3fnkG-{amAp-;T zZfeRY9PZgeP%AzO?_}flzGG!(Px;a>8d41W%{4U`VkQLdzf=i;g2d+YX9}W1`KBXF)Me&Z#o8(%S8xsi>q)DN}fj_!c?2ZFDnJV<(i;1QM?7&c6dazSZWj&(6;_ zc(}6@2L}uWYr

aS1EnvYZ+1?1V6v!i#hyC7+6!?b7gjFTA9c2{XXf(%_5$Qz z8yEcjHzN=@=36>|`O>hhxl_l5dJ~aD9J}7};WIvHQIL@hes0j$&j<=a1$E2o#j`++ zP;=P~&gi=pe8SHqG9SgoD{(?Ajl{t?0Rd_#Y=~!ZUGGj}l%O||#@E@+r^UqZ0c9V1 ziC#5yaoWIZwnl0l`G&5Swb0v3$=4Kg0D%*^ADIkDH;QW&F>kd|8&w>vz5!DVN+yWXYV|hEL3=f-_+d5 zfs{o>XO&Ww0;ncpZubC3iFG_*;OQ+r0y`QzUN86dB!@e8`uN1lYAQdzfuH!7`#&ea z0g@7z+tH(z@i~r!2B-C^wl5uB7L;1^GazdqgWn~f^vP>Wtk}-*RQA|%JJ%R@ff-fHaN8?*Cn>J)x7nr|j=D{t*Q*+=E^PfGT6Ji;P% zis|^x(ey=MCqLRHKKz1u@mufpgoIA*A*b1rA7~YO@pFq41wE67z}P7kn#Z!8SQ)K= zOX9V4D+S+sKq1dp>HJDX6RgI_9UYfszc+IqBN6YI8bBS2N-3n9!-1b7M@QHA0|0tb zGM0$sDbLG}0el9n7wb@3qn>i=wnaCz-FbH@s=WMxg~4+Qg?lTt#wnTm1QoqvCVKw9w% z<+Zd3YreZ4&YfeM$=Xf*pog%GpY;_fQPF>^bXYf~W-JWWaVIn(HLwHDAQPwitBHph z$8!ACnHG!A*j|MwN};|uvA%6#^b{tYlM&dm=VGZ0k^9U0>urPlLgkhMO1h?qU=EFP1gpRsQD?Wr9n$8 z8WLY_7G$xUSVN&d!5(^Ve>H$cK%N zu0i2g9}Ljq6bt{-F8U4=9!s9z1U@$#N$cC*2mHg?28-vZ`z-6H@g<;N^^R3`V#(+4 zUseYplDM)|flYs2C)4*oLFaL3j5ST7}HTCKpCUi(Cx+G1tEY_E7Um=*lVFhWd^(BA$G0;~75r>7SM{epn>A3(oEsN+bHNkHTjSGW*rD*3 z01%Z}u79}L>@$4?k+slo^OS1dn8^8l@?AK=_l{7^)4*@kf^tXha4?5Hm~0H5Kk12Q2zs6Gzt>bZ~ zSz7G3=vwhhKiX2`Cod+HSe4&{Mmr$1r^hFYussHeejqQe%46QWfD?Y(WO66uNK4(* zd$ic5KRhf+UPG$EIx1tDp*%vav+?l_2JZ!flyK2vZKPt(BV(>@{2as2XtPN4haq_+W{@^BK&DFX|C&g-JrZvb`_-& zptaQnNW0L5I!RFXi38w@2U=k<=nkLmlo?~_h>4lj;j9ZvhDXh|tuCiommbwB@%Dc; z?83B;Py0BceK;4^MCFF_mt8a5D|~hTOahBi{@SfjKAnQskca;a_*-{hSBr&h;jg^p zhGIZEf#oalF}gI@zv;$iO{+V>Os>Qw&_4zpAqexcrO)ZO2SHW zf*zcN-j=5$ZAyQ4PcH8$Rdy?)=cbCg*!B`Xtww@5C zgTt$5c=^pdaaSjWnGty@X@h-gwMh6l8=C{1L@7A2!mLouB-Xr-#24zGLAFFXsDy3*n2IN!l`Z(AIxlbBw2l3-ih)3+w{2%gqI)X4S}Is2ENJ zXiIWQDk#MdkxtLg%#aBda&(Yw=)467u_jfmeZEhcCz2gYB*Vb;qlr4?N;WRPRhtKnB$3GZ*IGxwz)2A7q zpIH_Y1sLS)ACD}f$oZp_<3&`2uHn_??f8NCM7MuGq;i$&0ClWA?9ys1l;R!Kc6sgS zNDwFrrSfC+Tiqi``BP9WJ^l<2_FFG!R}Wfxd6K}7{=`CK3>lZbn^$g7&Vc^CbNz%x zw*eoYjMMf&Q%REgojtHr*C5Z=+i!b9PEmoK6{p( zPv`kzETUWH9=J|l23o(&Pq=UO+^@5N*)7L$W;3jJVi#WSEDcqwmkQpY()sAWx361C z-QzGxOihR-PaYKJO-j)*Gf_9ry#mf43;JjAEzi`7yy}(Y!=&wnO>#b-o~+Z#X9{)- zqey{*?PR1qCDQhr$Uo!~Cl8-`9F@{Ur~e_s`8=gIyh*>s)9o~luRCn=1HZL)PT^0$ zv1LEETie`hbiA%wna)W$1bRSc$i&an&4+(pv@DgXs;ce5JJ-E9DRpXuTv+&5&l@h! z`Qdc-(hwD{5ucKe5I&I2B|ZkgQMxU3=n-CLhn`T>(5I87I<$L>&7|CRW~Tjir7PEG zNa)c!C+8y>{0bwY7Z(>IA{`GuG!s%&VOM`~xF|Ps6$Am3CB~M09V`Uxx5pA-?F1M# zn|WA*E|9}6G*lX~k8?y)@XMDB5pASHJZvwQyz1@3zBiKdI$FbblmMwGo&_&GCPsQJ z0(dZ-wpm-;iC^NLru<@p8+Jt{-du!)dbD_+gdV;U51C0)o+Bt2MeVi)a!CEXGfP{; zAd*K|e5{Qj@nfgdzPNN9kFt#TV0%}9A2T-MCNFPmZG8|+tDyel_IN0Dd|$&VWJKu3 z84#=3skrTCo6Ju#!}}K&bbTJqHGpd7@t0RDT!I@ezvo;?H`KfT`GNkZc|XG%l}A8n z5b8Z9+Hd&4xgUryQBs{bWv1`tdVFa{fa`V^OuqS}|L7#JjNuQ~!8QRd7F3JToy zBi(_p2)J@tHZ)0n_aC_se3A)oLk^~AcDdBBw zKYK#p^sZ5tBC#YfK3N+)D+(|H3cLK|!tDXig5*n>>njG86|0*>`VKr^tEaBAVypJR zZo=S$r}h;DrTzCMtTr$(G_`)hn-(fr=tLLUQAnVEPxJl7|8+5Gzow!R10~@w(i3Ko zE?{Por~3bKIn9aC4Gv0fo?$i!Kv)tbz67u{$ta<*L=CL(pI}z8D+h*Z{oe!s|0&m7A!;lT>$vnUovV_q>!I5?5vBoT zW6L?jPu5_5YoxeCDsz0bj}Mjm?&4&X+u{cO*w17XOlY44KHkN94ZB0f((YjM!Ip}Q zNt<}fw3$W6=YJ50@h3Is*d;KwZ7`2px^wN=hT0%typhB&CYE3!tl?lubxx#&f!b~$ zzIu2KP2IyWBTk4E|L#b2GVo;EM+cO2TT_u3_-4k&*5=kRFDVC*d~L^7|BuZqYaGm( zZa6RaVW;nI>Kr(HK2YVRYOofyp%QUNX=f}IMp44QtF)^%567Iq?DR2R5u<(GwHXTQ zI@2r#h56%>mnpSY)1^KaU9VA*YEZ#xK~O%o?PQ6q#MO|a%taPByN+b3$b6&snpdpJ zJ`*q>DuCe`Omki_ayX6DxnP2r;mMl7K@4&69y@(B04*^v^&B18hR1-kh?A+l#-4A{ zcp&Ud-fp=C>tGD_?SC3C$VhXKKvuh4w7jc^s)d={4?HAJOc4bnq)qMmP{mXFLOgf(@&qwk9TA}-oS*8 zmm5XO??3ctYHE$?h_rpWnxW?e^im(=2P{yFF8+*XMSAl;?wg^n>+UxE=S7=p zy}xI81eoBryHw8{y1$$3?vpz2Lw@(=qed0NE~ox@)R&F_2BFfaH}&9hHfVR`db>jq zk$r6KX+0P#0VB5YLqaS1s?x&35;Mg>Z-gO=c*SV7;{S*qom7gRSuKR>nMscRx5wF- zNWRKivon9JM(NmI>z`gtg-WOXBK2B2r(!TL>4~) z^^!X^uw;PbbGXHUnYsDK&JMf%Ohewr6O!ewk%zrN4r>zY9tnfH%S*dZsL_0okHB;V`AYh7ZF-A0!kKD6Sb+`amvv zc(^Q|%kg6Awl6p$X@$edR#qzfA2*i@q0V3%%E*2z2l7Xt=}i+vK39*-|F;(~+BNyZ zto=3qN=2=cec7t_$$^9T0JtlJTgs`T%M5l#&x@1O(SPocS57HQu+@)LNpN9*Rhm^4 zwpK%(L#h~v&LY$$h=lZ!=Po#$ug30XR0(f3oT9DGJGWp+gZU=ikQmMd+<5oONmR_# zf#d*N{!HwRIagg$kS;4S1@Jg6P7eMWUizYxT%yCw;S>e}Uu%xXC*p#bO8@2kuuoWw zPq{q2rRkAN;({$lMjWs_+6mZJp!gjQh;goGe3Am+DB$c_AOeYUzRJyB2VLq0*y)WG~&xH3lSK9%>g z9m>v`nOT0mCSY}yF&*6|4Nv9MamN8F3h2ymhtl%sD+cx!jrMFE?yujAocE@<`S{kZ z*ZttD>o>3^v4Fz7w;V`+9z$ZU>JLA%VI`l8DMi0;-FqV7kz@C1Wo<9|)sY5Bl7#5F zUu-+_g8PX1+BPK#Zc@^VUUSMPJV=~@HV?||gQ`#DepEN%608X`MuJzSJ=ot<2GWd~ z_to)92UeBE*)~Zjg<9svW?g2ALP`=Iq7z~qLGG~#746I{Z;R^%9V3Uzw@lV*t}%|7 ztL}WCH8ysI$GnZRI#Y!62@$2O@e$1Y3r!#j8T*Lj<;xb=`#kkS_qdRuNJ>8U(=U3d zr`@T!JYh1J*iJGsLJxKDsmaN-h@^_eAMcR(On#2C;Xi8eybl<>aw^2S+~K&IDE9$F z^9T%VJoJy<7rAeiz+NT;6(mS|l5#Is-Dd(B;tW4uYZ!)){UuHAdoa)*WeW82G@s=w z!IQ#xs|nN zgOwe8+&B{W9&hVQhuH|ebI;gV+>J67$3k$U8p0=7R)nrcM1^G&nW&JGkff&SfC#wu zghH66xqYSxRi!pAvRUyZ|8$4D{!sO+V>19w8e)cA?<{MI8IYz z<21p#+Ksn}J#t7zEcp?)v8ttGrLxST2eyC@;;Y2aoW9H6@mv*jGQi^5i9<|`qFUqt z5*;@JxUesN^wtDTStF5VV`D7%xn$5Pz;<~?p zMN5EBTa_U2*NgBX7he+O;3%)#zw+KExMU}lIa|CKm=ScE(Mc~U$*#tmGwhsIje&$S zX-9IinayQuySTWo7mJ9K?vu6k8i-dN+W&NMI3WQe<+Sd>s<)lK?hl;6@MXz;1>()X z^(IfrI3-dAP6s1Am4d&`!1rRe^xZ2RfqA^xM3YKvkQY_Ar=OJ9dRn{3f=QBH)T*Cc zyeXTNa7w-T^`TEA2s3e0{T0M)ERf}lt$W{tGAu-GMNX|rYFAl<(WRBGN%DL>=qq;7 z(ck;SJG7|mE;Vocr4ESBEX8Mu@Z`NcrLb{T)(oL+4C7=m5PY|~vXd4c?*>Amot~HU zc;5q?BnX=)x#AM^HfH6=HsROO)}o~*WJIj)=`*r$rBvnNt{Ilv9GHh!m)x|@Pg5)4 zhCDkF$k^cnK4gyf?~VAYP(}%WsJ3K#A_=tzI5uBwJX~}VfmAgh5gSVaN~&EuZR!51 z?&lU+BU!5eaP59_5FJ}RpCrYxGf#BKVmV##2mjtkqkDS2et*HKw?K`P4EO50T(Iq$ z3;I$EmT?Vg@8o!Nv3Xer=7-0&9E!VJUOYj?<}nzMU9_ZM(J}g}GdaR*OPT>`Ya^qN zPx!N49kNyFK5nag3#HiYzeMU)Y7#^37uJjn3lB58TN?Z5`+*lL_ubFeOh(EYoA?FJ zl|wnU71Ey2*E&x`*pFHxuH&EH6}gi=S=;MpK;_Bf>NTIpR@YCHOD_?$mu%6SI!~it=nLXY_BV?qI-Odss`F1RCF)I3*v0|Of4<2Gf zn*Klle0A;A(A{65!f*s^8rBa?;L@2sg$sJ!zQVlR;h&kA*#IXU{{1K5g)f${UkUp$ z7%u299=Gt1Xk1mzPK=}&F$cg3FII}w`*BtjHMNS3$xnYpKQ1l=H_p&FRVioVEds#yZ)q(!Lz zQyl->L4fae1jNKpg*~!R{=v-T02X0VV`ll|iSmy?f7jorNTW|KjzT9L|0r)>t;1e` zx;!R3RTJs(i^OLnMFQLrZTWCi3r6@bLzE1vxXEiS%dfurR(jz;F2VXt*05{!)ne0##4Hm(CUbkh)~FcheevYGYPCew~l+ z-a>Qmf*?_UyLtga`KG$q&7Kz}HSP#qG(Uf(Rl{QTApSm(RcE{Usiu|abq?wG%xLL` z#6AHEx&=N7_ZKFbNJzr5FIIeG0??PFDM|rvVJ(_LW%_8wK zs~A7qInz?PQ}l^2csaRX8S$0=MQ_3&vC8iH3Y9sFPASxz4au0`rtE3{_oA#I@Clj| zg5q0I>l?-sB-e*mPoE+o#iN1}xkx~5o`wRH43DK8Xal&iRBiQc_ahJgAHv=;DvmbT z8YOrjxO)ih?(PnQJA~lwt|7rKcyJip3GN!4;O_43&TZav&yTy-{nppNvdHxGbobNM zRl9cWZMtdGqZUMmfeq061pycCULaH|n%d?9U$9 z2EFgK49{a{1TP3-e7lk&y01Q)Ahb|oe(C#=ulgD#`4?Llt@%~`qe^JtXd@I9L_qMn z>2e~pQavC##C8D6CZ`O%{|sEI&p>vPjlRKyEqwQfMrE2l;IdO4@Ul(~019$7;<_vo zG~5qGv~i!@QfjGLS4EImZ?qk-fSca?uH8Y4D77LC<8%*E8xXF*0F19Vf7}R%ga`-^ z`cL4b2zuuIX%kNS@7PjWx0ASp(@Gy8iri2sfvc?`8~`;GPBx`YA3&Q|qQ_?Xnf}pV zLf_e=0G666VXMe0@fJg`%h!6i&CG!mI3x^kNZC#54Dy6jsR6;{R;s9<#rG_05pMTG z7b)CU{6pcDe6C4!?UDE27*ff%2NsYwNg!xN;|WtqLA;5oyWealx!PG7`jj)XNZ+O- z{4cl9a;ok#rS4qJ|?>^9fL6$Ihis~jC-Az-bY1W_~?_&5_v^!mu2 zQt?r>e*>NXQs7ywruC}bbl`T%lAiBRKD_u9zIt53kS-s9pgs#pME#=dM(BE`x9D1A z>L-u34ul(opFriK^z5SJwJTqzY}H2w{LO8my~LfuJ6yt*3cyIH0gUAFZwy&=o!U8| z$a3x{x|Iu}tz`o)K7Q~22-N_$!G%y3_!uTfXBYBkU}iCUQFYpk+z(QihaV8h44?$c z3HlGJZZ<7nFZ(pdhH5Rmpdhlz^VD+W$#FhC)ds+Fmtg3^aREEZ0=M%}^-^5LmRJLI z8mMxA-nFL9-^I)Kn;h#NbYvgJp&J9(%?x1tz=98~qXfh!@h?~D;r*Bl;Agc20WY$< zNT^P4L@powY;vwZWzpOQuLC+bO*CLRVggZKFMy6vh+FDfnxVh>ZSOLo*2W;^}nfyaWgrwDu<8zGMJT>-z_RZJ$439YQkw zq0-=0sM;4|6=z`gKRG3akoVh=zABO`wM26q0R%n>v}D1c^d3D1bmjZco$^tO5qcK)UnyYYq zeIG|RwcPDA^4Aiu$c(7>KKl@vgvajl$rpNXNoHzSTFG{*z2--}+?d}FfR)wcJ?-S+ zayp+6_io4qr8Pr-RA)lT=9Kt>7FsIuC0xdTV*cWj4jlwhAtFvy72yQJ_vxzik@D`C zG35ty)j|1^Qz|?BPViT8B+Z!P%S zk3-nZjOh#*uD?BYuo-D#ql#wMkpzSYSKeA0DmuI_M*dDm3Zk{yPEViB=lHrFat(}( ze659mB<#%20fC4hC1*0p!%5jxyh@gbh0XHhQN<_nE{_YrGCGM?v{Ra}2W={C_6^zL z_pMexCH^XeMl_VIr*0?IO9WH{Y&wtqqp$cD^Yde~qo>a)7?Y}oz(^FMJOwMb)vgj~ z2w1V`Bc!o8D?+K5n4PzZ+XHk;;Vc18fI0*IZOpGm9IB>v9Inrkdx19Tb)GxVrK z-cC=B`X~zk{S|#)e}V8R%KyazD5VV4U~2pGuL^M;KSrOV%@(n~-IU@$W(u0sLdEo3 z@6K60&DK?%a%AXRdCb{5IINA%byZ46Wuc?J7HmI@R2l$GpU?^9Bn{5CCzOKm$Jg{WdW*z1`Log8X;VE%sp`%3NZ^fcy>;@lipw$E1?&tloT>Vb3H-P_@^T5PrIP=$98 zuKN=I5V`qZ;;v6#Ku)WvQks|%!E^fYs{4B_ovob$;mq8kyDWf zH1e^L<;pnwi;0oBWdbhdyC!?`zj^7nxcaB3kI!HAKUtJ2ZxuRl>j4Tkj`8N>bs{o$ zjsl_b;r_q`OIC*YK&WFMCFODA*5U3%p0=ETfB=FS(R^D}u}WrL+kyR**(<;#3Jd>K zxDguK5S3Q@t35!s)@pC5QQJ(C_bI6y;+v&uo72HrWYR*Ntr*l=AK=br=05qZ^${b8 za=KEyacK9|Vm@~af;h(fY&|_W4Fl&Yx){idg~4>D)(!71e2`oo2&^k-AY=Tv)@bQGI0DQe+dPKfY;U}1GG)$OUDVap zWpN%&tt@fP6m9e+f$z@C^b75v+&FuA9x@b!vI(H z3;|C-JTu?+iBXd&!yoTl&8!}(rC~~KP}kae30c3iy-u!q8=u|A#`;Kj0tv9{GW(~i z{Mq;}8tF$!OuPTt?XbC$)0LBDJpkhh(5(`O$m?dR2WE!N`$LLMTq*ZRD$oG0J6$cE zwN|$jQ8^{zmE3WYkWm3eVuAIkA{jvZ_Novih4#O6CE>d~0bwX2kgs^+5)c#ucyA}L zT<2+NBZ~~eioy|)`28<`8N>mC51CX`gg^X`ZaMS>R8-;sz&0g0i^_z17h8j7FpT|m zx!CxaKdve?u?~sM3gZtC3^(<~!BWw}D8tc&L*%TJYbhQHK~HG&6>dgSDn5a}8hdBg zAVqUkYMfh%_He`())C&#td%^{K*o1Drdyq>o6|)#Q)V7FOXOJV!k6{JaGz*(aX|`gd^mPj&Udbln^<_R5$c^lvmiHFY^pdstH8 zB@ahlLD8%>7_9P#WkUJxY`M0!w%PM#sK`+j4E|ziVj-RVCoBc@`D+O#Bq7L``taog z&wqlPI?_rpOsE2dA76AExL7%x9B;lvv5#|Q6F&L!HJD)5D`YnhgjQW&*J771RX(>A zVL_#!6GnnO4(~V4t3d68D7PMFgXK5diwJAHaVYEzn37Am{{QTy$>8*;oqMg#u!uPF^II& z=MO%-e1L;CZ2PC|@w}WIWrj*#(H!Bm&d*eQX~n#Hu-W*rVXR<*|DtfY`qUKN7yd|e zWx%}4pvfbNDCHd#8DD+B7g!`Nwg!{_8Vc@x2^w~< zw2XK$z}3CV-RW??jtXjVXmoXBcSi1%!}@{U?r>Xg>*VNH9bIyDVH`u8v7OOEvu$DP z@-p!J>S*cUDo8jTwOVZL=p-)9Vg&d zoF0ElP&}Be@i#Dt);#e7b_5z-C$ptY$^CV8HTKJ8Fsk`+=v!I*%~DVem4<-HUSR*! z(RhA85QUoWh_eedL86=bYUD%@$7S+B%SK~%R!l0Y!{=fosD|nZ1z&^(x!@4U<%C@W z%HTZK)PB6i{W6DFuMkJ*S9wptJRmr*FVEiB{Qygtu;GRaG%3OxnC8{xOa{d z%rOPQ!c|-23R-5UuS3CyxnZTl_OmOO91V6UwJB`a;)-fYJd7d43mBP=m-Uu_3&`#>e&6?ayQuE zha_MWY*qr_KV7}~L-#+Bs^plgZR|gks=(#T2eOsipW$pZCXaamR%K;{S`RyU7PhQ% zgChm2ymjHANSfuPk`?p181sjzUvY`0=%*ra>PT1~(Z6Z=V7k#+&cTx{mym>}_oS-A z{}$)uUduNHiPpq+XPs0&)Fa#3{~%#Al^T1K3H$R&xOak!d1P?(TXt%7ZMEu|T`o|= z09677Io}MKOk5&OEDDf;G(%1xfuP=-wAse8eN6NFl3~aDB@6HEV~8Sj3lkvi{%><3 ze&5-m4uR=&0Q!IFK8WGHG6nK~GkjZ-ca84p_p%pa|99L;*rXx8HbszXiAW{;``$DE z9YFo#$FDciix`Q2IULB}_gr-b&?Ply&fZ>OQ)Q=GI{seW*Jx?eTWrhA%g%{-#Jem& z;qLd*^1-h~Bm7QHK(N>46Yxt9RTJ82t(y@K*y}89H$_f)%4^%oZ#6NcA}5d&JE$lc zsPTU6!zOepqZd*;GJNc1Ixq!}=JK&Q?7vzBIB-S?I&i`zqS^%SzYe1O{S{`xr*N&J z{QXG$1gOb|Z1mm}nQSkh%7zg3m9`38Owi-Ef*WT4qX+?kwXwn!Z7yW#Ut@RI9j-h_tOuket(6yq=8+&?%WNA|5sE2d&95%{a+>0 zQL&H69w-Scy80@g*W3O@;~)f`iCx@>(>{I=BmixIgkWkR7)60%QQ*B;otJV@S8-=A z9I;7!zBed{a?CULk0Cj)r3%Y8fX=zpU7&Uq$ZaDa<| z_FzBIW&WWHWKO zWpUJOjYV^?>t2hVF{tMij5bszv&6I(RhlIpLVkJAj~AKWWbf0LT-m~bgsIisM2a2R zVzo)_q*P4&H}hlE!=Uo}kc6r2Bk{RDDQ*nq8RMmQW6|PjMRM37*k~CJUkV za(6OozFk~yU|bCgey8#?It=r62RYjyK`|dD$)k#3(`5~8vz$*LR)&`cN%6fY6OYOS zQVzd+7$7LiZQ8#N-}FS!A-SN`I+B|3Movpv^D(E|5G^|W0a4@2c4!u}oSn{vG^w3J zb~`Z4Qub_~vR$70qv!jJH{c1}%}X8vUIs4xqAlAJg>IV)Ln);0L5&!#iY{6kaa;qn0#dJc50%=Uyn)$@(rkZ z6BC%sAOsCiBUCmkh_}OjO|Vb0FP>5nP}78nD&ErHbKSR=eHcsU(UkdZJhti!7~=jS zHvaZ)vPrdHk%-U3%X3l}5fM@6BN90U#lLip2B6^$1$V3a=bPydSfsa{Gb#eC^tMrGnIwsez88>@dbmS5=HH{|r8H z*44>b$L?0c^ySvgHHl-b2V)Nhh+JQKTf$)Zm&tNBC^p1W67y@h@EEgxXHTy!!B@i- z$=4LT;=rWDPw z{KDsE-nd+qoqShmn8)Huz+pXJo1b5IQlA)6!^$SBy*zPneZWS*Wt(FZF7g~lEMRDm7t7~<3=Bb7@Lo_r&&V`C zBj>wdKQs(n2Ef$oyGdKBx0{^fy5E^wE7>_=r=r`M{6mC2dwcQ}nLw-r(6mCUZaREt z+s}$@6Hm_BZw0x;sBDUetLEj1UT1A2?{ZPEug5A%IsHC3XkqKz#ZLFNaH*m&shPCM^dczZse39 zhAVyT4`LPV3`Xr%OZ`&Q5@X$LmW2FHyq@ms{GFb@2idyNJ|Tb1Syk~XLdzs!&R2wkN~&{ z;D2~&xHb8r+tm&3p%GEWAq$mtv`bb~&FU&DU&u-@`t%$8evCXaFwn=V#--c^mukMu z7d2q6d|i`l_1rnsRzid#bkbhr{{360-S6VJF99(jsKJpAcKSUZ5azhfG~cC_)g%+q zBBqEj2vVb>sZ)a=qQu#6!h0Kr0x1$ z%d82u`MvJUnU6tw@UL$Cc#0a#Bh@pb;6NPTCv~Lj_k#aDt}D!%UH=k);NLzYV&0g<@kJ7 z0#O&SKwGVZSYB~%ZjgX9qvZ``!P%$R-c!>ksxT;nfQnA*a(3Xa{G?yu z?XWaPVjT-Bi@7mjUjh5CzJO*R9V_~ZPg_%7@vTKQQz4fOiik{&aKHm)lvvhE?X!Z< z8eJhn3~k+OwxhkAOup6cHHml}o7rdSQjrKwpTi-Z~b@F(9+O{4NH1b5wNjuUv zkup4mu^j={xn(4dvDg3VBCg^iZ>OjjmFfza6^>uy;82crcgg^DC>oHfLvt~r)}@rc zm|lhzw1c&)NvAE_B8aA0t{M#2cbC+SsO7mCPBVbWh*3x8jxXy=^KfbP#a6qGjM6D4 zY(%>03ceS&w^V(rsxM$z(7QL1xd6y_Y?r%yM4nAH^zEsqdJmnhN1WsoR81`{bCrgb zfV&|ux`v2=;787cFN$+mtZFWp%I6Kt$%KW4#a*!G(+*Vc&{gW8)xq*Q2)_ZcIW!2) z$G}*~)MFixN60HFp&)y<-H!>`ueHnduv@+~bMU#JnVLJhmZK416mHh{MG+C8qd%{e zaJ)R;0(P=MYL)?5`&Csnw78!E?~S}Xv0CZgHm;k@Ud!G4W~M-K^Fpalnm zc(M6uy_Cvr&VE)SW~{@1hdh_;^2(_)Yjv(*4}{>LrM2YEWz%%qpo9y;0g4wLF)i7Z z4+&`y5JcddJe#0~+3)1;LGj+wj_D$dkCO)ce%W#zq@O~bIPcY`mq7Rnt@?Ap3_4St z05h%)=_9}I*AQ@Ta;26G)1dAS1vl=6J{ZsdP^DCQzC5xA1{bf7D&KTl+{7Flm=!i1 zn_Ob?@$qYcQ#Z^K1e7Ctq@q*Uo_c!ypWW{F{}6E54V6iKMxBhP>^JUrIaucT!m6Si z@S__EP!WAQ8k49o^LN=47wKLa7_iv5yCNbY6EoPBq<>}k!iEuc|!242~{ z0ESU~FSjl08{x=K1u6OH(pE=Po@9J{p0h=uCMPH7h5N=ueyFOd$RQsk!TOyP_-K#! z3}kuRg@m-;U&%R=TWpMczz}@3>^bP{cTe`?U}544nGn>=T;0r=TiAek3tQ}Zv66pE zPcIWodp+bexx9NLiin9?exBpn==ur!F%5xMs}T235C_?_5AiWI-Z(?>amVp4w^NesgWS;e-ZHlDod z5|&yuTFr|d6`78Ee5*L@l6?0Kdt^IpGJ)OE#G31G>|f`{yG|DCQUGb5fPjD{z({S) z^GcA%W8Yzncm?*;zW?d6r_%dmNZkuC1!+DePBW5HvwM5AWWETedb;M{S<%%g{I-_( z=D?F45P(xSmY%}v%Gze(BP)9-mR}x4%_gHQl~M4V39; z40-mo$ci5!T7_tvCa~)chWrkpq5FZ3_7v*6M@-QiG)?9PZPH~wpJKiw54kTgB!n8A zv=GoxY~2IG|2Vb>a|lB2pH+(2|Xs*J+JE$><2BA1y zQo`?iQb#aF>Kb0cimq}9pM+CmK$wKjyOr22*13cqrSVwBj-vj=K`r?GSW5HggT58% zw#X4SXIFt_ciAiH=#KSt`81YO0%sOtCSqKjTu0np*0a;|7J%YyNH3Tp||H=%PVZQdAuFeh(_l{x-^8v8dqEz!r z)MjpM2?Rq(UL_|gvz1ERS=s(b$x347?)n#i;}LKNuxPSP#mgg|sNOT z%A@K7Jzb4^E&?Y9D1X{jUope=-2rFJbTT)MIwpJxuE0yZEQGOkkPNrPf{wC5{x`?z6Gpl0 zKdPj6sme|SBBVeYbzBE%e9@Q?JF+hkQa%mjCCad|L+be1{K0CHs^*UwmW^{hq^B@Y zDOwH0_t@_bDh!PyQ#}azWd5;)RGdW^NyE(!;N!gaRBD3)3nWUl27e;*!7BO5QkOY` zq7w?Mr|G5IY%D1_$Z#n%!IRy5k@(Z2zxpMLq)qRX2bF8Ylj7Kl+#nPM@}(>=CD1sW z(ZI(6(OVXliZB)i{pE&LP=)5t0}v3@rU^y^7C?_0WTrE3Eo|Do<=RX8cA?f7JXr#w zS*-QUk75a=i+KXX1bVfFFMC+}zR&!KpdgiL2~6$F*oj_CAMQe;M?B@BbkN}Ys+ACmWOZ5Hqa z0QqFu(YDstWnKix!r=$4yS4=?uc7gC`u@S-e18APyJeXg1;!RCQnG-A&h(S%hOX@? zk%o5vuk6e~QDl`w9&(s(d6iJeWEhaj)v^nM8hD9}WY=I704#hhk*0p>U}d7=krDNb za3-(88c}Um9irKRl-S0_q~6-mk1jBvP<4C)xJX065K=eb=U6c#^VA^%1^U^lfa5BU zYKumP?Dj#!)9dxE6EB2?8U@0fSrtq;wx z$z|cA1QOhzIhK}eZYQek@QTgA#0d^s)UjgrregMJN{qkjn10x#-a89%Uqt>F3y=#z z2!b(0b!!E^N>PUkusPiY6(1X@O{+aTf!%UN@l!RUZfO2}rS7*C&Ayz34}6aw?-{gm z62(7ivTf=x8u%UiZ07jd^{!^-kD%XD+G@OFUgVlq8R48X8K4cUrNM79+6ymglh?W3 z>dTO$#* zKwGaQ{U8s?oB-7-#&(e6eI!~nOLU+Gtak}Ca0@JEj&HOajE0}mMZbezOp2$nZD~G8 z2FZxSVk%c6DdtQYro=`D#va3{ULsTBB@n=4BP>+GhEO$!!-z%;yAU~F{{6rcy$?&d z9R?;46^I5^Rvk+S`a8h4+@Uo$$H4CP2u(kPCz_A&`^xJfNNI+u^s zR`-3$>23Jpoiy_h8VC8Cg2|Z25JPc-K%JAa305}U@jC*SiZ~Y#y8?mGiY2VISqYA{ zNsY_p$IfMk{;WnO0eZ;bM)=t+>tkaaub_UJP3>E)MuYhoS%%vCkU>5&tVdh)VDAVl z3z5F5v>{qNEKhl)E;Yldd0o9j(@0;w&&z;KU z36od(n>*8f*Xu>~9~ScS%r!hkawg-R!P9l!^iGn#ratX-vL4C4xmJ`q^Ot+3tEz5B z1>FwZ@&~;}e}j@F3S~NDeGGDrdzjK-w^=1xs7#=UMWfY5!Q>Unzti+>WR;`8fpnz=3wYz^K|&H@)qeHNtfKooqeA-g5SvWnEVo9k_)jl_FXvtY?ym-s z9PVBz&y{rWs2ie60O25i+(T0k3>)rD-8X(_P7N-ZEo)qTy2bSS)zw=j0M?ljf#|@%&L#@ccMovQgS9 zs#EV(jU!TUc~3XU{sju@n)QUQ8rRcU?XfH_2}TKg5L&zU^gyYi%EVV16Fe8mILj* z6D==~Qz7kY`Xx+a=`qvKz8{3WYbAGdnzL-WQa6p)g@CB zC6qAck-f`{;|m6wUJ<%+0pOtk=ZkR3AXbz83F7?`PwV zq5<}?n#rGYWAD+=c|eoFjfa6)*}E)K|;lWcCgqoq0dgtg+IcT||rp8g!g zl5ZzR&6ZY`4IN7kGibV(&`iHDtp52_T!R0zfcK6D3iSGb*R_G@d&ADNsIEGoI@0orF8F(^jhS*ZgOY<(~Wjmi#RaoAq>kUyY%W8-&V9zsUH zPo$dO`_JeH)+g0*tq&pbV>CKgeD+;7aV1nRuF&EK()EQKalZYmHa%cMH#f#B18B%> zQ7bL#DET43e?Q|Uz@m7Ybg3tJp*^!oH1OukHqHcKUYhjo`jCikL+P}w{HE=_Sbpv^ z;<>~nMYRf6k0XQS==ptx7G{b{B?`V;fP)nYRzi>74=CDj5} zDxvpZeYY~`K9KCIAt7Z}4^|Z#g5!1|9qpOnIBPC>v+}$NnN%|wwqh535SA(eEh6)$ z9*2@ASHp{c?CoRX{VGOpf})NjG!phfaT0&$@RE_19RCKzaL3&THP$IR=KUbU#LS@M zgjYeqJM(wS&J81qFSH}adyyn zDYVpdTX=Rhp%7WIem7PjiK$ht^o$}kXPADN$BXoKhave~Wmxbe~X`ezfr5wYfPx>Etptk~v?lt4;pPpfBWM6_~^?DHfQPlQu8c?z`ZHl@T2 z;T7pf$qsP{ouZ_){@~u>yne#9Dt$9zM7Aq^y4iW{rSXVW#@E}BSw9Bc87*C%@+aez zuvo`I8IJVy2F8~L(Rz-vA1S`P?#4i$i@NLm8CZ2ds6wHJ{4PvA_<0jkK3TD>nvD)P z)0Jq_a0-;6%c8$KPFM^TZjKhb$nqC$iNuUNb=&-`vc9hvNdw&KK_E|@CR4+3Wv)WC zQ{DxrUd&9g!N-Wn^vzk}1qb{ntk5TEqe=nJ?N}<*^J|YNgB()&R zBdZu>5?8$=CyznlF&|8tH>by(MV_TG054asRz%;GOa4PkhFQ(4IIiZ8Y7opqU+)ki z{7*>9ofE7j=O)$OAfHCU3NB%jT zr9xYyPkrUt)Qv79rZ~u^(UF=!S5df=O=9Gfv@+~9)oC1>?MrAD z;Mw=6#$rU+O?)Iq$bY4xcXS*GmZG3T^>tJH14inF)S{5WLaa!7w*6`nZSzq=O7Sr( zeT809tIA@6f}u(LC&k(4?!c$gqD*2b7cPc+%Vwwu9Kd*cQq%uJ-$agNkR~{{TM1Vx zpAJ1ckY4vbqCC;n%&mU8n8f!@K}Yhjc4CVPg+$-U%Esrph-bM-M_!>L~%9ZJHNbPoP!oi*jbxm>i3)A8suzvDY*R}yvxFSi0 zW6m{Ia*gf90*w`vn21$MYtnjJ7eNGR6s8tJ9;AG1fhaLZUTmy>tc8GxL+9y)h`z02uBtWeLD56mhQLKj z%!Dedq6^L)+!>O=knv~aF_#~uihH#f0^`ckbj|m0Rrv5OvoQ{T7nuPTaXvG-Aqv?iUyz;#u^kZB3zi-B59|sKB z@`Ms$W1hw>&&U?;<-4bmVa+ovif`z*P3Hw|g9ais8VMrz+i7^t2#a;?CXg|glD|p) zrfSw^efV~)n=0D_Q}Z#xH;o%+Eo!nOXEKkI>h(3z&GE|(4A%Zw-P?-q^DngIZmr7` zlpb?HtaCU%)9B{*-*p`ERIXT|_EzDD;d7~rOV9j3qZmNJ)p?wAxyo)ZGuu7Feo=rL zq#4Bgwhs)Ot}(7?*eFS3 zZ#9-gF9OweOXDb1rhD7tRm6r$yZxGpwkJGpzbpIK{4Cbigq{hBNC3`3(JlT@*LA~i z2zw0#ET?o*QZYa8-JjfRptvXe1#NS-ZxphJ1c*?yE*CE z*S9C?SSF0voi^d#uF6(^XOPm-ZSBGI;FXKJUosVpS9q=@YkyB zUd9Ukxm0dQay^<7%pD8({Ve7MATqikVjSqrXWU{){K)I6;QI@;1isM?Z1VdB)F9!3 zEvGzHvF^<Mn5A{$HYX9L}>K~Xb==b z-M{-vGGWtEOY0(FMbdkTW858eVHWUkPm;*U$SQHN(8iPL&&!+raY?B<_f*|oci!1) zlc`i7l%>T6e?$2}9}(8_m) zmk7W&9WTVDqf2U?Q!p;GaUzjEv2fd{QltmwX@(W&`*`fc;|43o!K;B#^FOChygFD> zS;{K-$c||^_|x(K-Rz8?UQu+CaVE!`iKFR-(oNr#=?jdZL|FL4C8;bQE$4{T{LQ(k z+eT3T5BQHKSc^EB%H-M!I-Sr+u%{65`R!OAE`VHOrs9y<^U5F^aBPN`Ma2{e{DKaR zh9)BZ@ryP{?V8I-6nse*4hX^ubfeK}HwbfK6E zPFgqlYvp~}P;17{ivo40#Kz%dEAlS!5$(LhX>=ZL&2gYE^{bgJoNW4^X{9(`D(&{J z6_N+RLLN%`zR2%^cy{3f&M?d0Dku%k54kyRM}$8Tq94d z9{cDjndwd;4L(?nJu^MuF7G(2Itnb!Sn)tV*DEcvY0@n`GB03^Pcct>`aQl`^+ci^ z`!YukC)FzqYqY~sJRQ4^m*HJOd1d<^C26ib3oX?Lmu+(iwN=pKD*)p3ukCBE&tBBp z-0rW*j2#xLa?Y!nSLXkCw%D>(jOQrU+8;}>tmi_zheuxz+r8WDWqo^aguD+M{QcC; z?>M{l)kfPlRqi@nC|~Y7GBenmHIxj`R#0Ad#-W6ugHQx-WBdi*KG_gf-N9+tYt0F9 zgK9mqxUWOuULT}4y zYj{}ZTf0HqRelVAVxow?<0d2i)~Vdc-L>JxECWpX^eVcWEz!&)X&M?YTq!3f$8P_0 zmTF|<3EC(^FJRT%(x6|G&+je)qm>MbW8y0HRM9KUb_*vqf$XPl#e4PlTY`l6UC^v$4%_2H3o-bSv>(Bo9rN#kZPo(5WFIa%- zEngDs6T9Zh?i}sjh<86j`vn;}-xKYy3C!PEVtE18wiHf?VwbV3LA^)mp`=!#xI2QO zZJHYAV>%+&yQ7F)y@$0iC5$E8w%7aU#FWUTePa+e3>)U#Bum%R7TT;9R3s}&;ag%# zo0LyikIeN?lnIkNN5N?PP)2v|i)#|POj@%k(Dx`SAtfB*o=D;64=IakQ2kY*sA@FX zx0JaP`m;Uil$`zSkRtdqvXd0SjUIhy_fD`Bo*dsUo<~!N#VA2P1@|&%vb{N8Biv-* z>7*-BK`^}`-*kYxHwtoI(-`Xo2;%Fb-Do9pZA+gnQ5t%6v^LKRT7?XEob07N94>8UBo zR@+Wbe-UKgstC#RyuX28bxXt1evEH@ZcTnoDVCsIavLE+FbeN_XfNWePMh7quS5vH zi>0(x{klnp)-(TzG-kcm#H%^mMfl~As!Y_zi|94R-+XW0$$F=z?3d?RbA~}N4cpCU zuMW&S)b$;YFlxneXoL~F|K4UY8nBa~dYs0?r2H+`pcDmn?+wDBpvy0m#2GZ z;AHR$ly685b2j`l@`=(_wGbx#AW9}4rCiriPWCMhqUe18!&lKIktQfC<`kLI4U5l& z;rp_wDq`x{M*-(**)jd+pPI>eB6p5gOR1wUh!hmp5W#`YJ0G=dS}rV&iG$sK7kp1G_|n9q z=;vsSv6mA6BjSqMN!&v>>y5$t$x=Hr&}45=)c%8=Ih$g5A0r^JB(CK&?l(Ql8_mifrq%|0o~E9c<$`qM@-A-~(b_A0xpzWNHqb;JTL z`G+Ufx7L?-AS&d4=x%#@?X}q3|&?*F) z@M5H>C9me*dXH>6o;eYc`AYk2&Fd}!l+`Rve+6|{ntD@KqsK^3`?d>un~~HuYfVkM zDgtn8x$9OLPefnxI(4}F>SM>v%Yq&h%F50jHw{;OQ%5S~acKaRe>F?FpisM@wsQn4to3YgV=5%GNN>JEZWA#vZ z*6jSR@5bbL9fR-;D?s=M-M&woW|@ofslPQYEM~Eeumxx`8p_RC-&x;tsYi5tgFYJT zv=50}E4p(Z4R1I!N0vp|W}wfZt;*w>zmjzw{;5~vi%dmx&y(sLpsR5SkIWbtkN&lH zaozS)E=6TvLco^Bc6#^JchS|GRqiv2EuummoKrV5W{A|^hZA)X*HsOBSIq<7+4M%_ zxbP6=uzINyVfpI|-;EKK@$17_e_QC*)!X+Tde|F&$xz0{*M{oK34*LI$gArtTXDlscJ<%up2+tU!119pBS)WVQ4GPI>zbgxGdV;!z1s z8YwvJz5M7NtYKE}zZeAbC*q=)$t^VveQb8N5zU$>)aQy!tSr_n8+7De=)?^AVN_4N zaFdo2c;nJ4n)1D}qyqh4i=^P$Fa<#B`N1Wn3M#fbq!Lk9Ubx76lrDIPQlCHO;Z`qgnd@F_wOh82)+jZ3!kU95adHIgUR|6`^majn! zjY~cf>6m0_`^7Dj)lRk7sYMih>8IumvTSr_cuqnq%oC%E;vq=@d!q&Jxo`2wJ zkiZ?vux#|+VUX>@2Wyo1=kAz9!rHg7?Sm0?81Ha2MMNKYy=XC?pnB8BQOf&qEqZL^ zkfX!H0)-4#^U-vGIa^RrFj}sQ@xw8lm?P5hZ!R|0xQSxc)A=7LRAdP_43#bd+~X{^ z(uo%t^*WfVq}JQph#hNqXegljm{7moxI07wkAs3kvTSa6Hs{>ot8M82HW5NB=s%mz z=jXVECG<8iK_al(tZQ%3WXHh23mU_CyVILc<(i&-IgHj_)2<8r1cPcRj6mC4GLpqs z*ye2gfR8YyZm8-$4u=eZVAgfaWA3w?@K3kCweESP2Zh(ecdFp6^>rxU+;^wMP+y9Y zL(q33t5h;p6O; zeul1BGoPT>UmEkwt1UnnzN*2UF6FC5nf>p{@$=nk_tED^3GdV9r9>PbTd}JG|F-L_ z$Hm8#sRBda;YvinhxKtd~ByyEegU5%GTOi;YB}r<(zJwrjEuM)(Glg zC3ZEWIWjP?%AL-X7>!#pQ}};)`Ub{0!>!w1LBHzul8(L^RkNvf z9kPpR!j_Trbx$yuwKX1^SuiRXCtOqJgiyJ~xe6CYro{zV-c%zW&77DnB`^kL#fS;Q z$yQb3HUqazNSi(CQI6G1>s04j6foG&`I(s)N3{xBc(%9Y+u_XIc z<7XcR7KzbUW0V83QG`B%YZpIdl4QeCoEV^JnJC={)sJ(0p|Dfk>?7lO>RW;+XiY`| zDQ##BCTN5~;a<6w8~~X-^+nl;h`h0{V{$uEJW}Chvr*W7PQoo?V^P1b&J!>kHje3( ztBNUxbi?V+Nn%p|~8hEb`fiQ_!L2Zd*ZY{6If*{FgsVPH72Sdjkrq#@uCof!_ z_6&<@JZ79GF(70{J9_eY5?el)HlMOWRbgSWz~wN*wiOV=?n)$(T~2F>W^nU`%EnXaY#Cg-xFpoo+@x zjZQ~@LvX;`8P_m=Nhux(LUupG+N8+@v}Q0~4U7y;Yw`cN=3tFQh+ms`xpOE-svSeQ z6X-PCp@^vgCV~0k)!p4h@6F9k&8+NTa~Aq!sa^!w)c{PCa4Cz2@IO07*1ZuneT(AA zJQd|8!d~J|&!DpkD=w}UTT9!&t1EiJwH@O*9mUlHbye;(>63XTL+pOJ=6RnD7>yHuX) z)}P*pL0fGsW)O+3X91$6x!1UPL`2@5#`*)*YwcK94fZRi$(wys-nUO(Zd-e!dawI$ zUgs`*7d-=3<>j(P379!&xl{aPLXPM4?n`t0Z!1Wd?1azp!r&s@v9!Ap*ejrACh3$g z;*TNMZ9KD!^7>FRCK8rnOp3f3^GUAK*2_Yum{mNsD}o;W0F?sl3ourX|i!u(L`Az-p>FHA)R zR(MAM2={T)E>`>mtR7CsfSW)=X$;vjgs+~*4ezMC@}7!MGr^I|n4)zt$qu#6b%5>mn`kvJ|!ks{FCtP93k$+h>$txduZ$7d)5R*oSX zlnF|xx&c?wGpZA2*Z+vVvw%)QZvU96KmGM1@XK@(D%pze;SiAGE*p8QMhL=4$+t-Wp=a{GEWI)msY^v;UE-7uS&DMj zQp#GD_QxOPT+)ZN0v9@oQhax>;_H}>EyYAexUt>e*7kJbjt-D5Qt&wiH*aQ9YSt`R zd$L7J#OOUe46&!KizkWQ!osZ@!qw*Po@M}~+#D8{;kWGTk^;8VuLkECl=ji{4^5NR zphB`SV~)1cf*f2~u?|sTTzoNBPgx3TTzQEeu~~#@7E2NzahWW~iBI*3zn6OBLh8dI zGc@EHY-yI>c-?e#+eze~c#T&|6fJ(nuNiS#np?mG9Uyx=>N43;VevEec*sG|cZrX; zAqzFK2e{KatRKZ5v=nX6&ueA54f%5wTt+2hmn4VH6Sfg<*ICSC&@cPw0SvxJAbMZ5 z5by=WVxlOK|4?6fow}`=l8)QYO7qN|NFzrtXZgywE2ekI(k(^^iHdVFe9O#=8u$wV zCt9gQro6l+httPXT6zr3{`5brlbLIFYLceWpvqmpt`Cfp83>Hg#X}H7my=8EdgRFK zXt~?KtWDmFn%Y#k=)r@?(v3sOo9G%mnhij%wMRcbnhL{}nUHn2}dIGIhbd)&BF#P8~V6j!R_y#1%tH&rt`Bz zXqbNC+^UO3RNe^Z=ul8h=}vYQ2sAtj#qnv_EYLb1G82&Z{Mt;}&-Pn3{Oa;K3Ba3l zP!o+^op(*5V18t3Y5WXp0_`*-Jw8Y7Hf9kbesteloo6VXKU^_eGYGtgsPT_DhFm|~ zVpQznwI(5IK4&z77}v5sgGy<&7&NsY5d^59K z1(`-G=x}HyEj*$>-?|wTySNBR*Zb=(%>K$8p$W-J++P{1L+^FCwToMWy?h=m$TEp+ z$=pC!wFgX`Ae8ef?e5qRU?yUP=g317~Dxj1}G% zvd6uSJ>DV}KLCU7$PYF=kBA35iwJhazOg=vKwlV|$d;+cll8Bd>r#+sj*YxL0Dnj} zuFoA!<9om&5Czp|T_K+bpeuF;0a!yYH}AiqV3SQnJMHr<&3a48RZiwyN=L8F4vz)9 zd2^uqW5!6Z;voiWg1xe|G!sXHQe_M;1Ldg8_YJ+DmiesDR`B3Ojp{Rc-9Oe>A~!Ys zGd61?ACN-}YP+9b<3g|6&Jqq?S!+4&_LV9NR?G<*RebL{BYp43tVY5<#ZIw&_};v- zbUQCWK0^3xryOaheDZa=()l=;5s2m8Eo({XQHnTxdQpmfd!Sm{xZBqAk6c+^cb+5m zeH`Sz3VY3HdEL)|&5fG@n#pF?S9f&uIr+RRQQ25;dJpMc@*ZB3b+mVIceK1cm7KLS z{d&I$7yrTe0L8wzw^Ew*et27KN3RF7c9iuun0?cy!`|ztZzz_}>5&(u{f&L=bT#AG zYh%!MTdPf_)4uP0yn5+6^Mg5fUqBxMO8Q=L384ECBg|9SHoA@a#_;IC+Pn)CGjV`Xp!34w4@*ASMnK;_sDmCzx?pZ*-HaT! z#Zr(BjkaEgQG@)`Ah9X*4CIV9W@hv6V3Q4o@ZR}(FOuq9t8iYvulQBhG>8jZdIS|J zX34nfTFqfq+cehH&tXk2tf}yK4-G1A5R`w?T$v~!k>4p|Le?BGFq_260t-m#y{EA! zq5IbRwONuUTk?xHRDWXT9Tew5zX6`q`rxj2ytG|CIzLuh+vQf30*ukh6P@xU^_(3O zl~3g{h3{Ax{*|b=3t0wgD&0sp`e;NpZ;NCOSGfE1z_cUX_q0b(3)|FN(o@6{>mEZhv)%~v!8ofKBaCz1k``Dp6qnF=u*)*UrKaA#0RfK!! z?>U2QR4U6WGi$9}TwIhDGBY!kM##_2(qJT@5smCMUrw|KCeA(eCc7Ub{GZk3`nH->W*GA5U@t1v(Nsn*?wIYf64QC^!q#TfpJI# zH6d`Ce{=mOBKN(nqvfbI){itN>I>;!P^_OLBcFOJFH=77G`7Z9-P&)D+wW(On$IIk zNc^T*52$eF6s+aBI$vF|TlX14(cgP)@qIkrn`*la_0#NeJ*j5bvCY6`uHm>Uz&D-R zz+~flTan(rEA8-Wc3koMc=JPn3Ea8+*4^E0K-qMCGIWJUa10T!2MWz%bbG(Fce$v> zCvLq8IV@Q)7TV2J({p*p?!JH8@H-*KX$4QEx~~yRPCQ&rGS%Kr|6XZ(uM~K6`~$DK zuUCJJwRipnvv&8szff?DSQ)e;8zVslR`@`POggjM=DJ3snTm=EJlf_iGrFG0czk%| z-?GG=yeLJTCo3W;m`w21nB4RYA-YgaokC_uRfQ(eHFu=X^`r|}D@lDO4#9rMC0ORn z$v=~I5b2#xU*GSs>arH>Ye`4Kta21y0ZnP*?C7{ckUQVVe=A(OW6n3ox3$P{&v576 z{Dz5A0wBDL(|s7LLR9{L}DvW73Rm@5&9y|Kk_tMXU*wt z?Nya!ZQ|}t+IHFfPF!jODwNNKz*Rj=Uwlxr zoIWwA0&$d|mdF#BbA-R6;xlt-z)DAo~gkAQSm zwxdhXV$%G2Ja6}-YO%VN2j`AeNmaJ%j&P)l?Fwcr9Vyb;6gsbyDaS!CiG`ZEl4bXR zxj_$pB=f*Aosght9=!{BHD3KgpBz10w2uiDoh}o6#Z+;kJa(b2Bptw>&xxc7b8bR&OLobzFS+qS|{~7I0Y_hNCTzWd7rPg`<+uWZXyeum!z*r6}JoAkRO^o4tSSE}0d_VZ0OXI!* zy%5?Ty*>8X%KJXY_I!+hVqR~}_6?imI#?`zVYoOYM* z26(1JFt2yt2{wa{ynMJZ%hh%EGW@LC*!?)MhcH(Aa*`TxnJY&{>Pud0ot|8;p{UlUKibV%)@JwzG+^3ihz|{;|`? z;Kzo8i2kkD;HR^!G&wqn%6QgytKI;e61hxOB`y5wi2MR`4c+uUT3WNEcycjv?|od3 zeKdliED~KsL>hIasp>Xw?ohyRM6Lrj1;gp<58f{UCM(&yr*0m7VWt$DpOwAg=kNHb zU6&j!iz)CE^{SWp2OU85@RWM<(?`RqbS#KM_aTRBzHW~xOPdJGC4(}_D-}HP3CK|B z-(6mOPM1YPeSwUAhk0^J2;Xhci@0EB0ZfGUh%CWjDyC~fUr_50r&zvVcLn8-2fGq1 z5C0f)H5YE=WTyG;%FnX~Nku9>vgr6ZKQHQ_9xnz$w7GyM5%?rY70CLFv+LqlfAbP( zbg(X^$;J|m5DqyJ#u|_9T!5rqfAkzAPX4-Em`fnHdoQCZqq87 zn-tL(?TQJICO!~aRUMKTVHZLIY&6Q8veBMc-9B6ne)=EU( z&_hK+Q52ZL|f2Bx6xi;cJYMk!95JFS=8n+ z;jJRj)M9p8J2Bn%QvffYEJD||mY$U#3rt(jk1;dD? zK!j;rmp^|x&Kt)11|q@Q(SA=fyc`4u?H{A+il`OrLm)%r273CJUv#6@*VD5UCTpCI zqK3-GW*IiZZI!e)(r9k6ebAfCgFV74uKTyzm#5~%h_JN7`kI$&Gnj01`ok+&c7&cP z8Yj-oL@hQ-C0;K0Akd^`J66oVy*!YfMkHDBE7lzsJE}#DtzD{Uf2+RZuSJZ*_;c%F z!iu)DSreGHfbd_C$}{QJhK7J0l40R(=5ah@t!4`im<6b8DgsiRc1=|7E9c}PZXC%+ zTNH(QA2${=^h)Cfnjw3R9?$jA&a!?a8+D-O6yxQ*Yp!S0WP%^|6MG&DE7*6-h!Szm zm~>`l^P4eb97fT{R$ZS&GS`N@9Rm2cfIpW}6L>k2h(MYK9Hvb}vOS&WeO!0i?DX(t z+!ldPyoC#67^nT=6;&gS1=%O=5!FgXTmP%3|FR{a(K_~+lVC)JbqVVwk^_%K$_)L6 z{7}@%``zSM9kleW$PBCgd00*1Eik0s{>n8&PUI(6zB)Q-cHZD1+<-WBR3T9-q&(|> zY2ENP7J*=eIb)5@B|gY)=M|drq)CR%Mk_?hMkT*Qm~54;z3hBDXQV^eW6)FPZ%6cl zr1&2d#bu*lqpERvI+t@sL!ygocBAn6)E<=bCw5N1+ffjM1A|W#f3nUrH?`ar#Wu0Tz#d{Oie|_M23hw>L|>`1h97Y zM+c{%hs4x)X70IfG_4>h)OF@h(?yTC2l>FXAj{advt&ii)`jlrLXO^ENQ~8z^av{E z@}$AEfUja*!sq}Yta(;+I7SswU8qEPiCiy>-(M?DIjiqNkCydG&-@F+n0H`qmx}9U zR9Fcc-_$cCF#GyZB$iij0E5idYx6)Q#&dd@k)T{UqVFMUnlbkG{CKsn6 zoX>!RxfWt|8$t7xTg&F9#lpV9s6S=jzAwbSdpKR_;Y=NVq9v7;l?ChcDUj`S6-txD z(%Do0z5CNOlbC#1tR$3yhNhNUi~dd@^cUvGBIs5!ER+%&l0}JD&4c7(8c2u08xH>X?aF_Y-#jW}O{q)|3 zDsxsYyZ6q4j&C_!Hh8P=EckP)PFx_`hc@OWna0#Fs&#Oxfc`?1egAX&=~_A+ItU7=HDKuLwnFN(U8D3s=FEAq&`R7C*q*cwx=wKo%7= zOqVpH<+6G>Z~yBFU8>Q6D5)$7Qion-{ZEjC2_7Ifg>+?k6_>>-4X2~=@BzcVP&NRv zQ}Fbl45n_!N0mE$RJoIYWV#43XPLY{b(J5K{s3NLa{78DElpfJT0p{#TJqTaMVRPs zW>{54qa7Q1mmN@-+xTA31Oc%hD_$QXfrM)wZ-@oX31WRZO}FC|29bJTtm3j>=z+t; zVdV}fB{e~fyoxjKs`s=ZJmZEkUBLmyf}({N%rYEtalnkCyWi!Asl6z$aR7gd@J2i(XW+ro92GZzsSfKt_j2!xY z&>NDG_aU;iZ|~ao=4Wn_r3muS5`qG%F%(3${qj%^s^X+%Ue|dhtPIXO&v0beWNs$R zm5J{=&K0EQ>SmSeJ}S#rP=UR;ET+z`*5B!A^U4o8R(yc}sjc9UG1%6!m!Y9sbz$-@ znMy-L2&StalnpmXciovyQHNrv7inyipcRew-9X201Pr6B;4<#2#zb&B8)O&4F%7a} zVzpzAnfmLa4(61UA;hv+7R^HPZa@_>?oRUbK*h+Yi#7@TL^kg=hZ1oBHsFZO4k!z@ zO35+(DG@ggb+C;Nxfq0^5Tg7f#xW~a;T!9&A6o!DC9kS~%qq&LURDAF3Nc_YP%DzR zEe{e-H%$GI@oIBj?-|HMm^yYFXrHI2Zc%yR#b}nDYrJZxN~AwJ1N`N zd6~JfzG$qL8Pn0BhIR&~orHdV4B^q{Ajsi%vt`AifJUPAb+@oW*3@8R_+RO8-uchx z&~B~E&xe2ljFMsh{#-DyWGoO5D3}FSH7NrN5r3_zGrUA;9zZLci7A4BO@>jTpwTm8 zi%_`YU=5zzNYqIG+&Y1R<|t(vCdM-63dM>7O$yl~(I7DnBSx$RA;E&3jMjlhD@7{9 z05Cto6v>14pd%?}7Zp!BX@+aqb)NGthUn%}_)<%s6n|~d$C&%|Xuxm$$=X)ApJZyK zQmFJCI4~*sef+`8G`|3r<>vhc1qbms8<*jf?Sn^}j?)QBA_pxEbs?Q2MZXbZqXkit zXWNH%74UFfdC=mH>=8&sY;<%!e*cUg0S-7SsLb{i9Ln(xf!hplHgWy*FPRvOHd*A$ z$H8kUojyTdCtnx5bfA0sdYmABHoj*RBM4Qi!At&IZWq8^q0dtZ6-y;0%A){cL=S|l zL$0yE!qi4n6TuOqsd^<_P|@U&nU)5}CUr21F3nKgL^&#APjBJ?f~SyXAS!WTFkwx$ z4693-x)&5iDXOU2{3o)2l0biJjP%c{gSaLF_RBbd!cZibDsAoGR}}Q2tEr%E@;Ez` zn57QUMHMVv={?WxI_W~QQmD@6b#`1-S~F8aJ}E#T!-WV(E0sRnKRDa14I;Q2`5FTL zTO;asI?7~dg8IHz0830VnxP9qB`W%O(v@x>h*RLt)x(%U6;4p4r8#sziy5mtDTyPH z{T25}fAr!^UJG(a?*LsZk8}H+O5_JvtDQ+l0z(XUWl$S1du>T%{Vfff+BG2$-`#>S*6*h0t5R@zn@|1er(q{WX~E)a=YR9rw_Sw-_?nqL1At|R4(kFV?qUo1vl zo_CA(!4=Q6u0}=g#V}Bb+M%vs9q=tksZ_SQyc~1RnoU)0z)t3WhnwI(hr1O`IGv13 zsoPnCu2P88UEA4UAJ>l0L$8RAXw}erx*&b5J>1DQGguap$`bWV0q~a(j zB^jf@tc3Vj<>?KVkd8{htIL>>jZQPs5Q9+>9egyNq!!ti7lj`vkP=|`K=M|GL@D6d zZEhTv5#Q1VNyo)szz~)rfm!1Q$@c2T97}w=t}#_+qd9V~sv^~u_RYBHtY-y69Oe0t zy}iKW&{-#h>css(Ln}s2o4E#Nn#O)}%#@ElEbwuUV@AU7&;TA?-YJcQMyfauyXLg_ zX$gmV`Xp79j=K}#qV$C{*yMw%9XNn4oF=B#o$N-NAwNylGAb6q&Gmw^Rfef4v#pI0 zE86qF?iz)-@XNpb-luQEd#3m+$NAc%y{%CZii+RpzBVh9>ct>7i3Aq{TVu=RIH=X? zL)<#P*nXw{a=A0{GQqUFJqjV#9{ks$um6m$s}nc#C_r0OIbE~k@PRz7VJ{d(-3CifkUQLtTarEUY3VBtN5of=E*H?IB?-riyx2K6sw9_luF#oh`I+!*);^(M=tGh@-Q1CyhN5V~TD z5k5X5>L{fRu}vhFSAAwo%Sce(XUs+o2$}4a-vXIwe{qyF6bjI(T2Ja=)bn?_PRp(x zR*9q&(Z*{w^~MO)b6APZ4%T;)#oV0-vc;T0-dMOaG347Z6!FxHhv^j+Xy3!%Av6n+ z{?}IY2N4GvHS{`eW137?2v^}JJd%1_SO@$po$Tj?6<)bnZix_@D|cqjsgs>cj|xf- zaS9XqE>8dnFEX&He$!GW0>UDtn26fHv3udEgq4$f%N8LK;gQi+1+hYyqOcfENHTVxQe3(S3_oT9 z5nGTErd!RQzfphe+valh zVhq6TJ2Ky59fufJW*DxR{$wijBUxAwR>Z|A$IXNWGBny;!>ub{$_atY$Ydi)b%xDZ(?iKiH8%8ivFbDn&I43B7eEmH>spy zC*<(Y^>C!MFo4|(%$ElvDOS~mr8(((xDxfqtx0S7oGi%{?6LcGjA+rjddQns>>4zD z2z1ha(V;+Ov?ud#!G!8z6&q0iNJ-_lBNLco>8cZ_o}q2fGM$WcK2lQn~co`Ga|_UhsR zTSOxe#OD5K(0G+7tTF_8Y?BfiK9dH^Gao21Q4V7Q9(a?NX{`@PQOsYU(&E1+j0W4U zm`-`nsus~g)3fY~BhaM$75_dTS?HbgllN8dw0v>KV~l;eFgKNZ#okWjAhxQkOPO9Y zwyfwh0Oq=iuX!{~4w8u$qv>a<@FZj_$EA3&kQ;syc^R)j75S~5j3_s`A+5&yPfTRG zsYdN~rpc@4!k$l6^H;k$SXaO)UV>hC+{tLd6>|ojiat%8@}vsN5DKxf_hv&wm)*whV8)u2MExaJwVEuP@fUBZV zBSG-C48Ra4U51N(wo<WFUUUIH$(q{(46?M*T@Cy>R_P z;T}jmmqr+y5Xdsb>N_tVF|!=b3Y=V_4ytI!l3L4yP!y!PlV}s+%KSarl3Z^iEWAdM zIm$5|#IeKQImpeYacE==lyXw`&3mL(Mn{AK6ErO=DqC#UD8(v=$<8wE{yQ%WNBp4w zquZNi{>4_lvop4FQ|)xm=m-+OmjOC2ASuqlp!GrAe~WRJ^j8o;u{Vl7D?l5j)Dj4w zH18FpWN=Nex=X&C{oSnqphq=OCc7SiQ5gxTt+!5G2@#?v;ok0$dV8#mg+~%qDVRB) zFP}pMiKL`6$=S=t*G~LrCh?S(cDF{&PU)SA#1 z(}gnM&$NG2e`o0KNr3&6uLBTl{&tohf*JYWlgnSj$4JI7 zWBpn28HO{mmth59Hj7y|bbuI*SsA5`1lD89qKf*=MVrD2Am)Vcc~h7_h>dK_)Yq zo^F)NSH^&XopwaLD^gePA0G(AM-;YS#$A)cFubj*XDg9GXEO871rQtkM2o6?U7{cZ zD?!qki}Ulr{yM*zS$3(=%;wG5{}XXT{XzeDGyuG) zf|cm{pH7@S*%8rDp(e&MWRR)V_PZb~c%VBCX47W`D4}?`W^mu(m;g9`y`zqp zocUIEjQW8l(5gFOG>FUyWJ+^Q85wxn96z`1C6IQ?cK%#vW01QtisK=%n&$g|e~Z;ZzXX}Y9M)KnMw zOw|8ae@JA)#m)|UuiSpG?0gIL)8USG7%G3Zppvp2NgUuN4uH7&6L;kk9H@r1+`R)|dk2 zy>tWDs|piRf=qgNkx81MoKIe0OUNd$ zq!-bUrJ}5z2rhJlG4MfY2Bx7

1O*rkA5RG&-cY^G#}oaYRGacWQSE8>P56tZzC@ zSZyAaj0X4#sA%|M|OvcznI!HuhM*-%%@n z1-k5k?J$9yM+W)>;bWB!5WS4Wh>r!Ano*LOV!|Z-wW*G#Hrr1jo%u&BjpcFtl;O0a zkbODrZ3+Tb2vd$p9Qm&$DmpfIFFx4K30eyg}B%x_qbKD~5v%!v9nS%h)(y~|Jt2s1J&zw|i> z_5{T?OfiVLAvTq0kW1O8MMd)klVRWo6BR`8sBIG|Qig3V-Aov1CmPF8M_mg9V4RDH zi7^P9)_>fJlW>4{JpsKWQ-Da;7K@DFmlwLM{|vs<;I8R8C@~{e*<#DD&W0;;j1{LJ z)@)nzPXv$Pk0DUM3VItuVUH?{P87}gbouxT2zaZ&{X)cu<^Vtse+FWd<4Hh6;Eg4x zUXPawi{9Yeo~}oc^r7MvqL|b4;y{sSPa4nSZiCrBkkQZaJ9Rh}q3gTpWf0LYkXg^b z-4ot#zCa9EjM3_@I;P7E#L8$YeOy-QCH@mrcvM3Ahxa=l~`&Qmx}*hGal~!0;E9Mta)xf5JeUv(tc{x~Wc&JJa#3atlq$ z#2IBUWe>gFogDR(*za9~i!Y$6gi-vHpjRLrkw2+U|J7i%$RWI-ASY35X`&m?-R?zr*w!f;m^RA|a7sW~tBvRDOLhbf^t} zNiwp6SL^&b+-E0ns|-k%fZL3s5hwwn!UmAxMfa`2>dM%+vcrfY{~7lN5&pN^m;{%a z5%9Ft@k!_oPM$||jgf5dNW)&gNM7Z@l>m<9`|9edrlw|lM~DB6MCshy+nYR})1l1r zTmxOUbh>-h1j0$UV#_b@sue|5eA4^_*9=7cgK@N4&U}{Xtz4OYQANxgwE-9lye8qn zKf-xz_ww>tTT1>60(K0PNhd?gKRw@78>ntfkElJh6-s`H&8l!vgz{t+STY)_Jnj@C z`;YsmSexQEC;KB2r31v?Pp#u%5IO~5NA6*aX`p?qUf zYC2daS7%DM|0Ve-_E#K&L|og1n1}~jq}{^L*0qHQW#ZZwu(y4~ID~|RXNzEdx#z^8 zY$lksesOj|NPweV?>*4qmfEZKwI(A3ZQA82@@CVR5Pfs>4nf1$P;)RMm}G*%STu%k zHzGnM;syZ_g&$DQMpsD;A5)~yyS`3K8jfflSrPJ;Lu7T?a3YIUfA0oYOXjDB5tJ;B z@(s=MLSVJg&5N(SoU4}irZwd2b4GjW2${=$#gAVad4TLU494eKgAvrALJmFZk`m=+@>ii)R-lL7PYReaz5kHrgW^cQwP@x;JD>6>(-ffl_mQ zr{7dWrAdIX>I7m`S>vloj;BrL5bB^l!TPyn=`4)+bS@ty0m7S-`3VjIsXO|Qw1C>E z)%JFEy2XVR6P6MOx4uqcZ(Febe#fn4lexH_DP0Ql?GNpd4kHJL`%Lgr?qjmk7r%|w z8IL~6Tz^6d6ONa22L88kmPl-RuDAOsJ^LMs0(`-|JQXQ4xw9DSwTg|?|p2H*ES@T%)6 z*KcU=W~|%w1OoP)3Sr4^`KjybO+ZDtQXNsBm~T_*UndyM?Afm_NkmrZ7E~{J$55;I z167eaLPU?YD*V8^xTuwdck1RetyDu_Cv`;B3SnUPsDe)2wP5CP_Dsg*tI3VwOZ#AI&9|2S> z;OB|~5C=0AcCc2<{hR54=I;ii>}!HtMuIwCc4_ToW@CdBs+c}AXX&OWqPHn_OSh0Bx&WRGxHo_gHEE6wgTYS^gMzQV{ zPYVB+RDMF-(?I}d_I|HuA8lHs%5qhCb-0Qeb%vD97kGy<)B&34c}Mc@q!=Q4ktBiZey+CjT8TDi_3YmXCG=eu$Nr`X zr)R))^5bz63BRkWOTdia&rNK$`|b_UG&N}l)!NqSMQ03IvRw` z?F?oWVC#9}T5mm#>Gf`z<$B(x?Zy5mWr5NY`fJ?awqf%4sOP8aM`N{7Byh`TE0F)~V9fAWw5(Oi+s~@)@_xgGSPFvO%HqSdKwVqxl-16?n`Jq`p zi>aw}E7e|Fk5sX|IsXLVprHrDBy344tcXJgXH|!1;&NNhaSCgiU(#~y#tYbJ)e~ZF z=$VzY%{J*I!U4zT8K#|n`g4Owm8vxo0~lglgh6V6bARe*eGQ!S7=6}HE}?!Yc2E72^TJ6#+AnVKNN2WsTsw;xm;;D zY-BJKRUyyySFp|HYfNLDG?{8}HLW(})8bNb2z+m{`5dk4%&VxD*Ko3&pqZScKBp?KP+sw^?nO3-^V)q^G97cYZ%;)ayIFcfCXGnwH4)H zwFEk{cw#y0FT!Tpv0bIzCZ(H=TpvV?IkPG5KzmqFf!+8UHe_yn`U54P>dY@esU%MQRr*D0{e`MVZFnkFx zS{jI)dg`;-dg+ZFzIIq=8hU;1enHTP>Uvncn|jA3XX zj1#p)oi`p{d9DWYzpiC3b-kS4E$7(jxJ?|C3VAeD97c`#{+;r&^EgUA;`7^i3=f1+j|Vjh;Z+`B5cRJD{+=(UWGIGzE+c5;SJfEH3>e~JEE4jV(hN^P zbverGh8%_pr-<2{qOx#P(WqxU&~STD6zP-MO%X=#(vF+d)Sv%}9o1B-XE|q#fh;__ z?xZG_wT>%;C@GWaKiqC7#t;EEVy?+o0qW6%0#4yjT2AYnS+4*dAr!lMyU(fhvxiF0 z7n3h7*X<2#+T*3FWn0awflLSN>YN_ak@Wz_0#u^$63e7 zP$TL<42C+Fiiks(2)xRQ2o*J#$A|0393vX>yK)AH6lMpHp<&;lAvpvbq)4S`Q+`qzc;adG>eJQMDH(w zqHf){qjohKjSo(xIh0jIP`(ZmE&^e%`xV58>B{ameP0;*hO`{-WP1q0P{J-E8M?P4 z>~yJ{^AB_1?~l~<-sY@o+TME@x||PMx;>_N9|JUjzR@CmeM0*kTWz;_ko+8sVc+R= zo1Vx*eUlJ_2O_ccoHqUhAN-#0w9EBDc^>X9&G8%!#@0cgnBslgpPaN!>hl-Rx}Ih6 z+p7EQk#$7W{t_5U_yuYxsx8uVa^ue;v0j1{BA@FOSogJ`*7X2JI?&8=%PzT)TI{uN zS4tdk#4mdj#9X)iqFxt0wku~Ze)r2U^!V>@P|VCIZ##pvdM`uvO8#Uxsf>E^S)AT_g}nM&soV_N({*VhJfw-am=TQ1-i~SDE|uuzM|xBMkUD#-;vCb zLN;;I`Y@qHrZ)q7^CFl;Sn6VH%CB3hi(Mu%7OLs1Z=Oh|iOO#Eo7MBp?eY4=CPvsK z2S<-RzrO$>mk5x=ySoFol3<|#wN^s`RLQ0~Dy{IJpL+;o4Hv(+FC zbWI?IIobq5ijpA%q9roOGO&xI0T7`suqti3~9ycVeVxJfiQY4qhCtB$i5qENNi5{!d%e0-#>Xyp!EW7eYi z0p3wKN+naQ%qNo%Sj(GrmMfj2(m%IxjxRz4gFP1ASIcjIrn6^Dfd(JDA2*!2E>fn_ zI1A^$+I2st{JYd5KYynmgYVD6U3Y`)#%lA!1OJBCW0b|*rQfw(&+|lev=Sb%*M9yV ze74`VU>4N#VoT56-%Pt4ugR5b{{&20)nCoclj$s4^Np5&PVIc}Mzw68x3KlR(Z20G za9`4Sw(ETyw)C|e%!t3s(Fl;^p)FE~GP#^K)l7b?=jGH^F;sC;+q~Cj1nB;~ZA97n zLyM0MH*(MUW%KXVoMGsMRZ6xyCD-}5Mw7UiR-^snrzUG%7PFnpJqde&;bsA^|8^Ru zox@nj_ktp$u|XLcPOsDruC~PqSsDLzm!>MuLI(@65e0js#~Uq^4f&oYR>~LF$0ziCqQU{sqfC zh`b={g!u;gQRj@ z%(zh~HR|<-U>2El%6Sc}@b7}3%*C+I9CYbY!9rhAC2eeJ5~L;fX<3SQDLsFs7Cb{s zUue6Bt(VM3Y9D)%u$7{RJ2QvR)CIkZSAkPPle|rgT&=V}| zGGX$B(C@B%nkhSFTY$Uc$YFdWzL7Uoww>PLz>^K~rIk@ix=U8nq)UjMs*MH|UwL?} zMqXaeB>2=&M~fAL)dAPY=rc&KGSc?k_REd(0 zR^Ik?-8C^XJWo)evEjkD=y%9irJdiVp%3Jy*(p=nc=%?Mpzc3axvj9pPWq1rBS3{( z;0^07yHaMKUYyvhG??rSMz6KED~lq**ndRD&txSf2GvT)WBO=`2=R$3!NS4{$Jh1w z8207M+u3dTecWwHaQtIqXM}v$CqY&zj`N-)yvP8r`MEA5PL?mO+CnfCu^ItRbV_qUD5$+m4x#%9~*WZP}K*|yt^ zsm-?S+RbgY+iGjK_L+0e@A((LubKOE<9%IMY&iKZ9$Gfiza0?CVsVg_SMo zene}t*5^Ov!S5McQ)G4Ygd=dKkkV98Q1Hv*ZrzrmG`u9|@~{8j?FN^>?_ucc&%+d- zdLcl%)F%@oqs><|5=d=v9axR%X0>y0;Zo)i&D<(tFhR`#R{_Z2luk z|K}HLZkxHa4)^8i%1_XJqPM$0`h6zpP6~bBiOd8bpAt_+W!9p-;@DFTwlV z7k&<1onEWSRE|$~@$Fgcy*C`sA1{RWw7$HqrN|lk-Ia^=okzt;-!EFQPP(xP2nh-K zYRLp77dF|Gx_Wl__50#C#iQh{5;?&4h_4yJb1N2;Olfv--2pWIt>6 z`S&u;+@Fn>^=>EQH&w^wu0nPv3?N6b8{eoaBen|Y;5TAhr=c6^KqT!!hRqWZ3hSD6 zRuXC8?PO(suHMd%9~9_O+x3h3wY`?YA3AX%Q&dzESCQ$x$l%6)Ji+u2I*{`n zz6pntz)+YDjC|3xB?@cWPV)V$S-a2G_pe6g29%`egX}~QV%$S8D?shK?EJu#gaygH#@>(kMb13k z6U4;+lbbq$Q^0&!pE+fwjC+9zIfef2^rPzHMN}o7wlzfH7$IXi}3h6ZH0A(wuKGR@X>#gj==18ilu%haGK4it;NsiF`?e9 z?fLhU=*d?7)6bt!k8Y6uZmY>UZgs%Q%I_sZPxN(B3U1IHvVjV^KP@F8zDN?-zy16E zTn>>9SZ=H^4ds_L7E6RbW%D#Rqmbpiv@CD03wd4-wf_w};y$_A_?Eb?0D01gE9S#x z0DEIx-rxWDn!XSa>GiyB(cDr^Sd8@fP7hPt$@|Jphi;2e`cygRqZ`Z0O;?2MILp#= zLte!1?86}ZT00bVfw(0`rRMBFShQX6ZwPy*qTyzeQlXkYw;#o9hrK z)m{y!DdZ+JqeWPX7Bz?1zB^znPA!yFbW3OxZ+*MKVD75}m@i-~lA#pTJ&6k%{}Fc^ zjxxH3%#k}bHU`N{dOm{q_cbi6?+_>G=zaS>Ola6(RbTN~Q|z{(OgyGkiY(HBJ!(Lx|jNtnW5(QsA2v zlbZMboOw~#kF}P7qxNg%a=lr44?*og@FC-2q|vqK?wGB=^D!6}ed2?XqtOMy zyZ{*?&aa;ssB2FP91;DH633AEDxjXw(EE9K?82pEZ&wQa!-JCT_G|Yhd;g{5^Y-81 zHD}0XvcSk1m{?!<_w}=ojRXsp7-A?VF(lni|h{PmDgR*AsnLa zVnkwF?k6`t+UNWBJ%5w=4nYBYV5cB&SQel2mf!h_D8eR%*uA{T^545tvB{AzS4hug zJ0xHSEBKwr8WhKf9he6QldvgGLk8j-io-@!ltS>n4e#06+3oGk;D7}l6OxI{OAVWt{{M{!GYncw?!{Vx3E$y{g8QawT>g z0ar!q7kVgNN)`;W-uF}KjiN(^Qkhi^6lb1$MP?+fsqRJmb+{q}dE z`H`mebz?EZKtIhs8h3Up%9Amwa@?F zR#oUUjN;Ff-n{QLh5ygRx914E3q)ME@DTM4BXllJeHpf^k;A$8W3Brbq`xw6_l|- zMp~q>u&|=ip{L0&qV`2WMQ$H0ET-Z+$G?0wF9TOp)_j`nuK)X>){lQgD3rbwSJ%Z$ zh%WYg+sh$F)BDloWf7hs|D~1U?|XIYP0#zZYtSSQmzA;m#;92B*DpeAKe_|Xrh{H) z)ui{tZ`$(ueRu9`jk#?1$X$DG=eu41)G-GAsv{A5%4z<5yE)$G-L~`Tz_+c|Oo5(u zPUI%kHSn#8z|8Nub@hk9!3;-CGO66B%ZZEne%(t*P2<=5YWlXWHO6gLtL6r|wgGr< z%%oKh{jfj_S|g=s8#6HYQXyFs<+qyl4i1Lh4!k`NK%Pi2l%9ERBs?vAIi)}>uO?t4 z(V3H|RvVLa9iCGI6FH*81n|wj&Ok?_cu1ZxU%C8yoq<&Kecz|294>DM<_w8`;>|Xv zxz2^!FL9OCp_Oj1!I@eb`qp>6k>RD#hQzC|Ur`O+)Y*A9{Y1pwKt-}~oHzm)XqIVE zT46rykZVy4{B!Z{i^c-J<_N+Vs9)Q*ZOkd-_Ll$N{y0RQDCA_d;VX}I#m|x41tAMQ zEmS7_go*JgOG)@gOkYw&^rA~n&9(%NR&h#o9Ub_Z@w>ry&4fom4hYutJ98Fe+cU5I z@Czi9cdrq0RKpPYbL-O0*7=9zhw%LbO`qGVz4xOxHT!^C&}ZKCj*oz_kjgIsC2bWp zFZUTWdS54NPCP5~1BZ~l1U#*u;IkXMO;Az1T-~@tY#03rMd1*9@38FmxL*HfEH#>U zb$Q_xMrYBh3phWkoEJP{{e_9D2E2XRRH{__(&l$Zy>O?f{ zRCa2APmWyScx?5bHKgN(!@R)5bhY+tQ@u&R0Q7HzOtw&WC{g#Hjn{>8aZ2 z!oM*CW8Z&Xv-}NcAAqGRi2w0-^UF|_GODrnj=RkChc>64x05WqOtFXEf?Fya)c&qB zo10aL82{$e-zfjz!jCh}H=DLF2C$3z&N7@Iqap-Ro_8AVb@nZzEVs{B72jH#5i5r8 zN66tbK1MHBE7zKFG1#-%gnC_(I}N%Ng~2~lONbQIFB?`syY@*7Lzj(ZX0m-{V}t$l zX6G}AQ(aVKXE}lE%Yg()FeolJW=|2 z$My@xWjjTSBnlje<4=NItJ@j23~_|p7Ys!chSvO}Q;XhQJ^iN|l{oL(GQDgx6%0;8 z*CzhIPp9XV@7AvNafX?7YZVTI-q4elYl@*v=PP3}a)CdCIRe`_s))c2cPLcD9lkUn z99epa#(m0}$r=$-KhaG5P2r69;E%pmx}B6TuySiHEuG*=EjKL(46N8CE&lc~yI|qz zVs7mr41uHil#-fx)M9O~VwH-si= z-Zqi6srwMIEY+w6ErqHwuYQrq>w43rDn}=EPhPyTKasC1u?Q{w^C&Mn^}^M(V-=cy z&^f?bbV<~uGc;Y(-A%&yxM~)pqYQ((?gLL~PyRXy2M8B1Nnh27pvu2zzsk+Kp%?XG z1z*J3K|^RrzIqCUD#YZ!-9owlQX1NkSo3^G5K+UgZ2fF?VtO1Ha;o(}W^^Dd%-_Vw z?t!$;&|e@)YPhyi&&{^uiet7}qppu_<;zTK23k71T)*bp^cV54Wc){vj-`LPWkwLy z|A^U;9r>J(zgM^5v6p2ezmc10gMRmGw*rmi;^M+~Hb*HwzT^z~*a>P@!F%(@A-?DB z)s{$q(G$UEWm6;E# zQ}?!;iB%xmS>T|8 z@x?W|M=Hh!L|vp3Zw!w6Z9c%}Z!jNCT9-6Ek=h$_Os%1l6EEg8XCntO1dM3Sd! zYuwgY2QLjJ_fyUQxvKqox@#P_yS(%GofT=ZiKEPk8`|K1yHVgDTbb_tLBi;g$ggeo zPo=`RVh~ncFk6`=Wp`N&W=0O(?iIy$NC=xKu}kN&wp`x~dSLZ-?6 zUE#=F2_jMe`hz45ceNIAk+iL9vc@4-j=Cz_6j!(nE4K^}_wQW-vLj26D?CxvmbF!d zdq!3CtK940-+MpPCseLIUW?VMy1IyWxFE;D@cBRZV>x>-L%{5GG3(#|O0Dl81Rdl_ z!4`I{)_%leD257H^1W5`f`a=O^*aX*;CW6?S*vo?e#7s2(J zqtz-Jx^R$RqrQbVxdsl+3XHt6yU%dn=MLtp>#x8{C@`YhVU!tZ!|UUg2T}I9AB>%E zbbZ?CPpU)MvyB)(+Z~CoVyPtGY!vqqjFWUIT_~b)y=x0bm^2&l48ry&6p(=X6G?PK z(ZU7z5|+Hu;Fk^4xr64UqP{9Av4Zl}+V{-tgHJ=|TZEzqj5v=aZsN>}9eRU=Q6$_T zWLd&cDFm-g)9xqPJnMnKK>)wl|+1u6)2a*{S4oU#Bt1RPnPw zKJ6lplc|Kb5C$tyIN7B}=yxgnwdko_N#L-6l#n_8qzuYfB{Vo@F|kEBM|mO5z# zwrKPVq%7cL8+LMWFzgFl&pbZ%;)0AXGJP%}aq;_Q9W)bJQVU5T+UdVd@00O1@Ge9# zVI(e;S;dEp_Kx1$2;MUb35EQSC(0H099)5i4_rk5}&*@T#kQHo405)|Dqw#;6)_(TC9~E)!hih5m@B5xRF6XC#DZJWTGfR_96&<*rw#{ zIqZENqQ)ajqi~1ZwsRYh88Z4y5iL| z z8Za0p6n2MRT&~o;)Y_jY@LvLI&(av8`w;bEjtAlk6A`KFrtQ!eb62h;usP&J+HZov zWm=QIKm2`%BOf8;T2Z+a5@N`JOK?@xJ5z2fJyNDKVfMK;JktUaouM*}Rox!Iy-s%$ z2fqMVv7hIbfS>qhrU_36Hk2kq(RF*H(c+~^oD%5z&Tc4v!p@xP?^ZKd^=qNf_-}d% zI@9%}eT-~c>@tQZcu{T8S~HtF{y8&SFZsdeiB^=2@ax|Sj$Y&SK5)%bzcf8 zAh52x6DgNZFWwu&LH~r1RI0xXsL;@X#f=d>TRUFkKnj?rZnL-2l-}y&v>}QGt=;kX z)MiZj6s4|%CXi;MQ24#7q>LD~Co`eCHT3i#6nC34J#0%K4&f9-$$v5+fnf%Lz+a3= zH;U=Ex)NeoD9r<0n~HR4&dy)+_*^bePvz-SPi^M4Mr@RE>@e{rVDY|+_)X!CRijDh z7bzh{&bcvXP4~xA-`C^XORJtPlHO@_Wva{4eFP1kbLJbawmdzxLBIzn6`=R47f2}B zfvV0SO{z4xoYCGK7a{+JRSB*C3v?%S*vlIlJZ(1}t11IN?t4Z>KW?f@S*;1Jt0ZV&~o2 zBLe_bP9#|FC$3OXw+1XcIak?o7HWLSx@w3MxAIQ ztsA|FsLU_ds0`efq91q}r z@zd=~B!Wt5EP%FB{xho&_OZg&_bJ&2CT^8%Brm4&wudrnOYez^()BPATM`+8KuC0M zuU@2y1`LF+`&bi6j=Uox>y?rEw(rU@gTumlMOq^Uu1ZKqXwe-@J&Ie5B?|D&&*j;iPQxO{o|K}j<6-WnMbhC|80UXy1XxKNzfUH3>@k%aKeqKA z9~)=W2){);Z_snIfgK`q&d#pSHoCkYDiR>lp&TzmEzS-@p@Jq(CYH#QRhXns2y~BFU z`{zwAyi-`0QRr@Pd}YRfh&K4l0az9_#MGjyp+D&17y%kixwC(Nr-3DVMG!i)XJo3_ zKE4_0+3=QWxBs9~EI6-U8tTZjHTr>7iTtRXw4hQeRYkCU`%1Zt(?kLJ*9l(FU`-5;|%#3Y=W*5~{UC=$^ccq=7cAKdkfkmxw9B7{N56j631O zPL@a}NBG**^awn^hOu4;kFscP+QxD9| zbYhnk-t=~w)A-Dov1-A|uem-NKXVnf9#e-lmu&~)ikT3CGlo8yk*@ky3q|2OSV^-R+RtfjAx<+I5UU-4>Os zo^!iCu3|J&lV$oA3qI$I($djwhR+!1aTbtY-K|w*&I#`2Q7qka6UqrZXw|zocaSV@ z2=#(F(h$crcJU-aYG;;XKE8}$JEwFHHAu?C<`fVx2u~plu_M}Qn{7i4Z6kr8 zBW>FF3r_G!dgdBN=DD&k(8F!GY#Fp6BL;mQ|iK+h_Q4EHwBt*vv@c_Cnl6r<`<{ ze5t^Skh@@$YDFMjas)?S zS5kIYO&&@{w!5x2&q|ErR4Y^2q=pn#R{_dSG33Flyz}s+ilHw{$PqN&-j9&>RBSs6 zPAJ}Oc_mcBr8`~C^2&IO>tA`QacwIQqkwr z+&arzbu$WQP0J-9ZURW@eRhL0~Bo4;Ale>bHM5*I@EiV>U0p&lHzuCrYzez z6e{*i$*-F&it~<-ranamW%Abi1G1p1&{{OEx-34X_>d~#&^OM#`7?ZiGLH$;>42Bp z*I{zBVPuNZ;F!sQ=y|qLH8qux46KVn=o$BOR z+I~3VCE$wSVnw`}_5f)a=tY845&cjah9S!?{-`>7*acl!N|2xut;scQ&SVyF++wpo zV2ePH_G_7e-w*m2X6VRq7k@E8sT}BAMJyExYxluiiB`l2H$UEadXCUW)>^lZ19~sb zdk0nA-u@z1A+$sdf#f`ylMA`(R^#geS12B_wYsK*?892YqA?AzyK#BFyX<&8{uANv zfw=m};wkqX*`KOx?n zh*A}%82VKVqrzrL?W|>SX{qV+c30c??}LL9YZNU65%Ox-acI`0{Qv}%T5p_5hZ?!wev-jfK{k%E;QTo|$O9aN;Q zUnkTp{qX>*oxmi+hnHfoS^7=}pM-=5v4qC;5*du<1cT52$!^*=wkVB?xeX~z?QY1) z;N(__$SFyM8U5BwJCETp^fOu}KyPz9Mq?%G>q<29@Sjw&#}xusqlh#s^Vcfmp& z{QI_+{ccf1*FeJ_W$Eb95K|Kq6C*tWM0j&WhT&q+|MZZYq5stE_?ff7?l}Pfv8v#_ zEmsNh5jE2^$FW7Fxqj|Ym{$o==j?Nv%Cda9W*)!mMvuRz+($W~95@Ab!hu$%_(pT1 zhGKOFh1f$Ppk3AM*ToDNcfM|kGF^izimr5oyVEZ`3_-}=mXWvph0VYe`;2Lb@42UY zzUr=?$|5UwU%)r8x)oaD>tH9oOjE?eL8iWDyF;S49O$%KE? z3p+@A&!DT0AuNYXOl}`b9L&#}?B@b0TWc1r8F-ot#!j*^Fyh~f?|4EI%Sr@ zAHth1{SB(;nU$y-+b}S zRAdvFU@X4GMXn!JXVlix!byfDm zW1p6(kxgg0)5_+4hQWt92Qq=TmY2h?CG_=uUIvD|2PfGR7Ns)|<-lrzgR)?IOd$!x!v5)By!?Pi$; zH2RtJ{DRw$W56^j8|wnKaFn=V>xt-o7lq74b;d@yqy6HrlI=KJ+mUlb< zb{h>T??UTYM)%U($DeVhv{gn=F&L#ca+JI3+uI?Z(f;AMt&MMbL=DjS^KQ23LGyi;c?WmAZv27tOE*v4TDl1!i$3LLDCSMl8s5VCn;Vt9 z%%Ig(Er<)CQeaV*{n;h$mf6-AkM_Pt^2TEy(>Npc?eJYHra=l~#wE9t{Z#{1U%ZE) zP_RCE`2V{A;RqbRkx7;iVe&@q%pB^|5)02TG+V;ym|UBy7N8u8x3Cr=gDAau_$lf4 zBFU?c=&37Jv4~77yx`+G(;1_BTY4)&22yO9FX?++G_{vTmU4riyzes^V<{gYv+p?x zNvMj7a9}IMm!c9f(fwobATY=vKu@5kBzk0(@SII5DFTNelmn8*R?@*82DbTl)fzTh zJf8F3{Q~Lepb_lhQX-?$SQ=K;IfRG@1C^xflUcw^<%OLFcO9~+L7GVdM#cdIk`2bc z?|*?0t+gdseD=OUvjXs3m~hj;R;ylQtGM(kKg^X@dDggbQR!2eb_I0n3^j(yD%mn( zPoLYGFEckm{xY>*v{;&yea)%E$uWgZsLRT>wO&HuNZbXcGeR}MlCuzbX^bdN!qVo? zS18k^(WG&Cp+~3Btj-!p$d;4>==xhL(MZqbw#2qAF$g<`^2Rfg~*w z%P$gkhn*E_(z&^Uq~nKyh_N-VosD`#OuLA1j=55F-6XL%^<|m@q+dyZ^0>!y9(BIv znU|o9vE6K3r=mQ!0>pi^GhwemnDd^#I(FhXIxVIWdm0;$Ps->kDmhC3d|1nWd~-#2 z@^+6&;6`~&ALl%y)LsGA@3pTj1b!sGBtW`CMmx{C?uF6($j6bAkcdDGORZG zrpLYo&=pk>J}v0hQ$RSvigYA~=#GoV6x4vK@=y^Bf4BGszUV3w_~p({F-VYL&YzY{ zUCb%|s?CHXR{|R&z}`AaEz6GSh%3D(!BB=_vrU;gb9oUT%Go^!P zjq$=6ywXwXt6cJ&&ORvVv6?MzZlZjMqSjb=Mc79gmKOZVto!xMkU7VyO;A99aN$v1 zJh5msv0&!&vjijUwy|WlXFQO z^2dmUyC-&Y*jjFKGss3jc6SnW`W#b*HMlxB_#RBIOd<=!wXdzexVi}PJ==sRWa1h3YgG&H5%PqwpoV{t|R@26?i<8@#Svg#-wJO?>lX7_`4la>T z5?~IqRsEi3zEkd7@8U>%5zYwBp@K``bwn&kh-YXUYR2r<#{9pDi!Z|rdeNKb62Xsm zz$aB#fGgnD>dViOld1V0Y~JwQWv^1lG%N)ll5};-g&&chakFwJtwyA+i0OTbz>+rx z6?8MRZscXLEj9<^3XtCn)3jyapz(%OxU$C(-BFlOq}7T7EM3o^5vRDp+>Q)Vwf@*h z8-aOEWji#SY%eu0Kv)%~FXhspKn8&b}CocA=zQ5ki99-hkz^8&vPo*y)Gp9N01 zb!SA9ck^WSL}}VS7InuV8s!DyW@ve@lgu`?XoM_dc-F0s5UTC1H=jVVW)$d`28#+4 z?l1kfVpWa*?Wg~-++z^fkzypkvtW=QLYnkGo!I=s@@-|YUV{gZlVuMDaY5Xn2Ld~= zv9Z0q!3Kz9z)4XO(PJ}#(1sTETN*3CU}i!@)Tih6Ru?2Vnub#cLi}XZ+dWOD=yJ`& zGV0eVIiji9o^+cJU$6~@o}rWR0*8GzZ(<|)<{6pRQH-S5L7@ol_`%K!9EgKDYZDUca%Yt?u*d3{e%ETcXqoX^gZuC}N{1I|C6>lC-q}n{DF^ z;ck{b)1G#Dq(R(J4>-)0u}&9VN*BytDlsf}Sf<(J+d3?(<} zb;%H#s5(QtM#}7Ki8?>`v}(eXYryvzG`vsvaig!VuYO}DUxPqJ&CT2pKJ}g=PS*Pu zLKQsvn#4g1hm%`j`oEVd9%Hby&fruG=&IFLpls_ut7mtuIkJL4Fa%6Y~ zq@S0=X0uMQ41*vIqBA0UEJ0ZAWxi&#eKB)<^L^4OhQOKLQVV=+nl!BZ$dZYySU>MU zt1l1CSSMZ#x~IIuv~_t_b<@R)yvNY)(fC8FP*XFpb87KIrc&$-(WnCaZ?JGMA?N40 z;*{BqTB-y|bISh{M0k()a%I{*n{xK8z#-~ZzX`8 zovBC|eDnOyrPCxf@+r(sq+^9I(je;jEz{p98tULqkGfm(j0cv~o~<@$?y;XgpPh{R z%^?hGrt_c`-ip^K3PhX+%WJfJr&lDl0_6n#p~U?awt^r2b14|cE2aX)K|1WMV;1l& zIld^dv4HKwtOZNMNrIY1_%PfOnz};PJdNgRtBLjNpBm~UnMG{5ncsG3)RCct)cHBb zVl;HaZ{(v&aE2@i3R~<}TQ5lT3K;<&g-YljHXWt4uRJgfOeyMCrd6!AGYIMz6VKPoTdUw`?)5_0+P$mkRBPtuv>I`1qNvvj$aj1crb z4Q@HoJ*o~I5@ne}hSChAL<~4W85$XxlZ@z@Xqm^%$qyQmCP;%TRK6fx%32<1Q;G&7 z0?}wlW0-OQ`SKPG6lJ;&o8Sal()9Yy5Y1tyLW#jGWtpQy2#axt9M3ZrDCBiPz!Gn89gCxB#J(bD8B9U-=`hWOYdv8d@& z`8ql%xM3i16!_{ocg@(pBl5Zw%zUV^hREuZg_+ zMRfbq@k5&2y1_F5y$q&#tg?rA24qd|`}Im;0B2BFjydGq8@g$QpCr#b14WpUM3MFY zm0_SQ@Z0=DCK*oXp@nt6N^B!xd}B$JHO8Kk12e>hY#8?K4U))X)>d0t$v}#1VBwSM ziK@5CR=qY%Va4X^h4Jwh!l{)zgAN+SWlJXQoI^?&TNeakTKphP# z4O@LdJlrU{N}q)BIvXpBL=V&ow*0fZhg>ym1D(cFsYXs>sNCQIZew+KtVaD2N>95a zg|&vCeNJ>DN-7GIAnGQpp=_-RCzs0YQmznd*>XacIbz+3p0v}BZMpvW=lA1v{G-GW zMfUo0<(^;Arq$psu=^CQpVAQAm)^V1XJ6dH|Mvr9{D<8(w+x+N^xtoE>=?|q7Vynb zV7$N=%F>_IZtASIpUvgHBD;z-AL>d2!zYi(m$!&F6q`?73O?A(<>FL>0cWY;5OD$d zZ!I?P6#yyEldC3|D~D-$g9#jcW?z5&ip4fP%(B!sEH2&7swof{ zm)7oHIgyOQA!>H;;)sY?I$koV6Wxq>%r#TOoD(M#nfr_<5KXQG?a`ZTvkemA3kgT0 z9~$h~M6U~S7%J>UOgJL8KXfO(CY!9|z~2H=dyCtYErIp7tSlmoSRG2A00fwbfJYGK zNMN33stM^=JKzHKz3=;z^l)+u;;f0lBWEmOhN<=Xh7-dO#wcQ`Vz9d}9QC94m;tgV zm0LS*Bsw~um>4!`>_kd<+#_`$Y+8<9eua=?(@Nqb92CnQ+eG`l(v%hzJ0k{TI_J`W zMoU{utxDBmwMMycJYeR`g^#s0Y=HO=9yyehZyU z0>DFg)r%9=O%qc50ImQLLd3zoK}yVlj*zVwSwZ)bb3~8~&#rcv4tWZ9Q#?i}J=M^K zH0GWlI!)qL3~-~!;B#(2xAOB9J7`y=AHt-JQJ?Sv6xVGrEVia_@3JRQ_%U z9?NX9T)0W(#vdmJf6)mw@iKY3vsm?=e*?HdAVgzT_LFCz(NonzYDt0!hpc@wKfLWemQ5IDD{ zXj6%w#TpltWc2&xB}l@L#2L$?q@=Z)8wKUdK~fR$lSTs+&|@0n98(AJ+Xg!{gz$+{ z0bx`MA!iv*S5=y2aJMU z-^#-o<8Wp+%IZ;)a0+&$0&5OohA*+~&qf>cVdZ+DLT zg)154@#jMiJ`J@~D!dc_bhJHAHFN~n#ZT4Bgia&AK>*H~s#jh+!YF9qgEWRrDfKr# z;sEiK#5Ss;^EfmUOscw8#2@1c{#1U@1lN?fEgd*I;c0?%m-Qa5r0buy0^zkXA5abw z1;HmmFEX2nM1@*-lh&KtkHIDUaoBr2@J4)CH1la$dWQtIb6#*}A-lnGDwSVLeDhxo z8vh%q(L*5cMinkj;NzO3vyy+G4we9pHX=>h+4)vi;7gOr1%I?LRfC!^IfALwDog;7 z$9AsPp!xV4zbXoxF}>FR?*e4N%S3$1gxb6!gy?s8+{md^$y7yg;X{ie&l@D+v&XX} zOJ{IND2QSJY1sUCbAP3pIp}~eg6zl)4HR2O12O?Bqt-gr8&GH$O=P-GF8bc!~ zW&xRo3M$IcpkpJ$jqZ6w;_i?NYXZhr7PN`X4b&U+7bdC;yU58`&aA$1acaad<7oJQ z_A#ZiAm$eY(WI#kBV0SIwmIX^2hd>~`geMpBcDjRk%r!5whrnQ!3Jw3x=T}It)aVh zoZ#h0uc=$VyPpUNOT+FtL!FN+WktVZy~hjZH9R;b@T#ju$6$o8?`f6}`Q~O5oWr-DB_(OHX?2^9E4#?Kqw6!T6{$ zAAf(mASSk%&&g9A?H1Z#rLk9sID743W)NP}x;}uOqz}b*v5pn`euT^MaC9KvWC%5Q z-qgfPGRMnwzzTT^TSgoIvR+^Cq&yAvV?|BvOh96nfvJ~h2;$1cPn4xO^Xe)sJ(%VH z0Ws|$A|(y)pTTMDU$~XP#)jXam@A}1U3@XC;ww-bM>tqeV{CX3W>%)cT_C ziDuGfq^Xmc0S-*DPc$p+opK7+Szt>L>bj+dIu_f71*6;wWq?c7hja7;Pt@wsV7GYC zK&rfI^6*|;%ocFwH@M3@o~7yn@JC_s_%0}GZ5fKR{4U0;>wP*0XZ0b5136SBdrrjy zJpj*=4-ald0*+W-npjvI(n-w16_H-0>N>o@4V$)SR(a#qLI z!L(JwOdQfFsoz!xlnh{8tUsh!?+l>;O_?E%XEJDKNI-;*nwskv7mKaJ|F-Un5&v%8 zmEqVqBGZ>jeR?{X9#T1H4C1KaWq%v<@y2@v1qFF}dM+Q&_KIxhC<77^;f5*K%DDm2 zo;u%nAp)mdUPo1vH{9g6)CNlaWSDZ(Zz0do=OINEB}P$*^Bh~9QhV9vHs>Uj6$Bn)-Fk$||Kob&bUcB4g9LOYLrH^qtcdE!wQg&)}wp&X35^h4=@B-&9f2?V(f4 z$@8rR$AgDCO^>4(!!3jqX~~-XO$6Xfxf2P-v5yy4c5c{GZ=~-XFe%p$j1* zq8n!o4@5I#;0=VEC1|f8vlmyzDK7qw(sM8Zc$ozz8{$t~XuCHtdlbMAV+YS9c=U`} zG7W`xpk0B6d8U7;zV-hZsK|KnL_-lZYNX-p=~NQ6s@?hnA)U!F^!F}lx<&_q=o;8o z1N%s?i5wL)?om5>!hoop5+6e>Bnu`$0TwcU20g`f^QG)_L6PuygY0J{pW(*l0juvm z)SMmW+g-nQDe;iBLbexz3*9Ou%rAyMgnvw&xTK!RvF%B5{6!GRHqdg6iO0+!?KB=C z70&GOTp5^TjDe0dFGnE#lJWs$PW~MnSQ4&Tp`#{KO9!t{<*o6{nws9@Fk82OR?epmBb{qZt0A&zlGnTC;gX>h`(=nOG)=ify*XZ63O$LFX!QTr!h2%h8Ko-n+944;)_xj&dIs~(psMrbPp$1@>F?zbCz zgf-V0Bsn{Qrz1KtNGo zwtAJW(JIumwVh-lOpA>AoQ^auuL@GRW+qsMxN=UleAbF<`A2)ghHP}vrE?A-qbE+q z%3^}`gqox9*tFP@YK94pK5W*V!u{Hl&7&fkxdV0T!$PJm-HJta?z92bd<6##D-F+ilPe5`2o zW_NQL`R5sq=m-sux?@`2kG~@<)|?jGhnZ{$k+fne3|*ZFpbn4E4F4Ap#u#E`H*8*& z*+E*nGv{M?J-{fK>zv75D9?V>A_Bt0J?2%~VpUpO`bZiCBGJ9?)jcX%*sLiDSmlYb z(Ye;prH=JcT_k7UYIbYCx^_73Ro2~Ix^X!s-*>^iA+0Pl?q=3pX^j=aNik__KT0@= z<-1IHddbcW)2%6+YU}a?X(-vOy73qw2Q+Y2=_|nFS%B_=q2>0rxS$ur`5?G-#M)kz zS)y5NT&zx-?y771;{_3wNw;m|*c+qw3Y*#SLvVkkwFlS4=aawO?*iG8gU)D#!j!!t zo4OokJKNuo#7fAMbg{FM;R%sT9J!pDaNbg%)0A7nu>_4P!lFNv2!!y3eB&Ka*fvlK zhp}E)-Yncnb*D=Pr>=L0RBH-cN&HHW(_jPgMOJ{=*=nWXKFT%=Z9?J0(by8+8t%#x zFk{J5{c)D@Q#bgG@8`d(Get4V>t-pw=YKFFZ!O>C3C^Po8bTe{mJY~@-Z)BEj-ZRC zBpcBqQ_;r#_UOH51&CDANx{V8?j@0x{xe!ny@bd`*-=0C99|$<4D6vVD$blZX5kLR z5s;9wv}|Fc&0^AZ+a+u7-Jx!O2Bo;|aoCq%Cf z7{E=$j3xhCmndE1a6wWrpKqeW4=>Gy^AjK<%}Z0bm8n=RaT_NqWvDKBp2vODf5v?T!#f&AnBb?<2-_Do^gWql-?Ah`=4uI`9Vjp(}MD)2~lc`St;-&gLSIteU* z&#{?TTUbr9to;=D*^ z7u!G)xalKj8@vnU7su!4r&_aio2%S!)mD;l?AUPdtGk?`EyAV4bzD(Fl9}?w+V=?8 z%QS&n+MJ!(w&IwnwnlRK4D1Y>i2aq}sidBQJZ%~sDkSB^fcFt)rY&govHh|M^*mYn zoXG+Hyh<01QC>{d??9dk%efEQ?hLSlE)$pRf9`S5=ZaU_j*P%90+%By&ETkT=C91_NF2w+b#)f>Ic!Sl!zjt%n{I zBd+>#tz@P_-DvlgKDN_WC~XW#nEDhKx9y26pYry<1|j9&LosX-qaQ>z_e2J z6Q@l0yV1Eqxf>=68x;pP_< zX4q(Tbj{?*qYrLd7F%5l^fYFr508xP7LLUJc`P=qxErNdiQi1l zZjeh3G|L?;EF3}-|1euly6~&y+UHBVbc!YaO@ES?aUm(4cj@LR6dP!EprzH&V3Bps zq;yZG{c;ztBc$WcA^e;8u11DEgtm71?Zg>tn&?f&3t91X}}AGYbgg|5k;Y+B%7~Y@nA34Ph^F2zTvW3Sb{^olB?QF z_M5%8^HqBFGQ|6gv73CCXj1K>};@Tr+xSYi-md)ol8TCuB-EHzyk02TD>fY zZz3NbPZq!7Z_C-T3Jnp(C443gu6i&YRi?Sk5;Ne2(wLus3GaSYSY|@)1Y8E+gvxKb znK;1qm5VVcMXqV1N#EB~C`zsY+XK!-^qc4a#vAA}SsHwkWQ3Kpl~u`0hG+mt zV{}F#X0B|>FacHI3L(;jZwOMdW1uc#^e9dnIU(4VFmUx!H05)NyechLW^agrKPkp^ z+46uCmeIK|{taE!srZ#D2F!2VMObL^nxb1;8P>>qUuuKrkG6-0D9=_urLU-TTpGiM zzG0+9haj4aHUd|9R1W#ZSWAVe$`^htXhu?mA|vXVWjhw9^&-6BLamG9ex<4x8^^Mo zb@yYb@8K4ShdZKj*=>}pp;1ROAKe7v*{84AkpBNJ0F~N_krt>;r`cAdzCrKXyz9_& zdt*r6btHie#{8?bYmBgXThfnAQ<=qk59ip?M+yUMgb z+X@La5?1Klj+ve8xs-ih_pBt#+ID;EwIlDJX^#+9i$#OFE|=_GZnC9t2^wemKfd0v zIkPTW!;O=UI<{@6W81cE+qP}n>DacNbZpzsdER~Y{&cF&KUlS@)|z9EagXalY1QWL z+H{J)iFI{aIDn-aMM9$pY-`dvK(y!}J(bCZ&fd+TgFCmbx|XJf-s++)-YrGRd4cYk zKo8uoKIi|WA)`7ov}k#94}3lAnz9F)%E?Y1zIaZ@9ovTV2; z@{sbdeEwer2-JU!l`>!`aZ}FzbcSh>AG7>FK)1-@8i2qrHk<9`#Rcdd14^X-8XE%w zq()_Fi^2ILrjprIv~WN}xlqEAX+1&-A~<@9btk7L9G4kW#`*a!~c%kAd)_RCqi<26C zi1;;Kg-40m`MEI1F4>wk1?`H&0De}TA=S)2z8oUOp~+}|5CY~MXj+QRB1o>NLdr-e z=lWLCj4_~mS5nb|%K=)Fqe?*WT|aZyB#gZ6?^mzXt%|{EPLj~FzA^m6w&SmgQQ_=p zFaO>YrMec(am^wByZZOQeLfo0n)Ww(3w1E#h^uFN`UF2v>8(px0oPb3$QTm^y6=rg zSRbz}7X0z43r9rosHWB$@VaMfVymM)cpw}{>?wkLscu?IhP zDQ8?M!wiTF>f>6pMG4IYf-C2zJt=)U(t~-0aap9Y{G5?)sVF-dNUybKD0mQl`7FTL z#PHD6+(J2^DZ2{ZPo8ehdr|~Jcl_DK4xgx$4_@ZLZt@K|4z2|j>&oKsH!lB|xdjXq zaKBW?>TtLfwx9_t>}&|N;VI{K$b#<>q4}nHg@<*ZR_|4x79D8L!leS8(X}rC*!}08 zM_ylwVg>{@)QdB1$Uy^j9tbZqXajT~*0dzC;#I$^s0P>kj)38#&;$U=kw7a_X(H97hl6WIadTRJNAdB4A@mMB?H1MMQiLZ21)_v^t8 zjn?+!MqB|C3y)2Dgfb@r^mI0>+3MD_dXxKj(^;e7p1C(ehO_}MO1$Yj!cYU!-C>K( zhCU-zIM=LZoB3W7rjkKIb(I%316h7lU~-LRCb5~;K%_qxxs~!_Gd~oSUf0ESCLR`s zG)Db71}w+}P-G00*eFhTRD6}e0KOIz*3ftc!PPG6zHj(GtNvYb+w*PgNoe?9!R_y( z+}%xk&@-5nvcD>Arbe9oZG?gTgrPe2dBB#*U!5+y}d z`yENCkJXr%lIvPf^lxwZLD%4joPewSYIVEcn^$$U-k%q?ueX(nOpNupwKAH3r?5!t zec>!c_)GXJDmLTw5|tKCpin7FK0|J3Fgof+D^#u_s`TBt?&RB`BY}-~=V5^Q`_Yk5 z3|vW>ksKc2qWgi9;uBhGqAC8`EY^gj6gU_Ifv0kX+_MWE@d>$@XOM>^H?QAr|J~dQ zjz}$1jYQZ>u1I{gh?8-~jv##>F=nY2*g1WxPtlO)&2f%CWtq85w{{F9xY zWaLX|j(6SxrN8%+T9hbJ6;GO-?d~_*VZ+?2i17LDpP)}2qMC^C&S}R8g%=RghrT_V zvJGYoie-O|oiPIa>P)KR_5pp|ni{Qzvtn4+HANwyu`ag>Uy`V$=-p#pE_F8m*@DPTuz^?b~Xj+>r_hEBF1}PN{4T z2>z=lfEdDk7%wDlVs38!x`=-hP*!wX1|UXozxL^g9%-xjerS<nhqkoRl6~Y`O_L9Yq3(0t_-e3D{lMA1Rf} zmRH$Ci4|xOiut|3f=Ue()g$)r;1es9&CJx)R$Bo6zVH;M>yp5XCw>qsJG-!*U2eEY zkl_LL%y1JrSZ6U7R146>hJx+$j`#IYQS72kryGkHGmaw#!SGRwfe6dGHlkXqF#>3Q z)e?@ht1}>th((TE5uLKm@~p9CvRDl+RFHb^1Qx;L`)c(;;YR4BQN@xe@iG9s)*mTQ zNSYZ9|5UHh6Ez;o>+&yTJ>Ho!Cl36-^mj0$)%L^&EB%3n7-L-_=VKbf`w}bDfaq*# zcS!|Va$8SKmVSr^3UStDq>I?oih=tTqX@I?Lm%4BX)UJ6s2`!ovucd-ea#dhKUZhx zYOPlI?-9elGqE#(!H--HZ+B%S^t<-zoob-3fiY|bML-;^Sb(#k>irn#HRZ@aQ$KFE z@hAUy8|{~>`4N5u4wE;N-Cv6`|+<5=m&QA{=W9!cE3A~cuEDEsX1d!?3Fq|2p4n7 zTtz`f8cA?dLn>VxAhDM4=q1 zvYHDoYHcM?l08u1zD^Q{Q;p5I-!lq3Xe9SL9wYu87E`D5{8XdGMa^+|+1UH}$UFcetuyZ{XT`<@w(&ti5)#i^SN38grb5u(Tr#DS;+gYmi)|*O z;W8@#n+Fz4j1^H3#!!HE08$WZOGyyZ|K4sfQVWzF=#gow&{eZ0?8gWWlFcud;JULUKPCZ>{Ac$=307PyT>i-tSXETz-OFUL72jsDoZG zyoR;i7{VQOSl;M#xmfAx;a!7BDd_CnI7ZYh`m3IPq92S1wZY%ro{8G)GxWR`pEPf3 zBQ7v$0ltmN1tfx==Y~PWRpAaK(qgte(qF3x4i$?~MAcfN*zkalY7HLm9%@sfud0-A zv-|!hPMMNA*}eiUby<59qq^y@Xok-{;3UeK9GVho3?(0evy4DPz4eoGdwF-cn2H|v z1D+5fomo46wvBztFoNQ*NE%qL(BrQ>lKon`n?!iXC1DqLRW;0o!c8{)J>H#4v&!#8 z?8cyLwx_UU(53yuq}A3pwz+NSS_atQPW_@jHtRazo+zgpM?VFX731phrsE@`aZ=yr zj?Y?9zn2VDC6?CokQaI9zc&)OwJ1ZN9)3Z5?zm*JO)ys{OQ@sna`^%UwV2U^auOk8 z*3FumIJYEW*MDb%3xc}Abt8Gb42_!~$^r{{o^Jj}W-&J(4U*Pu>^vehM!F07Rpc zm7Q-aVMBL)zsEuR&&!il?|Z*UQ^z8Q$%lb>1Z!{PzZ_x8Rk7kK@ zkf{2+lk#QuUTp7~FHg?$djHt@o#TNd&7!XUv>U8f+vU14c*BJfo%o)l!MOVQktJFF zt3N(6!}q*;6B~!`4-m_>zqCLfskGwS$OI5=UO#eHhaumGDbejih9O*3|K+F%NM#1-8q-*=tE!ouOr%f~-44_Zh?8m`stCIVDN3Rd(=8^J{7t{@33$#l&#Dne$DV~l zXJluL|FuaMlXc^?6?iN{f&n5#^;t9n1^bJ#;g(dRKqdelsBodL52$NIe=J0>K%4?3 zgolTRAh{l*z)2c;MiYJ#2tQle(`=uT+Id}N@|0VQ{wDhV=tF0@qj||1!YTt zDM>rPWMC(9Bb(9^2t$2@@;4?~>dmZMSVh0SD+KSUgFi<+g8{;79HzS9g@4y$FcOO8reH+2H^L*hrYRr3E$S?x{ zBfpPRvy>_h2je~ul9gmbLl`mo#PB>`0mK>z>HgN9Hv==gfdA_S!~g(JcIU~IC)rM^ zR&#fCW_g@fkMtc+eS+(J-*;=9?M|JYUbejlFJbt;7%1B>Cm;A8D5z&a@Z9!Cl_~^^ zXf`Jfqc{&IAJ^302+!+QvwS<$8$Z{I|1Mu{y!c5^0?85EFxoyV)m_xgR#o^uzdN2T z@ccd_(!M|LyZyST5um;=PtX#H^K1qdWM;s8rG-HiY z>7tw*bKN#?ktQ{?Hsi`zB=qrlu#>Xz@RkAkt1Z`n8|C_Ys5lD*jU78qL8K`&M^}3Fdi@vD&e1{z?X2^QAYA^|=;2DDJt!6^ z;dW`V8gnc?c4hZK-X;cr+_{227R7Pn>Pmdw)cOj`drcD-A1#Q#jT~kEWH^C10(*+h z6)bq7gYbYp4DPCh0d0SYW7Zf-fbztUcgl>;TJk_e)w_6XzzYTXO?w+{Iu?;kEoRUY zn|$#i*|{2L+yms+uPR{Z1yN5M7ZtmmzV+p*0^^@AJ`AXk_`-UTY4e!9N|0zqH8X_C z9F|pYUG2C5cTt@|6jN;O;hM${Y8Q^CT0*SHU(F>%YD9X15sZICJ-_2mp3oMwt02{C zkXUdSBmu)|(X`>kms@Rqz-FgoV^#PmP@l8&%d_afUg(JEUMoluVr+zq_dMDAE9U=T)A5$w8l)D42!e_EPsY)2TE(WV!iRanXxZzOm8 zUCf;euX-WtuNaedufg(bNhUg$S3|C4-NbBI0)^3L0#QupdKLA~pUNJf^ zx&|JZ&mC>TH9;!0#gijtMj9G`)3vEBeULUSJz5w1fTE837>k`ol_6ca*F1J$&t>8B zcRIiy-Y(UiotvFu_ss0rB%C`am_wC`ce}_x%WsX5oabZgO9$5`kN2T#r8MSB>yMC! zjgZmc(iujVY3B|w!xZ3fj>}^|KhW#^#9r0=h^kO3<9(bPeEE42cs(ml;^pz%=3aXp zW?iM#Y&s=Z`Q~y%r}`9wme!y454Gh#mu{!?|AD|m zyvE|G&y@FvM}MeVm@C+&WtwA{U19tG?#K8=!7-{{lur5g{yp#bjrUf8UHdAi_JQ`A zX(sD<-@eoF;^%i8V|C-Ar!3;7Z`NN`*CYD6C`<+czzHY@T=RR~GkM0U2T}P@v?i9k zg^ksN{0Wr?eVaJ020FT9XUeGzvh~l@ zX~Tf-yK=#6Gr!O0HaIU`X2@`C{T2w*9u6&~9jauMGY(a<*qZtYYdEzlFx-St?NEMQ z*_cX!$O5^NN|6gG05CP?62kS!%u_R^3qX=D4*7&v3(IA##KOlRiNuN(6)35#jeUB2 z>;~*W)U($czwm&V3sQcX6?d{bFu_3Sr!Ec*(GT6F|GnMTU*ksPLNNAKuNi)>ln(=@ zU0jl95p>xKtQvc8EHcKW_s;i$HJcn?;N zp5TgH4E@8DM^r|?JjWkYE<9g>RBsLLcW~$HV1_O9r?NjBqH5oCf&t0E&=R9VNaCSM zhGo>AHg5ga7Vd@D_NA)dDmI~2K-&-UZa7O*rTlBj2|-B(ZcINtQcF_{kiQB9hSmP% z6V`5IGwJFm_)sQ_NCrw20zJ)XHeT5UOjD8*l>{7Y(k+E@Oj7NrR1leuBnxw7L}B@$ zjsx|b+HvJjhyDx;0Xj$0b>{8Lnm%02S4?HK+fTpwK{5`aB|VX7zAK?FjI9r`;{NV* zoE<1xg|OROFDnIs<$y)}E`WvVSxxdPwdN`Tmt|H+s1Xa{`*)M!LAm3z%?(A*iztJQ zMG;wtM6nO*0e;)A)Wl}1qq)EUnufe@p3F^u{z-pCT%mMjNttbF)vc)&IX5_@*Mx*} zII=n!){o{RT{xY|r!+yrt);c^IaFoxm{yYrDmIAEbu&ti*LxuY{m^Wdu8+32=a+7* zA=j+P(f0?w`{|xoB;HdA^K5xVg~j$lqT2S{oc;chyVa^$p7%>3KLjp($Ek{} zm9C2;D==uUzR|J&>Vgqq0=zqhONu}3Z*Rg-Zt(`7nRH;R1~Ho(8yibYO|Dm&EvdAo zoN$7Pc(9h4xxuSyR7H30g^edOR_57g&?5fR;pC8Bo*zy8P)!1G%#OnOp7Ciy{RCrX zDk_ZX+;PH#9ouWG8!vf3pO9;C?1){5!l3rQcr5&%q3A=mZL&HRFyqI(OIkWc zfS!c*&<2tNe4&H=GbBnDFxxWE&o!^jW@v~PceVDK5ST(Oz!>){EQ=M# zSwc(AD8`wkr$Ctg66xeepa>}>tKb&MzjH_pLS@_|gm@)Akff*kQ)>`tu2-&BXL`{5 zB|RdX4H(yppr-D$OrZ3*SL&Ecgl8itrK|US}ye zQr(~(?Bg<5XNssDU^cu5YY`3xB4NRZP?TwAjDl@V+yg#8ogw~4EBQUN-#jWDTPh&Uah90NdFQ8 z%h{`69q|DCZBp2(*F4Uus@QQ54)slxl|Ma{6vI`3L^O23KOZpY8>Ya0@m(PRY2U|3#d0|;kS$gl10 z_o~muAU@yOqmTQ*2~uzx#Bq3zX12I z5WgM(4%l_$qmLB-^)?kwyn1O#uErw0;do1}uKn}D3H|2n4aRS7r3-!?r>2tQFo|5x zPXtk@U;yuyk?GaK&?g!+cn;K3mnmbac(Fe7x23oIqEH_uMdjRGFvV)S%f-FP@Yv>C9d?4(wROBr`7i7mg|nub9Q@X?F$^d_0_qx-OXBqiIkKS3=N%& zvvVIbAw__=Qy)^2#gY?{3S!jAZm}s@w|&~1lj(Fehmj{chW|*ob}sbqWFQhjJv%u- z?n2AUO6nrjm@42Gw?P2nI%Fm1CESP6oV&>IWobcWWu^tQ>9k%w0|wMt0D%Gv!qy@t zvyu8~hQU9J!OQb&+l$k+^?%D-iyM3l>LI(=z-F;BaJaQ2w!SQh{S#x59qP4*mu}ZB!$TI0HC{F~a^g?jJR5f9%$w(rPI! zeLIAynBf+z=67E!J2Y9Lbi#(<^~4Vqeq~4A zLnu;;bVom5*gS_T9CwwRx8`^!F%TFWLb7ij84eirFp(=2jm=Y>M+6HR>>g)7Qv3M$ z^u5%am|I!l;^0V=Ajub~*&vW3ZCvZ19+7!!nr!NgVr}d!bHM~N1`x0ti>fOF1su=q z{wr2KxJIAZXZOR2F9mXq!eBs0nriEJ8dBVE;E?8(vW1+m&L{a`c9>Om=DJZLlo*qI z&r*I=H)-|Ym%_He7=mEa9X#bsjgjc!NHaSzsXIbLgI-qtr8fA&tKD+@IQf)4acyL# zwQu2?DnI5W4(^jJU*UQ3t+bx}o5j;#wVgcMQd z8PR*!P5|n2CbJuzJ(w82(jWI`gH*>+y1s{IG#b~}Ju6-avCaNAMS^Ffv57nNbp ze2YKvJe(Jc<32~OAN+TcyE?Z9Rre}~^YnMwO7*7w1Wye2=`YhU2{Ia>>Tf>5k6B*A z^}MWjWz8_R()<}P^1Hq{^W6X53BYLAfBD@P{dLyZd)H^Pnp_Fy>idoVA_&*$sv+n~ zpl_@9^eiQFk(%Ru-MVMR6kA~P^Bsom*|DD5^Eh?hs`pZCU55+wgRr87}he;LnLdcq#`%73y8>$RzE31tr3*=iA0BMixl0Z3{ z30KZ^S4u_YDmnwO6`HH0rIt5?fz8H7u9U&gP;99wBqT%6|e>s(16fRsCNap(b0uFo_fR}m3TpATk zHMK`cF|Fs6>igclhd|pVH(qR$9$C*<29G$f0cO(_uA`$?1mSGVt!%4kiyU|5Eb#$# z?h;MEhnC>BYfiZ>WEtcbWDuCEOa<;8%y8t72T>b9&|G9L+=y!{RR8o<|JN!i&?%BUxx*BH*JQ5-UgY5rB8CnM@7W*^1HQAFe1tm?aM%{Y zU2q6Ag(7Od65d2UDD?V82wS~|wEYf${X{2#%7W1=#M8xp(=0kG&P zmFx#+G0lb&AstD~4hB1Jua<|<4l_v1m~^)6i?sm@K_mo>NDzo#s?S$$6wy7XdqwgP zGVyH(I+;}Cr0z~e5y}hI44J{HE>%KX|LCq0@s>%6P*QJ!%gfd3EifbSP3UnChVJee zH&yPVe^FziTK98kU5Z5N_+;8x5wK81?q>aIYg7MXjudByj+6i_8ip6^p?{7UX#9G1 zcHF#Bb#%)ST;Nx`4rTfrAZ29o%l^SC+0sEK{i^BT}d%n+RUjSmtGT+YweBAmS-#orA*w)Z?r{iz} z^m+$+`u4+p7+sH}zo;A9?T~GTtN!h+$RN2xYp%X;TYP5aCJ}M@h zz=@hpX@Ozq*vN>uso#J9zVbUIm*dfczk%_6%__rhJGkrJJl`6<0>&o}`Y+UmJ!g#( zOT+{hH&M!-+|l_;Z_FPb=6?d=T2xhKrLPa<%rPRY3~#tDy&Cjazq+#2b0WC{4lZWi ze2HaTpJ;KF4~R;iwRA1ovI8$hwI7 zQEy_7VM&sm6UQuG$j~Rrz81kp$rY0i$^}~97QXSMRYVo|fwz4Wn5Paf!0}u(bkuT|yJX3A_lmZ)&9I_3_u|2c8pi8d=E zF^n38Ms=(`fGTRcNagmb9_t=(R)K*G%)!M4$Y0>X1QmAuSumZ31|6V`ED}vH(#70Q z=r~o^^}TJB=;S8k%b9E;g_M@5GiH8SxfeHd<_M zQHLng{QVtqA%jy{99U7E1!Ppp6J;WQi6L4%f<+ZU^fOfFlY{CcO1zZqJ`Ww-eSiCw zbD%{DOdIGIxC2x|lQSu;-s)-DYHwbm8GF$|V;(f2jHtR)LE0_mRj1MHx~JB#`?U>*#|qo@AtBY_DZy$XqB$FX1rdJ2 zp@i?V)Wt*2G?p7uv201X*Mhm?alrn#b>{$JX2*4 z+N_ck`{NW3G}6q{imVm?-MpiZu1}B72@acxmz17y+Q&_tj<|6Am7PV#%^Wr8?Cv8i zUb_>CK?PlsOG48YNq~GPnHV;zU#L+@MBhl`&}MNCE1a;iD#UUB9nc*1-Qzn)LOfr- z0h>k}m|;GVObOs_r9#MJq-mCL{XtBO%K^!`s+m`JQL*{r*P9I_^#aH?PS!Kowj91x z_E!NLbX%Ra?;aU6RL!i-{}uJModB0vq10Xo1@ukFM=qrj*3aW;XWjdm96$mBjwttt zEavkW%jLVXvC3t)-32(ZeDZvreINgh0^~ov&K8JfXc*;kyl;;sY1+;oS8mUb#ylp$ zD&YNn_&?N=w(mPyy55K}GO_??QeRDSA;&_qGzZ=;cja;p+D(JuI|9ACGk9`D(HE?(_Z;^_PgQNPV$NIyFlSsbUoj1 z@OiEuyWw9?8&^5c9oS3$8=dsx?YoZig%DB!i5PJH#=u4Ra1E(^P#?Xr1K096UN z!CCoiE_PLISPozqNiNIbb|y93!e&>V$N5k%=VLH9(OO_X8JhB+3+&Ns*5~v$+f;g| zed~+gDd1j&Jkjg-tZ*H$`gz9J`*B9fKb?I3zdxNdy5uDT4B}z5g_5zcU+``fvJ~f~ z1|-P80RQ-Pz-43-@TNtu)QHeT8Bep3B1Qna2u3PUy96Np1mgXDPO%ZAA^D2a@k6s@@krGwrnc#9VNO8lS=HO^GPDQje9{Bv>; zRDT#v$(Q`CVKTw(eL?GRRcHMZ=Zuwq#}}bCu@>CJ&LmbdWW~GQE!ZPhz!3%sKCPW) zR%VScCRD1^B#2vESXS0nXtbFt5TF!qf^R86Y<^svg6!ku5(Wsz8suv5azkW?F9#~8Q>Ir|5hh0!Jwy?@H5Fb#r9j^k2d)R8kIOuFb@<7+ zf_*STwoTzn*ra2|G>CV;=RQ#V?{5p+%_cPM5eV3wXWYWWWD>?XOE^H0a9V5_oD~xA zbc^*5nGto$_M<4v_*GqnLX`!g9Am5dk_XAIlV-NC)zLmnCx93?1B38Smn=?HQw9yk z96u#Av)jp#a>oNb_hs6UlV1tzj*^ds`AWbY0&|xB3~fN%y(3+x>aL_5%Y(lK)1V4z zHee9h#KKBZ!%7Q7BjXKaAjX;Cqbqly+tcWyGc=NjLJN%s&QVRUa)NEjR8Ae>UWm9@6E9+Ex`rNkN@@i%#9`~_sR z|K|7oHFm%UO5Eu2g{99H>csLk!bVqBQTq+3G9u8cqW-fNg}$-&w!Jr&dE-U=d2vq0 zyH5z3<-L&F>A3H_0eGkZ%`fuSo$n98hzvQqE_z*OygXgt@{SwQ=Nsx|J)GdAR z*=M!1uy#L8ILYzZmr?XSWKzchwyOC$z9h^S4qm=Y0ilVM$^zxzmo)6>Mq;{tU(JRe zTP1e2n%!8rTui?I<&+M9&et$HFMbHfZLUs%6pPy1+Z!8|zzF)vlQu&@NO)<>FABzz zF&oQf(%~kT_=QQ3$BjpX^H4;9hiyluGy?R2hUlX=EjIFz2aJJ1RKpcwALFR0*7Iia$Ae7e7a zf-n~Wq~2fA5)3H8Y6K8x_0}^5U@X^nz}#^0^n3ZK{(QhoRGpoj$m1w+7X`xpz7@42 zo4^zdMdP9B4WPNei{#>523h$#fCgfSNO(B(1S{djp)F1JE598EGO>ek{Ku|0B!vX# zX;Twnu&)a0WTCj@srf2!O$)$OBW{M;LosHeD$O=%Vlq4)D(-Ii`&CYSFV&z@6R7Fd zzZMQ2-der4wxdNA2Ba6oIgPVDeu`W6GWu|MaPYP&G$WwXh+}0@n{ih&PaG#`+#(2g zZjUxH_uZJH$X~I;6O)lsZU+N%qTH`~4T&xK0mY2XpAh<9zxA4TfPIIBAq371MloN@ zfWojrw+H)jXJdnzfdMKdzp_uADFfjJjTnnlSI`|A9%Dh!D4WBHYz_zUaH;TCE%=jw z3vENDRJ^-Jv77s4lbFNOINzfT`4Ael7-CbBx9g3QQ{bQSCIML?Hc+!c7Po1=dfwD| zm7@<&C)3TaxUESDn8e-QmAyTOq_ZxujuY6o&i8l&*wh|K+cXg8m_`HB@L;s$#P-}; z+W++emOru;<-!Rxf}w^288H`wclub1;i$ufjs>*F0TYr!g$R=^1yyy8!v@i(ohtJ@)C;-Uyv?#p>|w=49S zJPx~VuQ`?TdtH{B#P>R&w?kS<2x$YT^lo}ia%D&31$G+(Ej`zCQrEQ18voT%NuYcPV#-N)f*^V3a~wxeC(4AD9iPD28>#z$Y9N@ zT{$srrZn1KLR0FH@VRg9INsXkB0X;OGq>9u4gi5S z|6TtFU^(E-uC*O;Gk&h>+S&{rH#&K2F!xw_SgK$ifpQQP22~xnNI?oL#Jyj0>4E;D z1&}VZ5yu!3@@N5!;@Ft+rA2#CM>7`#-uM)8xw*E?Ht<7`Y}T7#F&TesI!eJ?o0^QJ zt!}!Xvw4gvOR4H-LuIhDTC4&1gDf7q>m>`NpaKL2hm0yr5 zrX)yr-_NlvrGPHv1__dcIY2@T*UO>>w&_nNA0if^9h^Ks;R;g_%ht1mzDr%+oz9~G zW{HqXlaFR*&o!;12@7u>3QHP>1FdaN;g7}IqNd{PAlXw`M`a#3P{T)|>BcS@vR}@~ z1P^r2V9BnDDo0je#Oy9>wX)gnb=5elOdg1ynSaYFw%KIGz{E~$tlmhtCk%Zin$<*~k>`%YEQo2SrK=lyd$LUrcvQq@jV z>c{QjtHDI@Ye7^ZJzU5UwTjBh%DQXduHon4>YD-)Sv);=={KMKux1!?y_le? zID?V>7-j;E_O z(a2SCt~qc_7)XUcFc&bYw|h;`4J3^NN>&Pqv0Bv&qDON`Z(%n8Ka~k3Mn>>FdNQc~ zOIE4I-AvI&`z0Hi&ykhVA|M#teWgUOGhF{${7+zx(Md zwt`kXx$)@~4^4gMf;N&ccWM21)F2IvgkL7uWg^8U6^Y27VwKl{%A`a>Rpk1Rh{4cK zt&QhAMyI8@S8w_!dIw6&)`LMB0Uy&Y%W(D^_<80@()9!rD>9U@=H>n3aa zJ@5>T#)`Q3AkyYLtZyDa4N3_R0p0tli6P$s_KxBv820~_a{o_u9teh2OKMo&*i}#O z2Y_7=fCzlLGG{@97kO1Ihpxnh(rR{5`Y)FsK2lkZN1$0=)rwE5tYeqXi3+A&ZUgYH zWu6rKH(RL_LheaGa?oP{tRrm^P;!+O_c+z#$~b>2i3V6e{>6WP_a7gd&JhX;ERhFj z)K$POFDcutHCg;?a4X#VDwVZ}9+^WoWx&g0w%b$^qDGETmCLIwRqW$Uk7PpdXMN)bGKUJWy2Q{NWI-MDmzra|iO}vuPRwI5diFOE z>_Jg!+pM;bPmwnXz6lz5I0HOXhBGI)p9k7HN~WhVQ-m_~(F%&5KxyQ`6o_#c(l1ER z4zwW;enj4VVRafO53Crd*BJixe@vEzjBPg2WxX1Wj-;E(fT<-v1(*&sZ91cIMf6es z0i9%uxC3tscy{Qxwx0`OB^P?6gA54PxkHzrnBUs?uF5S`uKopR z=osJp-;fag2wqyl&XOHIEJ zFiIIR_CwhO&sr%OTL^GN z)tP`fZSiwQgFXnuqI&;wppm|{hpBm-~$x_LI zUH-XUO{_-J4LBYb8!R)=m|2mkJ}koF858bw)|tGG2|NN?zG+XuR#E4()v zoPNRI{BxZ;CRml5%$;yUPc3k2zCG$v3tUu%C&pSZpclTnvZ7B#FaMSZ z7_MQ+4F$9VEn@|f$Zzxi%VjD_u(GNKWNYQzew#=1^Xnf^WsqgEVMO!`usg6L4Ii=e zFhPa+w~Vg>Ez48DO8J8J>5?SEJw5fyBN#WR({P$SOHbh-!S-3^V?oR2Fd8{?KY&#Q-XHb8b>>mVe;F-gC9s!q!xyt;_{YDK_{+eghZ&O@6_P^n zt)!~Xai&1c!-Ni85n0 zu@s%p2l|yzKX(f139b%5g;~`fNL1PhPKaCNR-UQeCG>%ktWCQpJ{T%1L6Q&1TXo%b z$y|a*p@aGbW2HqFdtZ|B0r}>;{wpqR*JCcEO)EYs#lX@m4kz55(xhy z2@XbpDgd6YP%d?hjSi2#oBip%n|XQHZ=U5aJwATve));RN0gu%p!Mr%yW#ESP0!kB z?w;lS2=?mZ7*(K(kjf?$PNmccnD=sb2cxP>JS3t}3FL(&)*<;@uY!C7!gQFn5wS`OKSj7+J737%#@?B@}0B6|fS+ps9kAYe0%*1BkpM()1P z@^QIXj<1qKtr?X{5x>_)BXECA0)#q5->|3-hhcbyEN_ zJ9i3CfzovTXsVDZk8fU<8!IGbq>TlIi>zr&*=suO1&*=BX-y(y&!jM4tlbz8|qyv)C5N#oK$&v9w`4r-T(td=9 zr$+Ihbdu`dS9)cYjp0KflOl(I#zjTEx24g` z*2m~U{{~2#&rP9;rR5Pl-M30>>s+<9qx+whjD1%~rmy*nqti297@* zmaQ8nGkIx;@HizK7crtmfbOWF161(q+HfmdX3V^EpwW}+`;GUiQUlH-m3Bmn1iJ%g z7I1|kP|p3;U(IL@m0G7NmeB+W!2wP?O%-GinM|2fb#(9Xp^SKIc9_mZkDa%amxvy; zt{g87CW*cZU275`E=X@wR5nTwb#q!w3S28mY z!9b&07OSQ#8Js<%-lqiZjMnZ0T=jKF$4K2N^xAzR{k%Sjqb9)_rURu#OdAn+7lSKl z^IA+RL*duN7-|B8Zb8Yd*RPR7pN&BAkvP`&S0gWV3Ec{yAFLY3qI*YQ{cBK5Wl0v+oXJz zxE9fDhJOUovQKYU3pV6h8g0v|2@zv4aX(e^{xTnk1Ake{rg#4#F%u5{%nm4ob;>Wr ztjJyQX>eKMfB;XKp@?pD8Hzy4ALd)f;;BnNSf){rmqaH+97qo+5y`zilyucxAg#2M zY_!wtRIO$ALd);^K@V0S?KI%CG4uAA*~-%yh+byH&||46#yc$f1Of_|;z2y30eFhF zVsW@$Cl>7)HaL_{6Wt!yp;2Lf8&PFqP27QCOTLMYbKP zNE$_9tLZD7c4Ed#QZgR?#PZaEcu-y8E+=U0l$lE@?Qz1RW0j{MF_GYwVq|rhW{+Z# zhheNyCds5A@lO*HUL~{A(&n@Cv!{ibSq@4ex*cQ>B)+U@Pv6A^4%TBvPc#9oQPf+8 zS{U)qpFfbJpx#{U*LE>CKc~cT*?h(Pf|jy znhTO?^V)(Vf0)FmiwzUZ=zCw^()2v;j2s$lTRFquwA<_v4e+63 zk2nIX?V-DdjPhjlEl6oV^VjX!wTU@@2>-7aaKv_pi~tA2Bis-NDW;Lzyk1kBK(m0! z(5;^TYhct2oM(S^YfI-+pmG68ID3|jjcs*xwf>Ir>_dX2p}X7X`_lt3H8v2UylpFd z^A?R5Q=oS3*i2U$;`dz}iX>;Z%G@&~As{8v5kg6lFvHeKEdgI`XwGR}8tmxeI+liu z)f$a@3q|L!N^7R8K#)wCe9ZQU<)DZiPrsBJ?=j*nflcFxU9@PGZr!kE*$?g)8=AKm zytC*@HN{~~dhX6z&GUNxXPbu<)gePAlQ~miSc^f~q3rdpHx-~e-ld;SQ$%)eoG#3Z zZL=@KK2-Ka^x2bEo4Q{sb(0s|P=`KhAe)r`Ae3LUFc4?nwr9`pvg=N*{cX%7KzWRYN&?{XYPnKf@o1zJqBS8Sg)0G*k56{ojR{S}c^{Lixf16IV@#C`G&im*`Q zfKWxPsV1(-U5YtpW{w9V56;%cmVo_s%(;_?NCYbabq0fePe1u8*R7BSb875#zX+ZD z1jz``#?_-Hue*+OZ)q!2EE}r%exDU`TYXBPT*b4hKTfzp&~a{k&trmXe5@;!hYI9g zKv%4!mc+mHd_GfS=9ANfB(J0YUuTm)-=@mz@xsMKo$zbJ?*tX;wqNw@rDkKBtC6m} z2(hwF^pfG%T-ERHskrE}AcTZjNs zlMHIp70Z?%^dGbw7dY5YG_;&kl#O~jO%GXMIa9K#imdWUj zE#Z5q4e%GSAtA%x-@O+3KQpe(gM>J>2t#=X>_`swfjNdgQLk}vu$TnfJ3B*12!r+q zV#?e|3Q2Sut^uX}LyPyka_|jVSWJL5Z-LPtn0VIwSPPdXtXubP+Y3$ycF7Lnj{?tV zPVosGMbr`q{lFehKqf$(c!%|kX~;H!c zfRsqIjlCV1LyBCe)rv|sdr>RSk-{r|8fAw>5T-<>O;Qx)HB#7b3#K=3diQeT@6W6c zJ*64>{*$%~wo?C`y1g0gLi_Y9n{)1hZ#X|50>trTD6{4g``dzjQZw{sMj{dK5$lKK z!=vC((j`iJNmxkbuzOXZvXm1kH(`dY zxbx0z$B4b*M@^$+-Fp}urt0vHqOp;Kqz3&4wcY{>J&F8I@poE# zC{trDySfmi>)y$_8&}j;1=P4-k?lu@Wfc+GqI&yg!;8|KeW6)|lbYC)p+^5KbWsT; znJ|;aiy)5^=Nd?fLYcO%h-2GnB&?)+kL1oRYmpqBZ4;){B81 z)_CnSSEf=ISpMFq5sTn!AIkZw={`*3COp#T%02ZxNl?nVrRm>X!Ua+!l#xvl^P$zQ zqzw9GOLseK5PYsE@BXpmVehGa*yHz9|5He9sAenS{XJn-M({)3Q*wHOkM=t%xA!|Z z10E);v-b7UmwmiN{8<5oCMgBX121BGa}zj#`%GCesV-LU`9fFke@Y_2Q7>=LKJc7w zjnI85fPu>JAjdIkKTste6pBoR$sWm#pzXMuyWM8!{aA|2;n0duP?Kyprsd$4S6>fG z($Ky|c+lxy9{d-S!qQZ%Q9~jasjw`~tOFEYUt0qLfe`JU1URz6-2<4*uK936z5}|4l`Y-evT5}Nu#T9LpVHE` zFpUnL@^Yi#i}=ljY3S`(nl3cf!J`(#vaX zG7rmRIy7I<0H1)sI8X}S&?r08rm$cmwn=7#62laf0O}?XReGXo_}mCu;joAOg5o4q zRpE}h@E_&Nyj)-r-ikq`W}W0xv^jVLD=(Dq*%TEs3BTEqh~qM10z{sC^}fMrOPYL) zXB~#V5m_W%QTs312Lu%jW74CvzdPuCB^OkIybae$jE+#Z>0Mt+2MUfm53}onw!8gI zV&%RA+hM(2Ofdw^6r4px2NFYXJ-l&aaT#Guu=H)ZiqtV{z#6(0W@`wlb8x5^9#G~NiWF|-+WCAh;RDu;onGA^4ioks|h}@ ztA>R0E+FZy@~Hi8ZrRh%NCGakHR9D!;0K9=dpF0qUWmCLZ)@~2G6gF&^3w#OHQvyL z^sv;>6I2n6s7v%3_{g=59LH1bqZncr9V72;=rib}S<*c_Y=q!|43;AxBMO}-Lqs$3 z>Q-z_ZvO94cjH;K4nD43m6vcG?5?-JbcogY^#{L!aoc(exOXB6m}Y&u-2oP#5kD>` z&%S(Dq=nO>e_kw|SiPsOv$?Z%^SYjB$IsD~HFri-YVp=RYK;hEgs5$QY0+1(EH)5k z`4Y@EL^WeV&xRd^+IFgxHRx*~+z_#Ya)AaC60#a#(6LGN34XUaHg_>AO`ww~K74z_ zU>`;(6}~+e_~+$YvVMJMYX^MLiZj{E(_XLZ@7uaQPIZ5KtJm-Uy*151cy}-GIFj;7 z((7@KU`X>;(4bDJ6h)Xj(H6~ z**e$?>-j+Q%2-u4u@u^ouf7tSB3{0(M@}1;lkAj~;Wd<4ynNZF9gQMcG+}P);tpbX zPr{%bcV^lKH11?$V{tm^L8~e+m{378%$_BE02K~a`0Og2PIxqLG}zii9}4D5vU|~j z&ZI?RmgJ?VV} z>^Hd?F7_3BRNy39lVW5QPU#e?axjSPg^A9HGV3nG1*?n)a(~=cK%S#f>i-MGi&Op= zDj#S7I0k4cq1aaWKK}u}{gZ(`KkwnMNV~Yv#m$6xsZ_+G!X@#QE z8KaOmkJM7-ZXYd)qmYLijN^I&jPKMygBLpM8Ywd57&3d%g)lb92zs~NRk)Iq(SXxJ zU^=r@iI!o6l-W{GCOSph5atUsYGzn4I#Z2XQ+L}L-qor{+^$Geslv&~XCHI``jW38B|98=17v(6s?dz@p1v0kGgp$?)T{f!Nn63CcFgcj9Mn%!1^*IY4uN{ckh#(|NY(WBOr_b zRm&*ymhLrDLI3|h|83a^727tJK`m^NDpbhTF~G!$_vifYoq+1yiao1Mn~tbd_wv$~ zNum71H|B=jeJ-zi!9WW;j$(M*V?2fG!L8bC!aa!`;>1EIkz?E+cnk?vIrTJ28#kB} zL;dLE18cy+#|HHufWZ?T68w8$anPXVMFVI9h^=0)J6u7p(^;&LC{aLNZ@@gz(NGE^ z1UM4wV0b!%q5l~FexQg>KJboPbL_DB{qDB5-p=06E{F%H`*BHz!N_NUW&b7jmM`E!b29i#AOqYqG=pyqnB4C`!;0JtcQ0Ci>LU%OHo4Qq>tP^fw zI~)%>_3JMhp~(K8o|0g4-BA({A-R9;w{B^%toT>{@?7}mtr6(0s1SSK5@D`qi>f4R z7+1i-??UeSX7_GhU2>XIo8Rvi zrN+5!Zfe<3!;Z^-ZJQ66*meYPGGxpaQTB~mH|23uIfK%*Q-J+rq z=YFc zq#q;}vhMnR@3eM-*z@O*B`V*e37x2u1|lTBeK1V zH+FUgu26_SGT&`|)VyP*3e1~Fvqv*af+gmIm{4*nKUT5Gb1X$eXw18=-^?_TCs9mhkA&+|Wb zwzi@#p;@-&mdsk-;-RzRC@*L?cOqMbOzO#vOB&+FWs=#uZUcRroq;jLe-_Q*rO2$e zzV!Kb=%z{7lp7Nunlo-NmQ-HIE;$lK%&QcY^%;YmUqX>Mn}eB4Y2G<#HAAlzKOE}b z(3phFCM!OG99x^2wJzVmR0@R?DADS`>`XYLoI47GEmC&Gg+WzfxYT0G9+ltZ{#OeK zXDtfNkHO#ziOZ}an9NEX1KSQ_9RU@-D)$dC@$<8@Lq|;YxP4XKx?UN+QZnf_w=(8w z++G$iD;UxUko9ngYjB5aZN+kINRSetQIu!uJ{Lc@H$DSqdg%0op=j(>rut^-EW0<; zK{;M|)_4zi_n*zB*4OlYw8W(iI)RkeVwNXSc63C^P5xXYr`^ej<@Dm@c%c4*D`&2_ zq4PyXf}DngNwAkkRiF4n)}b>3F3tbxvgv z06LZ2eGi^RceRx{Ews3KWyK-?;Ad8DOCMt)51N|{H}Ua^ph%}gfk>klN~wqI6G6fS zT!h{2{dwk$K+8FQ(d++hRo#5xYbiLd+|7NETH4a+eIp~67t&~pZoEo1T6xa7jzwpCZw(sz%kkaK2I|e5)v94U(pV1 zQcKWpyzK1Y`;Bo0J8tu8gX5S4QCV^16bXRbqN%kGpjcYFqTd_o`@Npa%*zpd!A_u# zwt0BA$q)xEI?}C^EGRU5tyPVi8_+*Qhh_0Wkg^M)YWSF?Q&sURQMQtqTZD#$J0>vP zhyP{A7-y}FBCEOmP9Y!VMgOFHf;io5NX^3kW0Fp8;~c{%=+IdNm%3=^_`TIyt-w!E zy^)q;dJn878V0PQT#{w()dY$?6e$)?Z7DudUl`%0YRMJ38|!yz{3^C4??V@@h{>Ii*Dd*y#go=emgN@2c(5UT zLd1sw_cZoFL(Hycv=*UE`4Cwwmq?WUEg6|>JLVhYs1k_U%N?~z(}ZVv zQ2|Y4px8UVks**ys0+ibVmi=BM2>oHnzjXOV*vus)brWu3pbXwiN8L zK5`05tY|8009H9#^Y|(+xRLb`ZO~-st|_cpFm2O+^p*ceiTdm6sCW)(V>ZPsKd$1w z%U}hi2IS+JgSAHB{v~lWhQ90!hKTB1kbfeep8NhT$z+QGe*oNdAk2pv2lcnR&)t~y zW3%)|{T)mSQcC;R^}seMMiT0Qc-U|ek#HP3aq%PxB6CvZdSZBX_G~&RQOVwv(KXn| z_OhL1$A^~edBOevd0QcaK&je=b2cqOtj#Pu0KlEyF0U9Yn7Z3!^IYTwNkGth1K9{w zZC%~Lx6OK;TQ|NTchB;r>!ZSwX0|Pr_wg9KBggZmv5N~TD@@_;>2wAgj~^8M>B!UA z*4LM#CmbGS>q`Y`u_P}o9t11O2+6U$idLUMq@H$P=TFGMYD7eYe&6pZ4kfkY#4y;- zKNkc+bPL=!C^$(9kx5&nQ`X5skyt^O$wD9`Fg(@=^d~TU|31(2WwvH_$LTNBMO?nv zxDo=pmAP|fn?HkJsg*s~8d!=OhkVkPUo_TLO>ZHvz$$pbaS~#c--QNcdRtlqHR>H$ z&Q5-RE7jEJuNl&b4>vNR)p61+2bfk2M&rvH%tsv;X;U+4#I4dE4WK(_<<0E#E2vQ( zDPG7)(;gLLAXpoj*45XW2)=dk#R@syDe4Zd`lZ7x7{7}#l{<560h2=iK91mW7jgG=}* z!W9UuIC6qL^}kKM_V#{Hno4`YXznQ`O$GY_AN@>Cjml&&7!qfkJuGGf6EC3G?Mf<9)kS5Owp`I9%s&*8PUC!5~MDTN!Qxh;vvRU4wKJ%?#gV4lQh+&M4H)2 z3mXuFu25B6z5H<#XVJ}YnF;9H_rfz=&+`G%XF=Yti{VL1*vSGsv_Cp=f&0s z8iJEKa^vF!%aHkv%K)B0cVAc%PlZcj4a;7Fk#N(n36>=pnwp7lfPGj*4XsfDUd=AR z%RBTl2wGS3BDrt~C3NUf0$c0dpDlVY%VVqX^r{UvW?iJ=bFZK9c5v|ojb!gRc5+l| z+Y!QFCXK2Zxz4JiC-Q7Gq7O}SocuK|o@pfO=EZNv5vzTYJ2py%i+4ehA9@#Wc<|tu z0N5kj&-L@V>g$S45e?@a^!V@)b{FB=p}DDITw#`98Wnyff)@IaGGAlC3_dr5lX<1N zBq-ug)l#*rG@e)r>+qwdda%XYpg&hA=A9<`L#qIq(EVtt(7i6A@tW#Pod{p&Mw#z7?RDPh}xyOodgqWuL|@rkIz}*X-)0 z6qGuaou&;QO%*)X{zXVy!AA{Q3+5yw6hu%uvQLRZM zR<+`k|Nq^X{72GP_c`FcU+I@-uI2QOmJLCYCp{x6XmTb$9GGV4{o6Uu=X1ZuSJ$&J z++@%63s(51cli!rEZ6Vxbh0?tEne+)w{c+2H+pU9YU*n3&12PGzU*1QR)LQcTf1}3 zp?f=6xVreDq>ug4pe6sDfbIv_yTeuc)X1bG^~cyj#I5RqFbrGfmwU!w`k`mMi=7_bI(HU zTJ!ehJNK?aZgur9&kIVuX%uYJLFI|n4c4EHba1B(O*Ch9Q8e0JbwCaH@wWNF@vlOf zm{TlFR-ClyQ1K(;UN=Rp3Fd=QEIQl)`x4`9+Ar4G&)6WZWv zXh{A!yd4OqWs1CoLkXVAVXP`i^O}u5>uMp0sDgf<*i9@yF`U#kPIP&fpm%d95z0SO z*yyTzwR`+36lhfN?SPH(R$LF5BCH+&S zk9;ig*Q)q*rGcM{a*PeQpy0Mb1RXskr5?0CKeaS+`o}u{7&CvbUpi* zu8sNFag*LCxE|e+XKx(rZSHLz?Cotx+TBjqcjx(D1dlW3)oEwo!Ec)IM&FR!hMah` z%Ff&)+E9j8K0YpHZljV7S$rVL8(u&tRb$fZ1q|koWJ66-MA#efwxnkG56D0QQBQ~C zj-I}ro)06a3byw42cysP0tu_iQbDyZapI6lsVg6VmW<7?3rHSag5B35 z773IIsbpflmPrS_OJHXA1=hfD3xI-&gu}!^{M#|1gC#*7w7=s;F#tDV>5d2pR6 z6)!w$Dvu+koRt*%Db~I0{g!2MCbnhER;Sw`iO-?;f-?)gtkmeVum*y#)~pDgRuL8$ z&^&z4G*hkO5c7IY;@O)1py8+EyzOGCDaax767ocz#2^QEq`VG$X%q-|Q`S{&O|1!G>;c4K|UC7H8c;cnLlS1Nky7ntU_s%0^z^dQeJp?OQgp1q}B9O zq@2Ac8=6e)nyW!oKR&NepFy=tpsN3B0m5RximIr{D3H=}OE5BBXu;-L`i(v#rK<{G zHI;~yeg|L9RqL5LJZbAYj+xacOx`Bns%bL4o1SB?8Uyl@tWrA@7AwkgT^Hzht)*0r zxm>f0ES^6f7m+5QDynxf=$GE?L z%aXM*xYOs)+;V}BJ0q4sk284Bz}FYI;=C)fnk@k^yOCL#vSeUVL&5dBgMOzsxfPN& z=p88o#GAb>o81CdBJ};l=fTcHup5SY4TJ-_KL$#$QfG5n9h}HkSO$gcF+}#}`AVnADl}!McF$7f) zw{-2goHG~-c--$~!$;ubj;i2+&T~bU242Tf-$Kj)WU|m{%Sq{n`|4)ka7hl&UVv#J zRKsjJSvAIo;M78*Cp69#dy_^p?e+z|re|HbO61+=n@&wP;1vBYpZ~oNgM)27TTTcM z$MF!*(3x0gItG#jESBNtw)MDF@d8Gu7CcLNBah!tz?ZxA1MiIy9CeaIBD`2iRqt5O zl*NX^7A$qMOkGhR*jhsI4U+u~W~zsZsY-H7uu-g2v=W^MhAt4l^jHxt?ng>R1Y;=X zb811E=3Ee@;~WH0XD#+}4 zAsl|HuSEyQJW)<;xySv@>lc2KXyflVIYU62cs3lSm;#t3kaO>4q_e2sqZGS5yNPJ~ zZ$1yhOn}!k`O&`!HTGK_ja-+X+&GxgV+e6r8*jmw)Qma2I{TI*RwEqCpQ6w5&rry- z#%jy7ZqW^^`n=av;gqzhCeUX+46~N$82i}PeyHNEYKr1W)-l|ja#%`TC3W+Pm8qrY zEH{$6=U7TXw%KKbQBo=+81X+8eb0$>Z>cZ@G!lQ@>f58vzbg)Lr7?Wp(_-7wjY0-q zhq?a}Tpg+OkgGyg)hUddqGD2S4Z2*z0$4I_ToWHi+M!v1`raM(`^=`MjF+rrTi5^F zVR*LyL{*j62rV=EFb{&rQZwjLv|(YUBapv9{@rd30ncBpIUIF>A!ht96#yDvViwc% zaN+iC?}uE$t&6V#x7sEr5~+MZ54I*eSsv5jrlzhISalZT`Op$7UWBRBFy*b-5=HI- z!0r7Z6P!CvFtBUG;UvM30g_OTP*6?*1#K|gNgu;Uq78g|cfSN$F z?d5|FJ&U!k@Dl}5!6&@oD;GB~>l5-Ad z_`muD8vD@DNENf?l)t|r-T}%gUV^(@LS4VV*6q7`S~?1WKq+8{?zCTc)|edZc$WE@ z;VawA)Y@GPXQ6N3$BsL*X1rMRG;CGvYn!@WLly4}Xxt<$NU) z{YW}*DvaBZ$i(}Iq3UvIf1fW>OfYUtjvZh6<`dxI8Tq#H;2U#C)?3-z+A43s@jyOf zsmF*>D7o8-iPt~x`o`U!d|DnmtKgv^dNP(l7DL$okl1JPqNGTrlcD_;8sZO92?Wx|C z_)sA^g0^4r3e1f}7kYW0oQnj9rjFG7GoIuD-Yr+;vi`ScO)i_DFoM_w_#>a*m8?&& z%t_uYJ-)v)%e_K~?^RDl2DOBx5n-JXel)>eDAmQf! z8%>Pv|7YmDHxt5%9EE9RAx@d~24G|L{7g+lzTe4@iJ;6pSJ@^dgAfXq_BR3St3Q9k zV#1=g^7_Pg_AJ>8EML^u3A;lb&6CYyHhrC3eP!7(GU{O%8xl~D12Oo6HM=TULW_Ybi8&Q7u>L~hBJGfb z#IpA2|3szPoQNnG8fErrv2Hr5CY~E|_IzxZYzQ29iM!iGR}Hsq4Fl%lJ6(Q{OJwc z1~X|+=-iZ@+1!GiBW_a|g~UW`t{a4@F6tjEX>VTlrgGW8;BLl`7tag)?s^>peic18 zKXMwBB*Dm$(k&HZ>XN6&ewi{L-zWSA-;dEe$mgP)=@7N6Fa!_y_tT>wkr!^d^6f!> z?O#Palh5#>#%syK^OqSq6a^;vf%p7Z5`UPrji@n+$)TJ&R6#{`lb37F3M(?aQuFTB z>z^U}2_TAs-AB(xNwEo&4Oewt|F~5nC2z?(W)rsC6H=RLZpMUCQ0rI~HIknQz6UEL zy`vp&$}qBYk`CoMNm^SpOfRaZNGtJKi71}D@E7GSq9$|hoS*&R|`B|>V@IM0qOcDsEa+hcR+YX4)U`zQ4u*LCaGB`lOn ze`!`2=OoZbcC9OW!ETQimq;g&frk7?VYAsN2eeWxNC6*Du>t?j`}RS| zNv!mt>p?PDOg%WMg7Tks?F}p-xxa^#2SiptvQt^8ZbfbZX6Qr@hKwWGOyn%U-R8P_ zIu2x$#{9>@Uf|<>n9ZA}oCG0PieZn~a|sO-I@*y8r`iRdRDpJOu-Hf5%V;mts>$G3 zgBV?kLekQ63}UuI?J9<$O5oUziZ=!GL6wm;M5TV^%l20n0LRtk>39NS{MaxuLW*TS z0DYxH9`g;=qQP=>#iAZQ2dj@c(BA7zeP6iG2tMw27sgeK49;$VR?~B!jinwr=&MAp zs&ta<3`j$-2z`_wiG^4sgm@9T3wsX_BSz@~rwVkU4ooCShd}o$M$XX7?z2YZ}v{d`R{x!B~S6EFU%DtdP+iL%!4fXqMo09 z83FMM1h)2e(lDWIuxum{f$r`e^Z0~>!0WBrarW&pjJ&fzRcJntk|W!iC{a+?gWPC|6fA})3k1n=!F)G&cgFY{8_J1Y zLg`jE#gC?b#gaI-=b2>4PTY%OF_?q92P&yKE9qCPHoTcywev-nl&=D8-7m zAl_zVa(u4KK?>2%$UTm9K5OH~aXgYg6jV7n^5q?0RFuR(7TN?7@&@qUpgnNCX(nuO z(~DBJYzri<58=SMuYn0Fr)k1$MVxu_0CR}H?;rNyx70M`s34LMpHub0>{9SqGHW6@ znpP(Adfm}Bj#LfD3&<#D{ra${;eC+)Q%}|`x%0xm6;tJFcFq}+V%fh_JAh7f2b6&n z2BCHzv;dhzL$^xl0_4H0_Xh@J- zs)3=4l6wV(@Xco&EbloDvW9OVH{)70^lgC1q|1JO7cq{H-IvYFyj*ZG=|V*J%DROZ8BB<743gwn+_(IXut z=;Wz^U%~BvSp2S%vfZcH$eBZ(BVL*-q@hbDKYmBkIHfkHv^bARF-t}ARr1FrC?2L_ z{hfq=a?W}1L#5=8F5IbcXRTE_U0+ikpUC;qk&;haUcOnblbww6{1VA4WHbdedd}zo zvMa@=C=rw8H2I2i6Ejif+jX8!%;S2Mp)}-9b)rR-`Znf9Y#N+uhPX7^5Z>tgTVbd@ z<`Vad*Z;CK7$(ssCqqJgI!&V2zTahgFHb}De_Tg=y4(Ek;g)@X%OSTq^S!q==RXT} zuY=z5=)hCVfSa#j+oFgTQar4o?*%UD7VwWK{z$()q+j+NY_Gxe-u2J9I+e`+f3<+Q zTYb-)rLQq|b>`wlPga6oaTLbsp_(pD3n^@|By^BNi*jigx_eE2cRQbeZ5xo<}oGY9SQxzX$2bkek?MD8T8rA~>R0*#7 zLV57`$S2dhb7(AsZpVTo4j|p0e+Yt=yC6xvWcwDGe`dl2=;GS&-@(O0#lFF%yLX&8 zO;k=7{;S5rxG!_Q+y+_e_9Z)xH6s>6d_0FoTjR>G8&#d-qFwuWb6A-nH?ZIocqFOd z=vnTAg*Z}cq!iR)82t3@Phq@95z2jOUUiY~UiQ;J?%3}wF^3XZV~#VllmaDtaAC*i zP1-y6V@hRt5u+3oM<~%uuNI{H0cH$jYs{_{?-NzYQY+;Yx(NrMZvV@z>C4AQvYc_% z%2Xs@Q%g3yW#&vYqM7jrTeJw1)H$ZMI>KqAF~~NFaXor3eKnT(Plul%svQ=SMzCia zRz)W5(1ZcUd)KX)Fp98vYP@%{2l0up+)n7DYdp}QWURf`5+N!xJNre!+n{61_U!IoZPpufbzYQMs%2s)}`98h=f)P!dqBU&|l6p4C`(RRp`F_ z1+M{~n93u&Hr$agW^^Z`I?{xyzBu&YV2f!*!t3seM5W%ihR!h`c@_ws2_jtvRS1TC z41BN8@6QF)JPvw>o{r9~TQ@vC+H9!?nU2JR%>&`JgU+6Jy5vVckD@k0eCdAmy+9!y ztT36P{>CBk4jv!Dl-I*kN)%*s(4np+C<+>04F!iDF%QgMXsk@+L8LQ0j#3gX0*{m^ zFR4T_Nm(v=G+|=G!rebhtbcZnZ9hjV95lQNr?bror}?e~&`Hqw-9ubFnNw{q5RbOd zle)D2W30N7sUk~TABVJ};Rj`u`EoUkLSu>Znobq_VB{&o0#uFk=#h2P^(yaFw~^2? zd{8{|9tsQv(Qn}Xp;FgBndoZ3?ui&E$0n>c7I*YKAb$1XltW` z;ddvTRN|UsN1GT&cEyTwSx5Fe5{(w?t`Pf>o_O=%AnlUxM)_4H^f3c^nZmYo$qlN; zNL6+=L+wny0&7XCY6|A!inJV%on2e(Y6iS$2H+YO|j2 z|AigsdyuH#9rsGEIB(mSNgHkMW9IR`W0Jtv*4jEc1fOOhwshs;X2Wg(#Ms{0oSrXB zp=n}gXUCBZoe_lAf)jM0X<$Hz5YcYGJCux2QMPOCa;gQt6$qm!s#L#BfCyPUh*Trp zzP(oEIaqw{fK%1rSq)ZE3#2bU5-lRBO= zK}2j@M~CLSZ=`W@G>+_V>IO&$U?q<~59qzzZ-Im79Pq!mHw1Ki$GdQ%$P;qbZ>v~A zXriD-vWS9>a_e6vd0gJTy$v2NZf)TVU9M=v^(@akZaAXtu&+xWqHt2Sw}OUZVk7c^`_hwsaoPeaWuGZ; z26)K28L{IjikZ0ai87T@S?hUVa zc#Xyj6%+}+O!NPSY*`ULZcvv*ZNLZBQja_c^V9@m9xtK=`tst@nKwsPn+94M+7>(h zDg|Le6$(4y_%x&?0*>HginU^gVECmTDVoGMF(|{!w0!B?D9MvYzXxpA*4QYtqMr{+ zhX-Kdmb2i&(gH1!bCb~X zWhLavi`!Q1PK}wSYwPQdK|NU#`Qpawwm(KnS+#w#{W5z&XxA+s+V&0I<>m2l5da#N zxzaEiIA-4{%Y<%$6(!1?;^wc;8e!y`X|#?KUTbr3^QxMxgLO(sR82Ih2Kk0D+F8} z7UN+t$Z&W`%ZMg_f0R#wFZ0f{Du%8!pRM)qoGyt}@zvDyI}~eWW}#iBJMeK~m%X&| z{EJjsdMXi`NBd>tJeGUp%S6pUD;q~)ksnLlY#uMX8Wb$T)vwG=$ULfUN^2J?8kOqO zJ&TTe&@`byZ6NTysnGZDy1#DmLK_@%ulPK$E6hTT_o+56&Td9^(^#@#3ECk1u!ie@ zyFIt+Z5s!+!N)bLG4aF>E~83k8pTR61uif+^NUyG^mvKlgr7ADM1;Tr3)4!V)cl;` zVqgsMXV&{#JA;nDADyrnZR{MALM(J06QGK6eVRly=IUS7hV+6)6I;;Nm6S1*Z4iw6 zmTlQ|)UR7*M@jR1?|4|Dv$FtS8yzDuXR&(q0AZjLswV%Ffa zH2z8@1cV0T9^X&r_H_~8zU_OZ*Ay=Gfyucy`)a4yV#aLpkBHu&yTN386L<~$-{U|M zPRSsaR$-XPwj7<>GnmJZp4>U+m%KW(vNg>Uv`w8r%#Eav0_2(ZAwGw4qf1KqoekAV zHDLXX27|D!m?q#bbLE$w+(L-4<0*%ZuD-lj31gaDDzxyfpxzA8101b6c7=-iF|dg0 zP}ZYMtJtv()xpLR%E?}dj-Y~wG!Ia|$e@9=i!L>$E_?~DIvmKuYtOt>aOYM??BZA$ z-E*RL_RUZv>3ED$iW3g(@zzyp<}0{+e^p58X)DqNB;WY&m`}MIt10KylKFvBv6fCc z&Rq+dhEkDD3{IA|sJufof!Wut(){=a3=AD^-El==L46ppZfnERJWyEq~oZ_EN5J? zE7{4g$H5w%GG-X4QAlUR-WvaXi~Bys8WcTa`}a=5U$KCJFZAF<2ID}i++z`sx+f=I;pvmrnOSHrAi3dhM|*8 zyfIh6Tx(7i5%aK7(nFWjcpnkp^&luKdCrJ3yPlTICL^!2E89xb;S3*CwjWO;WpxyC z@tV~oS_{xLrNkX;i5Dw!9ftFz%VjeA|0*?0=6gnE^9K1BBT2yNgY#DJpFj)KTz>9h zf5Tr)*+BEg(DS=qIZYP(b55E5R;l}@^WpeqFxWf)`YW)p2-(ZgGw^<<)jYD`E3VB?uQ1KOWU4)1mg{liW05P$(ZC>t zHLLOQ@%K91zeh0*c=`Fqsw!74#hgs~fNZBRwEB8mmn}WIX>hM~cz|%B*KH1REw}nh z(P6-npnQN5{sTamNB~NV;)C5p7ED(hXe$(ZkQ%^><{4eQnu6hvXF~#t6;0N$lcvp@ zEsJp--1@fxnbbm41*r;pQ{ItZ8hT(_N{JDfWWzr+6QI`DP;gHP*c}SeC$a&O6B;Li z+P|+4Y@28r^(ANq1*FwB66DX8_hgcB1NlkJC6qz4F4?-Tse2{WkZS+INoGfyDb57+ zA=nA=i{p^XlZ>EQiZk#spntbgcTrm)ZIwwE{_smLZ^?9L2$vb{!q*8~PY4pDqzR}H zXr?BJlOo|#$(wWkx{tdw&w+ynHf;bzoQkYh3 zPEuY|R+(V5^3aCbJQdj)YKuN|g{4{~2M!&kVWyPas2s^zZk)=c4^8rN?T5qHbDK~p zr{W>_$jsf4s_=Y*L0Hp8LwSe=5ihbNtNs4=J6su&3+DnCXFzTyX5!PmOOJXfet=g< zB%=8zKbDYUh{}sVbOzLTLYzDZZB9M=#A$U@|MY7fAyml9VcIyCs@U00`9!v4Bbro& zxqz0m{7_arYMF=~Od&h8G&<#X{+l5*a| zFB%k!9c}&Rn~BeeC7~h_1`B@5QW1Jyc8k_7vfMu@QLy-d8&&2e&4LmJ<29U}ZXf550+I`3zAT@c%c<%w z-;o=k{(&GgsnXG0aCy~^G*^p^&r3wRNb)KIxC***Y4wd&z}sL6nld5<1yVu-o(c zG|jNv>tey1<+`o6OV-J@uwvUbz|YR^L9gdmw0Tugk_L!T*bq^sHVu-@>{$fb6%2m! zzrVQ93?!6t-y~E56!9@bfU)xj>_JN|pM*&|jn89)@e;^`m781G3=Pm_c63aWV->p- z%nMEg(Al7Qpuci^law@4pQ{=7P6IG{Yn(6G%*NHPoe>M+flkqb@f75VjMX&#;v_V* zwP9h+`LbjCdc+L|=>yKF^AxTP?BTkKLTAY$S?I~^k#_Jyj;coNtk4nMi{VL2M_sC#ej`2)9SlPv?kQGh+#s7 znD#vMUa~d8xZ!Z1dZn}|Q}$^UaC~6;Xy8xRfK>>=g>j*qUK}#TG1iz;;DSbBZTv&2 zT3<^k2VwqY#hpa;6>^-y3-$Ulxv6l(M1=EywSarNGZXBtC`aw2Fu@m2O7dhJVp#e7 zChyI+U^M}i!xgxSe*J>E@F)}H#1aUCwd?!VFl*IS2 zgT$mWF3iJ4-Zm#MdhCKux`v}qv{P9-Dw+};>^D!@u}oh3S##QAZ8F@U*2d9V-IiAP zGwwpxlCYF!<~ZK->4J$0h+?uPgcn3_DPFD3ZB~_dAb9&~Rp-aVCNj*G>z$epNocCE z21FCict`KJA<-qBJ*U-Zek7Q6uk2YKvAH%};S4YlF~}$?`)qGgpx<@s1WH%%$J{45 z6G4u|RkR)N)ZvU#OMYQqE%)!H0j~<_KE84bu9^_7*1A*0y{1ejz^U6Nu=p(f6vZn6 zG@A|;9fLi6bh$Ml9YN$!Rb}{p$;?^J#GMx4Ev^<{l${e#l}|Ku3da4^!LzZt75)v` za2GWcDz(D20uCSdgfi8_12e%SMEzX(YRb#S&Aqk1-|h2G zpc#cG*s`gFbC3>)@YuY1`>vtRklWwU*qc}OY06$MiY8su?lspWJj*o=G>u6MynQQQ z%=8;1J%;OOCXi8Sgq(uY3hM?akp&3%$Mxqg7p;eL)e`qO3X%g#FbSYL*|KHT4ite( zYvu^+N1C$oI0XW4JLRf;A#bC-W+^POua98l@W)1o_)w|ePSLeVO3Uz6Tl3urh-Y+m zf8Wc~ukY^b1BNj}bUJp=@IyN1;_^%6Aq#FgaJrBi0&u}HFo=CY^Mi%BiM^JQZvPo~ zO`J(Y44AzS*y9lrCg&DsVyEGxB{@lw>;)$o2*Ns%Y7mRj|8-VTEYLMChPEeh_l0c6 zm!~31orwKwXthX{6yTIf<|bQBKBG}?;>{-=Ht=srEPNZX%-qJt#jSMzuBGt;oMkX( zN0W;f6qCkqvS)EjhMjCzN{3292*phL zl`Xk4rt7z1tALLQvwH1CE+OX!VfZ>{8n)NO*8t|cXrLvH26uONCj@tQg1dVlcyJip-5r9vyL)f~1b26Lo7wsA?%l_E z>Uruur%u&ZkmU&|lBIKngB1mS-btQ8KH(?yU)U$J^a37gw93XOUI(@KB84!>?IT!W zsZyg+L=#6CS)D63`ekf{Z1)yb8j>Ynp*VckLdQxER2qA7Ags@IFeQ^66oR&s*h3+6 z=;VTNI6_5CIfzSA(x|O+bgz_@5dfCfWU+rD$J!S)1tT};rXv&@^CSoJzPF>rd0UK< z;}@*;U^j$rXeptJI438yA!6__yb}{KFiA3&2xb$w>@qyu^}afn-#By%o>l#@HrVxR zNB)#;sUka*EGN6Lv2~J8b$?W6Zf=D$vLceu4Qy0zyY;ISf15ZA@Qf?q+=Z(ybSda@ z(k0?NIH;q}aA_{$q8J|dSU_fG{@TlgB>(&0Z@Phfx#nZywpTX5t>6QltSqDVBP-?IQkEo`y3tV@Id-J;gNc?T zNb=g_jdPyW<+YO5-1$73kZ%$)P_3Hm-c$MuWIOS2n3OW5EE+iiq_GnOW>B{Bv1i?SNowXpaI8EBdwK~=E!{XFjM9RX zSPK!3ro0kf@_0&RTygQTvHtc+i+4Xsds^(Yl5f3?PBYC28pNErbsS!zs1?8978x?C z*v|k-!gv|oUYj<3DPff+-8?o-7mX1(1q44&!>lNq;?gVw7EY|Sq(h>-odmqSF^G?p zl2|enCz_JLc z&zm755w|I?NJPHiYhFK*pn1z(v`X}x<;^aSU=4R71>EkCEhZPM3LQiA0IW%ok;Gpt zj~YRR0o;6HP@Vg%(Bn)qLIaxvDpo?B?E>xLYaRCVboaOE>M5tsX{ZZg-4a~ZDI^GkjnWzVTJ4F|=07#LEC)y0Q4_r!PxF4X6mNQk zXyGCYb>JBgtas>;tc*?iP8>%*emy_oJqT{!i_!f&C1aC>?=Tm9R6 zJO(ri0N~FNIGBd_HBB%n{G6&O`hPvIn*R-U71?}BRxNz0>@nzTPW_+zbz>Meglt7b;`Y%!9LndObj5%p6WPQe}|U{Qoxj(-^G z=Zi=S#}+R-RPc@DcYH9?MGi~;c!9r>?-R*ZmW)Niri!HF1Uyncj? z(6I0|(G3}1AqwcWhBP>YzpgwHOGU9W-$hIlGs&nSP?bHDB4OpY)$T98Kc3lDsiDQ` zS>N0;Iyk!O5pq*VL%CJ1+B|jdO!9-)kqUGRIUJW&JDh6bT2tb*n;t*fd)c|V%w{7a z4czC=m@N&^v#7vl!mKulw63-#cP5@uH-_ajA@QW44-b(F9hkdO+`xjN^D&o|D19b? zCuvrU<0?YLX+-Jdq#^1yhzlZxbiw_QBM^-`W8)m7mZvI>VvHKvj=^m%|DZZY(+*4S zP^7XvVJD4iHww@ADV;U^#qVxik*?{LQVDU1hw2K!1e&pAp(PaR^3I^)Ic)k+t5w=c z_qsoac8MLWW~W>b+xkMri2rG{JW6&ePU7J;WBu&x$A#dxRQhQ4? zPNbLpezfFRHV5CNEQZGZK$)Ol7?w&1CANcjv+D2d@L4%$GzML-8wovl*WcxcXy5|x z^^wM3JJ_z`!b_iSVx*#q_G9KGi7?1tWA;`>ub|8Zc>^yuNf@}I8KgQ@(@~M6`zZ$7y3}C32#&50-sGD&|Gtg6wZBr5Rph{xcpsCGOj@G|F(x&uSh^KOW zr-sQ9sM$gGIyR79ZRo>q;vf@+#g>+Jeen<^aQ3!NmHgpl=x^3QHlOOq^%}(`mY9<8 zhR}xWQvsjT-66pg<8-<_q#J#g@*5yMMB%N7=nxvtWJ!w6P7h8Rg=fgiE9+-=Wo7M` z|37FRDmY!b)qSbK0mlbrGhr&*Osy5O>b$R>|G~AW^DXO>5E0_8TQmli;~=$4MR4x) z=?IB2r(Tqplc^CFz0qN>t_SA=p^EozA4Wd1IV1!tZUb@T?^!dayd!(lEBKxb0mHd7 zo=X5%W20A7`tQfjHb@9W<-J_mM1EoZESIs)g&$ODHG>dTJc;Z9)R3@&n)KRVZIKLy zv(2zXfuTC$mK{1S5R$vv-J@`p6iAHWN>>W!ysLcWY17jzlGi6ugh~eUf-G@xR8GWU z{FO=sbUny7-HVbp2*qfy&4H&4v-|s>85Va;l^H71e~R{leZXF)9#=n3QdV3O9v9~P zWO&0|RR@K*c8?(`)cr!m1|wWr8@iqJKF(;0RDDCvy81}jg4Rn{Qx*#v8pIOEu1Yk#(a$SBdXv>WKO{HH~5d6 zoKTn5$2-q2AB2|bEc2meu44Igl-Udl5eX@Bcb4p#D~)2+f`caJ+;q79!TFP6D+KkC zqJwnCW6cTbukuCJ48cpNypbl7qO_=zjPb7;0XEd%^bQsZ66PPk>eJu?q&xU3gmOS- zRF7k)U?5T{|5D_^9GN2Y+#;BF?y7iP{Ej&k1NfGL0qg{>AUou6)$6f!QZ7t zvF;*X`RW7Z=Nr<|fa>PY;HJyL{rS33K!QTQ9mF3pc% zg|sr#@>2_0-m;vKFx1+jEk zan{)OKKrN^9~39QW?A z&Cb|mL$#h~3|9gbaM+AlKC9}S36HF+xwAE{CuHP*t<21TpjG4so3L&km~;sX?H6{1v&)!2<0C;kOba{0p4 z-N?ns&eq&ku8)U`<(|7%^<@sP85F-P@TW>dEH6Ll@ZlU zEewdVHBwufwy9w~gwhJCJB2Dy9b&`BSRl3$eobY~)tbU9SG1RN%^(*O)yJJ0M6PWT z_!4r|Mf)t(4-l(B(6AQcVjK3wsfDkqm(8R7j;hY#CrCo*^mBshfwMvz*7N>;6Hx+_< z&XkS8ywNMZ1A2i`I9r>WkOi2i8a*>VBcUQ*$Z~(96Tpt=>+%0!h^0&Hb}#O1Lh2-% zQ6KNeVM^X>Oko?-z1q%kFW6q4&XJ9~$-)sp<^_p$#c>8|+b{?t@}fS+Io}NjFzq@D zc_Z>da|ptb7V^7WcA!i)S~x{H4dEfX*hf5FP|#f-YP=gT(M2_PBVSkUi1iyYsmw6FEttQ=0pdvy9$LSTIJW;z#ej>Zj7 z5h9HYrlkN3xOA^^RNt|rD}^A{_^eKxG0qV$Akc>icsLk$J8?u7F&pmCLAWZLZt-a) z1pL@k_TjJtXDCQa4hKW;dyLw)ugArc-R|VRCf{Y}fS9WkotGe(5BO2tcWF3&2L9k( z#w{Y0r;OMi8j>*J+}q|YdG*D}!fTx8PC7Imh~|5W{d({9!zGv1g&pJC`^<1?4u8dk zUz$93V&TZv!3Y0b%VmBnnVu;AOZc-0iyjh^9!h?+~_WZAr?A z{*;=nth9A%N}635qZ^99FfPC@eM7rjQ->^z@iny=L6VKsk=o6s{>=^9usB>Lp3K5| zwbZM!1K5KD46{801irySWW>o4drI;+nxxC%9ilPx?-VC15iyvQck0;bqC+LBW|B4c z3>|h^4sFYN+xVMZM489}XWRlmZ)~QYh)qAj(vb0}w5eTOaj0Dc7A2gP8T|^Yes9W% zH{tSh_{Ul7_789PeS{{0iNefg8h2W{vDfLh~PQtZ8$j zS3;(Srf;`n3p!qtnW(Vy5`~RyGRW2s*uzK+8~=hD|K6OmDPrtK{8ym*CHyww4qtbNzu1`o5mYI1I*EhP!2sie*pyw$kuEA*+l&{cQi|)8T z!DW+E6JzBJ)0v*vA`35R%HABKvnreeiOJ!N;38@P{IfnZo}4Z%b)G25>c zV(-&ZNzJm|!Ec#>;<}I`WCT>xtrsuF!6NJ&82LhRb>uMU5S%-%q34#Pq#ITH=b@@l zK~p87TFAM$^JSyeE}ntu;UW48_y1kIdPe^t2RE}+GiAFBV=<%`8krOruZo8@~_~{^bj0D;&-=M$tIpZfbaXrzi zAs;x`&^^uTEX1o-KVWO?{}xk4TYWG(klVwUVAcGxl_{ecFqZpNpO&t+a1mR9U0(aq z>ftd<7b{`Uo~t+xRi|Vp;gZ;|DXZblorC-g7u*Axkur{skD?%GXkkDw|7W-`=ktjn zQX&$KL2KCgJJerhRU!qp8Di)qRZa}*)1ml6rlVS>;CnX-*WF+E67`7LGfAHxxj({c z3>o(96|kkL6XYTw=tE!&BwRdbRf!>m)J~Bi!y1LDXbG#tdPzB6TQT956!p%*_Dma& zyn^t^Qg=NUXUH2SXWW_>x&xU8*SRtB~tFVvbKv;oD53zErTQu@y z61B*sEzY{-geJ^Eap4oJ_t%zB?0lC&X^u#ZT^tqp7$&)J(?A_rA;}2>%=3^hWp2#0 zWcC10&tY{L=*_N%1`5USo%I49c{C`#A#fooopjc;LSs`c*-t^e6N~}35LX-NPR;TV z7!orBlbA_i2oNMAuKlqfg$${P-MXJGT_PqD#{KAN8dTg+_trSWF6ysG0xY2f5b6Sj zrp9MGujNWMkyM_6lZ6;{c3yV^O^f)+lvz;Y1|IU_E_@gFJ4qk$g=m^o#g{ZLIr$&7 zE6_)vm4r!5dQ`=jOXX$WQXNv_Hm0JEfUF4KI8?M$pN;=(i3DGr>N*yK`gt_<^%z%; zy~L>3P;@2WQ=rO+>!Mc;v{(YpF`YNpwuAw@g%Kp2P$&(i!HA+7?q6c1ICiy)xjUu) z`YVr^eC23!WB7}U?Bt0uH8Sx7;OYu(s%NSG`88zX;;eGa9L~_a9n#aJf{!%5JZ;jK zn^$;idprH5UN&O<&F!aQ{U0Y@>{4iJnL}khJZL*+;*z-^YV>V#p&B=4iB|7$;cZtN ze1iNFx^9z3PC?4D{idLX5<+X~deXqc2Jxk4?@foMz?BqRYSpStF7tY9ou!~Pd@#Bh z&ZiCSYEzKEU5M~vnidRIrdP0rCHRi53{%jtRS8ebMWh(;67eC=QOr$Ri%=Rx?_{WR zQ!!Cenb==Zy_x%2Nk;7^oMK8vq(cV1hT5F|E_as_15*7&ayjqk9)3?ig%vxN;?JBO zO&+%qTwNDw_izMV_4u@1Z}h~1cJh7~!37V1y}`3bx^aL2aAkeTx>9y*^`d=k-D7KO z!0B-)6Hm;$BZ z1rDc5HS$ycE?DUM0UxI_>+g=Wv6P?AA@%2O<;+b|cd=nidGigAVukH6q>#MJwO>LI zxQW;GEIM`ud;j0^#(GT>5%Pc!96Pw!(wc9^wMIp;dl`G&-{|GXDlY z2Ao56CKP0s?}%fOX8WCYDEc2tWw);Vq+uy>2a%SgpmXh z#hNQdb8+DA27cI^>yQSelQ^~8*hJU^YDUWx@X&Ot z4lOrWA#z=-7j9XlW3=YOVc{Xye})Ejwsuage1)fQzqQ{N!6cE#;N^8Mi_6({qhML^0yhlZ6AY!emX_INija}pCFKo;he@^Xnq(opwPqM> zd5Vm~zGoqHCfgCC4&`KL$GD~lW@CCWaST_afJs)ft|TH!3J)f77Xbz|zB(Eu+1}BvU*a#}EBYJuD}Go;hRi~fy0oz-;vG4zyr&MLv*kmd73z9h^&fr$kI z+5~v7YRoG!&07@<0>VoFLg;+Fz7mGuc0FI|K~%gnu?!?r4o~d{&uIR`?!@0wc1gE# z6$S>|aM)j}#9bn(?aD4p8v1(p>wpo}=CaJ2_&Lxeq4lCMXV_lK?h`m+?7w7ta6Sx- zjU|rXZfk%5RUTS9P$llXgv}5tJD8MSLI8k|MFXi80FB8 zB8N**j?sT{TFcJQkA&-wy|DU+U{z<2Ceau?n2#JEQu0&M_bakjIshoLt?ypQ*zPn290hz^_CGxyCVt*UbG|&p1CFD8fNqXF z{Gaouc`44Jw{KXUuQuK$>VW-p!cRnvaMiWF@3X)WGGI91Ia@g5txxfPwSb5BR*%{J z+Z#6-^&2mi@uI+<$tF(>;FjDuH0RB^|xMj(I>xJDSJ0p zR(JuxK`?gRiQV3fp)WW&^Q$DMey)F;Omz@u|LBO|&u_3tTw~wNIF_fsSUdkk{8ApE z^--xE zOPENASS!fV53du}DvAnc4}nczXT-=0q3gPe?VJ1kFcI8YPKoUA

0-7;Iivl?CII zc0-!mmz0oFph0z&@KUmg(%yeiZxZqX*A>Nr-`T`z z@Wb&(K1o0mbdYk5UUm$O#bwp6qQ#3xAC%k`O~$}qRakhmlWdI@y-4}cR}7n`m8YvV z-;#c7k<{y*GBY)f9rzUO)(k=SG>WgJ*VAieMw+=B#Fm44bgkNtp78K<(+XYAuo{{N zJ`=Oiy2B*npB_mOrU^SgqBgI zWwA(j4e(_+BS%W6LiXC3*H7l-spe*+o8haiKhh>ppc8l5L8dB_tdsZeg|ba)gEUI2 zbMc-eX>`2e-pc8exHx$#?3&sRfsM&p?8KYhS6vF4*Z<}>%B!YBG)gaj(mahX2CMzS zs+hQcia|_Dj+7&op}h%tqEkU*6VdWqqE|RPxtR<9wYkN7c?{fC>3#WV@`UUM<0an7 zz(2zPe!KUxd-jWSCVCH z`ktQw@9W#ICA0ol3!vR?AdFRqkF@Kfh3sXNuJ54Y)&6ZF zdKNUL0%~{SQ1Kt{>V19w5YBl9_6Uf4)&cpJ!pg2d|NIL5HbJKzedm+g!KWThZZBj2 z;P$TBdhV6+>osWGKNtkseg|eR8*go_y+Q*HM7lh0@3w*2pviJt=d&lm|E`)|*(X-P zp~~%Qkl!I>`#g6CcV1pzX951)+2EE<=*zgWd$8QJ(->Q0U-G?Y@t0)YxAzh`msOFpweiy=u+ysXZz_&&3<92GiGtoAsL^^1FOC-T=> zUx(+78Sof1eZJhF2G)PqF5P&ELL~`2{{*{Nh(_L@H46lYHTs+31$%XE{^VX?A%#Fp zi7@SefycP`X=w>d{0E%<)lEk3k1VfT5-Zp8(wR(UZdjr@_Mvo#Cr)xSP4;D^Ol2u8 z8v4|KzUprDc)o&SCzWl@pc@x-`OW?`S-fL!lyD47?mxf^BzVm}2z{I>>vLsQJS06+ z$LdqD+jY^!J=qde+t`88i`FjhRyjf?USQ=-%$A>)z7XzLgae9CdPF5BGknoRXx&3i zHqplRwL8j$T*a^@>MkAu=&mHq%B&x9OQ4$k(qDPKuu6)*|e^ z4#a#4y9N0A3g)x%N(I&CsFxI{+o10@IJsj=e|Y%0BtUpJl%N}#^AhjwS8dc&Lv>ML zLf0o;{FPI~2yiN-{gp$jJ5FwM=#}p@74esf*ShJUECI&_okM%cuIPZOS%D+Eg4~V6 zgEfG52aCj2k6ky@(_6eBKR?W(!^=Q#WvxP^gg!RM5vp}pC7GkV&aGMpKtVu)Z~!x-!1g;X+aVA z0h74Kbl)(ok#?t0v&H;)d>`B?k40VRL5&JmlKNfRDPlfnCVMBYcvrejlP>X-#>ty(md?T#I~D8MEpokKG20$6VA2L)ZHxBTb4lv?y`$G+SC=>C&^X<U90Jv-Ov=<^!K8WMv zVgM*q&j)yY2UdbjgGT!QVfy6n6$o^t87ZmMB$dJBcz)7YqnD5S%cttOGn!^DUF7hg zc7%s@%Ag_a2AHOrC*h+`id9Oc9tb9;dJ|1*h3*JkvruEZJr$z!a~DEU2L@eR$-$(X zGW!%{0WsotziXd-lO)_%4=|Bt=9=V!C=O!(V5HSB19kd{#M7EH^Wyp%8huY2ouDui zPZaY=J6H(Pt(e?v{$~DUFcW@lC$;gmF-U!nPsJ#bIm{Db_@?as_T=W|=E|P)yVabm z%Uz7jEJVyLVMdhj_v!wK9qd4|orSQ%kWld{dHhyvRE(ab!`#0FP(WAxxaI^w5-yIP zBhymH-&41>$~Ntro~@ZNe*LB$GaM=@FmG71Z@v-ZINp&GVzUTb!SOktg#HE-I9vkAYl zAD@S-42|kvTs*dExBl~(@Rif)Bgh8$Q`H(^=dx@uD?Sz48-FmsA%m7|7xo>^<&Js4 z2404WPrxPppq=s%BEG_a+Cbiy@JMLrQx*gFlz4n{2q~eg5yd^)zZZYM3V*B`W-8~(jE;}m6X=d7^nEA(hNFLamvC!KYw@I@%Sa98al9&09}bl7q}8wW zN!Gu$sNg-@=~c;i`#wl~jZ^?E#chArgbm+RS#c0+=lfF7fr_0UTwzguvp*h#t9ld}i=r_WlFU9kCsqiS8Ec`1X(MRxfzy309{Icut}=fiBb7mr$I-=fR&bYlAz zJOk)7M-Try>W?JDQ)~SG(UcDzNks(u z*Y{yv<9_@!eVz{-mbt|kDeiyoyUOm*U?w5Vb=_Z!K4ul9jf!8M|GP^b243q1dBjY| zqKATeh?b4@47Z2RJ$m0KuQxg=0BQb`A?LeEn)e6Q2_!=A3^F%vf1L_-Lzz#ZN%!B$mF>}r_yDC=(w$}wd* zm}%9?1Y(k5UYTgG@|*JJgfSeRi4KF`lbO4yaS*|spjAIy1q2Y% z-BOc&)wsLCXCXGQQvBpI!fUM+{0$Fr~2!Zt~S^0+Omb1RXR1Mp9e?o zySlpI%b;&~RA7H8#+>w`;v@P4^Dw_` zX-$|W22+<2WoJ=HBq~X(W2hIH(v*UuY{({SS$3Lx-$bFLIV+^BQuL{tBYNgMQG?m5 zJy6)ozKMhg2A+JPepn*b-wE6r{wd4%@}@*YRFub5x~d#KsEvxL81BI-Cy_WTwnT`o zMQ?$hfeewN#zZ4jK@ftb9)(Cu4#X*5o0M|Esso>e%S%_ydNW`@BKn#3(8T~F8ZB)U z>M+pU&e1tVbfL^eXN|6vHu{=&0;>P31!OcapZp}SD$_x}E zNE0H6AKo>@b*nnZT9b3zdnm588I7NJY#)jfRl?0|po!An)r`VwrxKYK)-i)WHBD=`+9p{*AU{~+kj_h>HZ_y zpmQ>+N5sKsIJoyaAh_DV&Mv#|hc@q`wvqQPo8LC5ShmRdIsc=7JMH=Q1+Wcz0Ocx= zssPUR!Fm<&C#gnLEqKEC4+P9(jHIR}&p5=b%egmMw5pg@LrW3JE8YCDHxdoT>WV9i z{Yt>&7-ZYOE$`zT|7yQF8tyWLi`-Z^*hw8lq{PUIlq0Q4N(sw5$b_#aEr);#quS!m z*M-YIF&I&Kvda~~E?NX(W3O(Avqx2I0uule*W1?z`naEs{#CvgRbORyt|Ih!)iR9& zfz0t;!{S>fdTvL2K~%ehmhDT`4zaGrD|+Xn+h2p#6zEY*S1kuAA`A`*TcFljK{>IB zWVJH*nh}S34VJU+PfQ#+@-6Od(^wSKl2&OCjqqM%oD8c88lXyj?YWc5&lQ6Py(n75 z$2}k-Vq__e^jQZGKmQWKV3oI6ww#HT_KUxsNj(SAp160BdxORQt;D8?#EDYaRg3RG zk@~IlwZ7u3Nmm|AuhmrT%NEY!E@xtO{2*a%N z&b{b%K+ncHLNfLNNk8Rd_qZ}t4w{DSpcHLJzLMBsZ$XuAYJ$L}XxYLg!ocoJv<_pL ztf7t|Vbdyc134fz@m@HFL|=79e6k_jVi5VPQb&T%W>==mT69>E-DR5DvJSuSYS@zM z!f5>84orDYJW<0Zoq+ehwTuAJ;lQYx*a{X|gs{2i4WhMR*$XQEZLwn-jG zQYSmYD6Ce$2}A@!Y_mbZQuXgaT-bJ zgm@)d+?B#^4oJYXR`}_fM&?0&-B$3XAuju8N2Z#ik2H~%% z75Lo{^&L(u(#6N4f#yI@JeEn|tbm{}|H}>!@GAe4M}&3#L7SPSv7se4-$b!r*!NYt z=b5SZ%@6bdmYUS*H1${(?p!)dk4{T7=4UJ;pA**8I31w15Ssj-6hHV?pvPwy3x|1;W0ZeOjDpIz(P z_G4M!^HAR%(yY(g2+g0wlb7Gis`LF9CXgU#zb7v@VfFA|8gVO=`7CGZ;yy zP1@(3yTMBPUvsWIX4R@MKktUXhB0hf@eOB*j?FGx09o-G$$)>gOA6!@xgjeF@eOGe z59}~IG^q^_jn{w|mfFM@d>eFhPogIG>LTT;SR8y4)OKWuFARf-Fg}W?1)0@Pp0|j^ z;+V(*1J#=Ym}=OgDCARx{bl8O40u|i5t=4>|Dq`vCC$b-G(~K=v-l(ms2$Lkhr&&Y ze^Z-m*>k;p=HW}|RUpt=Kx~^52;{TIJzZ~hJWk~5^Nbe#X5~!E#v@SYj{16w5|^th zqAcGWw{jppf0Ai5aF)gQ#A5L^eIHuF%OcR<-TKDj58T2UR7X<1;FW_{l=Gy z2JxCca81HWNi-N2ps%m5qN7N?vymdj6cYP)ig;YvFcUI8TEolDS9_goiBL3>KTial z)twEA zGef!0CD3Ca$Hnp|4Lq1HCc{1rF_9V3Q}&{gqY;c9yb`+Z>bMsmG;SbUQb93x=7E@_)u-MkIYvXTr{oU)hQfrEdxdwP1Auh9>H22Mb)_+RjI=(dOE4 zQZDf!K-Q}UH?EiOaLv+1U2}INAIKZXhN665s*{w9O$!IPB-lzaJh$xpw*5s{@+6<4 zu*)h>66(zxMPr7Ly=nq*}XnioM;F zGV_;uSI{&U5$~1U3I%3d>O_RWnC^21eImxKz=QKdLZM7PV-Qa3O6so(EtjXM?yoGS zee@QxNN}q45RnWL)(6N9O34Tsu6)(BKGb+>(L%1Nzl+bE_B2>) z7KcqaGHn?mq=1~<8v~zHO<4JSGl<)#=S0~xTGm{omEbem`q|o%6E9dadH&o%h42Ht z0DwXw4`?9ex@CZ@?Q9+BZWgo@4uCy~_?`@Y9f5%BefM4YK5t*)d_4SSVBKjH0}leo zn{SRm^MLFUq}Fj>KjHN*pJS%pGrPlYHlb~O`C_MJb| z$EC`%WE|)F5VFH?CHyD*^!;ZJt1XR1BP9$!&JkZ4w`jO>Kqpr`wG#!q9CC9>Oh(d; z{WG1>2D$j7qi#=Lo?^8jgqM~eD{YjhJhk1(_Hx4xNp>0-vSPKE%-qQ9>7pO(gHDC% z&_v|VO>Ars*>Q>n>esIz8g{nj^W~$D4R!+ldMY>iGW%)0F|Dr)=rGS5tLt7gvYNvV z|8e8I+?y<3w}j-O+({g_w?rQ~7ik~`SJa{y3+(1gPQ8wtqAMs@OO5lSNYY2Hz)+_= zp*G7f@L(r0ZSWg7KltU9+zV#M)BW*zb|Bm6`bIzs)$naEq6r4RzjAPNxZVxtkiW>k z1LGlB?dFs-6&*c_%HIAoAk;DCeq1UD(v2V;D=PeV?vti$YSLumaQKDXq$Yb)@Kq}e z++q;L9`wbi8#m4}CS}r!bHQq{S%1M-t0ulNs=vfK#3rl!_{dJvm^p2{;VpO*%SwlF zb7}kZMTMi_e)%U;KI)A1pmfPjtl@ zvWx6tDTt$>)^6*IAN{pG~L%MkFRO5L)qx2}u4*&OiWbD8~Zs`eHa#;qJ zRQI5uiy|rsCy;kf8AKz0FmM;8p2aYo6!grfgkJ9=C3Fy369JRsDO%dzl%_aWe`ILvkSTN%(u(Cb(9nxWJHqh2jjX_nmF$WGYC!{XHn`pYg;nFf%h%7?uzs zLqSW~c`jM=*C~D^He9L8%iYSQR^!N~t|lS-oPSlGY{M`idk>z9mbbZ!_^=n-je)k& zwP_e%u<3WtHi~6gqmG;>N?qdImQ^-yTZ}EwDbWOmdd%~QXFEw$b$RGRF_#d@ZZ@h4 zs?9yKe!Y1Mwu@P*O!Nq+->JeCsm-63$H&yg)ZKD$1gS6U&%UWdU5}i~_^=w}IzmT~G7(FQ|X5 zk>20_zZL**g@*RT&E19(nW6mW+WT4iDn|h1=>H*O(8A@Hh)rRvH;N5rT`ODRsQRN%rBf!O|3U|Y{CWSq2y;kVDG;&7*xdpi77lv1iJk{yrog` z&B&8cgSCi|r;tVU!gkb3kh?3@+vhETxqIcA!%_}46yHMc&|8H@pEu^_wtTsvaC76r zAt=@$E)u_5Q)s>7J}*$J|4=;YhO%@CjFU4C zkGN(N3djdB47ct|FxR}Kr+X~(dss*io$yDNkp{)Y_;7kyJ+aJNl-XTJrlJ*?Sv)_T z-F9=Y(08$EgU{Zm#`bLsUKZC@Is2ZB$s$ZOYVCbP!2jOsXr8ucNjq&sfj zQdiUJm|+RGqt9;%_5L6G1HSdrGZd_0 z?~2kdW>stNa}#-ogDpM9k_^|6M7G*gVI#(p+TYu}E|1GNv`ya!2_4C7 z4?&5?m|tn8tZA-UTYVs|d5@A#mJn{gMw5a+URjS;vE8BXAfJIqCzCXJ)DadLU06*t7#X4wwYFNk($qXF?vN^nx28JaB{Mh&P;{UB~30WpRjU-Y8G2WYasMoo-yjHX$|_HEvpnV z{&x*MnrJ~uj%C7yTKw3?sZiE;^_(Vbx^x<4K^)Rh*Ngd8Uv7M4OSB|J`FnICmS|9% zj~D|Z^b^{TyXUl%GvBxD4PLFTzmJd;d(ps~8^xyJ&Yq5Sup)ol0#>J+af+ge3X9=f~CS4D)Bj zg-%z;%9(aECkbKIQPTaONbiaG~Gb0KqFwRwZL!FI_bLtdWLsci zEn{=APMoBK)H{Evej7lDDpNz$`pm)Azz{>RtE+CS+d)8O%d)1U;SW26)3?oXE+#DD)LNcoMluoNKB<7=PPsbR=W ztXGA)e-OUB!lJ8kPiY%$MmAvBVgd?aOiKUiLkRaxqP(9n?zG{}5CwydVR9_P^HlN6eY&e8}Swva~V?O3qjquAfOcAn{h1_8&gqu{em zX<-CAf5Q0b^0%mk5(EjlyKs7@&tbq(dbA&1YU8P8(Epk6UOg_dcq&)P({UOEPQ|Q+ zD1~P$BdSNz1u^ub%5Y-03ax*&cKmHs4{dS7&)a9vAde<~1+&w!w;{#S-2YR6rZx$& zF>iY#YjVNJu!MSej;patdG2z;p__oogl0$1Elg|&I39z*5ROUYihT9=Gr#Hv77s7Do^&G^WK8F)EIol1!^cVXP(h-@_q)0qhU|h?60-`9+ zhXxU3tXOa)t@ReCiK54tkB-^_fxMeO0@EHFkH$yL89!Wemvk1C`djbG^(!s2ENKjT z&6?Rs&>PVL4ss^25^<-L*tJBa**Vz6(a%LxXjDdUHD2Mh5QpuT5F8O@wOw@f&0mGE10=c3yEp4N})MXV~{+OPN+^szrM zty+==oU!A9wdhjhk|r*&?ukDky9T9$&1LWGrs498ZkEw=CruzBqH zD0$%NM%uFw{lc)>{W`GLeZlv+U;QyQ z4NCvn>q36K6n|9<9wv~m;=iwe>RS;n6>Aq?={^tL+0M4v@5{dhZuXOPJGb^H{>0@m^~14CQN# zS>UZzVej4N)Zk-;p1Je-V(Vq&Rsegn?x0@5=M^;B+*a)H-EC~X`a1inE_Aii>k74A z1~pwR-x0T3Z=cs@cpR)QyA>wzetc4Vxt&x}86<+@ya$`MZg2h0&5PT$uHB$wAC-j* z#7%)GCoSGJ=BD$AvbB<7GM8^j>0Bbo(VqJhOSZb%7~WQ=G~i(Jj`Jr~x^4H*m#xEb zW&|U=mQ8aBMm_LS=$IkmM+~qr9Dcg@FQHyWA+A@Oy$EJDHc@fX4AEq+bKD<%j2up5PAI>y|zQ63pzrDTr9^f$P_PkLJ(>R13 zybh9&TGro#Y~*l_SC^L+)s;W70MMqjlhy(t*JRqPhP-Pg;$j|b8oZi&jx-I>J;`I6 znv|+5rpI=@)58kL&qlW=P3>C|+7tJlrO}uHl?$wySTzk?CI}m_rxF2hDaGBewXB>P zhH>M^Ziwy7orslhO`O?O{;UA?tc zzwt(53mx{^?gJ2|X!IDJ@QKNR9m^!R>gY;7b$z5bj|gWEAO}}YNUx!V1|(K7c$}%R zalk!?ta507q2|%w*05jV@3e)@o%ezjc7wj+vFuEJ1&A<5jHA?j>6Sxf6vIY#-CSw= zg1MgX@V)%3nkeE|aZ1%0q2i?97XUxqu_zQnLZ5MA#GKW**M$!WavBEX6!s=R$f}YYnM*v01OwqGIqHL4MmX8!= zNdOz1)iS7GSE4A|b8q=<)|#pm*J2*RYwkoN!Kx|nS5Mhu+cJRXsMS2H)B6LzjkT^K zfOd?xTwI38$`*~_eoSR3l5qj%@~I7eix2P7A^C$S0i2X0iA0zUxiQ?6k#SEN*&Ti0 z2i)gA=bG4F)Qc0KUl|rK>`l=Bhv`oX=7}o{Y-~El(!bqVTV{zY+Wu2&d~G4x)R82n zS}2gOk%3(xF4Krg<iiO6#j=QS6Z&o(PhdAg=vAZ`}JB^sWhg`&8~+N(7UDk z_vullX^Ya8_N> z_4Kg?)^0HV#UZWXvR31 zIm%px1x}O3Gy|hHWuh3g6UCvavn9&XAf`wS zz2yN0r-&tIM8NWhfJ&qJDF--NDA(wf0P}NgBVwW!h;2DDj@E+^<_7``{?y(<%l`)Q zCvgM?1bD5STL8gUdP_OdmK1c8$smD*qVuh$c$)jn^sNBJk0IX5FrN3a#ufwy$u}W7 zdj||CA{IOpkSzt(`el|({|M8Tu|yzX44l5UMZVO?ddkQ-Uh)J>pq`xDq@ahDjVs8J z+ZPeq4LZavvMww~Y0)*dLi~bH*8xf|TgkZBh(Sp|#=tu3k71O(10F+S;s2qwRN}JZcH$B!Yl*CL+do*EIn#$@5H^fkKh+?- zp%q;CIl(1DlfuvE^0bQZ%K0|?vhsZQBSD(Axj=`XlK%4OqcU0*p7K25isdwq<$ehr zJIE?AJc2mgPvGx+TK6+acvg79`8S$6__aD3>Q5#SK;&AM^Pcukyu7suNkIiVnlHVu zx@29U4|Z(dBw^2@!*IY+#5)Ez*@z+mfp;146H6s-zhJ~$#3*uZca7udS1nJn$!>z* zT(88n<0YMqWJo~=2}=4#C})%zk_*E3o+iyXEa9=+|fcNSo8cu{;UxT40A*?#ESFOw^xUZ6xe+I4_DbU8`s0u@~pz+;$i}%4BH*%7m6`!nlq7s~Z3EsVXo}GAp z)Lf8>b{Y!H;G4Fumm(!Bv}wA80?|M)n^xlWt&YU)}HH-5K#0){_qI zChrWnFGSas+bd|l7603M-W++-KC>%7k!2S^-%8&hTvsVF7AhzOADM-ggOcI~8GfJJk zFZewbjZ#+vg8>YAICm+qUj_5`RYQA31E~T~`{$7IDnj{EN=4-wTJm_|KUaSQ=%g~$ zkDvv@y+LcAE6`+&yY+pWP?BVPM!atn2pu*$+Q#y0n~gouAw!lBoA!O^Otv?ENPNAd zArLd;fOwKd**qd@`JlTKWA(DIVurSNw7;K%0b>wyYnTy>%zXX|gy(W4@wvUJ97B>sRDx7-c zLm90GWAgM2+i(muRfxitSrw)7g&QZzfZsui*B5MX2dHXHpLh*28cGc^2+C^i_&1u>tnnZkr z19pjbBBXineGU_pE&>1U?y_*QL}OOmqJzzpBzFar?2&>5J}#U+F;U|}`c;4DJ@A?#}Y(ttL| zxDYBUO^m8Ebj`gxrkcXiYs+BapNmcx=B7w@b(|y9I9UD*FmnTqvOzFpn_(eCS!Ag9 zrkyCIAa1QC_YmNxJE*Fn(V%LI5L^rUMVDgkX>YG57ZlNwPkTGB^+ zkV0xvl5L8)4D80pEOl$$V`2HDz;jrr!~@z_3B?kFtq`_7hvbSf)a8?AN(rn{wQT}2 z1PD|@kTwmt+2#0jc{A%E)O{p>~nwLUD%-156tadKR zv^J#`$YL!cV*vmH-S_`Auc0A;Ssj#XRP#Sv7W0$r$NFDopxT~X$0~m2Zu5W=Y_rx+5wdx3bH;?n1Jg0#`C?p0Ipr$#V9N#$1tiO zh$rv7Av4SBbf+6q={Z$+ouNAlCme_TnaeuH-R)LWpY0Yy=1!Ho#4jMr#|;Q=y&==9 z)M-jKt!g{$_q1e9zOPniJBePRTCn_)R4(fAURE7e}C0|l=Yc?#} zSAPvWd(a(vQOQnP)S>Ulm^I=TO`5e^ttXtqX1@8yF!NK%>-@iuQstb9Y3{=by_R{9 z6vCW$cXxX`A`EyBwgq|(jH;+~DOHHguS{|F2(&+x&bEQaW@&Ml-LtZ~cyOgqQd9XI z2?>fUsfyCi8#Tmg$}V0;-P38rbsil9zB{8Bl96n94QK#7M0xPjIRs?mHKNEVK5s z7cS6c8+x_7Mq1+zf12u?jf4oOHuPP+{`0e)ZEq_&NG<1PFIF7i>326b)iPmI(p^uh z2m$4>hn1a$;gCm#pJtY*Nki!oczw*xVKckin*Lm@KIm}&j>z^$sKfQN^+zT@jkWUD zbq^M<`H-?Wt+6149#ns?CT^Er8nsF$Fr{OF4>NI~#h^$AsOPjFP5V{X3S|1JQVtwF zf|uq&h&TBQ0Ehi}Lm0H}vGT_4N@-f}FN|3GeZG~~1#Qd_z9%yVZC|Is`B?2Y!lU!5 z!h=}^hIb8(i5SfRfYflKX2pMCg@0S(2+6JZiA%y}%kJ>$ZpZH6r|{CiXaTLz7mZAE zl|>9ANv_9INmcD|Li{_~gLYIs`>%(DWX5cIzx62?bc+4MR+b(@kwjU0#|rZ$|H-Fz zg_&Q5Tv`_H#=|(3jlaW-_tFxu?i>qjy}n=w&MfEu(jv3z; ztp>x3_bTA~6FZ#;{N2=3&Vl1;DIt-+Vqw3Y)l4yy@FAZOd19Z%dC$P5GJ)&n>gUd; zAehW>n>VV^HnG9+Be@ID@A6WOd+*DD&8@fww%Ug;v4>6het;#J8j|g>g8a!O`A9F( zx@7)Y8e_kp#Fy(Em9@7&x+jx}3=J4x_Dlm~qekK=h?q%dc(W(fm_eY5t;-|jf7>QW zUw^}{b0R#Up9C!~2_)3R#3aO;H)E9hl zDH?H(C8WyM0^PGGn>a8*e4B!*;flI`5(w*hS@6Ejq3~`Z3Ju|`XOY(`9$tY>OXo-` zV8b*=LTPZfUMcqt)5xFyhy(u_nAfaqK^QB5by3;g!|&|I?;*P6NjwFtknr1Uvlj2` zj?-9?<2Q$SlEZsDH%jeGIwy~RHst-qN{_MN*5k#5ANpkq=kDFiq}%*i_Xrh0(QWTS|o9FmmA?z2%hQs=R zP7lB|m>)Q|M1Sv2@8|fe z277)~kD)a5uVMu(aW&`N)h+yXx`kZ-G$>|1zV>zJZ>6fc_N2g2M4TyuR)@kZgPQ@r zXysGE{tDc{2=XCd2v2mCY4*NR>dIWr@fYa8=fqZ6YXVY+xX%AeI<=D&aYvf?TO8&V zTBn^$FC;u0Y#}`{l{#IACHI}>Z^mR`^iKJ&A;012Wk^02O$pgpF^`yuBNev_dXIj0 z{{jMJi6EIWZfRyT!qfAib~bO;?D`10e&aKCqcdC0!1F)?Kz3}u6_a5~uP4J&h;6T9 z01+|_q^q7dn1&=%iw=O!_JE27Q`8v(0p?|&v)@!b4|Ntk4ZU0FcTq|CFT{7KwLle| zpApE~n5w@Q{6s1dfV4l5Ox{~~lc9$*DYmfc=w(K0vuItRRSF#WuZHet$oV@#8#H1E zm-8lp#J64t>Gq}u?`C%|!hAoF_f&6c>$5rh`en|gFEFQE^X=GZ#(ek4aOi9s;z!}Q z%;aGSF$xF$K61=6erXWg;AY9Pb4KIK)lG#tFPZ$}>VIDjpZta0Xg=r$KE`c{My4|% zD=QB@FUx#5>y*0+jgwpSLKe3VYgD3l6OqGaF>2N&KbdEJ5IoiId3X;it%J3yHLaaE zv<;q9Sy<$Pfb0`hCjB>%Pis{ZRy&LQ+W#gpg=C)%16x-$v`K0A{QR_X-z7#fTGfLt zNrvV8R{9MGUlotpr^bA`Y2N5i=CQ_}ILyS~QDUGz;MvZTrtYs?MQ)7(iN;}>6Aq4` zy=!PX(NNP{Bv0?Nn?t;qPpU3owEOtLsGNgCcI&X2&x!QA{X%_7dPkHJ_Ni|JBq*phna0Dr-(#iFOF zD33rIbE`}>CVxg+6r&AlL5K|KPFu0%muHJWZf3ndNF=?h7W~BEdKDEFMjsq_3h7bU z*wRmz7qyzk2d4|eON)!Po0o>!(cfL%9gXAh;`25xC+4kd1q<-U^CjyMVfchNR+6*W zWLGO6hgM%dKRZuM)@>Tm<&Z-X-0~1|vR(4+ueL9HKRXlh+k+F@v@kp+vZz=Yf8f-^ z^RUNZk&_ikzH=gkpF*Cn5X?xYnP^I~WZkBsjqh4p4>USuI643&6Jc=18f9#VMzwKq z$Yf)7p-CkDIWWG5%H8$u-hd|SpfM6D+rW^pU>YiRiwZgzN^A`RX$Ij`@?63OLLcU@ z0E6K%M(%khzcs(_66b&H9!^0%pHFiEV}19RHv2(nQBg)>*q)0Pz75+Y3MtRUl|vlbzn*0L-gw|MnAlYCm z&$96oK#+Sqx~zmcL8P8X;%bybn5K1a&p3l_%79Qd^>OT$Jf4kp?*?~xh-Fc=BY%)V zfg#UIKrYTZhuvP9R2mm);8qvzKqHS?=@mDfVg5duu89}7O%YP)4>T#xv?xt2 z3U~wUL`y(Zk_DGZ#(wHSvF@V6E^KrR@-nf4)g@h}_!P}RzSj2W;3nU;4lFO!X@EDvKRMXfuE#7r{j7gButp-oN&8!c*W=SAcpeh$ z@W-e0#JKdVG)=jdoM;-;2#@ah>jQUKCy$i|HpfxpCJt4ulM6y~E6Rk$;k&%e`PwFxYS*O@`j|9KJ9vT{I-}qaV0BnyhV#FCt)n3MaZ3t&$REBnlcbXYdu@3b(X77%V_l6Qcr zgIbrk?-X7aT3QqCu9fx#e&L5SlHVBLF{&jyHfMaSMSUb;wU1`-I#S9_zHTTrvk%+k z&{mN5tLqHbSHp3pMwQE1>^yd$BR+1gC4=Dy&g#ObbVDJVWZ?i5e5OiovjMsRriRjv zAVhn_uiFdxh#-E0a9zn#b>#6cgjl6(T09@`&v5xpmkUg)F)x2t0t-(XOeO5Phrk?; zugJhm+BwS4tonrrau@2bjrx;R6@)0!$Cj>JO!<(B;aZsOIm=pp+MrtD)c+M%Y>xa- ztJlng7H^qvWG(Kkt)X&FIVoo>ZB_AP2qI?=GFx*T;V5rMX6NVSVKTwO!M)R`}HDl)aQ zLUAexjJirwXo|Gco5a4%wwYQ#Iu9K;YIuHyu=noa;af%VGQ^CysU_>M_cwQOx10^i z_P0T6;nY!+`PLLU5G?2BW@pog4Tj$DI5V!so#BjAKi?q=sri{+V;apFCO5~w6Kd2= z=xmL_DtiRKLyaiP8|LB@83WTGz#$!^l|u7T=FbhVhn`! zIibyVhZudD#F0c6IFU0k#VHXDtdmy`qAq(jYlH@~AU$V{-I^dVne{WczlQJ0%niI$ z{RVNEcA23fMl<03<6277j;UCi=_!gu)@O#ix`^s^-l7H+lnD-Fa0kNG{-ykhsC2fP z%eIBrNi-Z>=$A1afp(1Pq@W&T=)FeD!(O!{pIQ)D)`)xc^BZ}!ZcRc3^9gstLz)t&b+CBfXFpO? zr6p--nTcr>5~8f9s3EEo&RTN}`lS6XDLrQa(tpjXy3_QYdgmb?@pk(;OsI}{)qI2T zG7uGV$PO2sD{1vb^K&iZ5WnN8Md_oCl?!qp8hKp1AMB|E#%ZL~Ww9p9v$9H&FY5<* zanY#FNxCG~8CSA!RJE*`?uVhcsnVs45!du@(i~~%>8lpdHAi(~(W;NDkCsf3&}G0rXThs*ptB;hu;~MEM0$T` zuRK-%pL^u*jsJnJ9xj#H+MgI`*!}f53D{Y@jKHC4YJk&sYTrP_Vd{9l9q-@4ohhER zHh%D@TCwB6MSi2VqOVM$n+ESk*hjV4pM>5Jo)=cp`XQy8Y>JOfPghnfxd?;S9*zvt zKdb1Vre4FU@nJ2KV97yf1RkyMGkrAhSA}7^OMAT}yS0y<9h@i8{H~I@3imI|6teqbbQzVtxm%t z!o9Fu`DGtji%mc{5htCeKsGz=O2?zAZ^R_W8uz`QLSw|Jb>`rw!l9@GdJH4PLA~_| z5cOkKgNbE%5iz!KBGsn@b_)x~rq4IzcH)xg_GE=(PUzJ7_J;Y%3zvGkK-tt4Cc%-c z$g^hOlV6v^gqO(^?Gm~~ zrUNj-Q2*W;;emfpSv1Otex(~SU_m}4O^E(mf3mx{`LnIAskOCh;pCCPK94ciC_78? zt+&SW>K`7X;2p}+gjKw|>Q0%96g@R}UOEA-k{jo8*V5Vm%+o&t&s7yZDLaP7vE1_P zTa3`meCy;n*Pp#(&{nIXvRH*-HMA(-&+*K=3f+fB1A9U9?DT7qCF?pw6!Wr~3vSh% z2zs2O@UsfZu@bw(CQSGf;4!mo`fWy0xS@Xm67J`3Ob3q3)8jAGLZZg?3$|QIEXO4R zbvSJ3Dqh>y#NRe+JbgD>)if$fqqS9o#jMcp9!ga|Qnqm!4jp%FlFhUW=}POX?OXmQ zfdS#f0sqM_CNB0JcJ85}D!s`o7Rl#QDFz^weNQ6|D_dJz-Cu4G%(K0aaijZ65q@(C z(t!3{tgUT1N;3k@?j&J;=yEadY8Uu>^v`&;z#LGe?V!>K*M(8;5KK)=yxYQkngA0T zzCfR*0l(orG;Apo=07|99@qr|y(RNgZy#!vP_U;vc3@lu^CxWZMtg1(0yC-9O1;lD zBRF@n+sDs#zk$Fs1?r-a8VAuHuG|jHa$YWmbs9KE$Rb4>IUZ+N#=L<-1&jcrglTN0 z#SPLuFg@#)sK_=9k^xph^x6|oY?YM1q%2P|srZJvW4tTt^zE;Aw}y-OIJ&X(X*(*! zvaxFn*X&TpU0?mC7W=o4B9y4DX#he$g+dzmOXN+VEdfx!D4d=7Hs5&BDESo)JdgaK z2L^rmiS64(AmnNyHC%2Za+%?APAOfJ{TFLj#Q{1`9^w4Xk25-_6dJmAat8p~p=h48 zpxyF~h&qKkI9sF-mJX~8x_#asvOMAwNa5Sq$qSsyUgZm1_xT!N-&)S~O9}^31eYHt z<~Ubv>_8wv!VtN24m4ITe8gq2Yn-Ui#d05KdSh=HhQBg0j>jH)YpzQTPuHA^WYFEW z#Q11k+gSS(GTek+sU1An6;iq=B{f3KpR859kym(9wY2ij=|uH+F8`Ya1gnI=UK7Wq z`bN;2MdD-bZ(biHhb&PW`rTi^F zpZuaj$YZ3D7OKcnX*L0U47QsNhQzE_QB2o|O&8s#C8ov+cyzg=W{?Qma(JVb0kCaP z%d+b1$FD@5Ct)*?4v6$(_)C1np2uX-Zd^E}%p}vEhz;K(KzynYbh1~S3Y0-ABPcx z%aJa^ezm=*u4kbm%XowV8r}WF>0m4|ARqwLeETVfGm0;Mh9()>Ywv1GRW$GhL2rW) zMJ}LbQg?`sQjf|unl^{$hzhnNiIa*?dU$nh($Lfda}Ta;%z|reW5e?uyd?n{8}x6M zE}kB3b-Uf&>_mU*qr1^F0Bcze8wHLMS#(GBcp&u+u|eCWMA3l$UeM!a=f;v6IU<{o zc+INjVtP?6su5%qgIjlxBw<_`+=tCM^N6yVraty()f9E0;-@^kp_0K|B0$dY6A(*O zpn4=`M`&tE22aLLy0pSzS}bm(f@Fb#1`%VelWd`G?>WmfPqnDoS=p8r`g80!L*Ixy zLBz@%^28pz*;AQ)@7M5dmIJb|0NymA`A?mlccKARG4Ej1rR={|>MN=Xh7}0DAr3QC z^0;iY^sfe^KzyL;QIP$i7!6YA^XC=t&H1vUku) z^KIJ#?JSdwdy20flPPgj1?(Lz-e{b9Y9uu!$EBLv362*1qRx_%eQ=AkKMvu3*pn3n z14__gv^5NKWUb;^soBZT+by}mVqh>Sz>y|g`HloxySlZ;Q1*bXx!0U=lvxa5mxdz! zr$kuV@t6E{7=z-|DWS7JVPt?&HXueuR9P*|!A^6f5Z(B@;I{y6*#b`%6;V~`Fp4K6 zaGT8tDLA=erSSBudOS~B-A3*9y&gJB#XJE>|=X6=@8*_a{?cL3Iu zCOc&}f_dvMNl45$CGVl_y59W(6ZjS3F0fuF7O7x~Ip0dc{~Rua(_ON(ywbY1;pp-~ zvPGr$xT!S8gB~nvo;abeQ~`zIyZ7Oa(j6>y%m8v@Qm>c z4->v7R@6&Klvtx4%9!rJ*mMvcisl)1TWCT;s5G>KLaz83IKvrF3Yc_Y@a5*>xfyN5XPxq3u%L9`GcVmwqS%M+e@2$xkJo%BUD z-ch1q_QyGIIpF~>@9h|}5aOG&g-hBrubnvdAMC|%x+kERq*9Cpj7Uec#Lz1K;>i_B zGv^SNZ4V5x;}b+R1ZB38{5ZZELOgn1o_HLym4kZG8Q=BDYg7#D_sOOB)#G$iplNi$ z{~=uLN*JT1s9^ea3s!GIiO!>1mmLKsDvA}5W>x5uv!+fY zK$>~74*53g#tP*41RYYNCsi-H;XAlyJzR^9<`^!I{7B z;Xw}?saf`vc5e3-SA|-OdWvh+WFsO{tc3zhZEc(#V?6>HA{z?%i2sT^p&m<-gu4PN zsQe!YLykoGe;DchibjTOjKRKf-82yR-sHaHQl%?tFcd(#rF_vBL=E+mu9k~5U8#_u ztvAeLzvb76Fxk#~U?jQ*2gE1gRZ-vU*!3oj4>+d%U!Rl^^oarJJ1U@+uGiG%@Uls4 zasG{y&)4(eBwu;?hpWH?rJGLNgB^A9(-N9g8=W%n1ydE~)AD-GTb&r&d-q?!s`BtF zx7>n)xuw5@akSab$@Qx7%Tl`>X=d}5K*}Jh6IE0E`^oIEY?APvGl@(8^7OtH(LVeb zw`xNlqOv3oiip$~2u#iwi?20uS;*eg-~u7Cv}~mXUg`v`K8l!6ko>gR}1-WslpA|Y125<@tuy$Ya`m9V6!&v>eVNqNUr0mARDEFovekqcV z@AmcAVpcmd(EUJ4I|xgoqLshy<&ak5RaVjkwfk)R5q9}yXyl<)F1yNgntvO#fW(v+ zcrsg1Lo{P;ZvTi0|L>$>seA!SV#69yJ#OgmeoWY)bUgZdQ+G28m!Csuy%QH6MJSMv zI3PF_U$dsMFOj|#MtNI}8W0-@eIZK~MZXqX6q`dKF0DQAJ_KWC>G#(nx&{6^le^u7 z8ufcH7dJI2Eg@bU*GiidL6A;7TJWR`xBx+h4Uq5?9yW0L~2+-~hzE1evC`JW0= zH{_5s?x?S-L;iXbjgsw$yOtsd#tLo>`dc#;s3ORld3e0N2kp&T;I}Gs5(l;`tmJgA z8_=4s9bgX^za7Cg_+N;3n@3dnecmRiZsp`+hIMQPo*>Z~u3Xp8UsU|A7?)6O^hq*| z<`Mb|1F;nl)oKWO%A$fl_~fM#CaN z_U{}}nA1?HVp;Gv9VJE(U*Nu%L=9ww?&DM;mngD)+6~1b-xi@#4V@V5PX@~H1h(Ji zk&z^!{5DFF-rMJ@g;VD5LkVO5ar#aXV^(SoG#!o{ZlNrpJ%Rx41A#&?7g$7EqzGyl&LG{z0W9X+v$8W>b_jE^asFG=EVg1GGo}tzQS4euo+q-4H?#3HE^Ukoe4AV&atH!(bby~vLnt0Fa3DlQR|4kFLT>ToK&xp!CSiHU~~cBgF?$Itd6md7N0BsrJ_9n=znIWxAHY8j-X?P;}}?BR>TdP%ze{q zvGX$+qc87^u%Bo4w={er`?)SkHXM!Qrb#uMAcKF+DFeCEETZQ@e2&5a2t4?m2 z^oByfSt7sw$EMJg;q+>vOS#rzP#Pl3?a%Un0q{mKTrrahE~?Ee7ydpKi+%x2AO%`P zY`JLCvQTxm`zfZ)2i!Nqji!umQ@uH49bihU)UX(9f_Pe)%j`-vZyT41^vSy3r~5qiargvfuObvWgPg#*Q=*VJ8XXDg!=EWvk~ zBFjYsqJH|xzz9$nUIRUes2H^$ z2D}#$m5R~okug(#SiLw5dw9?@4-O#ne9PIH8U5$VVr-Z}(E={?;#+-Qec36=kyN7yXQ+y~af4_#(NeRCZ6;%~8uP`R@uX@jz_xCao zYdC=gPsd*FHCr2eIqn-INXOX>LUfhZxeMBw7UP5_)*-XlQgJFkFq}pD%-zDlt9A&v zjD+y&nTmjz(|Hw!@AAzA6k+J21T`bkwmTCRIWDitvv6IuXo z1Q&f;PDjFY+zMyrpN(6}7U1MAFXEcP>K9F5^4lEeB+r;~;~(vh!y9YE!voDE4y%mL zn@4X~A7Fvk`sLo>uWVbf&5lX&dawTaEXSSRr`h)V(D>+;`gH31=T77MXLiKZz>ch8 zV@duhrq4BIN8dfS#MHNdo-Pfa<@=|?>CoXwHHv-J<-c)_2_F*U^z`Oa87rHst6O}i zlk&*D%UdhrgtqOpr;x(pSXlj{E@rm5^SM!*hWxwNdy#q~P!I($xz8rp$d=g}l3a3+ z7(pCR4rpm?hdJDy`?C?_^yB69<&!RZhF^xjQz}@(c!%AASEA{z0C`=_^O?eQco9ew z4KC}prhk@H!Iz6$t)4aA?al|a-mcBKPb%EIj5hja`$E4E$a`1h#Eo7Gu2Um=)kDp* ztYx{@Qo`h)LrhR9HL%DKLzhSn0^{u7?9Rhz`lYy_e+f8DtS%abd=kxye4`9N(DN62 zLOkSodZS=n9sGl%f?5`b?sst~Absy05EF~NR+iI#Wqsv>g51P(wQkq{Qg1$OhZ-2# z+mu#0mqhm0H*=9J$Q--Qayca@$NTKs`~iIb=M}(7A0dzlQSO_(4~6Y}R6GIqhUZqcu$OOLfjE%8dt{hRe~WUNiL9eVUUk)l3}2_E$pODa2xT7k)5E!|I1<4ycOLoD(y zuv>Iw{R^-ZeWf^v*iaS*U|9Jj%=`=Yvsyj;i=JdR4KD5Z)=&r-8@*S;N|W43fbeHPkWtCNLc{4lAlA5d3M{hgs1f8QYD&-u){ z@BC2(@7D z=;5N=b9xo_A`9LdPJ!PFY9e^$*oRW*!OLO8 z1R`;e;8&QEo?EoU7$U&K1YcXE#ksEl3sXmp>yo2cG$eh4;i?lx_n-SE zojE_E9k|ha5U8DB4ENKO$RVYn#LFfY|GK8)}a$z4T)W8ru)^s(L zR3^LkH$@qa7CbQ}QExb63>0S#s0lRMB)3MxSQ;0znQ*e|-|`6nLe^Yc(#xSFU)P{v z)j6ZMh&dPXbt-S^QD){E-T2t29hzbrUrL3eQ2aL1@5hUY^y%rv@V@kTE)BE58^{{B z@KE8qTO`JY&Q<=mij9kLH$;*E@+lN#fkGed>8(FEJ02+YV9{Ik>1Ure=GZpzHx_P4 ztNW|%&ydQ8V8gYHZCXJAvX3I{T&Pogv$~kaiXV!RP$ZC^y8~f&-0OBvnXXvE$vGQx z0>lraR10R>m%>(lEqDCA5cM`|{2Nyx;zW%gxL>>_F zp&2B*qpfa(^93FpbO*!XLY~tfDeKQQ`hy~6yzH;&clh8JiH@F-w%!w46Su!@o z{`Qsb!EWzhZ_wYTJz2_1qNQlS7pXU_$wqV#ezOSG5K^x=KSgCgHq{6~hOqvVk@tDBpfWO(aPt`Baj07o=eY?XXK?nDEx+v_DN z;OBf67*?zCu`oPvSXVORC@lgb7z9f!^CSol8VnZ}7DZ5??DxlD>!4>5>wyp+P$%TF zDX1cJvk_;Z%v&N~LW6D0>>qLHvQwvD`Z}2s!^lV zDs-RmD34Q;1x6GI8dii$aSIz%KQYWi6U0l{!0d;H2yg)tbV*~njgm6J-f}^_Cg>no z^aMSt&7lM0Gvk`d8&B&^Q5ZhVHVHB=LsG|?5NP02YuHS- z3l=zC$O0;hY>B6kI30oT7=s(-1Q=bsba{7ouhnj@5XiB-yt+#NEHl;wMu15aoUpDM z{l>3##we-5Rf)tEji|Mu|B(rnYW&WrPIAU)3pwNk0y^tDR_{vwe%_u( zzuzPLw?FI~$6+kgeaz{Qz!jyBiGX(LpGW%9a?p~IOvJ1UBGoHKV|mAkj46xh@~F-; z@jw_8i=N1vO=u}{+(H%?!>N?~L$uV<{HE7fDe1r@Q7#XnOw_Ld^#OGDfH>D75$UNk z0mDh^LGS2GmoDCT)6LWZN!4);-il~O73AYPRaK} zj`{t)T>>&V9DocV@qs(x)QfIF(SVYuIDN&n#%ha_AhprL&dfP-?l+rs!G?oan;5I? zhhOZ7%h&b?L(5F>mUXOBhH6Q!0qDb*oSRYdKjz~hAsYL8moHzwu)n)i=EF>kgF;eH zh$Ui#^((~MgxMw;>>LpUt(4nwXCu@xZA_zp--rgn)+(oHI zlaP4hcAW8zYrS2k=1Mw*u_#ldapv0H0CJz+B5ekGY931Qu2o;iE^3aB2 z1?*(#mCBjP#>NKH2|=KQ8WWL9xDNfQ-ycAQmnTZI0bxHlT?vz8NW%G;odkIqCo~9If*+e1;8Ze?i)G44-Mg>ju7AoC}KXF1pd2nKFe%VNaLyQi-JBtaEeEjeuaJ49yB$DhE@eVI8 zLyO;H*;!dxr7_%NmFa!@e}*}RLzbr+(mSOq3Z6bOGqc{BS(`JhdJ;>*ct;{>Ubt?V z05F`+M|fr&C}KUVB%T8-Oj=V3NX;xqQembP!n(uD+h?j`CxGx1B{__dm>bJE$(SmZ z(Aw1imx@IN!;>1_S{@Kk{AOGN(KmN?(+hl!nZ~Hh5i6grB16Ae#$MG!gYW|SJPMAI( z_#JFHFRv`$c;k&1FJ5FzDNShuC8;9z$!)#1woX_E%g%@Wy_1X*&`uNGfSfMVY)M{L z>?O%|E1h_lVKYzZ{Srm|`z;>O^6@?BHTWjD)9NpoOU1zk0iO;M-+g5Mx z!fK&g5Ww-k0}nuP6LnKFbs|>KMcEJ(N01PmTtOeD38+KQRX-F&Dt0s*FV1hPv;ZYE zS*faI0F7CsY@~XNy+NZfV~vhc5gW{GXh|K& z1nQGRw%-R!BA1vK$HpqF*pSVsb)z=qu>5`>txc#1)JEwinz#f^3Z{6lGE4*Vy(VB8 zF+u^ok5Uv;TOt6gUjdj%#b9a{Wzg|Wl<5P}6BPv2&ZU1LM}o&hcLSpVIHk7$jAez{ zF%ppiRGZ~F#$!Dv1|1YEus(xE0zQb0CErtNDCnIe$}g*941WgEjWOT|6TlZ8S~xSQ zXNrkoI6EcAp;L8YN=CC7xs#?yA|V5giBuslQ&c1sh>{C_^-9quv8Jw6qU{`DEKLs# zv)@Vsa-30vtx)#xQx{ykcxgO-*qJ-dH_t6(9wo6gjzp+%?a!;cLG0m4VXv`-0f2W* zr5KWlJrCeUBN_Zm2PEzf@(&~kL&S{)8pl+{5amV|Jr5llo{1B!iIthnF^EjNp#__! z^iYU)oYz-Eg9?N*ih?z*{%uKD&h(A*4&HbG01yC4L_t(eXKigwnLhel=%(G>U21>x&MhbgLGNiMNNSXVb>b&f5R;dm>QZD=TGD@^;LolS7m+QMMF0Bj) zJ#(xJEmMn2%hTrH_3-TGx_|tch1;aFJPmL>L)+xAF8)B3IlyrYi+dczjYH0e82m_U zVmKg$L!Hl@JxjiEGO%Hdu;E6Yru~Dxy`5H@oJU)oA?cGxj(`WVhgB!OZgf*6TVZe1}R4g0idqz&Qz z-pq zr!&ddx3#rJZDLuU@)j{>ysub^0A|Xz*CW$oAnf_8xD|zFY@m>6jo2sOYMwfEiZ<>L z0kuJsvaBcRjGdVQEAa^+dAl!~BlH0_Fxd=hWGZnzKq{_2g7K#k=bgxoSQy*nP(^~< ztNN$ilw%*q0U?oP!B7FGPv)!w2>;Io4@+#7RT1_-nq4Bd*mf1_M2oicWU&BrQ0tVi z0_r~W5yUKTYY}1XX#2p{O~l)Yt`{pR0kS}+4Opvo1e?u94rEw&rk>0dHnuV^B)cfW zObR7jBq8er>Lzn;6oyTyRyrOT^^t*L6-t;u5R#zNnI@?XIiz)f*`SU=A4}yV$Z4?Q zje(AV33{$RP6}ViP#>33&J;BoIR+|6g6CR>9UY4Czgk-tQ}?GPEZ@l1N<4&2eHPCg zX*!!?vPsNI(GvpVEh&Dv&|Db|fxJR6rSy6fk~4@f3lS@JZP5f>SX`h5IhNhUnvbZx`|i7sRuXz$ zQpeF!>8gOHNd%3S6OQt1wU?I}sVR_ug;l8g0)6)A|3r1T-|Ze?hBEc7)o5|@7XzOF zheaiirPY_g63#%INF+{HNRX5hu^$oLmXItS zagvw}JjT52^$whbrG!*R)-+ko%5p5HK*L_ds_^So$f~CCLT)PQ(!5-srP*wE4|dG4 zE;K!tn0ugAJNBg7Yr4DbuijvVyS8$MWYWoMfSGFG8*}6O! zcE{uXc+{t9MqVXWU8Mh^O!F?>-m=aGz}s5a=#K#`n|P8k8=s~V^=4KxIEY6dD1I-*LMLAz=) zKCfK4BGQtU_Rvd9%m~0P9QLQV<`UmUKWlu4nt~Bn!$|{|mX>JOPW=uu7@-_R(20z% z=IY@#dITp6bd?~~1=hPD1!-(Z!c_CEWU7=g)>g9oX^Qj~#p)W>Yf0?<px3|B$N8fc85UC*ojX^?nf!P@e5}-gdRFeA)nBQR>4IZi;YpIH+24W}@$qqx+ z5>TV)N|8FJK&kPpJC96!a-6D_&(ySO61EolL zrr~^O0XUH>i|{xUFqEqeE`}++(aXP< z-i(lxXprcXRCl_mhZX3=(fO-mSd+A&L(Qyt6eD^zf(u;~o1-{|8pd$%s|*TeI^#Ij z=af-cM+49Vf`%E7da2o5NWPvN5D!KX(oyg1>=wd4xE6=Gv`aiqYL98I1t`C^V(a-? zxHUM^ritjf_>Oof$=rvr&I+5fO3}1SBvFV66ju;~ELehnIUmduLs!;X?R#eFB`iZr zXf3^G27?ig4zke+;27rsoe*+37E+awq2FjV2(PZ{uEN%aXv@`jz)iGEM4LX(R(%mlVPr~u4-BcmH`bT0?Qbp9h%&E ztk7Wf7PPHgSZEUf zIZ9;o(5sPiM~t@wc`j-Dvazvo{`~oS?zyMeA0SyN?a{jX2m9?^u}c#PJuwM6U1>YT zyCSyMgic%v7h`OZ{gFjJ**8o395EMZYtBld!5T5Ku?HKnnKd>xP7nsNzq_@+x4qbI zbUN)`cW3L;g}vP^78A_~jv)&wgF z?(OYv@9yn)X{TGlyC+KV!t4&Fw&dCh1}0+i@9ffxrJ1p7lL5gw2+7rCaB zz!Npal~19^^;Zf608B9v_d!$nraom)f-7rtC6axi42fW20@+AB1%WUo01yhLg~4xa zMEr44unFNuLD9AvW*Gm<>`;_Jai56^7zY>V$aGOLwdgIU&1Guqt&Qhwe3uyV&<4>_ z5!Vw3(R#e7mW7Mb-zsjX52bV)ftdg*(HN#NKxX6^s8AzoQfn+OF0Imj?ZgR|=opXV zTE%FEHHXHfOtiyOSt98ZJVHYNTz;scVn$3?#zo{ZZ4*~7Ch)yn;LczSn8~G;-vk6^ z_XflLZjT^@v84W?0Xr6HUYfSZ#LRC%&(Gq5CD*wM@Y zY*D`t54s0~UcW5Fmyk3NpXN1)%ZPHtu7yMtg^+cHTZvw#CChJ>5~-6RJcTpDR7^Ct zyu89oO|>i`jxyzZ>J1BhC3amf*s*9>YLwT>hG5x$BV%b^=4DnPAo5$k=^MWDyS@j1 ze%E*Y>tFokUwP-d-u;Om|Kfl0#V^Id_q^w>@B4w*#=rW67k}KxfBcJI{r~<4^9(Ov z{DuGMuDkC3^S8W}K7Zv`eCdM^UHHx4`rq{VRj>TkKmLm5A$$OE)u9dX}gvqc~XoqXYu2cA}X86SJ* z%*&tmJo@meKl`(P`=S@ohwpmJTi$l*(hZ%?%bxeVm%aJT-~3U}`^N7j!~Hd9LKr1CmE{}3XCOA0-EQE>=93GZfaHN87th(k?a|S-;oa+rG5BeW}QY4RY6~$Xeh=K&1`LDt=Vq1n{BEnxuOpUsX3s1A$jqNw9(9XKWBUH$@Eqvz>3d)RoeQP7I1MIRVA)oJgZMLunO0ZZFN1Z-2s5lm!8RtSd_ z*z>>w$W;pQQOR)wh(m5%Ru5uKW#@QksVvvl*U6A0qeA{Hj@U*k>Y=rTS;CoF<-AU23F8PkZX)6*8q=DVFer9_+We>ok~de zt7#M%WF#Y_3{s#nHMoh`S?gHykE$&5u@K8-47$FU99C!!xr`!f0}w`?2xbRNXxxZe z4^ut~Ma^st$x%~Ze`rD=k|HUt9syJa$6v+=q(MH@E)D3AJ$x6Jf&M>7F`UWA|eU1eg z6{2iB7U4{2LeZoE^?;{g6Zx}J#qw;O1c%8Ee*olcFt5RQvJjq-$4QKrqjeR==U*Qt z0J{9}r9-}Np&Yi>*Eb0AfUHk~)%TrT6{~vzNr~UoEd+W2F_0Yo)kAx8SYrp=#KI}uE z^8-KlpKibPS@h>;f7WNblYechY>;LV1$%+9^yo4;}Tf*<>$ z7m}Ti&-dMT|95@&_nD`0`LQ2gvkFkFd%5u6Mor_FHdx z)vLez?3pu<2~I(X<0pUOCw%!=el2~z&-WR*Q;Ok{`Wt0;n7Fmd*}Q9_iz8M5d?>juW5jTd;x@RY+ZTm zs<$TiLr!z6Ya7>c{@+^JXt#gmo_n6Zxw+D8zU#`BhYk+bnyqjBsOSA`z%OoJUHw;Y zdebXk@PZ57F8%qckAC4>EnM%ei_ zDzO9gYV(z8WRTFYM9PI=;aAOz{|y_v83siS(Xe$kc#M5u-%Njw`oqhYFE1}spLDWp z3CTpr6v6`JD>T z4_fhwAj6GteNH~o8WHH!mTa=LQ)m9U6_CkUHH%^!6*r;{c09lsGv~$BOVuL!#IjpeaRvl*ej0pf(!i zs^6(rz19Nv(h&|TNCI|zzF6M zL9&2_Ob11@#Sbv#YN1XvV!&vyOpGHy4H3aaHsYih1Bjl*I=;TX z#*rzKZC7pS3f+ic3ukKcab~ekX^6`7rBaWPr6rP}T8}B~-SO-;n8wyi?Mi--!0J`p z5Ua4H)wS{BhLIzE#9Ba>WtAj*iKITF1K!A&!Ak>HW=cbw4taWGc+N(;64Yz7Fz)Q^ zEJ^{c5f2+&!auu$haXL$# z#ZCvU!F~%tK!zi-k_Ny)jVOH3^~m;EP12=qmwWT_Wp*7L9BgiGnwU3K$I$0xUY>CI z<^TSxfAQA0eb1}El%-H*KB`OhPOLr?g%-}o*1PZ=Cnz2Kkx*|cKE z$N!y=`_xbQq-k`6%D27p-#q;p9H0L8KIKhs`ooX-@DC$&H3p%+%VFoH5VBmXU^UHv;sI#JPAJ76t}OgO`LFIp?%NZ-b-%14$FR6-CT>JKtMIoAiREkZ$NxvT2PbEt9d8bFBWKQTC6M8hk^n70LWht{0knb= z_FHA)9M(1U?l4o3hqvc(arxI_r!E!|Z#PR<^m_e^7cY^YmRxaUYeNzex%z??7GDsO zgw9uZ*0xfCgCb;grEUKp6zcH3eFB(<9L}@53nN^uW%@a+DLPXVaN1cpIDPJe$cEIA ziUBL_`TU)2*XHED7Rxo7sG_FBP{9Q^Ha2d$>1L6-h%l6JyNM^&uhdj#k&A9uqBArS z0qG|b8WZf;lp2cKNX}+PiUBL?s)71mrrwev=)s#KVrGCPA;wP*3k?JuAOe~_#6-{% zS`|SY$T&;KB}0z&rmSQ{K`SY(Lxoy!O~3(qkuv1(0P_OiMH$)c_!f8p!bwnh`Y;wK z6OW0@Drkc1n1FW#1IZ@}ku(Iwa`?nah+AZot$70wvS{ z;{wIYGCa%!yC@+OBw4Q|KU4_PK~7?TvzawW#9fZbGY@n|^iDWr%1N^1Ml&yW3-3o!ueKlVI>qkV6gweKeJUy zz@uUkM3VA?QI-KU@K}h5;yR=u_wcj`Cef7Q^vtrp<-#@`dB}{DYDq-cNC%(|pZ z8=)Q_jt9d&Ar2y1GwLNhv%J{OcpdYLhGoFG7>#8_@w%6I@uSqzt5AT`CNE|gr<9h^ zo?JqoXiS;mkXdy_(G+}#dx~#5rdNHx*X_~3=JQ#UC!x(iYhlp{bj!<>&|_j;zj_G; z1k=pRygbpup(ro^hM)SGpS$hWTUJ+B{`o)s;=g*=Uz_Wue9|ZUoik@n|K;1?e*Wya zZ~3OL`_ixY>iFAJCpSLlC7<;@-}joY|C+BPXV`E2*8ibZ@Y`SU%^&id=X~SKzs)>j z%O`*0C;Zgwf9_@9@YVnMM}P9;Ui8t*xH+-5_Ork6%k<%+KJp{(x#wOw;rn0nx*z>7 zKczo>&HwkqGcQQH=;_FmJKy{6|MY|Zapw7S^3hTqPtWq)6B~DKZ-4Hsw|v=ipZg`h z^Lw-oKi2Pm_3!_|_k8Tfz5NYu7#)kHcn)wJt8&mFO*}Z&azjr_eWy7YK6ZJ=lYj)0abj2NO{04QR$l`LEyVG3KWOXLr+S#OaC2sp*z|@56ASM=1+-JY#0$o zfF+=bNaIGR9>mRfG#jYneorjxY@?M$4P|wQtR^)6J$LRbz2NC(0|`tVHiwLVS8>8i zaQo@$fY*pRrX)a3#l6q^w|OE)CfN(jVjavLPW{;Fv|A2{KonHs z2Rx7qt%hn|O1qH>F*HG2RB@j`JxG*BVFN$_-Puy4Ody8=%O3zrOG+F9ld>u96UPF> z@;PWjXbGSeATJmmP#gd&-bM~^Ykl&XBt)rTRG3Jxvk1`>ID*Gf1Ozx>5=3FCI-oO1 zgGtVsBy~~J;ux87K?DTtsUrcdbwxB>l$k{-9vWGr09?$uNTN)%{;NRLvVo9-rA_)8lr3!yag6*-8Q+Pc>rw2g9!S0rMSSDVbp@i2RI1e{b+5`$f z4_|~7IfS;Uk}H#RWMtB}>#J+Dyh=4}%!N?f+C>Db*{=#){m?yi+zm4YK{`<|=5R<^ zy8;GIM+#GD{Dud9fOdxE3Tv{3=mXR7n418OPl|OwFmR7wX^e{yMEEsBJu+Y=>!F}( zRtQ;S;X8>=Lx71V+5p;PG1AcpvnFaMkG<0SkzRt}Ln$z){2%}vwnm!sL3emXVc6+H z7r{~`^28VeWl}xIW{oEOM}wQeGtMqH#?sBIqk~?5ILMG{+BSjtc;-^cp(rC)4u-s% z8P@J)2LmBu4DB=;i;JDb&Jta%-|vpb+zwZ^T;Zettgc8#5~dIcLO*AKTV}=9_BNeD z4Z*tL<57EIiC1;PK4ir(bvBck103`6{@$D4{3oCP4_Jt@qu3|L1)6XWe<{`+o74|HtQj?n~ltKm5Z!baA2mufOf(4?gtJ zm;MXJK4`!6i?9F5-~OH7y%5nfe(TqM`NE@~jPUPX{VF3!!LR+s|N6=Q`m^TgS6=Y^ z=N@_y>RVz2Ex8(#h3rnv!^$I=m)=#R-UaL zPEc~|%{Tw{@4ex>|HBWM>$1>`Xn!<*=hoIwz4y)^`{YkLztq_qjV|^2U-Ub_V~$Ii z102V>)OVuJ9BNdXL)>h(Zl!y-TJSfSwY4=er+0RCM{DCtmoJYcn+5?K`@6d|~9WO&WcXfETrn#5?$4LWoxv(iYhDSooex7$;-Lxh?kZyZ7#1?_fOY zvET;z#PYm%aIn2~d4G3rcW-aR>HRoxdN?k|v>z@+@1T_Quc_Du#)W4s!}F0z0+9y` zCNR4Pi!!&RDeQzB*aiAV++uYl(O`|Z+E47&;vRRZt&*4Fm=`u57oikf!V zPp7PhQW-GT=W$5I={J^(K-ks!^tYV9; zGs~LO4QnIOgfi#?QY8u9fDT9y*F@C2%M0>7BEKW2RHPb-D4=R>6fm%<^25=9YSauW zdWaK2ct|3dVhHsh%LhyfC0V!-(W9PTYJYhsPMxdQ7Zju2ZY>~|VJu~odxcIs2znfm z|A1XMnWbpKLg5d6R4|`{JBVs1I0C{LlJI!|01yC4L_t)Gc$~lqY$dTvm375{SizZA z$Xa6D#FgaZ%^FO2awY0th6oW0-j9xavT8I5ZKNzf&4%ESx!Dh#q3}O8@u5(rwCO@dtHtWt8m|?@frxPD2(=`fLTF?zV+|08spX%) z%Zpr-~7j90LLf%^y`1&!$0&x z2>u{A<5&LYZ+zUxe#{^L*`H4>>IwC-t@op z-~R`Fe*W`5LcjgJH~#*g{@Gu=_BH?Ed;jBWX?6OvPyLj?|Mx!mTVC<1zx&Cb`0DR? z)U95O1RZezW(Pv`uQLAnV_cAujbC&A+}SVs!k7O1Fa7sV`uG>W@1A?< zx2I2?ymoZsgFg7VKlsBxP9OGmcfS4GzR5g|3nA{`_>mt$@N}noab?nNa-ycK6-%->Y8u!ascE!W$oc$Q+|G2RP;h8zyX$5_OCjV^pjm zoaj6`mV=Vn)3?05a>M!a;{ki~2c!PZ_TEaC^as5wTUQpBmKWNKbk6G1a=Wv{c~wfb za26IjWbTj=B|KrTo5;HyPX1a*E`Xf3U*z^D6KkPKPD$EVk*U46ySu%!wYRrT4pFKX z?JW0qceXBF-nw#yz8MV$lAl$m-eh?i@T zi6Ue<5jCP&RN_FM==%icOzhpCf^M3HlEC&`Tv*sY$ai*jE?v5O`t%uC#eT;tb}W46 za!|9P@Kdx}sn}iRm|BqBeBItbcYkm1px^K9?(7o80UMeO2P33!N&TC062ZtBuJ#-9 z#cE_k8nV?AN}@hY7c)2_`y;jt6Un=Q2K2az#1ENB48cS;q+_;4^Tdhub7#-7hcd!G zj%ZJBAO?ekMie!mkM9s2J;y151q@0hrU1sL1pjjo0>K`Zv#a7bT0rtF_;4PoDxogH zuww`bKulbu+!@nDj8Hic zF@*l2wrsXCf*^5s=v?GvH0IvMPBYtTa|U8EPG|~HjW{gE<%Hmj^AQ<2tWn7H^%kx` zt%txbuCpu9Ym&7gbBNU{O+%CeM8!}-#*CL`6}o}Q0XPu2Tmlz%iBzbZP6rVy(&-Xi zCC04ffDTbPEkQ9$1tBF*f6+`D!A0)@1;sCPVk(HBn1YUHoDh#~2AG5uGIL^@0D{=4 z=q)Ck)jBMbpc7#t5Ja<_*oa*chXv`G0Dv%?>(ikmH#tLB1!wcQ`Q{s`qbrigVbj4J zRh)V1b9wrhro1*{xdlW;f)=G5kLVzE!ClVwHcfV$(6=H*L8FT)dud+McqXi^7}wE_ zM75mdDX?4%Kq|vdR!Al^(a5o>&~2#EXeBg>PYL8CN0^X8a;Wo`f;<8-BnWtQbv1@I zOn|zezTsj9P!tAbxIM3y#M!OTXYVKl9Um<2V18e)XIWe)j$Ef3V-{n`dkxXyZek z`<%^_r#|}m&;ODy`NH_=sE}*ryZ_yHJow;4U;Kq%u)DKIbNIi${ulnQk9pBg{PgQ@ zzvWpU^P(33;CN=0)0?Nh`_-?ybot7wU-gPV{qwhMZ*PC!_rLZtKjZJy!uEfD`*%O~ zMIXJlxBuDy;H3)-?N@x;H+|m^y!P|{!At(^EpMdWEfWj7<(76~``v>pR4!fG+u!9~ zTt3=A*t>H1(#6Lv?ramZF_cY}*dC&Y;s1l%lkMsxU>6p#+`3(NwT&;34tX?BTIOhW zp+nbvwr+~5UB@zDa%4vX9By{dgqb!prq7Hq0^F+q+NWg)i`_Z%<{@IfSX{TrZXWSHx)5N)gu%J zA{R_FV3)VeD<8WzbWvK(OrN@q|?2rMqJY3Nsg8>m|m$-*7sytf>;Lm-lB zc21{vksy4Zbp=!u)l8)e(s-lK1CbbX5&J;F1wPpaVpz3gV%ZVgPNPVae33{NTpMCc zOfd4bi!}&E$rrF8W0FV`*0PXbY%|n@w?D!wi^W4i4IhwiP#oT?g%|T4Bdx04hzyXfN$9~LZ%t55#KkAKg5?|j3X z{_x_(%P;xt&mj0C{^6y|m(A0@eE(}+X9VVW>s#LT`)~eZ`uul4=@a!^Lb}Nl_Q>GT z2j2hSAN}#4zW>38&Yn5_DWCF*Kl!7t`$u2=&v8B#LIhOCLd&oG>Tj5*WuXP^r+((= z-|)uYUs~$?lP~(mk6pU-@|S%b7#tt=q0jxJKl)QTXLoP!xzG8a8_%EruJ8QUZ-2)- zU-W|KpIBd~bPaJums;59n|JYwa_oAF^1HXzlrl8ZC0HfHbQ{vBubX5W`;Yd@L%V)SL%>hBkWgXCLw8l5G^mAt$y8=7Dl~ zeEg7MUDzZx4wY*F*$xF$j07gY09NZ)>u*DQ&3LqMSyjh zh6)uV<1bILlsJoF$8TJ+M z7v*%gkhD^xP6$D?MWWmj1ks=lu{vpN!0S0+oq(`I$EkAxW+?_C$$cv-gpWHAoIQ4%gv$%s@pA&hjgmkRXzzuHoVvQ&u%POyin%$uyzp2n+2nx{(wda^VS6 zj4mPaM+k;2OCc*Kp^X!Wi3tM<7`CAxf90pY+z^#5i${N86GZ~^a)b-LZp-@% zN?PbNGvWzgs*BVB>ZpRrvRG*`7@~gE0<;j(GH7G$uO>t?FZ1$L6lXvD!#?D>&-vg# zc=I3s(trC^J$&-S#)p2$b9Z-l|MG3`Fr4}IzK?p@*TSAZapJ@;{WlKlc=XZ7zWd*Q zKh28${k?Hsyx^lg@^|0xrqB88&oIxNve;Str2Z^=4n}|=J(!v->Y8vt*fgmv_{=|*WJJKyKjgY95f8R@$3H8g-0*a_T?9U`By&Z z*>^lK2FEk2oL*Q&CX{oX&V_DwRU(YO|LyOH&-t(KdGGiB<=fVp&42MB&;9gY`*kA* z3!VJWpZlDjc=vnEu_$wZ<5(4Uh*vEFC=SCuj`YackV;U`HX#KOOQt91&!4+=>EdWK zrZ)$@6=?HBJDcsTE32#P?e=0R>{l?9FIcMp01yC4L_t(?88b*t5uDPrB;nR9UN6Lf zF7pVw5K!p%dY3O<-r3!z?U@+O@A0+9LBlIIUF6VK1bD?tW{jw_)oa& z$towCb7y;p01KuBC)^BHBvj#hr=2B18!+bSUr_)p;1G5ZV?xqQR;H;jW(pxFYd8(93Jwd>zJyLs8kxsal%W6sr3})c+NVh*#-YPIHv9F~ zHbT2JBnRV`EPSYRPW);bn=o~XhZ1x2Ql;jm)zKe@x{dTBW8>KcQH-*RC8@HvXjkf( zEP`x=iNHG(AuL76$m;?*C9##@B{Wcw(p2OX7^#F*3hhms&4%cJ_KDJf%C!(jX|fiP&Ag4)A6w5%+V zatmi^rhrd1sC-J6D&{EMS%)$I*K9SGmpk-**2oC=9dYPIF&qm}SW0w;G`n#=JZ+qg zy9Zs`VXv>RBUWo-QkXEF?UdM%aWn0CSO4t7^60!qs{c4UNTm#KRqFbO9(<76?A*C? zz%=O_YOXMNN@VKcHk3;(E_Bi)qvfigk;CBF7;RIE3xW}`#3f!3iyaRQ#y9QAjp#xd z!9e^9mc-8@&BV)Mk`}`elB`qn^@l@RqO4FtXtklmwv1D2g-PlHLOL(=^2ExAKKD5< ze(}dV`;OZ;Po4aXPy5vO-F?s2_Vy!>Tv+OKAXt3I9k>3MpZMu7`_g|xAnE0;t#`ib zJ)ippUmRb7fcLNe+OPc9SANIe`{YlevHGjO{+nO>)&KGj|LBizx#gBGdgF1y3B+c&Ef8)2_ z@mKHug3tS$SH9xq?|8?%{_E?19>SuZ`;yNjREPkMZ~WS?`r?1~6(97$&;H=&e9*7` z`ft7JmEZD}U;T{--L83>mFIoLhyAmE`b9te`d_&F?t6ahhhM8`juvWxFZ~x^edhG$ zE57ZUI*W_{;B!8Umg~>`?9b}mE8v)x+g2-jB_AF|IbCBj!C?b*FaFOn-;m$t54A;X)@@X?T^>~ofd7)ap2 z5I_SR+1=WvFX?L}UZDMyr!terPi!V^CMClpOT>|{ilbNffIY$k=A4Ux2Jg=FQ7tB7 zSlHl>v{7;uJKbi}?Ri9*IL#f7QrRf5KRy#a|4al9$Xum9KypuNlEGjIiG;nqJp`eE ztO4WGD~1HZMPyGR8l^I~)Dlo%ubUP9VZ;m@Ns{mYyJU!rKoEx-X(qJS^zJ{l4Ls0b zC(w;*i%%zzkNV7+Gb<}ANnJ)!sSpzlYa@__YSonjq`*b2a#U5|gGT99RH^w#p2l!YzyYJ@s)!gmee}2qp$he(TRS1fL3D6hWlRMuCLWglL3m~3te?E|CZI(#6Z&M7@J|on6@N9ox2Tn>)5`qhaGT_Kt1ac4Mn?(%6lY#!16( zKks+mbH=#;gf-T>?rYBZo5m)-b%|7DkVS}T`1l94?*NN7@c`N8*0X9ZWZMq?RHE~h zR^~)*h~(i|2>68NA;tmwd??l7uSo{Z)%hN2F{_r9OH^t1Y4N}b2#t_Ca!yBzJF(2W znam3hR^uL79mp{=i5WHZIj+lcRqP@*8fzT8u@zHl+(-H4z!pSN0T>qwL7zd&iyjKK%ri*?gcbpc8-eZb#U~U(3 zC-V5amap~uzL^Ak==f*xm(EW7cA9w!dXGn0%dhs|JiShtZx?qzuMUHqRGSpO+01AdIrUr(|Mj0M$np7S z!1~wkjdUb%RcU^a2*RaOr@@+s)ZFv@IyV+)M+)IbCU26}KafBAq}OZ5;|mfMMdt8N z+DiEB;@IR8@9y`Ds-+YCE0n)!l8+iZ96s#UcIv=nzP0t8KJT+8##SB=)Disg?-&Qo z7WN&I*^LWp7oN}UUh85OyXxvwtLbgW1hpl4^72yc2btA*DX1AjT0p7zh*tM1hf9ZL ztZYB=Gi=wJC?3G5E_#OnK6l)Twugs(P@Ro&tE-{3<;l!>`8rr6qAS9Zp!GHZpe9+Q zAQ^DX^#YN&Qs_Ar&|zF=S?p^sGfI;MZ+KxE)Kb9`VEm2~Z%gPmO@zl1Ub-Qjd8t>?;(bxq(X$6V*Z*hLMtnwUsN0OW zqDjiMLQJ3=OPrAF#?#YtF;Ra7NMllyB%Uk*U{jw&m<*wQV9bSam$=GOtX!w(R|sEo zq@pWH@|gg$j*v~nY4yNsR!HG2l?4G_G9+~@IO5A~M_mUbh%QyN#UN?FDIx{IHv;zL z%ICstz?;R{qPmlM-)%x$W_8J#<_8KXE0N3%5)ZWv2ZzyqYSN3F`&Bo>alFh+#fp&- z41^Tf`s*U_2XN*lE-y6{({m9_$X5s!xi@Vy;xO5AWY+`s^SP+}xXpqQ8uKO>VI~AC z$@7t)d0=$#1%xWghD6tzVWdD#m^6@Hnwz-9GB^^G-jgXG{- zNZ?KPu9O5rDOJ?JWD}XVMF%{8Yun751q`iei`VtGwnDutvtz-l$_<&v;pWTO>&fZk zAYW$>p>rt{bIjcD2VuG`^b5i3u2bnaP@haDG6fHXFXZ=Fd34QGtM zflX@1)U^n@dtY_T+{R8Xllkm3;CWqb z?$ezP#2piDYLZjjFszKxlzaKlC%)_c6uay<)f+wV$W-#s+1O&N3z#>w6p4 zC3dPgKmbbA{fTBj^Vt$+)aI3xKU;w*OV1;NLo zQc(PLq&WGye|VF{=ven%xKIY8XLZ&<@5uNvNo6ccd$rUcakD8=lRj|`4FH7-R1f3d z_eiZojBgGh#N-W`%;t`kRL3pN`Wi1#*Eea`g#CI`-6|$n3x+Db?Foti98ld zj`!Yw)&8bu>rtX*|Z!ZVIo3cPSD zYBfEmdO7Cbv3bSVnRFuqT3`ik9`1|M6D2VN^2Ua!8tO){j!NTAO3RxTfDBqugpa!d1L>i85_ci2tApCEHf+e+J? zGV1J*@D{E|LV6cGMuyV9Z+>pkY@x+W%O8kfXpU}k!$VFo3eevQM_9(vGByNOuhQ++yM^`%0fxLW`wQ-E_r=LR94;r+^L*Jh> zuUJHFNS*tGw4vGqXO(Tz50fMT$6RAcl4q#P6d`V#P&IZT4x}dbuiomo^07Zv9Hk6X zW(c_)9AJI_ntw=R0m~8SgBAy}uoQN@TPk{TeoLLal#AlEr zw(0>3ZA3i&0xQS6eVCYxog+*>a~BHI(PW+EDKZA}%Cm)gkE=sbHv=j8=#qJ?#5~a( zI6r{3w?NN^KnPf@j>t`|ot23i=(KG>E#*!pWnaVjBrTfW{XQ2bA6@#RREgry#syvg=S`fx_vY z%Rp_|@%Nf-!_&pOZvTSAa(4{f;V4L(3q_@x1qa?PK@PK3OLMoV@)G(UlP*I&y^T!^ zWZY`v6&oTh`4P{#eF++cik3AqjmQ9o-{HRK!LR(uKKBLfz)&h-+v3HLPgr~Tdeo|Yr_`9bRVMCn;qslHTZo*5MN)A{Sv1 z=E1iNBqdZLS1Mt4P8pd>@oD`Sd+DIkhi*O6t^tm8lH+X8(xKxDPp> zwuOx>gu{=MO2+9JP1OJh&MHfhF%;3#lZ$gPopo*ef>DZq{rh7twBSR$ zO9D;rxX|cI_hlJm5e9vJdkDR@aE-b~n|yB9$J#aiBU!EW7;peu%Cx~Ap>2>9(|H{& zNX2o`m|`>{K5tGbH4H^hlC?Gc#-ZFl7+g}xNsIzlk9Pj1 z+H9?z?v@z#fDOq^+Kle-1;*_XFt@gsn(Ocv4?W95l@cL`O(oQvGD~a(w~{72?)3DS zo&l7hJH+H2g8#Dukxxt1f~lkG)he7k5YodSJwUv&zeERl^Y2$GBVQGwit<*x7^L>= zwA*(zCkF>mKv|+1Q;Auck`SR2tP6kL#7^;M=U&;kav9pp#`3ZGQA6HXYvG%vR*W=^ zJ1S*ktpX33j>1{XSmT6K%la&zZXw%zojVPR+l>k>4BJYIYG6|mLh0M(b0qZ%uU!f? z8K(TbzrI+2Yhp;bIFOorYSu29SV4%h{4OFTqU@J0L8?-x*WVa|+DdL?%S?#t zBJVfDs6sbjw93{8#zR=eJ+e?GM{o#oZWMi=sSKhdxFxOyjS1>5V|8_TAEDQy3I-s& zAP8#cRNK(rY{^G}@zB}2Tx87Ry@g_kCu$inZM{qnj*Q4Pm?^SbA1M1NTJ-&o2z9C~*TET`M&4GlWgCM44mO8-(ze6y+745ZfdRkF|>1T^6VVlUTSP?1z zMSzo$?E`-Zfe*qAg|HMM(PlY3jNTqRTh^HCz1`F2+CAxus^<7|o%5(>8H!FHfXH_n zD=-;rJ39ZUr!ZN#vAKC-Sa-SlK!y0KF{I7`AtBUOgrc?Ap&8tSrAx_$AxxnDkR>g7 zt$?59J=2310}+fpCK%~qt3+iRVt6fr96f~7C>I2K7RS|q0N_bZl!WYo#H9n6UhJLF znA5?vSOi_P;^H(*muJF3(SYIXnX*OOH=B&1J@XhH$~Zwx%w(j4`Gi!SMsf zBeHL+*Ncit7{<(?FVQ8Mmt{0?AW6`y`#qy)t0)$^pexImBkC0?!DMCP zckQBNxYG`*T=B@KVHu?kHTqXPSb^$e*@c|u6x%6ys6(oQm3#KID@bJxws}R`MkXeq z*>VJnAM&bcmHd*vXt({x6Xuj<`M^+_FP`Fj_OLxoV+ooZJ*5Pz@c)*kA+EQ9m%#h$3W_tMVCC-{cr^ zxOo*m8c${aD?9w1;ShM1CQkJ(|N1jG?*f+DI$xDu;+5o3(3fDk{uVu)8@?z(^3Ij} zM)E;6uCz!iI<&oUmQFr?(<~s3LOS}}L714Ye4{(6)KJ<^*eq?C652nE7Oay$-3;La zyE-1-LM4d({df5FpBMYCW(Hq_TuE@L=g>r(`^)g;eMdDAT+w_= z!_Ct~rU-TquOI9CNp7$?Zq%cMCzNtK2%K|b(Ed|>9LJwKg-%INL!i%7SuoZb$hQOk zC)g2()~t=4na-B#lzWuG6@!D4;M30Ye7PzIWR@BMV#=XykF@+mv!Dc!jENDqUnf_j zOg2+2Pc|3j%RBhh!VGo$RytlDC3yX_hshwX30Z)LYmq`QD2fl~UrV<6qu{(k*Z_C9 zSUX70SjVFZliB9l0=73l77PV5^Fw!*EuX!TfLu5_>0j{-3N93@YgOi_DZ~Oy;6~yr z)_f>%QN_GFSnUsp*K3|gEEVJy6mgiR&)(I*hh0tAVxt*a~PcMujE%akM0b~GnaFVBDz zL=^Tak~Go!2UNqyCM{a`nZFQW2xmjwGpvR-I0|3PFvN<1^}+qcU#D2My}cb8+8f~2 zzqu{?Bf>PEGjFz7m8Eu~!Ns`FFarFbr`n-e&f340LYs6Fe#pLj%MVWF5dONlvxp%) zof<22#|zn%w|h1x_pNwbOp$y#AhdpsO4rpj$(kNfov%c`7y*6bn^;Udovm(mdn?=G z3Qif7$e}t=cDNJrF(s5JDp4M42@RfHa!L{yrGsJAI^^y5h!4ocR~V zs5{QNRYY888V+toGFb{`Xt^H;sL8-bI%AFzF>{-3d-PjKC#!ws(WRUosl5c!7Z{bU(AmRIB%ru~p$2cn&DzKVp zgj`aVTpReFun%K!60s(`FfR!XeZ67W zxuk+i7{&syp=SPY)+p?Z5K$oFAftp*CmVxuO$qnB08v-?emq?c4FMb= zHyKpGoscVjpxI){Fw>#tHdip^=t)GK?QE3h36Iq^wlgBG!_3B@kBDE`g?fgHWW%K< zD~TpItaWx^=wIGkysH+t7JBE0wWsqC%uX%o^qh@y~azwQ54GWhd9Asa~YG zRR8#~>l^app;$(Ir^Iq8!{Ogf`ShTy7(t^@Ov13x?^l^j3%K*`lD3A(J8 z#$=@&GCiLwcIAcMUETH@DUaOY!u7cjlLo8_O}Fzw2n)c^JpXH?yNI@?wDSHe>>m^G zuy6&B4*Q!YS)8mUvRuAB;AvT)=Azndmlz({Eg|Nzr6VxC?+?BoBYBYVI=o+|kU`>m zO781QCrzIv|AMmQ9LaHnp*}J$ZJQcIIXXJ6ex0?5_xl@4wFcA=A3UYV^7YlZp(6WRPo+>V;ecW|o>e_k5xMj7ZYR<7Yl4 zAnNfI&p>f29?9_xDi}t1lRip`?g*Urt;2NqU=%~5$)M@L=b`KbLx5AXPV!RMnh~kj zoHm$N%A`9YCly_!e0Uzl@zysKsPeXn#NRvFe@U z7kCN&q<3L|+HYvk2^`TdIa;FO!9F6u3(qBL1sjf(DZ)l2FH)X_Aj#LKS>`7E3?-Ka zfysRlgT!qqVgs$JV)d4-$$YtL$EFaV`7ycBiaChlUjc2he(Lx>-pZ0mbiJh7sszdX z6yi+&WI0O+KnX#(8<`S^iPR%-iSe_1xRl^GqclN|B(l^VW@Ip;Wam*z6mW3%IJAkw z1H*g|0v(J4c}24JQ=32jdq;cJvptCXmV)UDuVM7QxR?X0e8;1!gMD~iQIWe3lB%1v zvENsf4dpZX;2~uF!YS0r@;lM{pdTbG;3s8td0Pp}9Ew3dpuXza#F-{qN*M0?TEhO7 zFD?9jed8yc5@>N}lwfxw0i%JSjlI7{$#e$GuGg+Vs_+Bbyb9InpHKRlvEK@5!9=Cs z!aXZJxqd;|7hUCje1z_-rsa*ZnDqTS&T;PEk7#RKsa>Bkq=Y6J>KAjOq%!FirmgK zSuMN}FQ$_n=Z?-i8>8*h_S?6Mne}@bB}MqOH2s9R=|}CScGm5i+LKtl<0`iCCMvu3 z#>V%X(a4H^fu}jA_<(m}pZC1OcThJ*$K=7?YMDeNl+9Ma@k9p6!s!1{_Tp`tA|)v@ zO|MtKxP$cqPoYcvI~N!!xuyF>JH2n4epYVz@D>p|Jh|AJv&I`ntgNa6lJZ>7J9J7~ zqwZ*i#zoL`F+7`)*PPd`am7t;&RV#elQ4A)&Zaq#{B=4}cIa|Zx2u#*kjd9Dk8z+> zW{6WEFGI~dtpVv`ElD)ldx#qX(qvGDTaJ?QL6i&3u9G?4_qRsarIF4E{f%UZawdrZ z9}Fa0n8r-R>tU^lp@kz{v5N^jv?qB5K4SRnxIkIYp&%))RB$sWmf+ILv7pllphPd+ zLeTH!s88tGBR2L)bVxYJiELzi;Z{MzC=w^`Gi5aF1i;N@P-l)1-OX@X$PAM|_duluxhZArgo_@yMc#@+SXR1X zaACScY9Bb5xhyp-U0lsk^)J}B7t~#{F*Vh^Cr=I5SvuSFy+xMjctgHNQt@kQWf${> zOxDXo8Kab!#QW=w)O#;DFOOJ=Nt3op@P0An4y#AZBy^s)NqRJnUYi%$bQZU-C9tur z#cf{=z|XSl`gUbAm#e2|TnrpT9N>$DpLIYfGE7Tu-k2yHM3@}*5OR{F|50r-F342X zoV{ASOC}<3LN#pNvTc9Ytr8d){oN+A1vM=hUqDV9&9Ag(O7dscxXC;*9+jMAV2*2b z&TCZ|zYo?-6s>YZ(Og;jOZc({dp%7gQEQ^n1RgVBFZYNR*(ZK@;<*-ECpL!Bz#w;u z;oBRppu6D-I>T?*uA99#p1Iu(5E4CrpMJ4+;jGrs`}BHPnQ!d>kt7WtT}8CxI9xv}@qRKdYcCIRK2x9#&i#^$6`M=S39uA;ku+(6U0O(pw0eLr_5yE4->yX@$Or zSFf+H%pF^cXYS_Yixd3_J;Ny+oyjO7mOM^Wd}S)OkVkk3$VN$MnXV%7!90YWR5WDT zGzj*M;&v)|lUcToeE9@3${DKhQ0I67BU41S{f}FUt;Hc%P5Djjsm72mY)AtMqQxxSEwrLfoPfhoS3Ji_EntmY#pqW|oqtpMk08KCmq zp*r@+nIs4ulmOl8dvBy$+uLsB_9~i$NP7@v3aPvTmF=`+{Z*y*7~u4V>5CI4 zTbqCT9~}c9K@#V#=LK`euIJ`t9A*!aP40SRTi`fb2epno|JX17$S6D0=!H`jevg`9 z6n0~SS6CX}IJ_=xUtBRKyuk$ef>3$1UbmMSI<;vGXAcJ!w12!wulZ!=QybJZlf9Z(B=3v@ATuGAZ61s?(x&8D+)bLsILtK=UT|>)G`xY>aeUwQy$Q>o;0`PwhBAW zzJ>Cv<#Qfw#OEX|h$Tgl`RjSVpu3Jwb1FZO4@(Id2S14fm6nOd+n@ztk>107FhG+w z-5if4rxb(F+XMK)Y5{2)JV^(zsNck5A@><9&*H|Q0#n0W=i4GflgeS6ld4%vmMFYw zBXNo5rHN7io}_y5nwY-Qszy*R_s*^}8H>|aH%s81;$)#1HilF)RbyuOqrMYK1oyJs zdP^fG*+6t;uP8(#X6fG9k&fv0LM-v#l@)r_rNDElMISRtVPd?eQ*>-1XncgrSdI~= zftzwnfjsJ{iW}MT?9>&^F8cD!8aI{r%uD@~v(QNF@B*5R{>>{;XP(&)8(2PwI8q_j zc}6aMe26wxuqU4_@@{88*IA@rvSGM{_$S`}Yumk{KX<&IXP;(?J9eelPJwOM1XvRB zB;;&OvQnRwHqy|GT zqH&41cb1b^Bu{|My`Z9^BYHxZ%=<1|d@1HB$y5<}%@sDE@tJ84!&Cp;`H#PPdV+29 zAi0)5$$HKKv22f5zhQU#=l9#engYL`{B6VM|2fS(K86YR9VQ@h{-f4qs>F^Gf}*b+ zi0$pt)3HWHeX=p4Y-MAUfs_OCQRe-9z3AWRZeF=S)@R-*=GIGBtoln4cq;amQ3?L3 z!l9qp&5Z6GsKiEN0f;1E%jOwXQd2t+m}8Pl1*j(|{{6jT7&!FwHRC8ef|8n!AzFyc zfc=#C@>GL!+X~N5CVseBrVe6v+Gj?DykDRH3z`^v0)K*J{p`D;mPvce-R zJ%Y8LvozekS!WiQsXzzI{kFQwv%YMp_JR;e+=HDZP5v~Tbp~ys)fQy-|S_86B*n{X?KNu*uIf=;xZf*Ra7 zKRSLWA}siVOQXxzcwa>tNoi}~BGE)3+mA{Uqc5|J(`pMQ3Si6TD$#I7V~_B^mJ^7A z)g(f@Ld{X7N7{hs1f1fEnjdD3?i`Ome7Jf*tNO2TK)q0jYfH~DRY183{GcsfP^MC?%KrUXb?ky6vnbs zFE(xb=j&)pMNO#WYwVD#Y^5~lp_m7?p=?OB|L!T&39+PoLYH1d*G~1atDMK%qcT)A z_zs9lL|io{Q#S}!U6=1DMvXm1cY^I6-}wya-`(8V+1T3ZIU6HX&fC(}wdK`&;N{ij z!CW|MGs zKn72u*Tz7`g~o!m`oobtD<=UkCrRmv{a$jACj6bU6cs97Bi8LMDt3}KD?z;NPAe0W z?^sF_|5Vh?DE;E!|JhLXDCN6=I1|5}n>=DA;>{Gee2OcB@HfmsNL)up$A#l#5G(It z-`{K3ZGQprz3x&Cfs_@wU(Y{>(9>i;yUo9`=DtJA+;m7;`?;2+N;HPQcBH%{2g==A z_wES@fvle_YpdZL_d?*`eFj@#r%9xiRdsL1hrdVgJw(*0MwjYnx$-J`n~Bw_$70pd z|I)F2z8;b2yBpk!$;Y|3;rMHF_+%XR0_?_Q+t~l@5rB#zvzlV+UY>eqhQY2u007sxK$Iz>^5^7QhO1FSx z;;v-@9ZZ+ok>raDXYj-^O>@EaR4*Y0)UXH{X#Swne3j@~FSgiES8EL5wAeHw_;Cwemg z`Sa&bq0%b!qfRPXdZScMsbwxW1_+W2#MyjFY&aa(cMLPCVD@;1cL;@8s5&H=607TCvE8Gl?maCPonK1{v&GX9UCDjo>mNfJh25ir#Xed=F7?OdaT5!#@0OqOXymgR84)qqF8M z+b{>_`65&^KA@c6e^I}PjQ{Cq=xvvctjRxomQ$qovwf1rsB_(YQ4dSv9oX@su|Ke} zx3@QN9y{Crn9^?a-a0mJAD$Q9@wAP$V~B<)pm@2cDNaopx-Rn@qi@4Y2&Pv-=W$!> zKKITBl%RZ_Kc>`y;?^^8dt}+GRbO#iLeT8nsB~MEXfuJuZ9$`!Ls-N}rA)1HaIx`+ zX@};u)0%S=f3yqsBj%V_lkvvzVtIkza$3?J4KsUNV5?Fgoj9%6PvP%`FnDyiK?tE% zBT_KN^wnO2zv}H-T@%%WylJ|%gytMtN-@M!vNe-cUk5NDT}9Y;axLk|7;TEy=MfJM z$n*-+$a2gjqS9#)!pG>n*(YQeX*4*)%3}GUA{oYJ-z7)BrfzK&d5pb|FiJxsRcx_E6+l+@ln@%FW91ZCQYBIo{F@_isQMs7<^Dvd#)V{O838) zB=D@?AA?z(6)O2c3uo>GdYs(+3J!dwCI>|`4)G9hG)lcg2c6jbGb5}hohIsKB=Bgl zM0=4bH1q%+eZ<8?$!J$FwZ%lc39iVSORg^%NRodupTtO+Ps{f0dXME&DPz*NG?+CV z{G~ZAY)b3_@|?mcr9*gL?_F5_1pPb&xN=tz<>CeEKhbruOlo42-CTF6PYI@%HrhE> z)fPX^(NjSy#G|9h6bP3><+S{Z|I7jZ^QHVZkbC32mGZ;s=UTF_ZEp78&;I;8Qu#a@ z3P)rU3d_!CX7Z#$l!PHog)3?~qx(VJ3sH?uIOk+;^OvIBOVx_?@#MBoG+^Kx6HhVr zNX8;1&0WMgEpwO|N~Cg#-IZ4y1L;jCiOuR20~RGu*>YjDvwa9NLt51grvWV>r+c9o z#s%Y!V@=?O%j~X8fTP$EO&4J7IxL$InuS`TGuZ5V@$WT;WW-$Ciq~b0ONCoDE;&(u zmT)5tg5ifPPUX&&3Saf8mkfQMwU(9(xjooB#Ihg6H;KFz$T=RrC7uZ?yAG^2vl@fs zrW*oeHi*MuwOwx%>ZD21mDYDG~A%1cv5FtEt9wm9A%@6r}g8Y z$tX+2O&kHQV)>VzEoFLA^5~>e(u9>HG!&EIL*YP4%P`MyKTGWDxm z&PCr34N^bxxo{ku3ilKO;<>0(Y%=XmQ}Rit&W+g-aCw#+2>12_34H2s4{}nM0SNiq zJRO7*65G~%(*_^8sl>=p7_ZjZY~UoJ%4R2oFZuv=P=I^(cnUPJa1%}z-2Ngr8?w}! z!YDmjP6VRrE;F+S40SB9fb!+`;nFi&06q^y$;ymc62x++WR!~N- zfy(X}$~#;J643}L{2|PmD7ZPSXH@yYbqeig>2_&a>SwEY{>42X3=voj0^T<~7tY>* z!1q&&?a=E#j5@RV!aqPD5kgvC9Go2LNS(WHu>px3G!re{R;ya@Sd+332$KF7AwmwO zOMq%uOl(v6gjCS1K9V+))=q@>4N z>>1G)eR*f3 zs{u}lb7_|ammuJ^|G41J?X_1wp|O|iLgJ$|j~tP_s-wW+W|u(0>I<`TTw76r$KQIH zOLCMNpa87OR}pHZB%~nrKaAh=!q}%{VenjLkcDXnvbpeyR@NU0oh3Xv4#)HZ!Tv6_ zzIf}-+D)OIpES{%ydLYnL1;{pRY|;PcpS~GONRt0e<&RveBJr7d4z)x_s3=M(Sav@ zjR8d+!Be22Q$lmM_7LFpysUWV2t`>_5_^HnKlv0g)?(>!Uyin9uou&vZ~lh! z?@I--3LIc~mv|>oF7qd)zgEoF7buQmN4U|QDaOP?z%XY}*08q>FY&GU6DUrThwc`< z(^poss<9s=)9BDuOYObq3xcH3(8NWd=p~YvG~p)>*+{31a$!DCRR@_(jTE%bRFvt^ z5442{N9mJC0ipb=u~Z|=vc&R^VcvXpF7gilE;KoU;Za514lr+DA8LvPIr*#G-VB8U zB<-H$qJa0zq)L_E+UH@5^W>Ofao&AYNnq7u4P9_LD>b7f3gh{4Q4D6YgB#BQ9tBlfhE z%51|`Zpi6E#Kl1cP*bMi%fCQ5GJzoH2!xE11vcD7n98zXZTBl+Bi;o<$kmX# z`41Np#h&n-$a`nJUZwyg*;aAx3r~Ccbd{xS{k1}CLcw1)OdOzEklo9$2z(YM=aiTNZghy?qAk8uJyU{UYuC?6u{wmH{ zMW~$apg(50PzdVChivE_(~=Am0y3J9+7Jn`@aEL+xr)DXKz>Ih`KFdQSU;o6AQN!Y zRgqxo2O!qoQL^j9p6PQiXf`}8ij-7cg7F@n4?`^3CUbRue;sHn{ilp4Q?W!IdpDKF zC|hPIElEhY4ARRk)hEpjnU=-MGQ^Fi392}k?F|Hf4(|)23g6v~#+>k~6qGji^(l8? z$`2Kq!cBUqtM1%)7SH#u1DNhVjbCJ`S5K{ZdlMOJHzq3dnvP3BH{0fAY$b%b zlXWP`9`ZR)AmP#Zyz>UH3hJ^DAG>Bcbiq7JVSl8uR-&M&Z3ljY%t*!m+8e4kMWU7`)r%48|QF?h|j#zZB|8%@JB_Rn^wJkd7KOGKj+7qwd$rp&Uee`xG;sL z#~4WDF!m0M zU*ho`j`m4~BF5VRB( zE5TT)GI9-Z80Ma?bN~(r-nTNVcQ>qpF*D${eW-Stsk`~XnO&O1Z9DQ` zhUXcCAn-3l^!zD|sSU$CP-Hj&!?qlJx*(=CG?c|@19vQjg!>&P1Kj(7I^?9+y+*~- zyeEKKlc)EHS>-&H4MllEJvnQe_`!F0r;HJIE?30=daFyLLYbL`WpUfDSp!85-EUxn zOfuV+FQ<(ML6!ZJHJcWgbY?eZ#m@45h!;*P30KVCVP(O!|6<7YB3OYW73*BSWqVNz z?h2AAxzAoftr&km1|L8aeR9X1pf{OKq=cE^8iitSe01o?b+pV~%Vj^HN8Txc^_7j3 z_P@S>B+f0|Fb>8>)>QKa;?5HL5{}hFp;UUKlzj>sRa}i}hUQi#4N(?uXcs>~ea!#) zEzHU|`>!8Yt4Rk@jo%#7j1g7z^Qk^RFK=7$Z%<5nMKiC`@)6|ZR+qQN^&#;PPBmL3 z1RAF*s)hZgh`|XK9UJ>nxp23lA!a^xqgh&fzP22-QO~x!N{BAIS-t6(O~^V}lvy!! zJj>G!SUMHLk+Ou7fz+nuHsD} zl0?%Tpor>9!2{0AeS_Oe?D^5j(S-mi>A0snuE-SlZUYfI9Hxu9Ass4*zF9UHSf6m{ z1}1mZqeOAY31e~!xou=noBCc`4)KWeoxc=M4al3UF_|MP4S5pa0H7I$E6Rb~9;a;S z?Tx#M>%>H-#aMP!C&MkL_}!>ZLp(VO2g#KP^-Jf~P?5ezlPjxM9EO5FxatmAs6OMpIa7t~ z=&%)hIwgsF(wNmX?FTfO9LYzG$y#tQR9R6E{~peiyx)|Fg%>unf?h{2q=j6!$SXU# zvj3c&pT@?ly4gGUy7qj1{Ov!<{{iCFg^fcYVTp1dYrnhUs*TxR71JxKC@lqoc3b}b z#41W9xYOA%k86$5kW+(cZ0yib$CmR@TB#_8F!=gVi+b9LUEcJ6pFyxOkhQD#*Dnos z$|247Xl!E#<$%B3Kq+1d=-z4XmrzF5pb!t$J9=t(MHD-;?B+~WD|YzrqjC5hMR6FA z@So>RLJuB>g&Uzsp>ezVQQmn6Xod$P{ znXD$uM`^tazLTp*&oIniu9qhlRf`6kO<8N4r6bq07Xf=X`lg{U#~S^Y-`>AYS#kN+ z`uvjP?3f3KhSiiGd)y z>l%Rlq?Qm-UrRltiTOJm4-*X8mb8yp97gXvj044uU?0^b%e=ih%Ml83FUqUb99W#S zho{47ysvk)g9br!Yf#@(EG|jOQCSYic%Iu5EVkx!})N)tY6=T4{EKaL#u@^<} z4G$d}SOPH51WXHC81m`@4~Pgzw8MZB zie%v)DEawlImL%nz|cx*;whoH<8*71nHCU^U~E}WD3aTGMey|Fl$J%^VPPS}LylhU z{ao&9@{yaa3?gB|VvS&fBEhSM`9A)u#|; z-soh zv*P`J{oIqoS;3~2`FvXE$zZL8-(E7PbNCCjCda^HtLV+5bD8V5n6G#AjW}2CrERU~ z^_v`|X~>7E>$kW#KHnI;`~`|TQf+azNJ$P+5BrU`%`^lGP*sBXKc2?{WWvhJ4W`J* z%Fdb{ME{yb6t9e$cW*+8gSZkMebtW87d- zg_SEsB3fbU3bB$D502}%F%vL0y6$D+?6wQja{CCm5C?o(nqUa^fkPxJW)hrO2g|un z5j8;+XqS`>0+4wZigEVV4HH!3-Nq9!WpVdC?Zi!+AL1IYw4;dM&ReAfHyKy;lP2Oy6gAS z)Hx}Zb!fonM4{QB-w%>|Kh;Yt>!kZF%ZjE#T7255U-y~;6YbZgboX4iYsGM2;Ii^n zo#{)l$aS%My$jxwyG%SefJkymD%@09B`4{kr8sZxdx}Wc%qAQ=7TOFZwk=akR44t= zF1z08H5mkNb`%bDOl?-8xU=&*?H@(GVk!kOFp; z1F%X*f3(|UkTSF5OKKqFb)DsD<)evUD3>aV9lQw3&^?Nj`Z0A5(9^)mGSU?cxwL5ZnX7-QC^Y-Q8VUT!Op1 zdvPdM+@ZlK(xSy%tQ06Mhj+`j&lvdy8EdWkdFH(5HCO41!XtMOc+=_Juzn#&NmXbr zc^T?evdF67vK!Ks)w40i0T0eq8bnBC<+&{}w(z<>iEep{hMw-neS7^E*WKL{6DD~V z>67TloQHNeOrerZ$%mVYcyb@iO0aX~4{*Ft1V`ontL94>BK2HF@0#f?DCYJ?SF}@X zu%~Uz5>3*vZNNuT)yLSb&2Q`K`gZl>$5pA&yXPY)$i<~G5d*`B83`BxhcLdUkck^V z9B7#96*ldcVoN+duM~*3H1B4sSkiC__61rZuh!2BUA#Cn*-alt*5mzFA)ku&;{d8U zZqZoysT-a`*9c0BV^OhcmCIKi`M~G1kzcd)EnQgQ6F$M7i&PG)i+*iAgFjm^6gkC! z7J%|IB1%Tpc*c~5mU(*SLF#ReL=?6;SZZO(w)>8J=TNa6`3!Q67j~r&zwAUf-jJIA z`FY#^r$D4(lNLJe?>`*#TN#QZ$?kRg^|F@glH&5y&ma9~7Ue51hw z)r7v1-bdxlcK*vjt7AirFZ>ncd8FSaFclPkOpaGf4rL=(lw1+cwg!bmv`eQeqikieGlUNq={xM! z((hBi$AWE&Jj;joAm!WX6dNailM#Fs(lE_tG+Z)&h;~P)C}btwxHFv(u|Q`*D#F#Z zd-!_RQSe8{x|8}lm}`$?FQ?5Y8aIJHG|HSPm|S0pB^gtnkt7|iP}YoqNpvzNbCn-d zO1#WMi?XISX}8%2re&yo?}|Gv&yRgX0NhR`Tp?%4YZgD@9g#Ue>KQ#|DifiWn3t`A z|6bAl9{6D&;Vte=`&kCcKzc3gY%sEK%M6%RHsErN&P7L}hL24z2Qz0ZnmkwdYXz6B z(=DQoN^%elP?7;bt-ozL)-zp@yxAB)JI~ixs;ZHks%T>))K&S4L{&=4hec8+`aV8B zncGoam_^I&*S*gkygR;nO)+)k56@|bc7`1|X8j2Rz!!ElIK`QlFI!)Oj?yvuRpD6a z)Z|Bo^}+Ejj4kx6SGr%NCtS?N(aQ;m70Z#Gt*oq^oSf|JVy>}A&t%~$Vt_oL8Dfkm z%PK2qIJ_i>Qs|4^J{||6lm)C8Qr5b05zs$)bydt0?NcP&HLz;r*z*y5q{awOo|I%i zkU@N1eVU&B;0NggvkrpM6eD+*mD+clVV0Rk3LlR^fHrkFvnGtRPo1#{zlU>(lNC*? zhOcnV?OM1}bU0nrh%+6JiPc}`R}$hF5aiK~f(-Sd!K0WeGSX2n?%qqd1_=CCX7zTT zJTr&a3A|>I#FAHL6>}uZXC{)wvtG%;$Xid+OI=yb!n~iAOX0(^#y*6E+pCUmD5A@s zSD-ynS>X2|Ar`ZD6FIgByE5 zUbOErZ$Vh2b51W>jCt&~O>Mvm|A9~Jm+-yfkunYO_id=mExiZ&w-VKk8%pspj6Xt5 zviL+O#_*wmvd!QAkXjkxm+hmaX?y+*jvq%;mTr+4^9JNQdMXch6LiXm?%PVFVfox1 z&BVm;|EiLj($DF`p_efV%q;zRGNSx&YzfuXCenS*<4(20{ zq{6My)L~MMoIpd-1`YTnr07OLeOC8^@TAD!DI>A`t1tpG@0u*FQCcGH6MCt5+$bS{ zko#d^MIJt|QUc_plT_c{g4Q%_F-cuE<>pkBi+S}?v3zVb*8(5e>)*|rm(Y!SHC`v@T7g*1O|H-OzSPXTBjJ=Uni$y}d-&^;q>5C#B4fx;W z(d$Op*rL1(Hk{ncJ5EcIGuxDlrqDc=%xR=t>Da8faU~aBHM6Si&1bL3n-ELvA~KJV zG}-?IqPm-D*DI;=O^4xw%+It&R@5=)=o|ICK)1PtEfD~K*xo%92X za5CxXqZu@`_%mx;RIhao20lW6MeykTX&p0dXV1u=-+s}jf5vl_SgB-V3n)vmLVU_6 zZ^jJr&B-yyF}g8-`5KLE`q4Y4=?%YF3Wx2)4rC2Ykb+KdlDmTGS=`j?{|bJNQIu=P zW~@7t9-r1$WPp8~*u*cPx9$8EQwg81y!MaxQgvSX9}(?oQs6hb7kZ`jjg1){EPps5 z<#OvrvS7oE5ypdEkz|u#a>lC}A(ZdVts@omE!)zgu!nNSd7Wvz%cXq{_IIQh9%;4U z1=L%LEoOLyNR!-KnW=ntGCa})Hh-cf)RbRo77^SwqpC~bhI^#0kiiR+1_xT_@JKXW z67JXO1j$0la>);>iUu#!N?AeW_(rI-)}4;$hPyiH7-bS~CQ$_EoXj#34k}WNF>#b0 z4S^}cIqA1N>exIVW1u9|Pmh z%rGhbeyV%g{dBjju23|(Obs{tV(Ot1{=cTS(t5ke43B8t0j6R*bKC#pjh)se`x4`4-WD9?B`8h=Bh3bE@!;y z2ZGNEl%?k(|6V>OCxdR?m>?I_0vF7PO26;Nme6K&h69Gts?Sf-resIQIYk0RkyZxp z4_LBG829<)&N88-ILN`H$dfplDE#226WMU}jB31kDM`4(N?UxpDDj}+M%7N`3MuRj z;tdS7;&FULkafJM-?zMS7;hO9F&5>dvTlLy)6*XN4InZN0(POllz~gMR2qjxC1;6y z(WJWIBj)$E~L87C?+QBGnEwi z4~-H;w8X!}u+|-8?2MT*-l2M>O`;pB`6NYetRX5SaZ^Qx>@ur~uEm{l^+a4-W{;bd zNO8vAs->FcVqf7No@RIhNkTdzs8C|l$XzplJ}2O*>WVo8XLsnVhF{x&tjKjXus$W1 zqZ?^I1VqCbq8!fwuorndt!PGLYG0#=VEb9JB!*eb2%<4IDHUf>bA7q_i3vHo>xjrI zM~tpyM{KUBWRpPN?^3bmw+jr-ROf1HoPA8i1kOnla3%CtHKVxSi40Y#qCnO@X-YUh^6^eNkD?BQ`WLK0 z|NoAde?Jap`LDxIh;96=Rm|T`Z$e!H$J7mip6qx`2tiji9b%z)ZMf3NcfBe5B%LZS zVxzwNr-Udq)(3|=25Dkdwe6MIu;%=v4^XPRQxqtf)Y*MPK-~rliqU)~<3_KR2xcI+ zpCVd|C?qxsomch|9^Kz5qdHePHcP)i8a<+GXKxs?dU!O0CC&S`x;sw9I-QdvQ!iA= zv>k_$*xc1cP#PlR-$L!T@&ojWw<@(42OABE`WLQV-prW~*I7v)Ej17o!!)A$T{2sH zv|kE`(GAFs#$9&nTG0wNjJ+8O&<|3=QjqjojpZt#cEfs7e;cw2up~51$X383mod_x zptbii8bX%lfGf~GLk^O>L+N7)c@&{6$>2gf+ zo5HjYFMsAmR?Fd-T=8!rwzSD;UZUkyqK@U>NhV-a;dHXKAY!9kBg-c4@d5;$6HX%> z+=kJjXE_cWVOggLp@|sG`E$q+8bHkMVGBAosX{{?F`zg8vnokVhB+kJ8>KL?9-oWS z1z|5%YNClwhBEMc7GV&rRZ$R+_*d_2dk6YlxB^$4{iA6;#e{FU$|Tp@F{ zecOA>3=Xwq2si!kTxF_!V(lBYzrXI>q2)|=3;h}MSw0R5ceLm_sI7FCxhpL>MCvmp z0y|13Q692B52@Dhqo*C2bl=g7tj_$N|L}lkmgja=iCh=%qcx3X#!}dvO0jIkhGgb` zL6UrvSwRQyiB!+V&X;ns9uu{)OytIsl(ydN+tq?nmUQGT9+vCH4oQc1Kl>zKxk&2| zMiQ@=8_Td_L`TVd5Oz5!cRh8g6sg7Ar;nNfSkP%QUgPyIprXgDcD9mRo|^A-&NMB+ zem7nbw!B*T(6^-`J!x{G8B4d$fsDKo{3X=}Is1kG*&rGa-T#lhVddp_@q8+gjREw} z1RO*K%tMob34`<}(fL zQ&g-bj$gkuYn?GozMR@p%EwF(E3TDsGj zg5yshZP5>EVla~^zJ+JHnHya-a-`rUIzd0U6|-GdTd{qbQGltZHjrEASiv&`SxIx5 z3Nyph3YWycaXfibW=K) zmoxHD*NnSN$eK8*nI2`OoRs-^(dgLfF>%RY9yu(tl9F~kwR9*MMA0bku=v|EBaRg~ z^3q2%M`gsK5t+zI7$vEmOG&YbB>bLOg%gPCcSsWAwVLJK#O2tlQC1VHjIKBMD;qoF zDH`LR2Pc;GOHlS*)KU3fFF|BRO4!5_zRYS{ZXmOf-d*5d-^iN$X|yR~echHwQCU`! zyfSL)5^dQjI-fad9YUiL`p$07us8zWB*`LD{V{`eBuV+{lyajsXC`M{u=#%2OHd8= z&&rz*fQ25t&;;Cwy?KkJAOOSbgQsAEA{9c@)HEnMyRP8{|3cx!Ak^h*1ZgCBjj^E4 zmoM+58)BYHKZbzATP^v2pHy_T-h7QVW(9YRzi~$WCdBVOy-XgB6^d9r3SEu;vZSg@ zA42g~;q?4<^Z_8Tg!1|8yd^QcnXx+bg32~!VY2r}n3zXEV{WgQ%HQ%W(gL9%Lk>Vy6|KJg}Nls_E1bS9$H(7V2@QnSXOXcXS5J1D_0BYAQFOVo2G zZ-4|O=lHxx2OqSLW9R1hguV4L%*I&GGzqI*q4 zkM>!Js3`73#+K52%!p+c7jQ9>+8uy55KjE`O?_=P!%y3BGhh@{k3PGj`1-UGQNPrNJ>^in1 z|8cd-S()h#>L@@z)zDeMuPB|cUN_zu$EB(446qc~y5wAz&(K{*sYJ*SNFz{0LRCuo zEaXgZ9LcHBYU|s_^gE)AQ7lrpkxoM~&NhyKQ9)ad#;CowWw3aWn+3~TXJy<2b`q-E za_LoI+w4DgN1b-5b33v8?$qRCdfq)@FddyX5m6Al84f{s6DR2t$lsnM0l*n;oAXOZ zc9=`b6nX08S)QAwqpou zP&qes`tNE0OJm`pv0%0EQst%sKx|RYgqK-rd{dgfUzuD{?1bPvZnGjbxm#EH{upFN;w(#5O=@7$1ty5JL zz&X-ceY|it-xIEBR8xs`rS&PA*7 z7o*>A)DR&~e2SJ&@5a z^=1=|U+R`Rg|i6zIgq+&>a4fA0{zPDtDj@6ND2dg8m63?zxS94a0Ueq=kgnZz?x=BcDqvNBEvr)16^g=l>E$S5NAG*G zVvTm2($``2sb+j823XMsyu+TAKNz!}Zpx@C4-#S@G(@@A+&fi|tYXZVpz%dU0?Coj zrb*BNZ0X1&eipkM%F|TxPdn%Jk+N{&W-H5m*q1k&o-~dld9Vc zT*3llbUX^ch9^S1Z?bq4e_c+(wc)s*8`!f_2xQN$80ao2iNyW#09c9 z0AmI~hDttnE_D{fH;JpG%EaXzk$~Uhlo~t^_$GL7ma^tUeQE#Vm%Pc=wrYc~n z_V(B5{^j*zVz;gka~UA;;q$HR&g|BD$Qhf}v%=mDJgdIUGSTR<`vLsbpRYzl1!IwW z2rt$n2_r{K^cXugJbG+jD+Tr$gv;>9ISz*U+^sIWHD`UFC%F~Qz)~u_g_HhhshOUg z^X2rwF%GF*9sy2{za;Qia;&9pzeAh8Hk6*p(3}>;?rmO^lzco<*5kLoUp?MQ`i0s9 z*tx`(D8D~VU1KHd&um35GwMsh@s>JzJnNdF31H6E>N5UZ&7-BAwIS?`AQDBxY z5Tp1UsK*^ipsS^ZyE84TT&L*+msp;*%B*|ZczkSkZK3Pgxp5I8T<+g*PPJft?qv=u zV8^9 z)tfMtygOfIM7*PeE*rC)(UvgrS6R2g`y_qD%e&+nF4vh6B8uu|mq$RTiodD(BJ=B+ z3%vq>n!}=fs@6Zu(`aVr!<{l%dMakpmG@F- zTh&Bh)dni4?!Y*T9>(>c*zD*;6En*{Vi*NX-g#o*p{vBj1tD7;Tac{o{2^F9E<@au zgRb3+>49(Mc=2UUwuF0DW8$l#qJDh+_itu_CTOdgr|^HoN{9dWaT_Z6PM`bp7%wQe z>5AB59QK4E7KVm~kkBYqz%PuCeT(_QQoq_giwc+;B83$h{CgPp7&LUnp+^g=s4ld9 zO&|H0roFl=yhr>gGeN*H zS%^$lz3W^o`qArrm8o3YgO!7$+FTNG;R30$PUIB=Ryi`|KN!)Oo0VNpEO_zy9R!Je z+N>#?sPv?TdhKBX7slOKIrt|ly*&iTk`0PYz$c;Wr-X-pM=X0Jb}oM1+GqN&ASv>8 z==Sb(RxWtsGPv3s(YjDm0Q{JP)HL4w(?1jeJHgw{E)4nt>aM)$|76e*ecADpH2*3D zN&Tw45u!X!Sy|69F)^|2S;1XP=Sv@)wOOV`fL;zpB?=l+ghqBK>H=udXCd^h7uC&p z#OHW@2OeMm!YnUmw)c!Cbt_7vd$@7r_DJ1u775X1yMIh--|{?e89$7zZ`XYXBLTw@ zx={uS{|GB(R)k0XqK!9nE?UhG?vY5!f2)5JWxeM>!{)?Ahr5+1PAzS;j!&mUaH}{O z8;(U*uN^64qs0>$LN<*IWslT|@q`mnbfB5QvCNgNQo>!!h)p6kBnv?6-4 zX1P9UgX_lpkM9>y_*%403DywwNIx}|TLHq1%*^^<|DmS663g(WCp8BeR*}@uD(WZe z6R^G6YP;gK0H%aDA&*a)IA>Dzf9@6axF7>RXbI^HQrcNCC#?KL z)wIY7t$n1pxA<4Pv$(tT_Q;eSQp^PYpW#)M8B9$ENLra3+l+F>k@tW4qrQLtetyo} z)z}M?36xu7?Vwz7Zl;&WlX1KPHOGhrIA=x~HFmEz&l!p&qm6Ux)q#pQ=)qAX#)d>$Ez zFju%DDZ{KJ7s{6Ua9zV^Bny>{n@)_(KiHj?(v}TEO28x4Nz{!TY`(*lDE~7$q<4h; zUCh-L4z<@CY*ZQ{#i%3|M`ef;OumH_?2Go~4jYwQ( zKfS9s(`@4&=ZJoJBa9fsS)Gm|%xSU!8O*hG2wy0M>9|-E$q4+|v|lv;zA~~i&t#1v zmyF53cjb`-HIk&khsmawyNGt{>#Ibh#Fc&7x8%G~T%1UPJi@(Ijp*cYrSBdp#p1m> zpG*49uluOSyg8H1I59wCIP=r8VWw+n@`zS@gn2}Lxh|UVahZOi4`-)!h2{=!-}6LQ zyADva-P$FtM(Sg1TJoL4=j8-t4D8)$K#PNP6DZ{|N7eZQokWe zAW8;}q|5;`$_m|r$^=IUT5480w}z@b5TCz1Au$r@xUwCYwj-a3t-~?JG4+3|fjVzj zXf9TEYwW4ltuY=1luXPJ1#yG{#P(FYR_djnM<7b9iAMU51bB;HXcrT5qI$c3YQW&q zV3)t_Q*yW@-ZHy|9ZW2fNuVzNf5wX{(ZQPXP((MgiUZQ^Bd-#k!_DZwHVyo|&Nq@6 z)b3%R?qIH2P@&kPKKXf$*ksPi5CQ>BS>>8@;+Q}DV$gFgtBIoP^_n3{h3Yw&4V2yK z$H^F~PLECbC2u_hKi}Jo#?_>ipb+m!tIvyT(R8UhC01^ZBweNzW0KYOOuL|EGy3K7 z2%TJjOs?Pu)*qp(jhWk{AvIx)#1r+>p&07qLU&^$V`q-;T>TvV4gG>E!jBs1b%uA? zmM2X}peaQ9&)VH#4-XFtm2JFo=SG{^SbLe>lKS+5`-4f+gsK!=UTV!QNLZXYP5+=d z>r8~LLWmTwDrL2E5~OUOwMquPLz|hE=6c;4fS9L8bIRZ!LEI^Rr1dcwVFC4+WiZVphuZR_qCDwS3e>zeA zdl6!>ltX6nxZptuQ|ag`+whqs6+;0HF(WiwBLjY#;SoPy@uFg8n9oZy?Pgd_8?K}gf9@<36XQ3 zo>$o3GCb~mXOk<1g>y{|Te^RVSiZ!&SsyN4JP`n6vhDY)X(5>FeM*QouSVhMFhpz$ zOKDDOPFx9o%tu^km_+246VB-uH#51bI==|6g3#*kkm0fR9~#g`A!>>nMW7YUzt3ol zI@0dZUEHcX!Cr-Eo6NKv6XU&nzXVcB>A4_~*8{9YiVas-Lhq+f8pNpjh5z?>8@)Z= ze!3}N|HxmX^2dx?GW<-q+~)K9CJBcvY*6=G%_S6DqKj@6d#zB_^K*^XERHz*2%c3b z;Wjia0oc>WBNx$n)7so*t!?cqlbqucc1@eo+ZfX}FWM|EaQ^S=S%~kGMUqSOw+gJM zkB*;)ATg{Y)T9DCo+X^g{`~}J6u~`g#K2$*9;o%^ne!tKfBw6dv`DpXc*F3dq+SsT z!x9`+cTv~g_G8mR|CV+VhPhrlR|YkadO~67E$57D>@}%V2Es2ahqA3IKpe8AsNF`t2xOkmwB4uLmAJ>qoGIFX8aAm&IClaq~la3SkO|I z79Ge0D%7M!Z++q?cUl@)l08_6OO>js&k>r(9RA{)$2J)g zriP=k(&B>t?h&A?{$kBDb#k<7T*9(t(^-MCsmiR%D^}~{DR6#0Sd$FtxE}=t;X$J3 z$}sUfBf#SUu9krS^Q=-~KW9Hbf^sEc(p1L9{Ja=mE~b=6p97j3A@^zzzRMiGYVo;o%W2k&nU{IB@75L`K)_hQN(&`dMrHA#8?gL z8#W&G5)QJlIh-W(5PkQIKTS{1x+{tdarC6;KuK)MtlGe{g)no{Bg|D0;>;=5ePp+2 zaW(?j>^t`B5|+SOqK)Ukp&nOj9LOf|@JK$u{Sl^%w~}{YdbJ>ii7T^n6)XtzWt{Tc zc~wJ>e<%vFCw65gRF#oAfKnyIEN6qyivZ!Yxbd|G0}&OY3cbNf0XyH`R?fG#U1aC~ zf1h&G|5igid{TLLoA*e?aT>q16JsZTZ_V<;elG$+wC$Ja@r*v%B}U5o?ebYhmf%3A7-&;rS0V2d6z^ZZvF$%~$zI@w$pEl>4>4vKc6DEB+Na3h#?ol+&f zSZQyUxslCtRyw{}rV44NM^jx73?Za4OTXWW>%$Y)c%ZbF(}g}HX4n>*RW8MOL{-i1 zDV(HBNmDrw!1D!#0zsUy*J$3yu=j>>w%Ku(8LesLQ!L6qx5_qn^LeMDb8|s%iHfw7 zG(MR;+1))O2)UcU*u%8o0w1WrncboM;gY3dM<_&hmRUntv~xZSza`n%NA2BCJf!h; z79A~Sf7g+(czo<3V|O>0;;2qb@tFj2M;op#zUX~t4! z<;-b!Prk?mkuR!NuNS)Kc-Bzf%eRY=@5TqbyY4%?Q}6fjDv@_=>=46E9*jX=jO**| zl}s6b6Fs(qK*Mcs9CoqVBgq@p{3XU7^DOvocK!otPc*@BE&{Wl+dIgoG18DtgO0?a zJ=?}bO_f=ZkV%?4(m{6*7*%It#*msyZuaF@TVEP4QNx6)Vu(v)W6xLLmG5i<*$b1s zOVpp-*f;eGT9Nl|tp!h)D73$3)~+71`@LMpV0B&@9|=vu8_+FGR)#t_D41-@nvu)L zf7|JwoxyaD?K$MLk3iNy=F*~Ff92_NaV<$4{NOUMTK{lV|&&h&Ca8$D_2^ANU;|ndB*;NTFDzB90 zCcbny{5n`f)W(`Y_uYIEL)b0xdsx+G5)i%*Z7FYrk=qRLul)Nbj?e3RhSCvb&IrmP z2eFBdw%-!L=cCz55{0u7waNL;5FpLc`fhzEe(%V_cw1q;8v8~Wqjo%eb6FCL<$O6T z#U&aLH85;MnuG|NfW7FCn8Kz9(%AU2rEt@ZNa16Fs$0SAv|{lN1=oMZ;^n=ed#a0i z$>S|Kv{G-VfHE$Qczb%%;|bxHrU2X>CLZ=$mKA_>sW0CC$#$tHW~_))$sNo(-Ey&I zA}^j~DN>v`?^@lUMSnnYV%MP(Zp{%*joO9sygV4MXcV6~C53Cy++%Ss?|+n@+N%HD zK}{}MT_jKt=pSg5@N-fJaa>V0&92RB@Nz3Q7{;2w{87elk;T)Loi}#ME!36^P=TEl8|RWBWs{=RI0Fr1QzH9l%!X zbB3RRKgBjz*iT+LR6hw2<0!a&5zU0AapM;J?(VFvS>mDJeqY33x1J z3%v#4D@tHJ2n( zGQ+7e22`$l;#$GGT?_zb1FJCtl}WPne?dLPGrq%cm-fws1O;Kt=h>N!kgXy@`23g9 zwF7%ADm$%7ru>Sp9|Y;`j7H3b;Z*O%m%+&*WjbY|=oA*ugb&>)1DkN$XYb4?F4w{E zbY?c9T+y&(GWXv7m#4epYHV0((-+0`2Vw1^^nJIAn4UhJz*4Wm=jV|36vmqi<0!BS9M`j{;Bh z4+pV6%Lsh%yqh5L(miwDUF4o!Bdqc4DjUx1i~>WhU=Z za-}8Nd!Ks!WJ2=PUEe+yWmm#`+DlwLy%omZD$$YNB-z7zgW#J_3l)XlhP}b2FQ!V) zAifaR0+#Ddc;`37I~yAZn_rIprmfKrR<;Z_XwLqm7c!4oolCERYkq1rJ>poCEg5t^ zyvCNhewtVTOv>jwR$!#-Z_~1z6fFa3>iI14O&5uz88?8cF#yG(_YIf%6c*3tzjc+o z)&*i|ypnghM7KJFg=+JwIGhb{#pBE1$YX9M(JNQfu zMoV|NA&V3ULgKLPB(keIv5vAZi!IXgDQ>24w#5P8%v?h*7?ZDpK#Y{a%GFeViHDi^ zQGO;V5^vdNrJ_G9>N8pTOh>l6fw?J=*3ky3IlQYa^MJ?YoU&jieZCGT0H>Ok0hb9Y z6&EcOfMH=5PLMU8UG0V%FNkOecPi_nx_1)4$5@GmT#Tm8jNDN>c3vc;he&?bB529rAM5RlIjRO4&&?uGAgNP6-e$=5bhr&0t_)C|Sa;S3VkJ zEqs%N$`AfC@?umCC-6!z9Q(+bjl~n!KahnJ| zznvlBAoazX^e;i{&1+W4o@}TW zQ>m?YYqZG9Jd`61$Z1OEI$-)u%iwg>j1>Ny+*}C!>~d_Dx&tm@-BGmKI6>(o8=^Z} zWR6ujJ(yL|*@Te1fX#`@6xr=`$frV#L3}UuEb1XXT14JYE{Nx+6kto3C>kAKNlWqS zOCConU(b9ZeXZ>oE{S_WmT(4&alez1UDR`KKetW8`vRq+r`OM4!{AK)&3=_Xem&WF z|8t|3$VtA^*{I!hE+<5;*NTkDbyyN%TqzKVAqWD2qyf>wl<_Dd!YJv0sEGWVHJg)z zGiL1tYkUPH3yulA@4h$olyrP!4*Afr$o4I?GM1-r?oi`*Nywu3pG*IT=jN{Wji?Re zl8m@?hwb@uznN(0_@!egG`st88- z0=Zr`nu$!(<)O)Xuoin1`(jDn+?4d)CEMj_ljy(PPCMs=f`{-#E zrzsajDGiG`X_3XFu~S%~ig_lFa&5#^mP>q_uIvhRfGVO)nhl1ZIUvj5k`W7$EUkEX z5-A!!bd#@T#YN$i(qNpE15-gd5dV<4DT3hbZi}^G?_+NtTbLe#Y7o0=EP^LUN#|n1 ziR?IC-}|IDiatCth6$H4*V21A|8mCTpw6RTq8*B^h>zf}TNtQ`*P!vmHTbf3gf=sd z2DJgGcvSCGq*F600#G4;5WboohC%G^Fiy2sq(Kl=Doe@Wr&!RIaVa{5+ix5~0s zPfRD2Suma%2cx}h|E}x>Pg6eozgj>Mbwz|Coux2~O5*kBBs*3ck~zc4+h{vXPt)mx zb^EEx&Kj@UHKYqoa7AcQmRdN!)u|X(p|mE&lI*uL-|gZo?u?4Bc)HI+Y+bHioooi` zK1F%Mr=iGSVkm{sENCIgiLlV*CzRj?`^1MPu0!U5Pu1b1Q{*(t;~8g3%qw}r^rW6d zWY#M2zO+;78SNI)$)W1QtRs|vTKE3ptjR^0nASOS$v5M5_gJ=7X5=$fN?vQ$FXe?( zyzcCo&YxldF%W@@#WykQF~xn5QH?C@4xVV=)Zzh}Tp~ zCJ-=aGYU3v*pq)QMw<5ivb@d6?k~L6 zyup#X?-g7oPX$j&QXnIYM=#)WR(%$r*3oLsP5vFj)$0h(=mu;ZDaBYl|4X!=z(}1suO$pNgy=V}65b@xF`sEq zbvzfL=HbdWm@*9V4U4tN$Z2$0S%nNA-2*e%el4OBAj+!Hp}8$5?$uS$2-b(zME?~v|VRhKe~BAJ;JDUckOy-2t5{wNwVke z4BX^QO4OOj#36H)M`!53yeIjbd5%6^5Y5FThpEV0?EmNTEEDzNC(nPq-`qKEqE;k` zB2LW>?ew%Cv4e+G2Uuu_H$L2i;_#E_juf<85@A5=jtqE}Q7ko18 zeq(Glmn2t#6_;Y?W@&$e}du9zMLWi zTCN(=*WKU7JB=_OO2xYXVw};y6S7+P?Qf*}SB;B*0%7ys3L|M({ywO4JC-dJ2I-B!EgONfouX$5 z*G$gEl8aGZt+QQDjFc^;aAv*ntCovWH);@xt1{#8l0o|bW1 zl?8qjAabWj5jgo}G7Z!7KT!h$n`gs91pP}YeS_}O*ZIHhY3o6h=nP}acpLPUWLixW zEWJ#cA8L@Qn~GEla1j0sVDw>>9L2g$yI$7{C*t%gcnWIDwdb@_m|vcFV)uQC>*Z8C zpZRTffC-Y%GR5`;?6k&}a1W{z`Q1j+{MhIEY%%ew0&*ChAvfT*yq>}kQOzHjc2aI? z0Z3yOc`#!6;^1i>7C*>fA?N>kR9Ad&t4Ev;Xrao0k2|sUT(q8_ILTdOgzDW$$>Sy# zNSY$}bs;OrP&iS#P z&%B*D14i-hM>z|I0RfV~QBB{76anT3u`yOM#HjrIYTJ~OD0z~k`kCW+gwE0*o~KTf ztKM4*S}*+Y`aI1TNr0Fx8>wB+{RIaLL5XGWy}=g7#6sP$EzC60SJ0k(P&%L3G!typyuGvX*8%n+RQAB}CjZ zxe&jUrW<~-s%B4+zQp&wGifO1H@6xPi9&h88`rW7ua6120sqtG32MSXIofc%^g-Tb z%tb2fr#v~Lla=grDm)H)o|E5JDy5L^VuGbWv%<=LDm;w&v&P|Zo_lO=#1ICiUio<-vDQ4B8Enf4% zx*syb-OzC{zXOX)O1`#Qjm^(vR`Td~B-_WQUPN)aQic=1SaP?H8E==CdSBn|Nr{ji zdZPa&>75Pa8Neb&56=3Xp}$Q(jyng7+TU$t4!^9O?!}z|6b#ZbY#%lec#8`J>l#fA zF*!pVt0~&~6Xcy^^j6#Qi-AZF(R;6;@>;$4H2u%AG4{u@kvg*4*+aS(B`ETkllVv+ zo=Dj3-9H&?7-xuC#D2+eEX0FVzrKEBv{uRd{V`1vr&6@)K=N5~h{-8S;~3;jsj?80 zI)-#ArCq%l+Kr3F#fh2qou?2@e>7&y@k5#=uhdFrxjjj)A~=%!0B*SYDX>RzVpP#H zS+tjy85o3AIpDhKG}~~=Oy65N{v=Ajvqb(N$Em}t#&-Da`_eum5V3w@Rv;1=l`30A zXG>w7j%2cKe3GdegW1?yUpgLTQbCp^lnW`1+bc5T%LOp|L%jiuP9~@-E<^%B1kUA| zY3u*v=^Gd-VZ&|P)nwbYCfl~ny|Z1DZCjIVO_=N^n-eG7=IuN8p8FqOJPXfSJ>6fy z*IvAk#6&M}?N+BtH8WI`2V?#H{a*#IUqI5m>-tD9kbr4rhFCh9pC;1`GQGPf9|hgg z-$YVRr>eB&p4u3-LQdna0pQi~%pKK*yttQ(ahNze94I+&r!trUnKub4)WV< zmBmRo03;8|h8d)Uz7Vn=%5Rnw$67S2WG9ZpR1!R77z>M6n!;PrV1UgmD;L8yL<6T@ zA>J+?obnH%?Hiw)Iw{{@xZ~()EWOtVXkCO(&kS^74=x6I&Os~F zBY+yzXmJ)6`=GP>0Wz(Dm zKNwhO(a6T5y@#_ajk$=6iQicjYe}2P=~B9hN3S}t}_zi;~QV)3YikNsd3&C%vnVkAUPx11BK0p z{^fTDNd*(SYY4@Cj48Jk|Eo2nC_W=PcW1Vy>%le}h~WH`Zie%f6kMXsB2iiV#X~UI zcq`%m%`)ZXx|cjbFD5z6@pt!~U|s+zQ3W|RtQ?54Q|d=G0!p4uF$g{v&UTy&G#C|E zgP}|PCl>EKYq6gWL5@m#Pk`K``WX^kx9Ea=m$?s_49KoCtj3e4T8L4jI`WNU7wTU> z#|BiJ`jc=ZJF~LmyGU!KI-(e9_S37pT&7FH9A`vTJI#te~~}TaLzR z5qmG_mbpYjzJy=A;af+5Q|~7l@jvh_17mJoIKm)c5^k^Dy!{_;OM&pw;wp`wkI=Xr zdIJl)!3Z*fli)RdI^I!htqwo-O1~;_TJr=tP?)92=t^-AuJ{rHyQILdrs~qomcUwGXA|mlcwZf9NHu4N25q5>|pn&v~&=Srx*C{*iud=PI zd|Op!cQ8r%0<8RHxKYNgzQ(Ch);B|_=lVoW)(L}&`7P$F@w(2`RLm!^IzMfiYO+IC zxQ!Xh99;9-dcU@5vo@-5ww|Uju&}h@3|^0L_m1dj6QsERdBAbUlSxbhOc*vvreE>h zzqbfl5(Jm58W5!1qMbi-uPD;cbUFgqF4 ze{gHoNHjzdmz13}c0Fy~=^)a=Gv98L=do5z$tp(CFP!)9H;NYB{&7Xc{ppgEGp=&I zw}FEym8$%L0`P}Hl$@HH8a|hO_RpWe?}a2Bg(R!Du5~!~$!v#EB1<=J3rcO%O~2wt zOryGXJ5AjFo;T~Ak9a^k8GQ#&&py&Nbs8f4wYBO^jc0E|$52cPyO&R{7IJ8toa9~wmCT29 zXe6OAt1Ym&R*CV@-S|WtiPULK(i&sRSbi#r0SFz>n|DPSq4s6NksLe%^v$q-K=v^} zk|a={i)o`kAx=Gr@@GR)&30S6{6q<~Oy;4=oTM%gMw81XyP~IVPK9?P-ANnLu|k)% z9bE)=vB()I&<7O9gVJ%YH&6tTp!z#?^!iMx5VUi1gJP~gEJmjXZaiw7QmRjVv@2=( zCKa;z_|Sm&RQ?}ciN_MP@Cl$SSK&4(GDuUy`gP^u`zwz+ohmK7d)Wq*5|Kp<nq8uKSYTdi_}u{tmU0u|GEE$}l?1rUtAD_SDThWgA6L;&hx%1JyV zuu}ZgJi;v)$hQXI%z~_RA*T~(KTqUyosM&bzUWzs? z=Jtxs4}6Y9_qmYAK%oBb?AoMzbv1N$vY#vu5AMzF1pN=ZmrrHsWx*0Vv0kt~yLJBl zzg$48TnXGxYInn9|>`xblouVKErMXFFCGUPa&!RBpGJJA5a01cb!j96_UZW(SXj5w>{87c| z(I){OtU!9ec$;yzb2^3l(&2z1u&O ziA@@#Sr0EfI$>R{MTFa+xlfD#wixo93R1N`FZ3JbVfP%Q5LMK~C+o!7Wj66F9W&E1 zd_QP?-*$Na`#p~L-y&DHmHkFo&+A$bWUr5x`9s;~e$VUs*7Kwqbv$xzo7t&^(1+5; zGx7VT1EKfX$abC3OR|vfVGY+n?!#b1(dQNM=Y;?BqCfJttpC0U?6ep_Ttof}u+pu` z>?^$a%eVn>fx~yv&%F=edKZPHd5K}sDX*~&mY+cgmOa+rEXf-O5t3^*VYEXscA`f? zIOHLQ>C7^%^drb3bvP$nuZPV<=^usg$9-h^_O4)ICteDRUhFzn>((eLa+;f$(x)KT?))f4R4Hje>y6g-koibWB$@~2=8^y#v=jsM_mTE7wmRjS(ho4$Mi zvs?O!w?zryV&rEWM0i?L4wO4Ayi=jXfIG|+ysqF)@lp)&R*uY`p3$*hhrR^zPM6#u9dl`i9P|SQI326qw#!t|j5ZK{a z)$xpo%jP=97_~!9N`l_;jPEUPs(@I9#KVEpWU%}s`l7Px-1Y=`Y{A-MbXi`qhFbNqqV{$A$!v4u z#a_5HM!92E?S}^Kz}K<<;O<7w(G`Jr`rDzh31?<7i;Uhy@(Nhj1W>y1Wn>N9!8wWQ zr2gAH7ajeB^aMy}wB_W&$YBYV8=9x%7Ee!4_jE*)si|5%UDkJ*l)yGS!1OSFNTVsb2jv8)8ED{+=3ME&ceY_rV0)NrZT<_5MKQ>U_d9dY-eG%H?@q zDU#x}-y{Qrj;YV#`;LgcYG3Dl?zVhcwfM}ZKYm#e`?J-s(`5Nj@Sf}QsqFJ!O7N*? zLi-B@!>bR2{b7?ngR#&($d3R)=ctaf%6)AvxEx)PuAFnocsE7;8t|H)QxB)KaT?P% zznMa9RJtKOYN`PR*K@-Ec|ZG^NiUUR^1@7I%0}7leFsHS68FQ^a+oStqP`j{;`^Yt zD>Rl5^)OQxODdl#N*HzJphZKiYOTBiClxi`qAaXEjJcK^me9-vI+;^2B1-_C%juGW zqLStbhdW(EAC&z__eLu$r@5(Rqygqey(coX$yqu2*|P~j)q0jB0^NS%4+>`|{6>d)0c;^`9PesSFvb+Hmw%#!SVP}0k7@l)A zjh6UspMXFt*#im8RUk1>#FV&B5vfWvUbZjiXE$RvVhRD%w72Y^9F_{7`<&c_-7a{* zH&2eAle05^{p>6mibLPFLfDnf>#u&J35ZK2^o26N4QhxGg*YHuLWww^t>qs5Y!tl^ z^?{?#!pt!``97+dl;9`P-7dUeZXI>YE8$Q^B!H?Yi9}-(U$3k?27*Z(t z7KW8)Gd_QAMlfd_v^qXzmfsCXPt zJ~AafW`G@wwlp%9rj!?{)@DZQdX1}D2&*LOUP@p0?3_p7NPj##L>sVRn@j&hed>=yVlA(vCj z;ip)LFDu&4>|I0niTvolg=t5O+1P%vL3sJU_7DiXTScZ6j(7|Q{M=TOI7O@RZI8ig znS|i-dQrM#bC?74wQX~v;w_6OeXf$O5CKnV+`=rqDop8M5IaxbQMh|IWAF5>OxcXh zcjtGv)ko)Y97m)m9z)a7tBDA~kJR#nhQQ-SQe)=8IHgj?M5frutmoM5d+C z%>ysUb8~4~a&KmqXTl1wVZ2g`yJ-seK;j+@(~fPge69UGeY?n)o>%^FOLZ5WdpAaJ z)1S!1o_{(%0dH&XyQPjM|x{*p*#EUS}_}FjS9V`bG zLD^VkA-zzyh7+EZi;IUtqgIn_6pK`okM;P} z5oDVPjEje(lV))!|7al&zGnd`hk0hfy7I>*mUIShHdx zhv|gX+V1qeK*l;5Vkc349V@V}Qny(*);W+Z@VMwyhJ|9p$Va9;iHDe_dk|`SxZD$u zeV8baL&8vuqBg|9Bx%7$K{X3X!Ie7_(GRzRq76p2g6MgcSyk!!;7HEI6avju^lv&l zO3=tvuMvw9>*_5kph$g;onnLwBLBc}&#U1$!?i@E3VS!#Eyo!ZwSOe6*(&ztQJgia zG&^n(ne)O@)(o#tSGOXCnhNhHmjkckc4C+Jj%02>8!$F;NWVuN8zhtQsz1X?f zUtJNnu<8@Hsh!^_OagX;QUy~) z1V`cq%_ODQF_O?3$|a$ycc77p1fBO({)SV6>e37(_}mK4zbX!2ImN_Ifx&W>ma?{? zOU&U^f-@%ym7+m8g^o(Mp170&q4zXopVEVYnT1J=^X;>Wf6=D-8^h8~v0TD=qVWOjwBm*2n0F89OY`h#z%I6}1<@IkRmMGgeET?P13 zw|f8rF^8s?%p$m^^>4I^qpLK>54<&{C0#8lueQ6*@MAIHQ0>-;5B1`hIEjIr#{SR? zXN41bLlo!z_oy}kSoU`#7q5psNy}t23gMm z=%U`=1=OMVX~x0QT&zOi0PGmZo{-q?m=ck9k#e#O9X2?O463I@I%bmakf>#A1lBmy`cd0*-1z23lx^Ft}z?Z>xQog;=ljiH=-`AIHzO zGtTc-fxlhvn;r3j%pk#%>QR?sV{eMC1&s$+N5`WJ;v#6Vz!bLk1X1bJ(9?fREgMPD zF8bR=c*|lK6|on&CeKDwm_;e@z)J6p*aS;jS(?g!2QQ(VR9&yqDpEzAIuqxNBuBsT zVE%QGu$n~(wBAJNyn=-KP$Z2<{Cl&8c{__9=0r2>t^(A@Si7-F5lCLSwNW$~!k5fNb}zLWn_ zg5C@WkP@D>FgMQ_!y87GRH0p9XiVxF(o$;0WKfQdmb;S8$;(y}qDqXrH1o2vZRHo}_f=!wDid!}Y91wzJGdrw94Mqm=_4dNO5U0h zVNHdnDF6A$o1%S_jT4qlcB>^kh{WA@@r(7UF^#5cZ68M?lF1j##n~DERXT3QEMv_wHpvJ>z_R5b&Aacq9<; zit!jR?vi3(Js3`&$7h4ws3*sP+iU7(H86J_TfmIl!!FV!K(2^XlMdwc<&#j7#y8XC zPzy89=u>hfs;|ahC!p5;rWYh&D7)5^F`}t*4v2Ow0T1iU938dJ^syvgH+5#~->(0wCA@sTD+mq817#}HsMEn|ya0T_Nbty|18zJhAL{Z}2CBfqimb^nX z?kgrmIVa3X@Swz{#AGS6oU9u{EdAMJ8*Sg@TZDCRj1R zxIX}zFb4EZb_7q304{MFV?~p|Oh3Z_>f4=~cvOtqtCZttbxPxMyT)B+;T}F_Xy4+D zPVAGrI*Z3s3cU$izq%1T;OB~}f?X)nyCc^yUv^Le1L>E@$y`r1Ib_f3^Fs8y+L3cBLmAk1l-cTl^4)O$pkV8~IBps!wpnXyG{N}fZ$=4`Blowdf^qC)+r>zHq_ z_cR1frQjW6g@Y~0vD2_N+aP8?KT`u74e<-Tg{YGbu*hQF)E!4YRBoVDx5m)t`T-Gt z>HM$T=w_EU@p{+mRpNkL^thd^=~g8I=`7i{R)Df+VOYtEaQ}z z9A4j(iMhGP7@t$}Wn19@w(MCbPzVyOQin{(h5}alRIY)xwtiMtRz_}C`C{e1?6+Yk z0qjK8zLgzEwnl<&T@~@NO4SD8%rN*;eI_8f#7$Ue!9fINc#(tX@JU1%SHCS9Bl!FV zqRZ%BC7koZ@~cYj^9qbuR3X=n;i-zD7i1?)B#VisB;2+QpSuB4^kE!f2hv$*)2cq; z&DhNIxG*Pu^Db1W4;C!mf49nL<$b%0Ib`$kyFbX;3lFLLdh#;FpZGmm$a8!1Qd-66 za}2xtQNckkL+CYXx%;1<(QBcHT9D5N8}T8sgU2oMM2+5b9*_9`hxf%>RnJ?%{~tVm zGn*ZSk;BD3_8(&|psfc5%-R;Rc<#+g5qoTl2y^uoL%=Eaj5O@dq!$ z59{JWQ4xY}JDEkP0aO#(q|KPUGzz~K+H7qt&JCnWSGJ|ej!3m6J;a24!Z6Sj?fVVo zZH0(oAyc9QuZLc8w8@sk^JGKk6EAp!UXR3%mvSgp3NtIIGOY zGyH~V4a;Vr)33x~*UyEeSy*S0H&IuP=`OmE(y3d(Ax9V~Q8BI{s(%7wHz@-5qG4d4 z);y$&R*81qX=zsOYGG85fn%N^NX|8GD0=6R97lPi&nzxDwCgh5YIlQYXwhr2nI zb)Hmax7Z;*P+6UgB|7b37i>5Io7Nh1`@`Zzo0iI06!BKhcsEp(CY&kw_)vb$QEK0h z-M#?3XhseL_E}M&ZI5(UA}K>bqHTFnT`n#sn&vZIz4{h+J-KkhEwQ>uTmIfqorglW z;gQkP-JiPdhc@I(i5`TE(eJ)PHW%%?YZi)md-L6ov0MIo+krhFS9$%Ka-!;SM3YVy z>z-?qT1L0)KR&jFxL=ME>-_hlU#K%+-u{{RzxO#eu(NP5lF~)X?P3i5H)&FSO`4fx zJHMHJy5a8>&eBP&W!Qgjd%f@``upqf>`iCxu#m`^as_zj{nA3>M6wMuQ%a%y+3I?K z9H@Kq$|wjeMlHNSE5Q;#U}`Aw>W*47h#@E1rpO2Bl5nAtm2DX?8$)|dkV!lnVl$`N z0sA_zdJkh~`}en}v2!B|c9#nTtr^e{D}a0~{9?_8$36y41suB0H)|Ba4!8ul_FIOp z-q)cOY)hL(oKs}+dciw_M#uJ%f)%G#jS+i{GEX8Dvc6c`+ZLfs=U{iB9|TXWlnWUE zu~1;#K=D!xr6yUX;Og4tsk8eJUsJPuTgm|jwF-j+Zp_S%LsKTS$T4R1z{v3-?@8a-QKcx))S zz}!>?4UId@m1!@A)+|1TDUqTYCSA;$T_vi|Oon;he@d#A@vcDe_UE7o|ApQ3BcE)l zGk!(6QwD+n)sxt?vJ^RD02jfMP7%?E-Gjws3xjN4MsMRmw1yQGo3|-QggCJD@2}ws zp52KE%>3^P)xNygPTfUH0ch5IXAg7t%sC<|TlJv#hv*ig&fOgW?+38IA4f&;HSAjX zL*sjY=k;}hYMC(Q*n4Iqs&;X!v5nqsh^Y+g4wFw`gb!?L5Suqs6i{c`yXJoRut zEW0Xkr~=2w-he=j7Hu$$=Cun~-SertJ68xXs(*+Fv4FUBw2Dp9-^p`A$g)sc6%jsc z?+bC&Gc#Aad9Qnl?be;xeJB5`` z6{jcq!-oe4DLa__J+?0T)&R2o-?E${#=c07z_BlqqckF3ThqQBbSu&(sHRo3_;9iM z*b9L?F+Gjn_!!i;f$AHCZwGWL%3+1FbCA??&X#o~`Gad55iCyk7}!hf%av3& zhG0XBfLoMEdoi?Nb3f7u7aoI_@1H6+WZkBlS6*YpC8f#QV-c)2GtVvQvU;|@KCJMw zYVq&jAjR2_0)CdGLWnQvQnjGYEBI?zY%kD4?9rTsu66Dxcw{{c(t4pAb^1&MTvb`X zkA%P|Aq1fIBHAKyNUkL}aL_P~VASi-=Vx(G6ei`!k=K8gW)y|GudTmV2+cS8Ma1~Z z$rY<(R8J1A;G1P~?a?c)7gF8hzsD4(HLLwQ-L+(3HAmwCKhD$babrrik^n=()g^H2 z9}`Q7SETmOtNp5)%IyLdK(+M`LpnDF-DV2L;LTB8X=Z?lUAgAVH{hmNiB^JBE#!~6 zW@a@x7OGBtY58omyDG__^_T#W2=NggAJ;*+It_%5D}|w- z19W>5cv8#^{;aHElQ*v~OjKON?JxVt+&&wwi60391|Lx%%m?K0n^S9-W;PVzD#n z1wt%yOi+Zmv2hT2Wwt1kX9MdqhuWquZZB#+wp=gNfCgIc4ow5n1hofae^xM!_V?8$ zC-Q}ajU3foZ-T47`7$M90WXXh#3%VGVUhLb7}58M-lQsO#bRxTHXTUvU=|Hph%pmx zF;0`vkskvUi9nWW&GsB7=*p|6AR{#67&`1|N1it|uy7->j8r=DRc*Gr3joSOi*5?R0@|NE? zS8u-Ae|9r|6Vorlb_VP3-3bOQk>qPm$-hSdFRgR78*k@&$U-kT8DW-X*ujfzjku{@ z_r$DTDw}mD9lw2GdVxclft)XXi?WI3?ig#?X!Adn7-gaw=a)218fDF2r5&gWg<#89 zTJ)<@ElO=t$L{XbT|BKoPKhvt2{KJ8br8KO6{+;8jqoh2@*mK8vMch_6BE@&J$}|+ zS$ElFb|{g~!B1~2Z4rwXr-xvKceS%|VVqDxrXBM0#JW9RJVjFtA;EPMO}TG2W~{u!9+R?s(5q$EwkM9&}_yK^WMr4)FXIh zQsAZH3i=_|q-vmaXjf4BjMG+R*IAjex#cZb2}tTD;fJp;MZRR+nwhXjkpqZ}I8lO2 zDk?HQo(ZkIiUg|^Jl%2yhzKuR)fy;1fHrl+n(TlTV3NWW4qx2q`xiAdV}A|;Q`9_9 zl{TF^6hcgYL}WK5IC#9U^~zJEX2)BC6eWy)RL6Fk#2@Pfago(FI!-2D^bZy(I^qb7 zq&09sdoO3Lt1{E5am6=S%;Yyqasp2%dFpTQx$krO{h${|AZjzC051E9p*Uy1Br2(kvWIx%$)AE?@x`MI}i#(0Oq9P{Sr$#~HR z>`_RDdp^@<=GDl0a<#EHxZ@OpZo#U?gBRQvnFmA~#Qq_RO8!cR@vbF#ia<5B>K86I z5|kb4Uq6$t(fm@}zZ`<^VjVw(UXEr{Q&Yjspzy3koBr1qaO`$WDItL1QZ-wyCA*)h zi4Im^uT;u|h;MP1khYqxr{{AeZYzLGZ`L!VX)-7Q0@<``#S!vBaqW;159A); z(8^M#FPChKn&7~=l!RBT6ONn}s2+}%L0vA1@ms`kuIZai)1_|#bct>MvyR36P;4Cl z`|*swWKWM=!$(OvhgJoZfggJozn@9cRd0E@>iZcO{@8sgev>@%;ob>E5InW1Ekge1)5x10XS7<0|lb)Umb`|(gj+<9qFf%QHv^jbup z=Qhc)+y(qICD$V%Nb;YlI+y3m?a4%QyjF~t9Q~L!8H=b7wQ0sH1JZlg7W2O7x)hwb zWl38B8YkM9p%+XEzcT-Idl5K>`Elf8j8!y6u+AB&S{B+8X-jvA0B;z{8dx{Af1SQ= zA31|2_U2w9ih2gT_d?8%k^y5oxgQD*eRVNqMG|#Lk><=bKDd^!+lL70>f#~IlrW3vht<}M6PxoW zFSFt%nrtsl$2x)r1cAqY1-{~N)Q=AImqrzw!y#{LnCZR z|A{Uh)<#2`Nr6dJ6BTZ+ad)9TZ$31~5B-feRx$Lu%sr{eSpH@2*hW4>aD8m`3MQG1 z;6w@CMyi*<7e{K#;uqZsC>6AY;7xpQHQtG&4M!O~%-r`Gks($vJLgs7KKpQ<->oHE z;#K2!wsK9GvTxry5P_pVXHU9%EpF1NQZ;{NIO+o(6VK-~<~BWN zPkuZlSP59aK!evWhgpnL?43_1flhE$;NYW)#R8J5w82+UFLX)MctX%-2UBMRX|w19 zFg8oOUd^rMu6srJz2s+Ln{r&H+J-7HZ2V>n`9YW6y$jF9BTJLp2PnU8j&Juflua|-TFC&v%zhwl!XA9gO zUVe7DlN@|8gu~9Hqd42Y^9PZ*yRA1QTRu9YjTtFPT}{3|d#){?+X}T2n{DPJl0xQa z2H}m2MN6XffG8e}DaR@@l0zSMRwYdKTbgJT6dbcO;!b4SD&bu)VG}QF=fc@?K!yr! zx|{^JTOq(w0#F%*%Os*SVBox?nv9zlEg>vmeo(qxqsPU=GujzRdM=&_eq6J;2W?kq zM9+O_!3VyL(_AniSnc(1yDKcp0D@}gM~S6ttm@BRX_*e&#D~pSMSbs)@4{jGcSCH4xYUn{k|`&0DdggDSp0hM_^NjKKyQ{`|d6n|A*NC z>e2-HgYMNzAo;M=%{CU|673WoItf@!dqj8T{CkAxxJAPU0|mEXa^RO>!Kfq_tG`*0 z(;HEkkD!WP*Br9qmpetZoe?7*F&^23u$3ai9!WXa`lEU-1t--?G|D)DT%#B*apxdp zI*CVu2^)-TJqMpT`ewU~JRLg34<Pw*!ezJvth8BgIDUDF436<&<5>(GUV}lOgj39+1cJl3Bjr1t|fH_SGj)7ufpP znFMN)`?dEvLh|^|?aL5rR?1W1oyPw%&57`q`>z$kBTP%+AMBM&kZco?r|Ix97@Hks z*@p8ttpJSTZ(l}Dm%QWuTw_}8G(bJ6F$~u0JN)fL{adVAlOyQzvNF3HEf?Ko-|vAd zY#I$89g35Fq}s4@o=Q-F)|N&ZDK=&S&YcZe?|6gcbv!%bnmf0W!LU6})I?qx-VlL{{~zyA*q3(*UiPI$+|Ule zsQD!Y=%s%4Ub4R*PVD!Trq}Je<@@L+pa6DF9u$OuNr7*aw;%iXoyo_bMAj?%7gi!0 zd1@tzZ3;P=TeRKCF-gHIukQgcct!Y|5ffG^6%}Xg7BdS#au$cncSEsmjWIPUR(JB! zGLfm^o8EwN#YiS0NQ;|CpxJ46*zMFjIwc0Z`VO_^WtXA_3EeRy-(zPxqVwK{yPNr` z58><_ss~A+Z+W%4nZVaEc16mcGMF0KzHU^F=Wh&#S9B|N)Vb?Z`CACtIli) z(d0x1F(@DU@nhV#SPGU_;7Hoelt2v(JRiUC;Wo0+BO2*a4u&~gl8i=ARuYHQ9?9^r zG$fL#h+G0SH1CqC8l86~(4lPdNj03p4GCm2?*aK5FMYqlH%9%@!=zDT0P0!&%n`>QN@uE$(HQ!@MfN!U-+i5!Ux&wK^V zE)M|-i4RY>0e2_4K93~tXzkCoGvXCXi(!->DAt;CvkA1Zeka}~C?w?nFu^Z;J82H@ zchaIK2X6r)rPESDfs#f`e>80Joxz#)Pil{KV(V_8uvOT1LxLR#s0Mfj8Jh41Z(pn| zHxP%P1?>?eZ*aSBHhALE-h-MajD}5XiCJ$v19Z^I#aGXJZN+QVf~c}(yk+V%t2Ah= z>renM6O_dOE_X;gCzEo91;BQMPkq@J zJ3q53WwHG5Z*LcJ1n%y;w>}O4Zk7K2jVGDBqJUVIk~w}1*^0RPpvC4sR+IV^T0 zvyv?&ey?tG&@&rh+gD9XMw2?7Hu10Im)M*JOBE&!VTJ0Y(_b#X*na&&N3XA|#U39T zasIgyG3WS@U!Ff0nT1%H%|hNqdL|tp_7}tr1VIbXI$OLz;WHH1fd`i$<`(e|Rm=|% zZ4}yD(n3caQ`OE#d)0?yQG00i=-0EL^L!h!sK1Es#xadnN8YiicJDq5{Pz!o2s&Rk zpd`iDeVc8Oc0Nt)b3SzQl^oNp|8Gnr)jh6ck+g*8cBc52boUk$q)k2+T$Y04x-<{4 z!eM4ETh<`AHpyWxW;ylw_x3keQXqi=&T=)9MBwur)D&t(O)s%LT4E&$e8_RTz%ln} z8%5(nn`%lx7=cuB7&}Wbj1wW_KO7Ind71haWUd-@x&Q}ejY^D>Q0D4wb!NG8nJI_R zA{AFh0_=WMf*5<|E7=bc)S++UN#`1>X}<%TWWgOBNstYKWUHQzaT=#xT3{L-$m#w7 z#ak9;>H!k7G&AC_Ke;8Oj3I`tVGLqvzsHzH&6bU-&q0vjlim>yD{>Wxf3JYxz>B@A z{aHL7TF$W;;M6EOs)QasKbANzGwm^)AslH%L&^624b&IcSdNkYGRh}hg>ujYjCgUG zx+Pa%@#4@hV1RZ>+K>*w#!`TZ z9tuu5+Ny?0co6jWgZ*)Ox=&IBPo-sk7{kq3#BiVZjuE%g*IG%C-XN80gh$4b!bL?Y z4W)SL{gioh0m%<$@eq>O3S&@Yxisc0$%GJdNtYkI(8UD{S*ATN`k-*aH1d1%rYuUmgQfX0S z=I!r>=fv9kxs8RDM&tv0{BhBVFCU&p=4tT#JW&zHxG;8d_8x!pU$IR;%aZaI$owmp zI}5N4(G%F0oQH;J4a|>QvFO~mz<_Jmt8}H43tgrZzK$^7tNQsf5)`~>Ck2e5$MPDn z3vcIq#pRZ5LuC=u!%&)@=rK^ODlcgD+`(CB z=?PxQVnRC^Io1rNYAn@D#klNA6=IGwq*_rajXwiEydKV0)yfo=6cwTN;6hvO!crxI zG&&m*HgtvAEC1-SBSX&g9Vf-~k#|({SA7Q>i~~?94CxDi;pKlkp&LR8oQmst+WGI_VgfPX3vGB(g+o^8)gfM)M7q_p2Kw;s7uBvpM5UA@3%=FCaX?){hDP*iHxXxyO(^O#(93oHRoS@<-ZW=%OlK3uOX$P>z1hL7Kh@WLNo_!TS(xbOyRH{PD;|bWCIu88yCaMerFNm8zW{YX_0K&sPQHugFB!Ek9>Ox(p8|GSx~n z5mVhV@W61Lz)T2!iHWMijMhADRv@dxGGkP%Z>IJ^Xvcs={>4f{Fbj&-@~gqcc@kWF zJN#8;tjL5uhdK1Tz*9NI)<3I{eKqjPiS~&k_iU+f^%MY5FeE)n0GwLKlbsoR_)h2L zTsCziC!;s(3glM*X^K-7Ab*$-fQpTI)OK1jz2+Ilas4=6Up13t`lxvdX6Z(?$R1);f)UftY#l|G?26N3D4oq(q8>1*4oX>xKXzvz|e%5h597hcF-X?H*5%%oQj}o z;e_p%%e)I4dWKqw3SIij!x$ewx_tNW&vI6W#6D=;7gzJWU<=}KLt|+7VF>IK^22ba z7#a#bCPlI%FtJ4Ld`tJW9Mh7VmW~*)m zb`A+++fvePUwX^BOIivFXu9;vT?W-DF&m7)Z7<6J$rGf|~#r zqOWf8uT;G~CTD$p>LbGnmsJBCUY4=l;8d4?h@eNfjU>taY%HS;u&HACtN~L3f-Yl- zwlEEQ@x6~;X2e51%*#k|F<8=zVRAJBRPvHzP1ZiKaDNHY0u(k~()&TB$UXN+?#ghb z1j%hYXBFOwKDfeT0ZW|F@iuR1Y;8*Y{3fB*gNmCF#O-as3};C|>VvbbZfsUnnpfgE z$uShpc7+B3kB_pKDUuS14qvR2VUkV~E9WteguWB`sgVh?g$7jK&vQNje6hPHj~hqD zCJ@8Gz?0?V`4QT!6(Q6oC7*FM>4j&a*tiGC#=v%w&sz&CtNmub4LB;{M0E@L=Fp>)#@Gqjm-yfBllex!^=4i2 z6A$&Oy#G4J#y@BTDUm$Dv2KpSTkMa?{#vx}ldDo4TBn1OiSxed8KGb)lTLSoGV&*d zp(oiNZgs8nqJ_A5?l~Jb6}7hO!TJW|MQ}S6!C#g0upK&pFpBwSkD{CXA6oezimU#A z5KI5TxJ{_yvq%C*Rt6^_4o)8oF0s$v%*Kz6U;dwf7O{3=O`s`y>lovbVdmRmAM#-* zC!eLL4;ZWiv)$DbFwcd29g-?~KW+q!kB392Zdqhiq==RBXipWWHzY%eV4x4y0WvXm zcX#)4ZxNiO(HcuKNQ!!p;hF?%DB*#2Zb-{(a%bKXWlceBTSt6!7Zk#65Fw5-#eJ9` zULm$H zDj$=H+1?T@Tb**Zx4gj+?e7u?m`tRX&ZAqP9G zG$^0q)*Tr;Dy|GyX|hL7=k@9*H239XTB3LVRaj$A3KnVXe12X9Yd(Af24E) z_pzixd*4pHe-qogX;|7A`oB-K8AH{Var%pB+UMtX27_jm>aC6QnF3w7 zAS&Zoso^4SR3Om%DEyg*-*7w(F5@2RC~e-scx-_EsmD@j#86wzkG+F46KmZ(Ru9uL zt@;9~Orzhd3YdgFe+V`9HFmb&Xg@x-+CyhF*8inZswJaKDx0N)-`g-&ck)|r-?Sr2 zX67gNa*)jm$ENRM-*z)PRJ>PHjieGRCyw{~Fb|@HNEwhpGO=cSj5_z?k}pN^f*9|E zZyj<5aiyC)a+~KQt;bK;p<(B37Zl{D1lh7LMOVg8`Bf-U@tfh*>OR;G6&tHQhQ9QFrmB@oPGAj-oHyoN-562 zysO2_bW8F@hO`*%V3y+I)A3n(A4V8*5>q0rxBL|Ljw1M%4T zaU)?U_3qm~`;}9|`S<9B?6~K%CJ0H?*NYh5=T~_RgCb&R$%e7yAjoBCwU{7iBC<45 zx9J9ytCp#w_GL|sch36zcXphsR#`FXC2>q8^ZScvU`M@1eBqMsiwp4Pj#;HV7OXHd zh9!&jzvV@6|FEB$RdK)-aD9egNMhXtHZ$zdBb-=uWYFRF@>T%C)drqG zcKykfLo-^zU+6rmXlVuEn)P{KOYzsgVUK`EIwj4s)BiUnWHNr4KtxmOM=^YYTPkf~ z6s)SSw){VXx7to?)ykCmu)ZHtO__4iBS!B<5qRY_6Y_8E+N^Rw4fK4TyiY)!8<~GE zSvgW|)2~{VOJzM$1^OPo+lxa3h_x*(kisU<~xQR}mQBvN3heXBkBBi@&-HO9@S-V|;eR#sYBccUQ!1 zB0cL=W8o#D2J7uDOFA!-8-8B-d$vwq(;9~yqBp^A#AnW+HRdT|5|`2BRbxiq3u+uE zS6u0PA<^}nt~f8$QKlr)xPCv3($FsOeC z7FFa7zKl>-sG?pCp)!y^)N1p||E83n7UJHvZ#md9$iYK4x>!NjZAr{ViIFS9yGO~- zJ?Fe}F|kI$2Lt`O$L7Na<4+DabX~)7eCSA58>oV7P~psZ!Ar()@}3cgVk|+LXMq|F zD@u`P<3Ei1!>5`)u@Y`X?gv9QJTgGk<2$mA2V~jmn}?qHdzxI8dU#89NuFdH;u_3U zU2dLaV9zh2IKXSb{Ib}%+FNg!N=+0ViNc%Tv03-+EpGl}DP-@M`EqMby zDT;GERnCVX$!Cbu!-lxB%FU^j`iJ_rR-IUB9Ikx`L(xH_Zb!NrF9H~b!5 z9i9Cour(D-m;hQveN&Op)a5*Pg44GoU@~b)D_d=ozYHywY9*hr*#%K*M`o2k;`Gk`vMypbJspl{Uq>Ik|JS#gtr$?6xJl z3kO%o)Zj0pa#R<35H^UuxBr_%2@2x4YZ%3pHSjTKSfqzAq*^n#13s=G5%6~Dc|i!z zq`Lokrmyv2b3`@tP)Sjc#gUa&{Xpjm7J7 z%+~=NKNrkaiOyo&nDGh4n8x$o9w>{xJL8lR{AmI`=M7&3c%kExc|AH03H8fKsc(!~ zDtW+1jAw4lO)=R@96IciE{MZgN{ZKxfTw#UqibRYp;YLJgh7)ib1OZCqmb-Ds?pt(%b|6vNa zet{E0A28J9Ah0pTK#78wvB?w@6XRm>fEl6iAXa>$i9NN!xRlBj{Z9*6+|9_WV5O;y zG@>6;A`#Ku)%KBYSW@VsU$wiraF z-dT3QIGAbFjp@h;SaRE@o%HRJyIyc+J!BSql=j}hQu zrg5c~WjK{WHOq+i;bslE1ngb*`G54k)#Yqv$&N;l!JX{kzrW6@#Z>ZIa$oBKPr4LO z+6itK;5}Od_*+A;w#JSyy6qr##}pKl#<36>Dsf%Ev($@ps^kOiS8Yk z=+Q`q`<;&pEZl;eF6cj|}@lSSbMiXeug4i=abE~!*0@02I z=d9qK$PbLtbsD7V#LBSyIvwst0|oqI($J32BqOpW8vVNw1uc`O(O<^Q9U|)S^Ip`t z2}KKv;Qt!lnq%Z@o!=?CfYHZWLkohikrc#1qBBcYI%(D`J`gUIERzK!!GuP# z$RHEX?EjQzuq%a{Y8i9%?gZYkx@%jYDPFKouUI`SG*Tr7gj2(WCahSBgsNu*c9pqW=~3A?#?9 z<|O;XpueZp>3qc5HUEAO@e}zBT(R#oMesXWFton&`W^Rhb&%9HGpFyiDt#3EL7x;+ z@NuAtG;&6(kM}?BHzg7rgr6sajf5xuHA}Zd0guZST}OV8ORhC9Eli?BpE&ztl*>k- z%f5TbEl+yO`g>c$qcVXKm?jQVrUOzd4U)Rjr9C0T{nbjf@`?(;kY*k~l*9pYEF{EV zZfODN9xJaXS80s_c}7mIH}BL>vKOur)xkGc*U0&Z5J=W{C6ZRMl6N8(e97=Ug|=Oz z+FZ@UYf0at{g1Z+%zzFFIfRRSboeP1ObZ$iyPhC|dTwS;FFt>9TafJW*}xGy+AW)NI? z*ic9hf(WfuIEXAv*ugyNN(Q}dR)Q2fM9hK1y&r)>C6+7PanZ^Akt_2)Grb>T(Bfc9b9$_eW_UF4b_wmA{qS$5ZHW+y< z8{U>pZv;{bC383?7V{zsI8v7}Q?Gd#0)3Fr0Lx-dwh`nQX(B#4P=W>`X#;PJVlcIx z`Cn&qGqtRqo9MXV!?RgC8O|%q0d^bgOmE|~NaO|pu=ZlD2~{>$HWd~17ErKwo=glw z@Ry&-Qo24fH_;kmt&i{=Ow`p*xM(>z!Ofxr(VYm=Sg!_)|QW# z%RZ}<`ozz5!UBHRbN`>?%tU@4D<6-oElM?caYpgz}+S%9N@+|h4o z^L>ij&q*-%IikJK_u#k=5L@_+{rQ5-Q}`&BW$>To-3C?KCH;5-egwJ+t^qty@jE}) z$1D=l1bmH%44r%C4FP+y2EeJi?SWUAZw9yLz8BqrW}DA>sHv)fPep*V$4j2mt-IrQ zus!BgR)OG0u;8n`SHBRr!Dq~BK2N~q-^qi(uc9CL7aY11!_UtKJnqMdi1-}ct7FLT z-ToF?i-N~=)q1sn%U;12hl6tW3(nuVvxfoy@# zm-9bUfxN!&Y;0RIm;a&<#s%&z16#lL^BlcTe1F?@ziI}2Tk6zdV1D1P_I=JGfUNOUO~FGCX5V4x%@02p6sV~O02A{yjri{&Z&O0w>rH*N=X%zW5hG^S;^F1H zz%ZPYwdG>_gH^EIVR&plV87w!^_g>bB4_7YG*j>bM+>@Wl%VIPzDToaVAjz6;N@$6 z=MDa}dC>Q3O)!9vZ8X^5SK$5RCpNL?oBpQf@DJ^Z{lCwn`R}dOzDI1W`|a;*!p)|O zdxwS(&%md_FJHp0g9(yTC3T7azSaz&N#0;4=ja9nV zzYz%d+%~T~FSoi8*@2ms7dXarO5!OM-w{ly>=s`xxEVEhxtr>aJaWf$qwgTul+>&1 zEK&nE`usQRWjz!W<$;6{8WC_oZQC|R4i9H$9uG&%iuBHvrzHBhNtgIh>zdkRR5?!d zyX!0T(Y3Wm{}{7VBneX5R>I*=H69_X(WpI6=)%_M!&1!Tf_*g)=%{ZRq52g%@= zltjrd6&DAy1sF9b$Tz95pSlt{+Za{=A^aF_+%^->gI#5}9|t+)@_GL;)|h}fi3-1i z)3}scbiz6kQ!*-rZISn?%YQOJl}p&KGFFR#MNpVbn@{Tnl3SQm)tG=cHz>q)XClaV z*aLZkwFUl^+vh|MHyhK?)@%C!s*zr?+`qjArs=JWeZw15OEVvXuTK;C9VW$fg^#bhUKfepV;RhDFjvBi}H;P3Uvar8;#?HHZM`d!^V+F zOA+rqR5;2m(u6_k%zc1?D1hS?V)b(1Q5EfviS@kc@K~UG*QQg7$7R zl0d{KLaq8_xpX7lCWn&}&sF;M&MeK`I-rb@Pjx<`T%p694ub~LA*Vt&I-xJLE<}=w zX7o#OW9*4)6A`Z{pTrWZqwJxkzb#mat(GPsd9=vZp#U*z9MAt&3y9F1K1mj=vr)ji zs%w#n)8^B=V&!(?+t`$ld_&ozv4);ZqGa^! z)T~PAA?**A?N_B25g0oyyTCnr3=>xX*f>VPu~M1Ng_RY3@T)bCP0At3=KpCmy-hXS zLGI$VTd)pP61I#6?8XbVPakz?s71F+ zH_Lyol|R{Reevwl1`|W*7jV1Xy}J|m`n2;!_;V#V4Gn;AAg#<-ovr5xyhsP(_B|~# zDkvy?4+j3IF0AdmI0l?u^Ez;QJTCUR2o?3-$?yF5_CM9k9^VVTTM9yw!(|Kujeq+i zevKA<4)O+^jyrmt)$~0d&G$V-27EFK+(IT2@Vu7X`0upM`wr^2?|=w?-R%5arBWt* z{R{krub-zk{J9T(*?v0^d}>h<6*9(&pWOL=`T+tZ2!arRA`g6>|2{3~x!KY4SZ)En z2MYg$EqKS8A)&fGw=M8IvERX=gKXI7#$AXc{JtybdYq~u>^R{Kon_ZTXhpAF=M21( zMdWv4EO_8|^qLk$+C9IUrr#H zdEY(RRr@utFK~+ap9{;tyZ-O{{_up0?ikHs07w6wy}|c^$_?}oH1L&oRp2KAz-G+u z7r8OKtF`_9X!wyF2&%BO*qdlu)fZ%D|I+P$zcKdfJE8n;_<5#Xdwr5P@C~$@6O!Uc1(CG(>dtf=5JepXgspPDPpuw$Elv-kxa#SVMGxMNA{(iI5P&XsZ6 zZ_?2mG@(<-X<*W<;5D>q^HU(q@Se~?&*2C-#B&bp5YUsSu3kXr$AAYO$Hi?IOoW?Z zDw}K^&Thp7a+UCiICC+v0}ddQO4W?NYRLTOb`dM=68+}f%QtM zd|XDwJ$tl7Jk8YTJ*Qu160IBWajnFqoN*z%k({Monx<+jQ zUnN?bax8=C1dfs}pV2GwSJkXUL1*p6L*sv105#Ya*7Ip{w`{-c^aN{{(2Vh95j49j z9oU2C5Zt-DhD}+kNLc7zCby24NPU-KMJjjg`6tAnR4RA&tATF-%=fTiOfR8r= z`}ouYXirN|PYy||W_Y?$1!81rI3fAP+NHNNv-H#t9sH-m&Y5bVSB_z{dAFh?A4#aD z0<;&6W|608t4VWxn8b`L9( zz}xF1K_AZP)WyTV2mBKMKT{KEL4}1yyT04xf{W1eRUhH^wfE_FO@lwfOvuL+**Z73rPA|dUn0Tn%;8@rn8dcUmZ##po%i?5z2R%f>&|a+#z@53BlF75{r&ah zDVLNG@G`KK?7kX?-p}*R{x5li+dWBwG|5D~Fpuk2-u8eC+F00!2#uoYhAs~moAcG5 zgMN-3@MiPPQOX4Vv3hbz1i>N5BFw@i#LG=qW@czZ#?H@}CtS{-rND2;J=ZoJ!**lV zQ*eLHK^cHO`^og@vEXMm^4shxfC~1(au|K_y_aACGPqFk*E8{TKbh{Sy!j}t{U zbp|xXxlenc6OmrWkJJRXrrs$3F&!$Us0i_%hE?q(bf{D{Hva+px-R1PCG`7G4orcv z_pg#%4D48!+kn3*z`)TqfbkwRed9K-sWNJ+Fq)`axhSNKJ>Va0LY&g6dEp{3gMXbsQX|Kbjr4JX6X&|zC?LtbdB2|K znOJ~vEnh2LV$UPMDj{udA0loB*Qv3EhT#9RHbQH0Y9L_lEAS;z8+!`V+}!N){%Gwn zf@A`|9?R-7D1h(O=Th$EmqV2xCci9zmb~A|1bR+;aY9>t9lo$#0|?BWqH|%(0vbsoj6F`g4S}rjTjo%QxYdcbtI5Xy~^f&j94=fpA&2) zcpLLvi8Uzx95F7!3T_l`*wR}J`nky0PzcG}1_yNO_OCJ)_b&>%`J75Qo-WrtIK2pC z`*V-3UWU`;Tcg?I{jSMM9pIowSfI!@{`66hhJ;;`rSMp~V?BmuS4c&V6Ob_Db<`D~qk!IkqeiyzCoM?1w51ban zIgU~%1GmJSuCyq|WtmjT|6vbEA4mzj?7jJzM*Q3x7koe2c}XCQ+g~iq zpXR0u{oFMCZn%9}aSTZJmR`|HSgvnwT`s11dyKwKAw>B$k>^43djU)|ghAAR=l6Rj z@OJa|X?NK7=jZ1s)6>A`4e@KZ;QjtYU~rJDLJ|LGOjGa&RPa9a=fSfeX;osU_YnF$ zF4_McAnn|UGX|pjy*~ZiHk?)y6Nd$ab26CWwE#(3uSUx?ZC6 z&gaz~$7qKF#og}&JndX{T~6T|yiffnT1ENu%-AXOZP_LaBT0~`z?Px@-%V$T_Y#bo z!*?Qr0MiIml`eyDqXlsxo>Y>+o?}xi0+YOc{6j$UV{k1$RKes5GBQSEK!bXZZxG~! z8KK|C^bhZLTrrLY72r+(6hm9T_S+u!v{1U8b}EWbJcUFPyb_Av8f5*au9{*S3u;#+TzK+c+jUioc^$OM%O0;8kzt&0A*eX&0tkGJzZMuGg+k zZvwHV-^WWF-WTyJ>Ic4H=Y>;ktbk%UH9g5~w^E4)(+Iwb)5xQnFC#OaFyHb}T0A-& znbRB$s*k>oZO^G@SzAvpY!qnWC(19@CY>fVQ9_wP)lT8pJmHMypT{pAG*+J;dO@*6 z1_q);j)jl`O|LFe$pj!2HhzA1D#3!lv$9M;!?J=tz$z@1a4-%~TV8LgQYnG&t^T-Z z0Ee2k!43CrS-*7YOD51WB~7eX?y7lNIG7yg!F-jD5ow+94x7sTOTlV~8bJHQ{%a!K zS^<2_6fwcFx#za5q32{y_v6uK0R1NZ4wM;slmmQFU-ZigpQ z{}&ydTt~S=-03n+f$6x69DyItTg5&BpVI+AGyHr_%TTMvG5UC(BOX*@8|~@0gPJV>fqdv zYJUT^BFBoYPQXToD2Z^r<2(Cc07bZBzxG#88`~`xfO7x8T|w=Z2Ol~v5+9Uke74ko zp8o|;z^!!YdSh4g`p?YS_GbZ&h9g5>b8DvTkxtuxNDH;PoSG0`q#%bSW_HLrQ2D<> zG5QIvw@K1h7ANW#F=uIFw0Q@NXBwkYfThth4>x2dx~#Jx$8l`QA!o9~rZavVi zeCArS8#TVkY#bpp}%I;dKaGn8eWP_aSEPRpJMxqV;^mBKO$w`tppB_92Z! zJb|<%tzFP_9}tH&ysAmw+U`@+gpm4P|J#yR!t3?^dYfcHR|>Qs$dd&9Bfm;3wp)CzbnGB^x0%E zVTr~G62WB?;H5!9pgJ!@e5sR9VR-QG4DBUbLU#Eb8wgHV+@m*zisk+5PE(D1I79E{ z${7n_5AFk$5V=4h^200%73lw|EC76167<7@HmFiG4#|=!Yi<5>flG#?joYQyaF5m{ zKZ81YH&3i0ilsaus9ra;6AFoVKhkqQMayZwp;o0vB@Bb&ChUIx1QBHA^cgeF0B{_H zTn+a1`YHgL*a7AU9~Tg?UlP%e<66Lws5zLVTKP9I!R>K$&*zjd;6o6^$`a$n>XiE6DMEE871DCXP+}ytR1CSth(1hqNI6oifzlZ!icf6NQ1OY#R zw-wKyiT>C2{u#28P#{REKixOoAC`f8Kc3H*zW^FXJk5BnY(xeqNaaYpqt`{O;rIEx z-)9Lx!TkzX4E(ISc~7p%_s8$?6nNf4grh$Pj4fC9v<>;$*>979&#r*+0TP_RDK)46 z?&#%vW}bPna>Uchd+x#UwXgjLiRx-P7B=uBS&((xEj;xLpu`3S0<_?}@9zGG;>`Z8 z2D4C~?_p)UuS>@7jrRV4rPaRA^&-jlL*DkSPsQz*hk5UB*hE!K+^Lj@^Bezz%P#m$ zPd&h{)%Tqkc+;*!`;~y+Nc%BS@WsrW|GX3)^UwQ&lhc0ved*^M(-@w|`4bBWFH|#p zoTcF7a7SpK1p&m+@bi9k0KyAxfVsut>m%Sg(+Fq)>J#s7Yx^s26DZ;X@V6Rv*`HPe z?*dpeDzxlOeNw%f%|GgG0ldmh3p&N_e_wg?CB@)l^&qDxMzazg(Tss_<&J&_x|9h8 zVx;ir(00R@=B;USoowx^oEA|P?HHUxq^7rp0!w+*fEa?q$55Sr`DEd~UHlH?qC)J0 z{E+2Zv3|f=8DqemEQjN^T8#!wmf<3BC2%*#VPPyxk6tI=VD%~tQ#C&RBOLQXK|^}W z&2Zr@htDar@MhcabF9j|71Y3B(aTp#3FU%iPBP1yaFsIJI|bKDuyp77j9P`~>1O*? z-&u_%!G`X|lZ=(U0_vZ4(}k%&9Vh!9ojpqQ{k!-(?FZzmIaLy|9#eXp2}zb+|DZSi zrv)HM7nYWe7lN_K@Aj8M>fVOGnaiNXcB&Qr6b7!dK>PY!`-ISY*%?(i5%+uplPXff z!(6^jlhx8!piPc;`J^EZLgBc}c)^U%25}HXyCkqqaIHBiC!{+7&rFrknBtZ8jZs*& zTrv~aBS{3~7gNTfEK`(mmOT;{E4O%DeXzXdIBA6`M@gh9($WGWU#fwS2hD-vB*1G+ zVeOl}4o5h_2_uEIk*TQR`!#dvIn9D^Ine}>m&37NdFXaXtwZPS!C~G{i_xspUlPBr z_$6pKmpR~B4X*|BiBG8fGFJaXA-|86VW@wrh#a6A%u0rz7{ryGkalANwT=uo0l}kt z#uD=@hOY&pD*IDAH=+Y|T@jFV06fqI$F38LV0P#7OSTyHjz=@mH4Fv4R^2%^x%6Wd zGK(sgK25Z!?&YNUMItsfy;i2)$M03}nj8CSjPw-cxf?c#f6i1Eh-V53j*`)HOM>2X zJ`X)NWJP8li3^s6ZR7CYZBDzLoCa4RH5rLLMY7db8^j30Kn?wybNN`4P+S3t2m zE~wIQ;5%(8IGURutu>i`JYV@9CWp>i)Gz~Q2lPgBk2dR85Cg^Z+Ck__l+gRn3p~o7 ztN@Y4^H|=B7baTO4lV%IC_R#k&!Zb9|7$j%7ANENyarP%(WqH0l`UMXec1+)uj=)j z7xe_B`TSqDH}$l$^Kx|i9q<>qhqG=**Yc^AXUKBrCH(rhR8!<&yoz8pKrng4;YN^h zVAUlyWt)I#Ez!MADJdu^lQC)*snVw#KJ(kNoA`QJxOv53wc78BML(`NP8b`11fSP{ zUGqWS&z~3r)*-)dJ?AcHuy}tS4ZlAFzm5t5LyiE}b;X#TXL!ddHJgIZoPH)#Zo>5r z@9_g&dyUK{$Z>G;#-rb8O>&#~YnJWLH-U$9yf=LQ=WNRHe;kr#E5fxlMZ`6BVmjGh z|0S>-)g47)A3yR^nTVQTS|~fJ>}v<*Y4>V$@5j(`IQ#?RWfJXTLAgArkI{*k3Ebx7 z$;23tk?O!7JL;6s{;GDh*NXf}V~@pkNyokZ+f+WasV=HkJC(Z;C}0sIubGj_d!-OY zp}N02z^YP@`jj}2uT{rsrziF1X&PE@j_=#K6#pxf^j-nXz%m#CoT~&W6Tr_EvIRf7 zHA~`wwylU0PV(7d7aIe1f=FA8^|KEr9y;nJ)`IjH44RK&5Cxm!Bt|#KeIrs;_`MIP zYgZof=Jc&egmo%Rx6hfct-1@RB5P&`EjRKoGYYL919@yH?&{tpA5@A}M#hfQeq1!W z#UJGt=HhKl4JoA8;P$aX^kz`iq6u^QTiH`R6p0dUh51c=@(1M>p-+rJd*01|1fo5b zY9UjCCcT!g+`)>8J|&ElFm@n-`>Hw2{y_M0yG$1)iX> z^p}FqLM84>11-8DrE~?>3zK|BMZwVk#ByGNgF&8s24+8LLd-Jnmyy z(q4{N?=(36afA++j$p7Rk1W*4Gu@H<06c>cm2g4)9q}zp85LlTNyR%&v$PDiHfQYN~V9n$Brno*~*ze>0T zW~jYGq8jnqz?RE2{cR?Fe8;fpGh@@Cx%08Xs^PM&sglMxT|~x8MN3}8no%7}&@c^i z>BGRB-B-lZ=5KpbT@5i0FR#BLmOD<;Qu9{moWxfKp@JQ!1eoOlbc~ZY$up64z(}fVS@MS?uyKF)`N_<_IwMm4I}Rs!x|?M8u;y z^8@8UuPWPqO(uI@iMUI9#&PFFGi%?1=c88M*%9m#@$>Pfw(YCk$zy@5W7Yjqs(T&M zI^uDU@lmza3;m+X$5#-aXaOPRic;N%JOkPbj}lOAL>Yt)BM%cVL9Ci8C2E7&N{%(# zGkgQZGe-MEg@C4y?Evf%BdzWYG~6^Ji2K*fs)&AqeI$&B8K?cWR-J~$NjV}RkKg%UY%R@C5px$*V)(EUm1O~DZ^|@v zoFxHotno_DC=d7p@9Y#@Od_rzw*ijbrf~vZuhV2e*VOFHjGVkYAeQSg%Ibg@LWn>B zNFEu>JN(woQ|*;#2@gPj!ij4VAq&<_z%WC6fJR5)p2NCly+;ov*WLimJT-v^J+g6^ z3s!AVlvf;H^}(l63NI*JM?I$Zr~=%iD|CS1--f>_o$1?0%GS%ytTV0Y+jTM^*eVQd z#}UQb2-NlQ`MDp8cnz@xzXrGNAfwG_DkB4xV_m|rXN$r)v}ZzJQCFtro1YZPUarVV zN)NSu(C)Z{070v9wE&Yn^h|q%-xA3j*C8537^&`za?U_%!|s;p-IvJZq&|=jah;NP z;G&sLELkwI^})|2p=Uxx$%xRwU}f9uU`7RMCv%R9OyZ)9wjUZEbT%gzXZ{LVobzT@ z-=`H?83v2Z>C|8_5Jl{kI(EhWj4zp`C?^+!jR%Nebcc3LXxsr1Nm(SY;(O}S$V$Sw z#YW4h?x7Q!1ble%NRLPeD^x5LqJ8Vy(_rTL%hn@8aW7K}YjvTr8A*`%@x^Tv{tMI) zzu1x_ne2#X5d8Xp$o>97jEKDoXDOyZ3jWB8l&4EEN8qG;u7DxJzD+^|i3Yj@c)z1y zJ_y#jHMf$e&5>b_5~Q#!&u+Q0hB;7nU?As6d}xqcr;-@~i&z=YuSnu*dLoZoq~e35 zvo&W@t)7UH^%JQ+EG0_z_Dde#fk-7Fglu0lAq)J6K?4cbFKH^20s6t!~56xMpfgtP)DmhSSk>EJv110#BKm<-F;_q>Sw5n< z(tK1+OO|tqf27!MwG~aCY~G7HvOQi6LymgN-Ex(m1c5)CHjPgmp$Prp{f1bA$$-)l z#KdHNPbB#!@!Bwer6Vz-knYM;=+&uLrzHo=ggwm=o>amsvYSvRBP)*hBwhFaD=`}K zQVAkf74lM}SvYtRZ_}$>(TJ9<&upTg28H5+#F+K}5}eTgO>mBkU3pM4>~24wE}9|= zyaHWX$KA!mh;u3r^Ijn##C{wXp4XV9z+tfmy^`OFGSc_i%zw#5To@(|EdBk~2;Y zin-x*!Hy>NeL=i7oG`=%(dE);=2?H<)b>wGbWk344qd5CraP}(+NaQI?eFNq1O)D; zrP83iQ_98&h*A)cWSleI3JnerCJe<5F0SenI$@Ad_XCK|Z+hCvDk9{|HelUHhxYc? zRxd!-sqCmsqXili+%BbXE47IrHWGD`dgw!Z0BaYmB}*u!l1FLsl49j~(>h*l9+ly- zw4tyyP2^9Lc`l$nHf5rfgnt@7X zQbzO-PC-N37T$z2Xe+WAO3`Lnu52dF8EP|2h-n>%`+*^Z`N+C4lX{FgSj|T8&JpSk z48&gA51=YEzc|j?tpxF4;RtrYNF+e-Jgxq;Sa}z?4cIIhq$H`2Ot6*3lj(XzpaF@2 zW|nd!J@VX^q}+7mm#`O}JJAQ&>-yw|N6cY-l+1vYnP71g3AGh+42q@2>U_FwxNtpvBA$1GqM+{l= zu8~%ojh$VeTGM8EgA{P%d+dvKFEgiU@c`*s2UF3Yz-Y622Mt$-o7`fz)oweS*v${N z$HBSyC&&RiAv>yyZHd`PD;L!pz*Awei7pK_MjKE*pRm$lRIeU2sREOfHLui6RJJP!9__y` zUK5bc`CrK)t3IHo2b|of6PT83026?^-fq9yGaXL|_Q(B=2H|;<@p&~_^cQE3laofl z`xK&8QDc3k6PWQx8MAu8c@T;1Ir*6J5LbKqC`Bf8^An0*qBfO_t$(S{&c$SZKbB#m z$rSRkjp@J?x<%)e$#a@phMLpC>0?f=s~DULoCwF@Z7;E3f_q%u%M_M-4KtKZTt8Idi}6vB#W9Kw4x<=d3JEf)L77%4Dk7eF+9hDF=E!nl1Gq)h?52!U zrZrC@=&q*K)T;HaQxGKJTIl7P<#RyENE&Jncw74XJW3^li&Pk_FiDk6_B2zlG6I4I zG20BBO{iz|f(q2aF31byQV5LN;F-vJBc5K33$#gx(42EguXUJo+QB88r*+FZn5bK6 z%7jzXj6cd)vt4iDH~T4Wg%~_72BK9#*C?n>1Je_Er&wD z*7wvY_=qZ(!m%t=jUSGOE-$-2ZUTc0Gc)`OsRrdaEDnNl2|t!0c+G);0Dy?eqg|< zZ(z-->7%-o3Z#Js8&rT2E74?xza(g0NQtIGL$nMLB#Ek;3NTJ-)PXb;e;2W+h zlJ~&>(F<)YfHhc~W2d0_4It3FU#w7-sv$La{ZSmL`~?t!L!4sAo3Q=wN9sdV;c{)i z4Z^l|qMe5yyRJ%Cfk)|w)H0X`gjzt%Vd?w^;bLLBqNl-Cqwv3^)-(uhQdmnP5i|Ul z_!`xz4y{PZHt?Qc)un^N*d&vBJi+r|=KhR-w-51eAt(uYs7f+4{(IhFNvu{OKT;Oz9eS>-*yP zOobCzYiSpf?x5mSJ#Y%Mm>Ld43F%G z%B5UD7{%D?bqww_$&X?H^`8KoO)1SzUFpusNriDVG0=KN${r*pj%G$_syJ@hUt0ou zVIYtt)xWr?ZP5~86mDLhBy!YKsOBX|C*f{*hc&^E#pRZIncjA{-Se*_i_{q+_fRor zGZ@PYBe{t`KdF8D>&C{^wmnk_u%>4X1lAkW_bI1KxpD*FDe*6LCe`IN^<6eDE+|Y1 z0JtT@Jp~0t78CeDFx6ynjTx77I$!iLP?4P>Ipn6RRnm0f1RB#6Hreb$57hA*39wZt zCK&2{^}xo2q4x+(>-cM4eG%y`VG!4M+=Zvz^e75mH0-GR1qYz)U=)|bE{LU*${Jq# z7qUDZI&GUyl^zNnsl)&IuuXm`vFW0`e zgIl-2PlT;h9x>q|%(feiT@QDq?o%~^>7gfW$^sS2vT+s?X~S7?ieodgvsW8!us{g{ zb#yTr4-Q^BNwKh!J>B3U;0X9xy!OSFw{dlT5S@p;Dz&l^t^pl?o5geiCv#fWE!jTMj*@G2_c*X6qHakVGKZPNT%rYK$0 zDbpJ+ZH%*06CDmgPQo>&#Rbojc`;lYnyWI_dEAYnp0#)B>h|@Bl;4#n_(Lik=+q=$Nl zqnu{tKt(uJsp<=-RnVr@STBK0@FB|EHCTd%Uq|OYw6Mpo znUtk|W}sr#wEnf(?FzE|hX?Ur^04%OZ!&K8^T%Ysm-qm$ohH-qWZl9XBbfPJSe)l& z(klyxFwLuTp|J&&Iq0-nlU~=75~Es}y7mBG^(fd5(Q0)XUFm^)3SPbYS$@XZw6t3qyjRJTpw69AG5JBRT0w}_6JOFr z@ME|-nw^eb2E*4V2Dq&nB)i%-?kO-~OM@wrppk+>5zt96aG-15itXjX`hes~NdK}+ znQ{ZGJheq?S$ELXP*_+K_;S}Fd$ve&Ibwv7(xc=QGV>A3yKl<@1TbE%EIH^FkCgWL z7R)0RI^N)stD=pj>-OD)hoLFFzm_zndjf(j<7?@%4jRie3>@(_?miztAw>kCPSvH; zIMaW{U>S9Ldi8UDL|TsZmG`3s(~@)5R|jrf zeUuP!Hww#IYueJ{8vhRV$OI>p#UYo1Yja|WK7^f?02S%?Cqd7IR4~uUr)I~Y4;NKYcl4L!wFp@0yFrLLH-OGgkvUB7tRcQuc5~yW%t(E=> zl_Ag(_QmD3FcgU{LIN}bT&el5p7tyW`-B`;Gm1Cf9a$`BI+10bk! z@$=JPdJ?}fByjs?j!7Ih-vQS5`I6u7^fnrmasVj& zV+Z5Z0`PU5wR3<;eI5QuSAfxNxNeYLl{p zg1*y=GI&-vr(dHp6F-4;P1#a_{LhVR-ky-CgN>xYTHt;s&839cQ5@A=;|kPjTaMBE z1(G94PP#t{z`s$5FGvEZT&9t#M&a?Ax)$7xTd$_H_GbW2==ziA2EuX07|!Wj{(Tw^ zx>27&eQMbT{!yaQI7Hmp*}8MxMxAzI7TGtLeF0fZ7{&~Y3c`rx^j(eaF-aHLa~oep zp}VTW$mlS95}}U3RC#TMH$7$E~IF%2={{ese1@wS8OziVM)M_l1-+~L(&qb@WySePW~X}@LAd8M;GNmEed z&Pzdt{t-(>sSe8MMYY-Yp)s+$>`?t8yJauexcfLa;<BA5;ZG`fIZ+Q36amrk%oSIX@J=RcV`Ky@p73r*~SDIIX&u-!ddo^z4H0;WYXN zChD*&ZkMhbsfwMj7CAv}h=?^4w83dxlZdylKyO&awyHueFs^Gnk2c#Tr z8Yaito&^Q`_i~TDPO8QA`(PAtKf>3!M!3j79H*or6W3%|gTbgC-2S5|pf%;Ij#m>P zu4*jYf1wAp3Q@rmbul?v$PV-VIhTE*ihYl~7I5n@A3SCYl3cf7G|Ls%SmptEF3rXk zI%-)wvl@S{NpJzMkT1OlNAeC$QH$tD2Y8W|z%OPm#3NN8 z=B+X&HE2+GcC!yfbtUjiNp}Nlrs!HMNCImBWjTxzNSIh->)cD19T7>33VtesxMZnoEZhp zdFPCJR0Pimvax~5a1Veu9MPYsP&A4#^(VAxh4~t!E+DI# z5A@ab_YT6d7w8WiH6k>%@e0bCYw#vmwj&X-gj_14vcS2Xlnf?CgID?alp5lr&X_L^ ztx~Wdp&AVTVCjS1gzsUh$_KT+g$iH*5P}AP;X-S}+jlfIk#j^+Q$i;sTA?ClI)W_j zYsdb7G}FjExY;K!Fw5p>T=lnQy(h{yhRLq?0MLd0#h@j`f=V;0c~Xl!kN3x^43&!&JGP#y2-3IIFS@ z5nzLqx-ta{1=i54c#$SRMXWqRi^4olbjgUCiHsHys=~rq)Pq}M*x4nYN@s`d83LBG zK2N=!O_ZZi(0<_ zS}VQ$;nNHuiD8lrT(1FB?`;&FiB(t?8lNB%IdQ3r%xQ&h!*`Z-mc(;Es~HoBN9K?F zYM}RqmQD>?K||Jk+M+D~-nZ9QII;R;V8M5PPK5e~!s`{SZ_4_x%QInK4QEn9S|g? z270sVok<=-9>&6QTT`q%n1*Nsw%OzZR}~uFymky2TP~f}Z(j zN;R9Rg}L8T+z=GYN8)#`-PjM~adta&9NN~fraa|Rt7>Q5MIv0o2ZwlN4=ABh_EPo; z^t=kt%r@m)GSZELskZXV5CFv?fsR)UM__xWXrP+(0xf0~BP|IO6tS5a+yEssOd~(1 z3i&3pUV2j%m;?Fo;7V}x1QW?2p;<<-0yKH?bNT6B9T`9OR3EDiSc1SG6QmV!;0j*q zRlNexxn5`wOZSqFZS%0hPI=l4&J5z^Aulf zt+KjBi=Y3~@ExDa7C>aT(+!;b=u2@bd9x}R4_pPH72M0p%KCsaBOZ~8PAzu#rUHgNnhcL$j_ zz!*@m)Ps)_ogZ5Hpj%5Mn}5#s96rqvLNcOgGOb@Cf#DZL4i;v!!R7nRBBwxC;@hG06}pQ3%*)XJMJQWbaFpTIzDJSxdt@3_8~1#saNCd_^voN6eN7ZLDO2`)AJchMUZ*cmzx2!Wj9A$ln)g zv#R1YYJvJ3Ikr=ycuQB?ySr*D8(k8~T=SU+{ZEl#CEs4&(ghrnjrs8mzZR3qu>i7R zj$k#EAs4WXHaUcJRtUwJOOgra8$?k^PNH@)tSG79kex0iw6^lZvc)_1!AR7>2IfRm zovEoH62w)mpJ+b?8(xZ471Gzg3a_LkaJ~*fW>tHb1}kq<-xKOVBpiN9(~>u9S4%Iz zIQ57b{~zM0@P9pPpl{6N18xOU-1Wi=qNDPP{+-6U>#WV{Wr~sYS^uZvV0RW2QYi^D zp`Y9WBUQWb=oPd&bX}dE_jPlD)MJu;J$fX&zGS_v!*oxaAM6MB z9o@X>3Pm&M;D5uyt|(@CluUvO;`=WfmQzIJ&msF%$_v=x z#$xQ1>9;sCCoQRS*ocRJ#dyhEc0(xQbfnP=e$%EiaHaDHy&8;N1sFhZbJ7ZiF1Mpg zaHx$)i=zEa17!e^Mg2t-$fON|JUo{wX!c%QT-0f`5B?SL=LZEAhANgxgbcb(K2_MG zVc32`i*##a&htjSEWDYbv^O9H9py3te7TMvA+w5o>T=8Jj{(~AxPwaL;2PJKrGH=Z zu59IQ<(9xpD&mvgnfD5|JnIB}wZyP6Fr_fr0X=N7R36Xhxjz(`HFY$PiUqW&R@b+~ ze?gxGSiYJA)|Z@>kijV-7xqH+F%A=S*JE!T5+`n@4x59*n~pqAHm7H22Qz_R_S$B@ zCvmnIm=AST3Z73^N_xTvY0k8I3TFInwg|@;>@ZZ2(Bh|XQzM+XdY#>ji zpFyc)#hNi2$b$aX`Y%2=>qrCv-ArQ|qG+;~lX<_|4C>8P-TAs~W3Ok@i{I{#;b^2n zqoHl@SyA*}t%qA%Us+l0^~aUQ(@zJmmQiJ>d>=<1@nA6AcS0laG-V)7q#m`0S)A0$z{^o$?5msH%XR)cRvI4?49R!H8^2 z$(Y2QwmB+?o+3i9-1l#&u&F~RyGXNhp29F!xp;Z8~g5!1w*};DB@wqOC4dic}Il7AG?aRoP7KHX7aLflvoN6Gmd4 zi+lQv@`6v5=rpR|8m!pH%1Zj;WGpj6fX{HD(vF3-o*sr~&v&E#9OZ9TDi+Iutt=KYt?^;4Nm+fCQ)>GM3mTpIYbnB2ikcPK$wdCD*gyD*+JS@|nK#x~JDl&%_`%UDJDXZ*$Z2ChOy~ zIh;OSwb|r4YR2Vz!K6K%I$7go-`MQq@h#@GKl#gN6X*!$YP{t7wc{6w&)?+#GLV^k z->gKtdpp6k^LQTrIDKMV&d(ySnvr2GC#NVky+l>IH;q{4y0NCj9BS~H#lBIo8~&+~cCwN|sk zQoJ5>d9r+(7ph^aYP83|Xki1VaSAD`v|OG?39%dvNTGP%h6xciIl`Bgo?ag|pT^gw z@mnZUUz#5QVDL0`#ax6MeUDu|-5StNd~X6WFy& z$7+(b!SmnMiaItKflCl2?9#9>j$8Y!>j0JAHn$PB-N4RO|%2{h*!qM(4hJ@_+j6|7JEYE z!u?i=98zae7gQ!mlt#v+PqY#%;(`rf^JsWE42NNTVjo=Gqq z*-RM*BiwVv723fCbk>r%?5?rTLQxsTh&6jg;OPpNR71nyhbE8bQuTKGC#%FZqe0Me zEucH(`Fssbrb{wiEc#qzfW@;$qY^V3&%*=!cAz6^*|ved)+}etML}#Xicm9AhJ2{a zVs~#|Y^+M;@nb1e3B`QQkdu){2%(-fIXRgmhwU^gCGN4z+iB=nSt0LL@kxI4UXQLo zx%bn1XT!a2C8OtL9?HC=ep@{ z=ONyz{`dW6&z&c;V;^6)V!3{==SSoy;@#;-&qK9M-pc}C&(n|U@ZPiKU95eb1>235 zhYA}501>4Hr<}sGvrgq&Lue}?cc*31u5|hye514DwOH}5i7OQ12@5yO& zPv`5d%%i^LC>z;(g~{v$Z3xb^E?Ou}zVssiVuOsi|QsgD%g9 zQm6K*dcZyvV}F&3e`iIv<5^y3t=CC8GF}Q!&*ZHtfA0xed7wagu3K|H_tyA6x6V)9 zkbmCdP`9gNq{UP(f_Wk{cNBe)6LZcxCF^TFDJ`s&)7BiEl+lCZ3a<7XK^TG*x`xtd zp&cN^ffy;!h)U{8Xi_G!KlNFTiX(W?4V6IB0@XaKDW|H;8(|<7-9Agj$Uim2F%cDJADs6)Pk;AreTIu5u* z(pAZ3X%x19gl+&6*KPXkYBkF?5J#yY4Wt7Tuu`lOo)NJwh5K?NLnqluK#Q6c(g{uz zW1httFb{G}JHM7E+a~5Q6elrT=TxlkuB{$kpzrzy z`{^=-Q6&0{D3+~xLCq%oBL zoIsifh*{Ey72(;OQN7fKm%M%)P^gXM*_Q% zuKdpkc|c`8!lpO(#46h#j}^^&u$g-A?2_4>0X@Kxya3XctH<{Za65Nqo(w8!2ieU( z0A_D@Xlbf>wf7#+;8Vw05R@8H7!{B67nEsPdcb(0R{4q|z5sXz+~@mRVK~<>vDKK) zUmq9Cv%ZnG>Cb{-gpFf_i5=ZieHp zx{(raEpsYseD}9oedQ@n2G&)%7I}g}ul$`1vahU_E{+WP4+DdX+u2qWB_J%t|F<+nEuxVJX0WNrBE&aP7oRo2+W zv4D_o&(SynQ3$3Pg174{YB2-1x?gNSgMf8a&$ahwwrE+Z`#ZO_&DreCOq03%4SDf} zxg<9sQr<6L)#BXW-3v`)ZTpL@#yv7r7u_Vdf=9j7YBD*x!*RDVKbwcwDy-RJO)VuO zwp)5<-xW%U$nwuP@$WbhNHP0~sLO4pLa=}qe1HyMtiG!Y98ysUb*~&Z_I(IkAMaT$ z40SpUB>MJ<$%SamGexFbAIn9jW%orq^Dl~7aILw_h2;)SKz?wuSD4z`df=18iV7)S zWM#y&3o@??1dG}c8TL}zAe85BI?h-YWN30SyM70&QX0lyJ;zR2hm3ObdBG}c)uEr}LXDSHcsvw#PYa_fR;;F>xB3yp zGOsNS+MVf#N2;DydqUF<$$hc#_{#t^TD^X`{IZqniEkw=h&WBptCipg3Sokd9!H3GQOfm{Ti|!VI{ou(*j9Og`&NaPrp0Q+CPYeuGP#x$gXRk2HQw$&{e6veLlY30NPV~X zdjDsR;B;4Gq31N*_~zUitUx;JefM(J{{39Oj*#kv(HCtY^fHXJ&rKgyz-}8fhi{Ii3)J+G3p!-I3 z$#5v}crtM9K1wSLaP#Co^dh&Pv-txfM0d65H9bVUm+^Ln|7IJPJH7T13R;-kqj_;- zMs0TbP2;g*3Z~%0|X;7kW3t)%y2p^UitSt&eIQ zp^@q7Iy}ri{#E>qx2?IM{kjyPnWO5eAJ4|i%jlWdz-Xx8W+PF{Gjb z$nAi#_qe6UptAC5WAzHM+fb2L8T zqC$UMuO6KGp>-6J*!O8tcN}`3v;GHhuBvHCnvHpk7T;alZ!XHUm`}JNq#rW=i+elZ zdv~w(XaRn>5GoFw$afo3>M~>?r1EFPJ$S{gtj{VKrX6negZ#$WHo^CJu_Lh3s2)D5 zQx_YMLvF^#qFN>~pnC%mv0*zmIm>vr`=~9yhnvA8G=rRwIl|S z#o_51NLwDmFFo5M^XrRL@%#-IZri0#*)4Wk9bf=Np8Z1aU!4c*Os{!dy{COU-kNDx z?~Dfg-dO@P`gh%buRhMBEDU-N7b=hFsITJ*Bp?$cpSOD*o|ob#YcN-noJuzAn+++0 zr4OSldL3rL#;P3OkLDHqj{FDEz0YlSOO+$-_?W_3g#6!!stvy3eX7-LYCX33KvbD+ z655dU{&l5akntA^+rK|b?tBJ+A1&RDA_{&2#~D>QEe$E&M*=UqEL!w?#B7&bgkB!* zCQjClUvkZ1aND1ms(>wF3K2bS@AnW&WR&t{D%sZ?>&tv0?|1IbZXkNg8Dv4m%S3;_o?rccTHw!KzwUR;KUZFDZ{|?mMRbZDlRq{@ z>AmeuoQ&#~1!1=l*rux+$aq%G&8l^9xE{!<5d2plJLH~jKr)s3}$Vf zN*gHOgR@u0eLeg7&3#SSR@*Be?f_SaCQw)f5MRicYvvVPsrl7`L+0u@0v!-L5JZhi zv*5>8h>1QGW8W}681his$_XXhRT6SJ*K37M_(@{8{Cz(M++;K6XOi1+xVB?MI7?kI z_D}ln$_E)dG+5~PFm-&)EcC^SoMvN?6Y(+?4}-_3BR5CS4b7T%@c2g~3h6O)8+BM6 ztON}K(Ib%1g=NSn4KgD#l49^n4`2RbPNYNY2uW*_ZONEz^B3S*==Bes5Tr`O1Y zED(?RV*wyu=N67{ia;#iYDtFs@o8T3l_z~#nXd86)nQtDIFv+=RUI>-CN>!Tr9fdUl`3q5kYY1loevoGBEkc!C5PoV|ntQ&PF*YMi z1Dg?@iP?2Gv6Xw*eQVClAje?p4U0PsPVuJgO?D&LWU(c;D$T%UJhii?%k7N9`{o<5 z0Gm0f=kpwCm5P$7_x%kR(CDt~^|W;LzWuVZ;?DYKu`ZoDwKDiPfOS?&xLxJ^17-pT zx5zkR^6;phEWoiTi!Pv7t@6pO5@#5qj|Qlpz+G|~u=5DK+^=>Q2ZRk-8hfSXVgb*| z9Iy|=>bi0T)`xV%Jx3hB+JS{EpYMU??zX}EMl0dQ_gsHC(U_2w*De8%WIRGYgYo3E z-nBW}3EI2mAD(8{(Gu^QPOz*d7M_O#&0d^mvAWE1H9X5n5jWSPfwh;tZD3(LIkh!X zng5ZL{p_-Q=05@_oHQxiH-@1oCfs17SAI*@ozK(;S3cej28zr4C zw@|GinJM5K0ee-ucJYVHnp3faMUB0*;5&}@p=;>mANL>Qw&5+{)~s2&o&-`fT$`mx zWlGZo1EJ(_-h2>4T(;pFCK?`qMzIPr$SBj2doBTgJ>%-%pC`;Hy~oxU=COjZ&kcM2 zKJ>-WlX=Y;yom66k#-Z^VqsSj0~f**W>sTSPmoZs8S&}%Y=9se2S+L@l>wY;rHgQ4 zcq5%z2(LE7nrxGydtFS&=JSjg-EWKg$T*{~%v7*3E&ol0&9 zdCI)-mpKbcv>)}!L60yJ%&-1;;ZrT5i2gj>Af*hS*T^%%0~QkJ>Cg!5Fg^BQVQ~>t z)DBTDC%F_dMXI&{m=@zZ-#xM}{B|QkqXQfx&^=NY$T+7Vi72im2qNe?xR$a{g{5x=tIMrBG2K@k8pbugmZc?=dA)J99i% z{a)B}w#~CW4-VXFFHae{mqQPLhYd{<5#JMeg(A}bIC=58L-X&kKWv%abcePpE9Ah3 zGIhj~E_i360iW8lFlRpezU!e>>E4YFPXCR{b-r#YjsyDYZW0eW< ziSan?dXH`L8Pn+;X6nh}A5j7IEX!l}3qh}szSoWZ5dH?w0=r25@qiYQy2F~6oAnGD#rk%aHOl{8w?=Far#-fV+F`X8FGMEyXw;)5+qzdC9+smpXKrV zoELc6b~|>q_jv91Zqe<6L(Jp$x?QNu0~)G*H3d$8blab%)%sa`nfq3=z|_qg4B$A% z?tA33mBsaMT8x^_=4I)R#`>h`vROYxo=g~*XzYH^bwfSx%%%%-On_tGJvb@atu_1` zuKn!M*L}U95(>eKq0STXhAqh77+QiexbqTo6ux~q-i@}qZ=U^kjSbAxR{j4@(6zoX zMyS^Qv}KYV39U6)G#p`43FT5v(_~iYGb>({ZqAb`Q1C~)3JoC=e3s&vH?dGIA!S>- zRGD(MuA`*fPB%SPY31lDkxr1_FbDF@mrkP|BztVJi@B`LhdW}A9OFhQc$@GtwI|Hg zX4@If&Uk@SMKO5$7;J8*P^hKeb@CgK$KYH0t+ulGv3k6764J+F@)3oz0_f{m`;b^q zas9-yfdA71Ql0!xW+K2M-UW~h&Scvenkh2B?yIG^o}~(mCe)&{AbqIFgknC)8PLZq z`dOb7>p+R;p`@9NED!>CEP=Q=g3s1EW?&$0nu?SLZmdMJHU=t`hF2yX`j<5_h#U=m z6DXwm>m)LHwi}Kr@|q|BG%rU{o@7TSC^F)Pff{UA56@NaHVK)m zk(dcpsOV62fmn?W$su9@*c4FCg;M_1Ue~Jh_oZi<{l%Zf^mQq`@aB&IAkX4%6O9*V%_FKI!K840EDQn0eFm~tr3pRUW-|1ftHiAHS7?(QhoFq*vnhD}*}0xRE-9EF zL3f+IMewV(@_TlopCNa#J!!499`Y?fkd6txnAyW~ZmM-I#+PXQ5^!Ru5u(-tnks`Z zh0HRnC*<+cDtmk}b@iS+8sDdwF7)vRRP}d1k9&ia-GD+3obWiX#B0s^`JOtu-w!l8 zS3ZrTuId7b9zTXv94~)#Di98Or^+11O)%L+tR0N?`!dg;a@ywQ=^%OZ7yd(9!Ak}> zY`LFu{q01X{jZBFp^^P@qL_ciH^4~p*{((ZYwbu{p!KHhs-@QVW@vtJc>|oNoQo23 zHc!y&2w+}0Or841oP2(fNrxs+q$5)IkI5H?aZ^&QVkI!ntaP3AHehw6UupbFm# zIK8*mL<(re>6omb=O+=c7sKJzr9=_TX;&VY--n|82<-{_vZ-d42B+_PI)5l;Xau8; z9p^w@K}CI3hL)4Z7SztAynjHpC~<3A>ye!(i9d^ zgpF&c4$11k_t<~vLtI(P*ehePeV81*S7e@}S+AQa4MS-xB*UMZqi)oWj=7VadWE8! zL@xq0VFmdNOjSRCmp58er^o0+-@P=NNafr|ms+lGs{@9*0wkL?#0+b+SV{6NBOf;t zCeSz*myFVIl=xteRMc+^>9igcTC?3c)5_`8*QCU)F##+Et0RA$hckpirBsZv#102V z0L+dRl!UJVNNY4<4=A2Y)$9ReZ9mC%gEhf_-v&UEnvJPo>XPf9!=MkqVtkQ7LEpto zMZ&j@!6yCsbuVrBk6GL{CqUig z7q*ikSaVGVt}WdZ47~|%@t?i06z|XoCgxtl+H3~1ur1nz^JFbU&z9R?LuInl`3tKN zA}r~xInj)#-N)RA3NfDrYt=Kr)D11*kCQ^61P{QcEUfu6>^S_y|K}_@ePKC-Npw~$ z9Lrm8t$+y=E?0je))JlhOGFhk4{GNx%}7nbC=~yXB}uLdU@$E!E;r{0JPZ$xu28O% z4mN+!dqtAhq6zP|aiwfJPik==J6s^%iTxLg-Nz7p; zJpeXfKq!Bfzr*en18M7t;oUO-DF=&S&AGPcHw#DSQ;{r@K-(|JFLq&U01LmvOLeW! zHWGU_UGk!{p!^9z*@ANCU`nc)!jf zizRQm|78Q^RsR)NE}xIeu~KClIhh76kHG)#ba1?be&3g0yN<)v1|L_l1|KWl_TL+Z z)Ds(_dZJp8Mu|{=+ppWXy0m-jH$B%MI}n01bvf;}=k2Sj=oxlgH#IrhrAxFskD^ZH zrmH@tc0b3PXZ_dHt~-G&*2}YfFlG@hyG_q;_nu)Z1Ih8DsgD2p@K&GWWFNc(IGN{~XWG@4_OJd2Q@OQVkP4R=7cYX=FPG zH!x$TwW!qG2c>|)5P>3P*TvLyUnFH-MOY-gGMhp2eO^4qb#!PLXUr~@YuR#}Tu$*T=knUxm-sZvCUF*dt-4?S? z*ap5j0Bac6Rn_t6nF&H{375Y4; zHf_3>X&;tBj7HhEd{U}8+AZk45UnF=619iseDxwDp%obDUhk|2abl5RkG3ty7EMbakkZ|(hVZ;XRuJ;KSiH!coI!3yrFZ ziXHX~nd@;Ngkx%ScD1=4_x5l-8UL-1$BO-#U6VKb+2 za}R?d#DTE*r1JDW661&A_>l!sjSsTQt53Mmy;eRD$c!cx+H;fPlJJ3~kv$Wbg{*i|o)HEtQK!Hm=x>7nt;I29j>Zu_L#x*fociznf-sYZ6p3^!2x+Ah~d-w01 zkJ&t4kiJnkeW0@u>e<0m+}YCzIO(t5tSeC~-vMf_E&4v^iD2GD0!>)NG3i~;3_QQOOM0hTF=R)$1R}hRrTN8Q>H%lT>fMn_|B;DeGUATUC)1g zpDgqhXA~Z88RnHNjz=fPczG5@Cqh@Cl-LZ#(X3Gk0|h$L4+H}Ky6&&0p;NjKzUq@w zF`sNCapN~dng;KAP9ntaj)qYDf}&m7+PXsE+H%--uH9}hgQE-VY3oAk`8i1Fy98q+ zbfeKN8L|U^N6}q=T11&s;QF{IMvIgXyl4J*9Bnb%(*^^O2oS8%mi03|34DHL1J* z#~Xt~qd?pQbUL(YBO=}Daj!LA{XxC!;~3ez(39dL&5Xo8hJG`+^X^!5& z^OFHXfI_oiCJ;_~O~8loFj9y;HkYLaV=u76cmkw=5hLPTyw3(KdFV%#T1V?+)D5*a zA!E@qIL3e^qTzgak?&B2=JEA=7fwd*yDs9`v*X=wn-<69xjUjH|7HFO^9kJ0Z|ds$ zOv3851ah@vBVhob_0)^ysd6;eYd7^P3j(>Roig+14VOP^Osx7{EY;&xp{Q1Qt*VuP ze5_55)N{IYZ@kU%-;on%m_e66a770?Mp~?p({pJS9^4xvlfMgkJGz4Na1g|*?p>Eo zl_jV8?%UpZPWe3ugL>FzO!M42n!3nBC$*s=>HE+>S%VM-4yO!_~&UUQBvDsG4T;?8J-+h{6)pnyP@(ph4keS&mUT7hVrwL?rr|z_C*qW-= zSd@JbhMrI|_0fIs=5g>Zo)XnlZq+FITK;cl;w1g_AX$7fp8b$0g$3=P1(+y^cyX5-$vQ|1|h{c@{iyng}H>b6}MDjFvu9f ze;FFG^yuGwimJ?YjJKf43FHB=MudUdaKY`OphVcK0nXs70f$p>l3o!OZOE9yE9pLI z$xEZ+mI*-w$f&<1{z7k;p8Zi4Y>v{1Wp8E{!1zv{$3PcJ88OMmiKFF%_oq5jov(|MkJjB z7r{e0kJ}M60rBjHvXpI19<4OM$Z+?an!yFc>64ZKz?4T{pHlc!zcdwAWu#A=yp6W^ zOhhxqFteH6is+T&aSvA{fodz*q}|ZWbq}B~ueiN7KL;t>x_yC2?mVni`xns#%+&E~ z>Vk?N(Pc1Ucz_tsChpF~)2bf`n%hnNZCr4UC-AIYCT|+-UsKU0FWpJ${5&v%)GBwax->RbO*oB@W zLv$6&NhL0$wZ;KrMvUR0{kH)CI2j6a49!>N%p2Su^cjZ5OpY}5gJtaR%^ynvcZlg! zEh(mSS7IbuU>D@*Fl0x$jAY2M;zuRK~Ieu4xsrYEz{S!5)TsTP1-M?p~NWH)yyCjc)(`Yy~LBq?Lcy7BCx;>+i= z_Zc0v?RQY;geB9lGJB*DxIYk$gPL>pv)Nx)9bT02%p;xp%=If*&sqK*5+}my+S?{B z486}sqp>8x*Tx`A4#}k)_;v&=VHe8Q;iOtZ%E#sTnj4xmdIrHe*O)!zKp+ZOq=G`h z;Zz7^e{pRj2f$T175AtQ5RsJe-I#H!Py}L87^a}1p^e59gChtaYsb}Nv48e2d`-T$m4jBWviza z#7;-i4X>U@|7}1&Aw5os9Jv?J4$Z^ity?9PT=h=SyuSy0>ZTmPM>|A;v!=ldkq!I0 zfXOi!iFUk5jinSB1to_8P;bLJ-I|nJ^bdOH(A>OWEjOje90C zeNC%&&q*n#(jUZSDY7brw+;k6tgH>cc6-=n`O(OQQIH(1YZx_bUDvqYb4 zCqW08A?lL545VKRrSQekm>(5E_CbfTLqkLRMX+VfNKu5hAk3KRJX2gDz~PIYr%EFE z3M!TC(W#YE2oaRQWQ?CRl61xZd0LQsp5X$7(qk;|rp{x?bnGc|MjPduq;+Cy5EQ><@GU1+9)C8_yWZ=+Tcg)0{ClCN-;|2@3Ip}8Ib)&*gWKA!XPF&ALWq0|dS*9`yhfZz&Y|rkD}lta_L2%=x?=W70}m zl-B-pv(Q|vi0ni&iAc}kJTrp6d7;oEWoDY?ra4f4;$P@8u(Dy;#09wVz6C-BfJaa6 z#0rjjX1O*4qY^=iRg9z>d0?Uid&f^P(1rFeL1tq`?N_cu)(kT|QVRgVXAm^HHI^=7 z1Rh%UpaU#@UO?J82qV2ejy1;W3WdaYRc3liy@dDKPi=$4ZsT5hlXatfWBvSGnzSmR zj1!B{nP3~$KO?B&pT1K~=2KjxlfGeQnWB4}>B+z6C^^;qTwK_x$9!0(Otu!$42}xx z5ITKuE|o_X3w#81sfl2v?TE7@z@e06iA_F06)X$6;B0KR?K-Pswidchm+G1l`EX7$ z3|(sYlj|?E!;Z1-T?mHtaM8XaDWTFpWeR3K}nPw16TY^x`{*%g`&bUG@bte`FQK4nx+~eEslw&ikVTl@_rIK`x8#j zdlF29N7e232khmAFW!}y2vrbX9R;`bJ@oHomvuYdJ*$Y;2%^O3RK#lp4 z7RNX(YtE&Bk?*J!@^fR8z7_HtC8!y0Yv-GmYDr=iLt$a0=?9&jVeoMj<7p22;xe(O z1u>CT&_ei8H`ZY8Behb1h+L>TX%QP>-0K#26g4(BQd)z#c*8Cl4*27uY&WeZ*OO-$ zbIrLzV55mBScHM6{8auC5eq|KOr;pUW0*BrU_GAJpSC|P9wbVpQlDl-Q6L}{q(V$2 z0xA^eW=s!%XtjtIrotj30B`jBX40hPk-F(Aq&|bWRTxQ9t6uG#9t|^uUj+W2_-xvF zW{9a!gSypuCmXyndi<-2e{7XyIccQ0E_7390W zXCgTtI9Gd$I;&MoKT0#HRsZ~b3PvhxrLwfKi>f6oYSeMWv=sqm_hZ``Z>L*^Heb$K z;4t!bm-7A5>CR^99`N17X|}kNX%CY;BW?%ASR?S;66p+p{Y=_$7$7pe$)3M@uQrFd zeyNeMZCLe>PR@>az~0glIs@xrP|;kXPaAeJEc#_ooOS2s!ZpTiNmgj1wC*WUG(tCY-JuSv@=z+I;1Q0MqI)N8)(eeO z!yH+nj?qx^u;6mXEw#X&M)tfrFe+gjJ7sQtM4!zIW1un#_t3WVA${0{f{u5xqC>zM zE=e6{_1irsm#iaH@Eg$%Kk6C|Sg=l!8&l#3vWFY97YYQ*1(;Uh@^8aC^E4XCZX}MI zRdxw&TsRh?{+VuxjwRv#v#y4bh;@R#`6uM(yqs9BJ1`7O-G)D24krt6T{hITlZ}n$ zY@p9?3G@(ii%IxH zw8L$dP%>jh4XJL$h8d$##XM>h7DK{qn9a(Y>1f(I+R-~K?9$j~M59yo<1>zr`@hHZ ze}9O`V`bChn5GhRT{({xzY#TCDQ_!3twC@Wm-&ppMXwTB{pH!-b1sM z^9~^*GBK=3Lb)gCHhBOXflrkN0+;Ch^wV#W<89kgY>n+SAFcv%FGY72_8rp}#6H8~ zE-_e->W8S^DTk!zCapi~0 zJyypokIr)6RKhk|&mg%zwx%p@*{)L?r1InR%aZ#kvY4eFma706j=k0Z*x))!!%Sp~snGUwbD8Rjqar6DR?LKFe0~cVz z$LsXOdt3FsJdfYAKl}Ym;_`Z#Q$52sDyqu&%GT{Q0hSwdM=$^+;mlGp75|5YXTzIX^NY0IZr0D z(Og&U6@&A^kmlUnb{#t2DgC2iKQcC%TPY{G6ALK|&E4!51`eFq^6qJZY7E#$4(bFG z1R2sAPBa|z zlW!$5xvBMEIzcW)l}s+fIl%F0a|?EMq-%GJSQ>+36Fy_tj9vWrV#{0Ad)d1%!;nxb zS+7AQN|8W7TX=>0bN((r4jH|Pm zfSOTQ;&iltw6wHkqiOAlec?P#&J9!*-F}=aY-XgwoFR6elGnI#*sa{6ITTE9Ji>rF zz3A0RYW-8DXsNrBjLb*oVd&~Bt4?xQ7qnKv+>}iS(cUI;P*7z*4)&mvhX@ zDq*zoWEt?F&|8b3%2;@+4(miU+qF_P?hVj|#GbnYJ`hseNaXC{>)DAt+I1a$_2ks; zdfze|AjEwg28Rp52|I9OU|KC06|P=-p^=OVum6lBP*gG$=jK*ItG5!7nh;50&LyF8 zCdFRPvifBYP#zDlNRB3*1UcCyyQ>1sK%dzlWk2B*8Y?d>2FDftiqp=g6yx{?W<-Jj zRPAN1d27*$4~izZ4~O~pzQcSQD!m$5*}g*u8OPeN&3T@vK^rYJoe`U{085-0;S5CYb2S`N9M8pOOL2j6j`&?Qb2%-;iP zRyeWX7J5SlAp6WvKI0I>EuFl`%3EM@ReWMR)4kQH_X5 zZ)OdlBIX=KZ?TakB`B+|a+!G9H5S~$xO()P$uy8Xph!~q)z`K8a|opa`T>w_m^2uB zhAz>IAZH$Jo{@bGwcjT)Cu#rX?<=0)@bMOeb<64Uo*S!oZGTLTf0Ht4wYy_0AzHsg zg1Z@E8{_;wTV@x@tlKQX&5>rta77`pRo*=B7gE7%f-2VVD7-jpl?)i@VUe;n@kv{SvJxf_Si^9d*9HXj!#t4{Yc+C&~dHb zI~+kutx3n%k*e0k8`rYXNz+a?EWb8`;gOG3e~VRo;}RA zpufR>VaSOTRhr{w8aqxW#$+iRf=E3x{Gg{F7^22#v#YC(%i);B$qwr1E`e)p@&9LE^1a+SMN631sgCr05dC?8}YvELFaz}2-){0;<)I()wAQgw|zq+ zHdKSJJpCgizCBQWe7P#rS#vdls|WX=a)%R-mfSJJ+NJk|A#;0`r3ewj1Oo#@nW*+3}9IEZ3B zkeE?+tp-JoylAM$#o0wliC@TLedazzorUGBq`XSd@i&WIndrt_=4m(nCXM!&8Jdbr z^v|BfaWGCes;4sKl{}AUb+i%a?2{E56489!%fduXJBa$*<5{DVLjU{2Lmczhh%osH z&N2K?n7=LSf~?EhnDnTV{Fq1-k<{lzKd>{Nj}m0;8*LK_P~tyMH!Pb zL%U24^3VlUGe78sgq^N0^kgXmCAmBF4Jw-otGQ+PbXygtgCnVdyTv`%lx4~IZb(@` zzj39Ac8sfkK8AfD$iG|1jHG`s6!V+HGH|*CKP(UP!4Fk{v=1cBOEl5~m_A=RreU1Q z3cbGT(nrMAzNevDNQd{Ns0f6ded318I&PFD+kIgbTjc>JuGoJr-v8P>!^&~loabmh zwU8opWzSe9tsp1e8Ull@^c$e)khzHUPqIRyuMP#eF#bo5*`b$7&}d6t0eIoqT9T8L z@P4i7N1m(#4sQ`k&oFNbg$YZSs7uDXw8$tB79;mtIL(-XYEdIQ$59E!MI#YC=V#d; zGp`=+v!_eP9QZ{?x|X%>i8$$I4BZ`f<`eSyX=SIC`hNhLn>P|%NVRPQZ?(pK`hsy3 zB|Vnt(Lc06>83J!K5!vRmFr4wcnU*kAV!(I);Y(nQjvill87I}C^Nt7N=nO?6I*M4 zlW3Bboy*95Inih8rNUNyNtaxLtr`@8h|N;bNaX@vV%*Qn%?*i9?g1-@E7iqrWE@R| zfMP{3tYD0Uec*>-QX(s^C|9w)k#uu3@XVmXB!NcBTOojA-Onm*!*~@8_z2KP3k1pc zAxvUvz@tX!UDmwwyY=!7_lPW*u(#~`&;w6kgk(cSy^ay7B)!dN^&bN^x20wIa z+#W5@Z#0vLomdluk~LL`>P_o!r)2q&Ll%eQag=HfcaKEQ!|NC1N z&369Gb^EGWJXwcewiHr4Vx5iTNykOTY2?CImy?!uZvTuWV>N;}>SbCc$uwDZJI+R1 zlwh@9asFG+sbNXOyd8|Nh~{UEjCVr$!1d>Yf(Kh$g;#!)H1Dl9(|<)?S?T<=aSEVi zu-_Uu6>0z>iho-Rh20Fa4~-wc(zVKO{VE2p5MOzg%J|XV!DDgeR`{nEBLl;Cv`=Ui z_L<`(J9DNusk`PS@%8E|`{_FqpwEWWz~^YTwA0+3+!?)mj-JnXvbhU)ZVu2jUNAG4 zF%`6S@nW7>QYKgT`QdlNhSiuQcU5RF$SVo~i2k{*PXhGs()|-?4DX!8OZ^MNdKu{C z@m8cJK=w(?v(;u*8^lU`(RV zkzpm>H@}AyCJPiCGnfB~;BudQ%J}Y1LR9ceq;H0zWjQzD%8f+24V!tLNrf!&mbSJd z72Gz4qJ$?V@UTfa4v5hZj6zuv0QBnU3^5vRZBF0i5Pi+icRGL(cjQFu(;=SyCbT1c zn{OA%)kwk^17j&u*a59?5m4X zG4?viO+Z~@E@RzbYPimO?)@Hn)MCDDU&OnEyN%np4@R#3EZam_V<=a11QVYWbFg$L zpb)PaN8VuMuCkAC@khH}6k9xDmI_P!0P+}vL@ukm2O;GZM$Educs>Z0oj`~ z2M@+IZsl_Zg}DYAiL;aHK6qUaw}P8aGa%WU&N|OA~;0pc`FOgp`S)2WOyvWeTa zu130xs%e(LkIadkqE+61t?FqBsMO>zkGp#?%wNv~PIz-ECq zRM&EQJYEnEd42}Yp7eG?k5V~FZVsU?2%6M>omZ^$^+b^ZW4$1EqG+r@O-?R}rJTNl z@eQH#h-jY{z14}n@L{ZPNvr;e=cWXH{zQDaYP(n(*eHEAelL3>3$s{+lwqQalpmC0 zeJF@Z>;1aEOb0IjFNRWA4+^P{|+ir0#Gd!}q2uzo*eDgMu`19=?+_GQuxx6l1z>!A z$EU6K*LfpUb(M-uM|bLZwS5Ta^`M<1&|+B5Kc4Vgqy_VPvT8;f5C7yCcfwV&XMtZG zJ`?|)>b!)P2^}7qKoQwxM?OX>AECpbu9`CKQo;{-e)`Dh)G?g#2|7Z|EkDhxUjr%+ z%&%Bq)-Zj1hY^H#7|YEi&GszE@(E*aFFPnpqv|np)BPzVIPc zg}91W+OVJa9VnIY!z&Lz_YjZanOm}BR6#Bk7XMgl(cA>;8)O*t?M34zZd&=+ja3#% zoY)G{jK8ch_pT>$Wg#iCRG##_6O$^je?1c6u?>5|3f1QR67m;OA<|uK&}-Dp7!jS8 zl5(Y*&M1G)$>g*l0$W?#!2E1W-OT|NN@R?uAIcC6E`g}y@`X;izp!Q}CUqs-Qwshq zpNI}#QR7W^3*_YHU!d_{#?B@AgY(X%?OKf>@&(_(qbVg%?A^ZENqy4DCRr$#QYDl1 zZ%rpWai@Y#&uVbT#~=l7QMwh2sdt#3MeMZbb^)#CiUq{3Pt5i=zZab9tZ*Lkh)76= z&vIXwwlQ$&F~m`0vi_>pEf^agBcEkU9l(32T;ljbrA$>?8=Y<{+lJ7f!1{>WMta1y z*Sc0SLN^h>|J}+_2!fesVubtW|G0pu(hIH3eWSJ4ykjY`gvUgg`>yn)i;#+|>aEr( zF`Q!kGL1^YOYif-Za7ohsK!A(WY<*G$X{lw?W>SPc@rg8>7zi7_7c-~a#qWjc*=dN z8gUSzx3jG+nq+FIeI!oWId4=5WZiYZiJhW0i{?-30L7G>@=FTQRoe;1sjsU1oOIwt z0D%zZ`?DAz)wud9_gs#BxfD^l#I?)}PLVCfcp==(4xqY8X+|Skzo?ojC0`vITT17w$0m7xOSo~aK>~}GZ28y=NXANe_`ReEZ&Yea2oarZj- zL(z(A7)KzO6)~otf`C?IXAF|k#E@=qJd@#E&BDmJu9}wr8cCnwi?*vEcsO9zYAIr6 zUNbP%Z?d}Kj0$aYXe{0Rf#%?8003ZO4h|?L`I>JS^F-4SlzkFyT}e9Dyyv*FGbS}+ zRAdj4)K=K4WwCn0sY2&(d};ia{J8#$J9DF59JW1^R#dJg-EcnnsmLhM;LaQ zZ77d%a+0#Gd!;YqWc~#csr489QE_Mlw`)Qp{)Km&615u-aNxjk)W+1@?(z@+%8mx? z`D8|uxPH-4bE1rmAbE_PcYE@J-VfKuzPb*D9KX+pa29-LkE4u-Q_rc-(srvV&;T57 z4JP4GTSM9_4J@DbvP$ZTt7ukS>!dr!(m@$IMaT0WVIFRAlWm)4g>91cm%q}S=jMoy zEr)tNa=kDD2z5RYZnp`8WeH>0Z(G$EadhJE>L*oFg4CpHSdEfq`)H#H5PmeUn?9e{Z4FP0c-SVTCA&SGXLLqCtU7-coQ4@gB1_t3#H#Wf0U-*?G|#eh$KYOyuvY`mbv8|Avy5HrQ_Bz24zi9ef!;VVnm>LQ5dhS^9@2?Gc z>_b*t9NvPtzebLJSET*R8m<<#SYfqvk+cmv0qeX(BoFn2RaXjJ-XzgmtzpN?y*@Q? zu;${Jh821K_?A1tj-^f4^_)mZRmm2cMU&@J$`WJ3s`2~hHQESS;tYLK%`X(YQnsg* zNgT{{bSYHGBoWjM)ZbrOS^41Lpj0_Wo_WKE(UT&84)38@={iT=rOFT61p_`Wdb2wG z(;-XSwMwbhoTD$&m}=G4sz;N`QFBn82e14k0zp#qFvLZAk59P0@PPj zpGEl?vA~pPP;=s-`rE;DA791>p7^`A1m06q^^pqr&FD0S?{ca?NJE1aO!IpjQIU=m z;77->Y&u6AJ2|M91JAyf3Ev*h)4h~tbg;0^>c*F?m8pMibK5mZg$HLKg`=|xFt4y= zi3(T{xCShf8)V}9(JqeZ{A$@WMo)7^KB}5c$E$ejQ)_&ik#(H!qPK1(r*Qp7&EucfYE%IK$eR6qIhxaa`sTU<)22u6ScVRj#P1#O{$G{^Q61 zsyt^os}}egUG&~CS7kCAF&f#FIP%=Gn_9anKY4+5R$B6`9;H7+6y}6zG)x>Ehgfs4 zSXTzACCGhvzAI;v#((nN;S0H%S$>03Xf@5+SNOWJEXLxwMPkFeM@J%dULf}2y<4ZW zf`WpCg!&2*2bP3uL7*rg+a<>wIL4Osu(u^$Ki$pLpdwDzCU@!uSp~nN4+AKp=dM)+ zbr9G|$BU)os{^o~8I7?sRU^A7Iq%;kc(^gO0c?}A3tTEVb3xf$8AG9{uYpI1@n+nM z#_@#X1!XAT;XrxDT=`~_dY4{4RwOm&b}+?&ZEAjWinR5{6p z<7W&d&R3HW$*_o046%}R#gqoLK}eM9DfT(P`JYyCe`C@=bJq~q=Q&nsReNVC7ST>* zKEs}*kc{-Tl^xQ^GS0t}3etJ;$Rs1XV^bW?G|kNCO8kLoF zc2uNDolWCm?Y??v6*p?+vAkvw%bwJ@zc5YZ@c^#v3ZAMePl>t8s`M}vs{3n8_0i>v zfT~%ufp>%`gX{BGj(=e`y?<|oN=d`jv^2f2BpM_He*6q*-?qU@qc3u4vYk(Bo#kTp z9NjF&IDfG-f4m%(%{$yKL> z_L{KYX)r|{2cpEEEZ?-pvBV@)e>y@U7tW-EiZ+y!quY+CPG&Fq4Xal>PhG9^=mHC+3a5ARk58|P%+4L#4K*k zxSE5X)T3UDcp$i)Xt3HNlN;l$!}?sB_nr;W&)xtr0a$U07&=B^P`=9$pe!loR^3BaaK5b7M5*|1~$5It-Lkyt7`;^<#8S zh_NL;iH>vaj~zrkNIPqs9Tq3$CYp!_!@Ivy7`$hReK;ETmVsX$Gjxe=DcUgOtMo=`J;tCbK(N>xU z*MRiW!O55lR`6at>qY&uT^`S9w0CI0PGLwPFI* zGu3s|RIj=0%Ue3Z&p9PiWrOSkEr)XGr9@RuG||2Yt@2TRi>Od3{I#_u(a9-2j z-cR<=?HMJQ!hVEr(KqR0!biD9+g%B6SvwRNG_S(Avew%IpZ8dCXFD95h>E5k;|wJ3zKDqAWKCU+&|#}b z##lZFT_zcqlKui@8_6VC2)*Q@XfUU;Lui@Tzp7|}ah`H3Jr?L{9kU8Kqjai-^$;qC zk!&B)Cn0^cosW10DLG~KG!-+K*yQq&k3wDa#*`NR1DJd{iLr+BI-DbqpcNc0+EDdu zEk_@p`2Cl*nB+lf--}f?b8t5pe45d|v1GI}bG|+*BlO(GP97sq6Y2z5)u0>I%>-b+ z!Dv6h<74%&4bDOqBsH=6IGuRBENdc_65yGk!wWIF33n%+kDgQ$aS59Hq$O%>LPWXgSx;hVxxsx4S!dUdk*?=2+$ECrM~WPaq)3^?D9k< zJ#kYeW$=@Y`BEFzxTG(6=1=!oKz}#_z#CK#MyH08QwVZiEb>y{!1|J|kXN_pe&m>~ z+!&NE`*&7GT@{F$ws{Yq6MjH*uqAy^9Q-<2S9xF1phER!;OkQ@k7}C!r*kQ>yNV1pUP z#Qvv3Jq;&P)VZc~8po|T2{9GNIt&AMVD6hTnbx$ekNg?(O~NUA(w(S_vUI_Clb<&t zSn#LHaxOdQ$TfA9wqCz$%y#WEmAN-dgoSE=5{Zl$DX?8T*#Z(%0!z2~TQ@ zeGVzLjuZ!5BPxtObkVqRp8|4)3w#m7P71b#i&I;RPuOzB8BBDpJ<@Uvzq9>h^uKda zdVU0N&o1>Xh9F!%s#K_cCPoxkH;Z$#(y(z;XmOrPRcPfOx5(RLsY}zC+QhcLA2jK; zW5T`E>{||a(tYXk-#KMOjn!W4t|UrvKjKpvQM@BS7JpTRckXL4*q&bU$7cxr!Lh_i z*Wm!!a-6BUYI6O6gVCdA;3@R?-@d$eT~q_%xPOSJC3!pI>Dz90*pZW?_ewBBn$9== zkGHkJ;wSH*7wT{E6Gw&2zC#%n#MuqS1Y@<7QZ>lJBOjuob3U$@gDR)Iu}zlLc=4am zb{u_64I8o)YasNGU3@&j`XuU|pUKlzQswhr8jHPr9Ur2ut8ma0*Pc<-aS6+4v$A== zbZ!M(^+@eZc{jt<7kR$y7A_vr0$ADEp%Swc#Ops zZVUYaBHyZMD5e_q!3+c^}|ZZye$Yrycw(DYqZTq#lmxDA>i%?DwYhu9KDb zuq6)wRws1|HC;{2WTcfSe#k$3w@Q5BK9vg^LS(nE7uWt@qc-rL!g#A9HfkMV&a!?X zPz*w%=Us3`x<6YO254)v2$X*3F2&+drCYxo%&^mgJb9KECh)|oi<>e%qLGa z`aOgSek%#m<>N|CLNstzFhqUq5rqn5X)jpq9G)%R5HEU)ByG<;HEeFYAD$IR<=P*L z2fYHb%RXjZvD$44E0uWt)bTW<_~iBVjzeJFyelcufoNv8%=eE^;K z)&l4uM!IXRRrZJ!n0-&$B=OZJ$m94t3jD@SBWc=95Odk}ZN27A0yrY7QFq@nL001OXF`Dtozi)E62`sR!GJCC@`c zoudRZi-6qYO`}AIBc0L=a@-4IdL@1VSwaQ5as5$4vn|~wv1NZxd3_ng$KR%|Odjk7 zn}2nEZ^XIj*pXQ)chU9c_vJ2gh=_mRY?k+AZ(@(wf#6su!S(-^nRbzaC=^aSXk>ab z)J96hC}!V(YDoA67UiX4ffZ+UXkITs&NDNWJR{NNwQWgTqv5_;(^ujBuWtNk1nidg zJ!fOfZ`k5~N@j@9)kFYyi;NpUu+H)#$1YkHLm!CEDaQJU3lb z2}qeZU`459%r%Ct@)IR|M+IjSGi!|+RVYOj!(%RUzfzjKsb3qUi*CRPV35jZ)rAqvE92kC9jvyVrdQ7CIj5nl+>Q7)VNeM}s*BS@xl85bgt`VsQr$1p^VbgI^)QW}-#ORm3 zrVT+aV@Q-TV%5jV_e3Ax(75U9$CLXm=*~dRs4`yg@(bU4mfB!pM-Qzb1~GzXu)%b?j=_dexf3mF z&x<##Mf&I;NeGEOc6vj8bj^l?7MHe#t0-!@_*<16j%zn>W6~ zb^U3AqFE!NO4GcJcb8$2UxmWwB8Le`3bj2_mic~Gl;!Gku*8);Um+fKZ z$JK;)_^_&?k(p`%AJ^l%8mdGG?k{PEhVb;NnW{&{!AC-$BkjYX-z9{@c}-Lib}rTB zMGk#RFzDGieAm`g$bk0Uz`?z2txdz#wod^Cy?6xah3}?abZZcunY&-k0gX8%Pa_sQ z3cj+BF_;3nbJ7e>lHxY%lsj#|MVDly^H-4+z_i~`SXczVz|bFDB^Xztznm+R!v4je z2GkzaiBhhaXg?!fX<8vnoFhV(v<5%Bc^05VZTRU7ViD@NqDDv1M=q9%%mlcBgAB)v zW>h!Oz`@nzK2)Q(S1oSmGmKK#(z<>Mp;8f7YWM3DvWsx<9?pST$NU`Fb< zpy)a=!yhE1`iq@}EFY$^0>+-$8L`vO6W+JY_uz0F9+4Tim=bM;*L%7sayZ7cZnS7V zUp9XBgdGr1+DjltTmEMDbOMi*;gENAlm4fWqyO&1ltL`+Dm#q{VUd-?SQc_vg-7>n zoE)*3ct#1P!tghD3@H95^?*?xYp?OD6N-llHpM$Q`qk>1xgKXR$K3nOu`lUNozNQg$RGUw-&hVls;~Oo zLQSf+)cSLc9$$*fcfwQxL24&cSy@9Q9ZTEUa%o0LL&vo7YK(Tkcfd`<@})W=jq|4b zSUcHv|6#o)?B5X!wzchVSCT!=mxW@CerEW%B#Vs~vVo+4cZDbw&36bXoFq00Sj z@!iBt0DQw7_1U^{5F)WlkzCg1B5@);;VQqU#Ig3`v3G=A^|y_{U=Bs=hFw~$FeM(g z?rRs7fw%n6yU-5atQP%5)1Ck`WofW|BWFyf`IZnm^G(K{=O$ecm%#Bir{##qKl>0k zA4@=RBzpztUzNkK$mRqRttO9L-2!9l!7pC_!%2%YsGB!C{qPZJA;9INKV;k4U#1+N zF=K1ENE;dH*w_R@i(@24T4nthLb)}f_VmPMWoJ5e_uBuq0JLo`ET1e%ZY`xBQgfEe zKWuKa@}s%tC|CJvgyJD>1p19FXK~`oWDR;qyRvaUd`21nfT#@8-mwv=X#0x{`kP*+ zG|P9iWwjRmAsZo^pR*~(OpCj=#a~zR=gIpvWVJMM%shz|ByI^#YxpdOoTQ7Yh}d+9 zGQbOeFas^QIkJLL5-4#wIaCO)E!RUM%`j1h&xGn|u>hZ`Y6i72GUgAEM?9I?Cc12s zASBf%fCo`XphipT>kc&fRGVVw*}_6UPV;|U0IJCTBDTLmClb$h-te}I6`GEU=3#G3 zqf3B;+CC47%f=~T8S>D4P_BtUHh9AAt!O~PWa{2Pf#?Ao?CSj;RwK6L{sO;$u-q2F z80Xtzo$DYd1J5Bo6}T%vr0(ien*XPU|FtT1oF!|H{+g+`(TKImov_|;+8A5lYY(F2xTdG?&(#!p(GxCtv1f>SxmJH=;%RL~Z9Gi?>Q~rh8 z-Wu@>7*H6s!|(0B*O$I$e?S(ug+TaORn6lIF+YN0) z&u=gmJ3AlSQga@yi(spNJaRpuok@m!buJ0#?$y_s=!3uZ!zT7UgsJ?Q`OYE*^|v*ZZ9H24MBcHy4Xl&?DsaE_NA-y%i^gYlzUC5{L%|T2Twd&{OQN>0F;QkVXE48-x>hGUTBH2=5$a?h{GRRwTQ zf(@>RX&!OI6P@~f%Mj)vw%uepDfO}CB4xVcHo=V&h!S|=oZ@N zx2XWcO=_{54Q9%NUEfOubq3|LOmV1O#NZGEYdDfO5Ye@{EUXrV!cv_(4gX`O>EcjL z5)0ThHKrfl*gDxT$ZTAF%F_7A@1??>J!6qzLVXH#w1sPcI@zWODeMn$@1L!;I}4t| zEn(9?G3vOD+afcH+P!#(y^ET=Ed^;equQBhQm<U-mvkz+;w*>?*Q(XLnV{= z>0Xs=WN)7$4MH~6Dw>%qlHVoog=G-R*?-Jrd6Td8b)w=n!6AkjW}eCGJjXoN~d9M=DW`3 zgW@_fco7qEs@AtroC{%6@=MIXSM(r~j%&m=$nM5G(w z_(YnT<2lg^S9-l6|p3|rF zvJ7=xe|9yhEI+@U^JU+OPY}>4T2H2K+lEGiG9EsqV5{$+iw~AnT=InJhY%Eh=F{2; za__Vcp)n#pZh37$%fO#yJJ&qG#j)n+<0JVBjohK#rg63Y+M=k$=|AN{Ar)!PwB|}^ zeCil#i(G3|E{3(m{B)8IYHzOocMC{>v`{Z>Xo z{KhWEcFobg^w2JD3J!PTE2Ubl`CB#GU62UFS+sFoaBPOoQ-m<_SrMS|wVpUH8ACPet{Pr3IEQdaZPn2l-;oksXR(lA?WL+a7Y)7VZZ4+|h^kOn; z9)^VEMm7CnG?nzKqZ^m0Bi{g#(OTkZ2h8 zBc~R%Gf@35jiN~DU1=l5M|U!NTImk13($#FwP+VQ_ChQZ&X?;YlAcv@u;Hiqe190q zvo?fmo+r{=4(Vz8LBKJNwLwKA03Yn2wdN(6YHwX`%v4yovF;&W8DZ2s_gSBS8oDyL zCmMa*Nu+3R9D@%FA6{X@eY0M}-$DuhufELGag@Chi%M*SQSe|hHr{jNy^v`}cNI%< z1}sjKK`G#5lU)I-$MaLTs_XGfaq@$%WFL81^%QwreIBgc54A@~J#|x~^-e(D>{Y>KL%<@x&IT-%p!G@>h>dgL9 z6AD+^Q=^ee$JBJQ8o$5{7bc> zPkK9cX}2*8*)9&hhMHvV$ZgpRR?(H)Y<{;z)+xIymS5hHXOk*gk4rDBgk=l?Ryfqj zQ8f`GHNG@TkIg=I!-wob1)X$A29#u@Ag@R+^bgHa*Q>{%@6QcF?L(?^^!p(-aI$ea zVB)CZ+7k0ms;m*qu@I~?+z0g)AxW`4rm^SD31U@<@E*`Bhq`6A3e#r{3^7qDp|4=D zvV;ep@iDcyxH!j+x2DQB?JlM0FREm8$`q-d4Y*|Satvn!cgL5E7Hbwv)O}2sTqd2; zQuKE}@Px5niw$5+YTz2$CPU?}SSay~wb4f>IjxZ6xuvtnWZbo+ka`Z3pYFtkf|InG1w>Nu(4werM(N-#oalI^81?-!~jYOJ?^O+Rqh zf7QZFH|#+h#vViGiqa=$g>MypeOj1_r#$=|RnC3HpJ$~#17n1F40joZ?2fcXs~k#6 zN){GJyohwFzWEg_`*tW+b8F1;g!rtF+Qw7rzC6=(kNaHNp=$ zkN7#9V$Rx>tUIx@z{qc(WRmTKW<;x9g2a3;J^OPt6z$8^$&99}E;swg< z&osh3{=7)0{V>V)92d1d?UvC~hpcR2eMVmq&;l=l)x5#Mz}DkhrL2hwe_kWr(NC@O z?vt>E2;%2a79=X%4P=TpE*yP}ld4#mwehFO76>x_#7Lvgah$yu#MbbIIuWYMl=oGt z@)u}4&&3Cwh+uX)Dg9GGOnYr{vWNKS3kFB zO&QPRk2J8Y^?Q?T-#8?BS!~%2b=3*UaxQ8D1Fbzg)h zvlA9myJtInKB>XbCnUN&%n8i~GnaT*Cc@p^+#Ib9bEFc1K$MlSv}ND{&~K$B0Vk*d zHMr1(a23rjO9(xU3_g$Cd$A$mGQ>{75BbZvL_T$o;kaAwL+RRAHq4Px=?tQ$U`2GN zt#!Q%wqa9V;94SlulyC>C6O@gHztzY`4WUjiHj>hI1-x&$o@(2UK(gaSWKiIm$k;? zzr}uSZhGyZo-8JDZdR@-6`0s$B=CNQlLI^Ai6WHZkEEWir}RZbJ4!=RM`=b7AWs5_yEKPskTV|<#slajys&}}l11j$`omV?DVZ(# zWs>JA@eR*!d(x?L4hyI`I^wdQy_XjuaWr`dD>0Y`P*mu1D$Mb$%_yEW3%>1tFOxVkyN&d&uQoOx_4C!Ew^Uc&x^UPl| ztaO5T=}B1Qk2wZJe4U^Iptqh>UfFPb8om#*$V(I5cd>{UshlfLN zK=bS}0+%fOG=POe6MM+iJbs*M^u#_Y-^$=n2?zH#j{Y~ES-uYHDHZD~UoOu~(YYKb z>4CUBq_y&_%+wNhUF6Varefwbr3@ZMfkUEv-8KD03lU8HN-W%5^aipf*!o&M|IOm+ zQnT7WSlM4X$q+i6h?>LbWpW%>OHUQl+)p)*+Jch=g~D3*T+ETK#ngfh3;i#EI&@xc zZp46QPh_*YIooD0(nBN9AR`g-f^>ZZvP#UB_AC^B{phpoV!mJ-sd=nmM2pNvdBRjB zE>f|fBlzELU*;QnlW0h>?N;6@^wYYs2dULq(bJj3r@aRI`%7T$7yqO*DM!x7TK~^# z4`5P6i2+{WI)aLyVb2i$IIGEcR@Q6ZjY}V8V{mckUaFPFMrD&%<(w#x(`!*BSOQ(`_<VX2Gacq{Fm_w|cILL?q{h|nm0_f>MvE~(+LMXq`Npof1~Wb^tg3gwOgq5I_M6Bd z1VUerBkdgj&8qnG4$ZZY@9KC6TaM6iH)kN(xLroBQj6*@0tQwB z%*7m$669v6Rv1JKQUSMsgH3->y_o?;nVPBSmP%=3%pV)%HMp8AdtZ_AtQyT<7O%4( zQhM(+Zu;%II7A)q(274OWbpD`E@)pS zXE3_A2LH|wnl)n^>q)laF@lq%Ope8#PckW$N@qY1xlW4C5la0t6jG8q)6QN|SSrV^ zQJo-cl9h?7%{-ha^~mp1adf^J)t$%7t5fAVv!>ELFd2II__N**}Nwe*{DjrGYAYDa>?HmxW zF5{|o;+7+%h)#7a#mUZmFTnX&jV4YsnlD3LYWC~1fQ{899bu(j3X#cdSh(ysnv8b$ zR}G6^(abwJGuG18pcH;<;We2(vN3i_6Us$}*w2i}*!Nf&lSn_Pu}BvdT3nC5{k%vj zr5{vcDbR-HJIuECaAf9yi*^dkac-Q|F))(dj>0*ne6_7z=71=5U(DC!+TCe%r3amr+_nKeGdN>_!Xo?Wb-vncHF0#r_<@FoW-vRnLbmX*WXT4B%cC58H4$c7 zs|52a@_0`@=)_6{hCdh{eBM(K`&X|6DnhY8cP*5Zo9QauN+5y<{|gL=p2*FAq(&$jI4gSoEa;d z6nmX=Em$?d6-=_7XywDP79#+{@6bm4g-ikmEg3BPeyWO zz_O8TZ6U&vyRrtqd5#jDPk))@gx0xnDXJ!vRsO@UL$B>?fNkmSY9PWlnBQZ7D=Rko zD`-_juV6s?A~Bm+u7g-u?1ddxNzqVH^ifMizhoXfK8wcH;#jL4-6Urw@P!Qf6;9ZI z?{dFUd`JI+Ilh{&#cU$R5v9z=g6Kq-YcZK(G+&4?7A8pV-?Vvob=?v@x`QQV z2PZ_XXxXqor>p`hfw&*>j_Ch;JQ4d`Bucd0;G(4o%70}WYpV`_UW(Y{P$V(?!$*=7 z4hK7HkQpai%y)mW;lHGB1KlqZ2Dnwo-_4HD2+Qe96OFNtX3M#tyR1~gVC9c!$655>_Gd~s+S{YWcMkoLktx|4!H5$jQ?d$rcE9`;(KC^YrAa*b4*Jx93uyC(xv)m3!VduRKL$ZHT|vi~z0*-hDuXEAy+ z;}z>l#zzL$5|Q@mhP)3I9Ocx1fP&S1bK^;-gG@t^IYt#1%Fy-8I`aR%1%vwG*-~Wyc?WFoCj<#iE>AbGgv~&ko14@l+ z151izEIcAQ&W+ukce@}1W$4dw&a>2_%{mR8=yHmzwl$(1A7;TYJDxrjl!nZs#v&=_ zq&*cfyH>3Kxlb82i}^mW{6kV!!_>v=y^qS|Tn|Z_W{H>UC#qEMSPWo&(9 z9Uta;*1=0?R`R?r6{L^KatOlP@OT`dB4&lq{mEf)WWruR22wPd2)vazCtFIGCN5LI z$-Tl+tYni~=_y}cq7-GfDf2#b_7U;tI38j%TPQ0lIisjqXqhvm)`KT3*wVrfo*lL1 zFad14Pzps*S4s+YTIkTafHbp=eh(uuU?NIsZdG$cMB~N0T?NF9 zpm>le#VstYtI(NzX^?Zap(k*IN;QqY=i)Co)KzR0Sj@U5i+Nf8`|`;Ktoaj4qng$q zBh@16IyxuWB&4LutyF4)5KCZ|7%eQy`W((=lvGrbl9Q9s(9;Osw-lVzJ6xp?^&pE5 z`L~N+INU9L6z$f{@I?U;Z-;#Tm73)?AqD43xABgjD&aHyTUnc{)Q}*pn_G}qWs_QD z#drCy0bFJBi9HxzF?QyXh!{db8LRzeio8TpFquc2?P<}Q{s)Tu;dimqRi5XQoQksD zO@zTztW~rradQ^TV~$aWblsS)Raul6A4sX?-;p7zGsPX_?g&6F+T+PI1COtMf|L*s zkEJb!Ac37#N6koPzko}BQe-5&hDq%qrV6wOg=3%8nwzcZKQ6@=hdvp+(0o*<7 zPvpU&bI;R(L7NIF8jbp_ny9qQA$bE)$w*C>wl3i1NMXWY&NXIQqi;1OiB|ZljD9NsPF6&g!uTp zfK#!+_aH~u>ny~{Ut7XT9 z>Px#eZP70%aL}q1NjMt?OWA!R(jZHnLsFy@U`UVE@sZ)ezr|5V_bLCX)YYzyubUY& zT`#)mXPL#|LLb`Iw4~qMOo%m%*WZ7{O|40lE2tz!u*WY@WlAwGjB>kJNT6Yg`ZW!% zMWaThMjMbcgjR(RojqRd!c0*k0Q_KdbXy=&A+0Ln?eKcLjDI`sfi0z_ z#u~LNDK=(=68^`(ydr}WUlX7Xw+NKdGkvnxF6o|PG9$4Q?zG~R4B>t52?S4z zLtcQaE?Axl`uD{K+-^A7s??uLy4twxZotaqCpw!GDBmKPhn#@nK+!a;lqyxU3(eQ0 zAkSg(^m1I3oIjA@+#54*2nBw7Tc`kbJWg>SQFcsNIkUy#lCP~VZDs6cqSmNa2~)4o z-HRs1r*!p{gNY{P3Z$Ydn_4rO8QJAx+l>73Y!FL z*?W9!rzQh?1znV`xioTCuP{xGRu0Mld00}Xvy)>H9f?@Eo8?NtD#iQuIr}{0x|dRc zv2CFmNoZ&&qmy86q~2K5E5gnc9sed)C9JY0N&nK9AJ5#8rG{CZeLyWa+||3Gc9%%$ z7Z^ogNtI3<{A{R>Ua6?o3wlt}C^}JCU9OY*txx3Y@>CTG#x5 z%49Wi&H1O3v)9|B+mnTpPhuBC{=>WTHt6IOjFTVqS6`TBSbGM)jsIHU1uG-yV@C@8 zPAFc=hN{Tc>=dorCP_xT{!hN^sz?1#ikD)_FOUS$Q*MxVw6RVYC_}y6$eAW`W z3-uRzN;G%Tz758`PTuTNFKnYRQ^yybBS(&)H~WC;{E8S_9GOQc8r91M1EBP=(;{f+ zLM2}-IF6b=3=rHQ_%bWod(-8V#Eid=Zx@>|%FK()GIaeK7qvPJ-5awjrZ$G3 zbDqm8=4tAR{{0@AD7%9l|4_;4-0g<~GC0ZeHP*atpugw;umHLBRLf|dBef<$+gz@N zT%|=?Wrm0`CRC$A^~9+>e%KAD8$8ld--I(^Mi6p{qXeT1F1)^$pw(b@Q+EoivN1BQ zz(=*2tKAXF?XK7Rwb1PlddKxfH(=c8gNN;wYJ-=A?vYZ++ptBAIQL~&A!w&S>LxSw z(hGr$zAcTNJ&`3Y@F6t7)g1e<$`Qdy9H%COPHzy^>L~7Nwt}-cG1-|wI`S+BvG_7l zxkow#nTs94Fb%pvZy6%nq~3 zTzE9#??@KGgr^lU{BHi&FC*9EOsR_~R|*L)(*eBq zt5oE-!j|2{4!2@#?7}Kbjh{YQ8nT#z9Pm)YW}O9M-BD8EH!y#rEx+zP4@M_TjBt0s zw!oMYT?X8spA7yUizVeWQMNRRvWv1s(~uywn6*p`MBbS(k}CY>Shea|_*arK$p}XBO^>xC&CJvggRg<6Pv<5HUm2E4IsB+VPRo7Cj7?Ok*f2P;2@QRQTPrGM-^rY z?(q0n$Z)zBhaEW8GSyqFM81i4hc8r09*?R$1t=^q;r;KzHg5j?C$5opBGlFMzf1E~ zq#lBL-$Ud(lgfF&+3nBoxB`Xh=WfC2;At>&!y50oImn!AlcR96c~Feq;WR`i6VYb+Fp6J ze>}rzZ&Fpsj?#qdDr-Xn-K3|*dDZ$6`K_pTBs@wqd8c*n(BV4ncglw^)F+Ff7kJCa;!qK2_gkw_So!pt1>mMfT6mh=Y0fRH8Raiz^+ z`8M|0ubWdyND*0}gcg@XqOrrvAdrg=R70D?S*e!&Ysv@XTA0}}^qZu-b%pt(!KDJ{ zbyKW>mEP5@%p@B%?cK0%CMSjk5gnNhB59y>hspp74~(bwwca1#_ E1N}I4rWD9- zr){<{E8im1yighFa=wXI^)DXL*fy4ypcTLnVoCWBY4*#caJL%<7Rd6%)fYwQQ})!T zJLTbAar<`&>4^dl#HY~A-E#>Ox9D^wE-4|`2}k((i`-0Jjht9&l)4SG(4hN2HsNn+ z_@eH2lSFk9_HD$=`dVDIQ!ZsX5N@li)z8Q4k_`L!8!;c&S{~GvN;%D`8OpNL@^1$c zj8wN@qnf?@;4LHO?0E+wnRIn19uZ`In%lMgZ7Mc04^{1+DqHfs#5S9p%ck|tt#er3 zc47w6rmW;lk(}J4qA)ldp*i^35iFe{d~76nQnLNwl-b3Gb7eY>XKvbdN-9TF@iKI+ zmtxK4$$=b5@tCAjrXa{})(oQoi-bW*X1Y|`Se(8NSIHYkqS#C{9mGrwb=65<&Ik`K z4v~?|Q-%lROd-o(+(d|#xnBq9iYI4hDP^G)L=}DnTbH((?@%joeyoU~mK>d8Nv9_j z+m)#4qligoV=6WOimM&1)=r+l3tx+hKODm{Rw6SWEn9JPzmP0+MV%QxceH+^syiRy z{k9Pnjj(|U`!k*K?0b9tmj;)G%0z&o^!%Xrmrd`aeyGEWEbN#-h8~X-X1aOV zR3D8jR4*G&i%8Jkg6Z!$jMOhS;aeOxkyX1SZcI|2)4Z)m6AJKSaMvr@{TnR!LHzYt z_4RaQD;yyN1!0z{S z*z3CSS={P%yCv(DKS1nCL6z74?FwXabK3j%I3jj|{IKrd@ag(D;*x4$kqt4MSxoCn9<+V(A_A>x&PDB#S?XdQVSH)84BpIR^}+6+VSN zbVc?I_#r6vye2{>b)Z!Jo^RfRe??C&Gl=iPi@$90gSuV`925VQFQApDsS~Xx#AQ?{ z364I(jQ6E>|7e_bl$nPUX7t1L4ShDGfTGD?|AL`eFL2vC1sJ_n5O~RgV5=1)|xCj_Tm^i zYLt`I+YIjx#gW>HEGLbRmBcvy%Fi{an(s8V;N^+p&1g7%+0***JHPmnP-F1Od-RM0 z#T|E;vwmg{TL~oU)=bpb(nEStJ-J<)b`@`nlnculkeTrlB{&Q} z$jBkdG}<~V)2msfFUn8rlVk1|;nqqBq*)WPsriL>5UO!t6bni?!+nE&!^b}e6o-N_ z_wlhcHC6kVFuN!n_YA#ZWbM68E?8^ID8qytrr1r@iB@B1lkKTP`~~-;Yo}isbiG}q zS-lP!F*!L22@5{Vu`^|)tT}vSQ{m3}AJsfH-)D7XJVfM<1T~)55Z>Z1;-g)XR4~)_ z`MjZC*(vih`1*3Ftel>&k zxz$BERZphKPT7dc)SSuzQ?AG$-Zs01y{K56g^|-cp8}qR8-0z}H}jq!v%CkoKnM;06eJo#4+ zGKD6R@UI_s-4w$nBqfU8HzoHRSLT@>0>df=cx6Q7DD~t0nV^MPkQunWUi|Uz;zREi zyo>*7-f`XOx1VpBhAzI>X+hyVJtxT!q&L&uSBl4N@!tEpMz^=8hwfYW8BoA6iAz^3 zQj){Bx1hqAJ5_*=$OyjuDjki{5(E`6WkGRsl;_g3UxFDp+WUHXYI0e+#S{I$=crV- z>&1%V`7&A@+&S_0RI}*W()7L1^8La0E_9?Mg05}fncz*2ys0JCZ{7EI4Z%*#CGdkK5%q<>3nc{xLpXjX<2vi@h`6+AOnD$9I^^J9mbax-zidr#5xDEdBtC z)c6N=>bpe6tbw6aD+J_JY?O>k9+X)U1Et?2L4<#u6d+JVU|I`BDb}66{P~Cmzyae~ zy?=wnRtsArm^9imANiiIQ-?3spYQEIAT=a14z1J({VXAC36|NF<(kmIY_hYbIf?*v zKNR|PN70t$+o>~P+ev@4$R;{QBF0v94jW-*=m1i$TD@d)Ol|GU3akW!!j+0f&tKc& zqgTf|cvam}p4Rrev{d}pK`%Fn9YFy691Qs$9GV#Xe7M}b{WJsKeCUC+fA8I8G3kV{ zc3JU#*m!^nZn6L;$&2imnY@kbetVww?tNZl0h%seo@d{Fg8np` z&FA|sDD#V-6U`|87U$x+Zj2U-_<%LwVDi8YR96PB?~;4$FZUqKP^rJAetf_A-J9J* z54K}_ARgq=i`XXz{flnP{~k41Sbh5y6tFPGZ;Tn-4nOi){Edoi<0qu9Wm;6cn+GEXgVFbVwwfg+Z(uh`nGj8i40y} zhP*-|XXHhmkx_4Mwtk+RHg;Pa5@Gs1TD+aUy6{^KGtehfh+Fp<-)@_9KmXhcn-G1z zQhggg6rWmp2k639`vn^3ujA+b4-4pPfH!cS38${ss_rvK;%TqT%!Aib+*L7?F5}MlHPWScPJ0SmE{J^t0r6&!uCr!liM=p{I!Gz z24@p_b7QFsrS#9WKT9kVfK2n~>nma6Ky`4%RupsT4@%}bx-I&1c8+bvGb%6XLnrj@ z7B$Ac=Fb5Eq}JlISTGAsjg785zmUhjb8!iVY{ym^RTR~EjGE~$>%+J#!@EwUZxt28#d()P-zSxJ-M?IBTMqJU1mo}c| z$&zqY>#0wMPv$okVD?e(vrXmT^<~yM+V@sF7A`OLccoychKB2Q-W^T^`VW5GObdrr zSqR1HH0g1Tg#ehO6_P8jYx>N`0h5e+4uKg;HUDy6G8O57V;0?)CzF>uNYEXH=z*JR z$K?R33FJrcGvJNvwbQomWcEU5lKV9<98`P<@f*dVs^w-pja5*+lf;k|HQ!TzHBQz1F% zHx1-W6@tsx^Ss#5eL(RRI>~Q*kJtf5Y?9h-)wAq_W*M z$Svk~K+I>j^-R%vh-v)nKjVMlAtx^{{#b&^P5RdGysP`Rji}qZg9)hy>Xp%BN064? z;YNu2dBtBaUiuARH*{Z)5jXD+{6Rf9_~?1x9cMhW^ijwkepNZG^>{c23|6rF>UZn( zzAR9%WwA(j7=g<`gL9C(pAD815#UWoC}z+~nHhcX*hiDV8By<`^NFIhpO7=}uHz#3 zuTwK(ps=D(-spzh|3Tr?1F|_7gon7&uB@zD4vRl{_N?8KQHMw zX2Lkp8FFK}B-L?GiaP%?c|M=;g%#~i{uL+w{2k^9foZk ztd~oZT29qb)ysUZ=(DSFz=DC+XP%22KCCI}E&9Fn6SI!8$Y`c+XY4T54}6s~rIlsc zSBD)^#CV5ToVw`O97YQbl|I)fDl{Hta7L$ zqTNH*ZFG>#dlMS>rt`1>47^f*?QUW=yv>!7-;GYDCj0F*!0=uU^LOu`nm~%e5ith} zBj87Ld;Ip3gI+AGgIKd4YkQwPc45s~WuPD+kTGnX%=0@?(6VsQJ~RQjkk6xb!&@4A z;1i2ABsn;Mjf1%Nili4~t>|g;Q0%@*73%K*P@jgBovD{L*^Nw;+e{>Azaue-+anIGYCV|Boev+{~6D70v6DRi3bJv z8LU#^S*ha44CA55Mj3CVA0aXd(Ca62IPrE4u!f&nV+3wvnp`z!y)4C>?TUi5hA+2X zfDQO@h`m25wgdX{kwNUL2=deq0YShxD?v{a#(rZ%1XGifV%I6j#bv``yW8ClfVcU+C-QQ)mWgBz#Umzsia(uQ0nw+KcYOww~v0#CriN)-fH4Zujl~xZF_I-F zwlMe4_UhStT!2u0Esk*W7ftvoeI+k2tiSK%9)X&@5!!b(nD3?FitR zq2(NHnM)!VGi6NxgY2N^&`{CK9s`4llJp1(5fK39L}L7#%B3D?)~JtKeg*oF8(Il) zqepML-6aPp^^mLj4D5y~_Uyzp478(_p*|n72VfI+116ZGc$4RA?X9QHe(C6oj@Kmi z;3t;ew^P*+vHNN7Ztz0ZCu>93KBn(mC6vFI!nc6!qtOXlU&GF1W!Jw-N6Cl#y-vF@* z?D~^)s(~j9!B@J$9>=J`H&mFu99CGpcS~!);jw)75sLk0trtB1z3T}*+4pzZ_G#}c zn53ci&4v|`A*rZ%q3ETYfis#Hp_r}kZG9_Y;PSDFM%8D&g`VFjETeDb5iSLO8M2p4 z5%iEJJxHVKKOO%b;wcxQB7pj)H$w;%=Hz7u%z?UgCbEM44lhK#w|v2PCLN9e=GQqh zka}{DnwOWT)9qw4opj)@-bbA_Sk(}c8b$ds|8`hpTk{G4pPDGR5^;ro@<7jd5eAT4 zs-EUa4$%tUZg-WM#kmN>DD7w~@iZdny&puG*TO&xcDs`>Bj4fSp=g1`dt*#t!4Et# zw^I79ETR+7M(%9{{jXgFX|Um(ICL7O8oDTUlo{J%m2r6LdY`gkQxi0OM2_#-!=wBv zVWt`LhM=r*F^D*6btsXvYEEm}Sd?VGBy5}#4Npn#ML?aoyKj~R2n`-+`UFiz!Ws7a zIe#QC>vvBO*r`vlvhJtFSf{6n7;hUJZ5*&HD1IbvWk4zGABS>;s%diD9R}K)NCEi( zTuk!90teAMCrL?BsHTIGuUPGXNHIV~MP+=J&`c-26DlK^>`S<$jQ)1J@(? z9Qs@Nqz`#7z?6Y=Sm?orMOxHz*79##1hP|v#CtDOAkQn@MOO*JKy*Sd1f;tGg29WQ z({l}YJ%!vf>yCE7N=wh1!z?u^MXhyQ<;?hft9gM3u$-c|v06L-!M*Y+8A4hwCJbE)i_)4h?~8mdqOu&@lT+hSoTS_Os-QYA z#dQPzZi{-0-RZMG&iY#sW^vz>cbsSD=@qSX*xhJvbx_{`9T}ISM=OCKNT0H%A*d6a zgebnl`5*{+9TI(#6AOHNUjKLZRf1#LL>%qo0fe zizpDAk4%$JoPS2=U=X>H2DOOulge*(v8OrX@BUtRdBCurvGO&Xu;orXmt2lhx^mPD zeaO4o=_&d8FR>lnh}BV6Nw`X#VJx;e=8C>A+!c|-Nbu=z18koOqFX_7l`X^9G3h`9 z1)?U#aP#Ylg|I);43&nUx&F+$a_*yTl~ZGwUpBghhU86&n<+MzMI5Up5h0Y*hBtmiydR{4<=6_mT#8X^wB5gG8`F;S03f zFsXc*!gDolwvrNDL@NU~yN9v^lDVQOuHpgq3OY6~2OLv^$Rz!mwT~`bj_4O*&7|q0 zzkjNeER@eK-@|GNj^>N?V3LkR5*Q=>CSha)P7Qt~=ZM#63^Hg%plf29-252Z47fHC zSWR_}Ng&rzI5om{COD_w<0+R&Rh*3+s6a4)P+pb{3az#O|4zZ^iT!zCzkyd8tLWib`g;S4Nq4yO7{ zlxCEA&-NR-u7*iwL_u8ykOl(LCk53Xz-Su$MDZe){-jE#3ETL20)NdBE*pbVnVx~Z zq)@kO#|of_e-QNUd7nMA3qX1r{5Ql^omOuPKmi1viGWRGT6pt*$wnOVB#&XkcvBER zZZN!G^rly)&Jad}nGsr&sA(vP?S6RPOG%LN4^SCO9bfji?<61<4Io5H5_axACv)+0 zYWsmDo|l}IxUggc0rwWCGKd`KeBZdfQM3Zy`DMwz18Kc(`&sY(E@{L7CZj2cqs`Z$ zQ$x`6vT(AjrDXx~l*BJ~d_2>zTx&^wckU8!w_0n(sHq3_dNBcssHgkK!bvu4w~*B* zdZ4gIg=|yaVK9^DwC*B_=ykw0G9#b)=-L0<3$7#beX{#xFBK)3HS9lJVRL5~*cdI$vdzTOLmN&S5Y zI{XgE-h14KbX_|4m!idutf>S&kG_0*cd&9AeBZ(#CP{A-sTs2IVY%IGkXC70g5M=) z6()`E5n_%mhxiu=hNc>b`=+WkFJ2Q@qQ~L9PI|c3Gopa-V{!fU+A! z;%u5q`n1hm`1TtDQZ;$Kloz=Lz`W;q-ESQSBj-Xc-FNK_;&;b3XPbe4s>JQQdY%Cm zf$`e^qWhtFhB2C;H33i6cRUR;Q}k+AVf6M754{!-*q^<;1aE;0&)2?p-%~*T2EQ)3 zRFUi&X)SRPC;BaOS?uCmUQpaPsD86Ytt>M3zoQ|)fm=ne-`y6S|8tTWEz*qF9&}ws zA#xF`3SL3nx|=l#T%Rk8cK-3VYNqFw4)Qe3zxWlEdtrouo_}6{@#H0X6Wk8YhlCKk zEdiBFu+3nBQ^el;avP)9@LD0tcwkD4WPU9ymg z(4Yanh>gc-UMnVKUtsp{zB^G6p{W=d2?@LxEF%z~lC0t+o1Z#Xr@Hjnt~2Z*@7uE_ z+jCuHRpae(Q}kZhoPU{)i&m{VW7%DJ*8v?lgRY|Wk>ij#cD-P_BXdL(&Xtc#gxXg8 zcH3`ZZZ_7;rq$_~*sLyw)5=@z4>tMgX8dyOnjPm^RapA5T7HgQpElDf(aw)ZSgnjC z`X$j9BzydSxvTNT!BEFZWpkLBv*M#-25iHc?FTLcelLGe_rd~05`{R1!x7N4J=>B3 zg9JsS=#F1!cHM zX{l{#lxgbr7iUY{B&{K$igK0-@hyRmiL%8F%ckBgL~dWWJyV%SDfJ6=YtYoNj*2N^ zcfx`I8>>XJc9QN&{g_JH9XeI>%YckZF{w4CuSfiVC9b#YD1uV^zsWWT1m3D@Pz`?8 zEZHI#_CxE49c0>#oC!GakbvuRL@V3AYj7AL;CRm*!XWx&_kGK4q$kkdr9(aURs1M5 z-uUw6@bw%5{t4_BoO|?dH-Vt1v9{$;K~DsF;sRc0dghosAaoZ$U?nUP_n#~y{|xF6 zZ}6QGUy*+}6QptCb3b+Jee8=Cd)#L?kxUWD=JMJ%YjZ#Yab)3@;+wBcft$C+q4<}jydm2Qre<)nc!#L!YmS&gJx-bf+4}vF+=c zx(&88P!%mh^k;NUSl6IzQ_V6Q>+<8SV``Us|(MYklXFVVp-RV2oN4L8CI6jyOy0{_Ogh=TR&RLkeo@Dezbphn>s7ie^(wX3H z!icy^Clwk7n#c||CS;ERvPP=>Ynw9QC9R0oe18Mv2d)zPOeCud-*s0HxjI(;giw{8 z`VZ7WHMz|L<}nqM2N%ee19L|6)2eNI_FyjD7no$fF0Q6f!JM#!fz=&YP|yUCKSsdw zE=9-1DuvkN-{hcmoFYCxPC*e7d&9tx{01FuDmQMXEv!=*MoBheztn zkNXpfvz1On>uK(pPS7T-&6Z#M-gV(y{+1)psR?*|U&fwP;IwN*W@zr*vsfv}HB5^&yFl zz2?psR6{p2JV6xv@ox*&OU0O>T^UB9sqOl+h-sl%sB}a*=_fm!B$!T0d#>00H-cpg zlB1(I6&>)E1^O{}Aq`v$Kb5LBnDOI53sdioKgi28 zSWqWDZuH?ROWf}6?kw!#!9n>J%8wik3S;~jRtOh41IR~3K~tK2Q7B&wii!d$M%3R6 z6Ag`2)bAGH?s+D^H3XlF2ObJkJ}q%ccPv{+HYOHib{R!A}G>MS=7QO)L#B z&r{#m1CCLRFZqM-2>#vPo3Dir?-n&`mrTb3-vX@~=OgVun@L8g`3}V;DV9zv?3Kk&B zM(m{r^5Lv&_kK+Y1+VWm*5cBp$Jl@BVTw;_yXbiZamCpm6-+=aTx6KnFi~f zWhz<&)bofrcHB9jK|Iy5+fe-BozUNOGPW;7>GVvAZzElL^Ys!#_g$S+>wvI!#zusu zT>3++BP+LUVQeE>+PI#LPDO-DWUkGn^4AedhbJ<^%z}(zn!1Q zUQx2~IxLX?KtHwqwDwnSX-U*tu(2)$|5}SMX|n9oy~W7xRdt2buXhTxtO92RvE~DD zD)u&U@YJwN{l#os0P~k(ArFk9rq^(}dVaAy#9HK)Itv3bdBQW;`3cpQak^23^r@cK z#*{vkf*8kQ*7|F%WbgEPbWQ0_XW?i(Rw^cFhoovhf${qp~+Qxjd=*YL-{e|UA`wUvw^cat(}v|Ld3V8TQ;REq! zpl8^;yYQB70MBRjyli3q*xN07v@BiW?|nK41wz2VH~hig526o7SBoqZmn@h9pmh&G z2As`!-#O8H*+SxshBDLu{F21E%Wa`j8Bn22Y`ePIF72z3#2{~E>27)GO z>j3bY?(6H;D_cS_0Qdx4N>eECUyuH!`NvG6-k=t}uJ5Zk5w`VQXRwF?Tyy})Ut=CT zX(RT~btw8^hUp)3t^>#=z`v`+_GkMCXIG21d=~1zZ{BYseu4>hJ$j7ADSC|HdFTLu z?p@pSh(UcXQoef_@m7arKq_~>=F%H3n2%CzlVXc80WZeWp2NoPb=D247YCDj=zSJ> ztw}Da=rj(SxP!}a{Egjb$j_&+#E|l$s)!z>ni&i(m%mKP3R~>8F zonrX{{oXSo9wP!*wLwU<%`?ez8!bmkElyzwD}tGtx>SnLkK3d7aT+t|A#aBsJt(X*GDPw_nHI+0!hZ7 zv@)gDK&RD z{C)2;|5!6y04?P)=qq~F+LHT>ICW?k19MQ}lJ1j7Ih4w*Cvb9xtds-)rwfQCZA>9R zVUIYxXbM5yiVu-VDUTRpi|MLqk1H3CcjQoR!oleOD`0cZIO zkvO25o*sc=ny649i@=MotUm^v()z~OJSxAH(|EdIA=sh@3eY?p&x)B|{1y?fX#D?Z zI>+G3)@}>Owr$%+$LzRcJ007$ZL?$Bwr#UxJGr~hS2tDpnbh8Ey_|E5XBfm^7bLex z05{0hiD@hPE_q)3U@dpGKh|@cV%PiF$F%Ld6(85iV%iyP?2){ld*`?8@LohQ=1qt+i-oaVJTV+5iEUjCLq*q;8U`S%zoFw2~_ z!vMgvZ`m6+^VH9cHQ*N^=q8GL7{*Glo(Q304D+FIjh%E7_HMcAaD#<^bgR=+=08Ti zWW2I{{^eosGtKz#w=FaqAd`&d>V8@8#kxR;hyZ|QX_K| z1rG+fF^*r+U1=T?YZ^sQ=U}uf4Wy?{Iq$w3Dk^sI-Ij+7W@C@zT6Cto#t2h6EDFDT zo)=%8SQNZ>0|WLHgkae{i^PeWkn%HNmxaKf;tu+eD==n*wI7)9tB#IJ2}J~5eMV9y zq#b>!`9j>|eCy*YFhMSBEC0jm%Z?JF>z|xkd5w!bfw9#q4*YQBXiI6D$ z#0BRIA3b&h}4jy|OZyDCeSS7rTt(A*!+`fyUk-qd-n(rrpO*aQLq`-{Yg zxcAOLa$MA+yagJsobp1d}r zXU~kYvj44Io~BK-li7Q%e&?Imr`8|L-0?-ShJ2*|hL3qu48rG!S1kIb%*i$IK$%6>G4w<1Hriv}@~ zNJvp(t9~ux;H;v-=)-uLvV542a?{zO!Mr8KW5o|8B^3djh5&a9U3K+Y4HXBirGp1r zo)TpP`xMbMXhC+(d&yrT_Wwm`(P93J($cTPJ>(_ATto8^5#KRqYkDmGfLx4{v_t!G z+v<2S4Tw`$WUE)L7-NRTFHtzF+%2A{nlIqC{#myO-a)?c$BI*q7fH(?Q@C)Y8*YMG zP>@gxaM5_k8>Ij<@x8gBV+dSB2Z_5acWd4q6tJKKGAv|jjx<@$uavlByZl3<`pAIc zS@+Ouv9f5;NqPClt)o%$Lc*V?#8pn0VFrmbu}3mVDt8p?{70)(6>DKONgygwGYN`f z1-kgC-VeZ0JjysaLArG5H&Ypk42T}}3f%?Uiu3|MjVR-YctmB%tooo(NO%{2yjv?x zOQiwIfFxUCsTi`f2^<1D2sW=+6c@8j_8;e0&a&v4(Y$JB*Wd`>W zpeEuePMj*LfTLEJa=AB`SvHJ_;M?1}hG`y4Lqm`kBvQ9tN)ttm&n8o|n>4ujg@A0Z zutwQG;9U!;>$~ZQ3V(jw{-BStp^nzfK10ioY9Tb?U8Q=WlCnpGiE1(G!Qt1XP@Q@^ zISgPN7d0%dlN^XfQx^I*7JF}M2UgzKplF!&w;4q%zFURKA;)$pWra7Wx&nEP@BF5x2Yk(Y$e1L=xle)5->1zd3HDMnx{5q>9snz z2#uu0iRFi44yvboih0?o6Nw-tm8_@cwVOK>~)`bY5$?`f0TIYU#u&E>XaG{6x|rzL>zy8X z8cM~T(5s^Y5sJf?BUPQH5wW9LHuT;c%@Y z+4aIfFftPAm;&}pK5DxpS&m&t0|Vf+n7%%taeWgttR8I#R>Sx|V5?A{s%)?_%v=w- za5yxDas6>{b5RjunIoY}Xbw#4=<*&a^^nN2&bxq+g-U|Zg{XG5+vWalN>w z_=$EnKky^*OnfAsa+HtQKyPqA`j(}Hg5X_(QQ0KTy%Z;+7~BN_gd-yM?0*rJRq6km3KzNQw;_c4R4qRqU2G%{{je>0U7 zb{NYRLbq(KJY1X}sd8Cg%(iF=E{%*pi>v!}->sbQcS03&C51e44+;*Wv0mv`F{-Dv zuI@FO>W6yvnsE{{g)tM&!SsaMq2SC_>ii!#vn0%IQ!8Igjo5d+{dvVH9 zFQa@!0T$g|Fyk=$9(cS-P5hq=Fa`S`D?09s-yXaO#ICXE3rUr)b^56-qJ*LNSsEM* z9wHZDP*VJR{zcjd2~ZTj3tN7Ll(a0Yr=(?J`R`?EDJ2 z4-*%Zrja@-6YyyrmqsWR@{r_bL~}b zZR)eYJmBO|68sq<^P>8fL92SQSgv8B|@XcK|$XSgs^>bz2v#lZLKz24Mt)v4~&d1 z76ZC$E=;VQ)-A(WR9q^p`-Dl-n$|H&=pZvH6*22)N_!c|*2IRJCBCoyP2KbM(Y*}3 zbdnlP33^U1tskMUQbw@VD;y|hUNBHW6NeU%AzAdet|@meNyc5PVk=M7ZPl#qM}L3J z0*S&RMUIkhAh4Yg)uA|Qxr0xGO0i1Ev(H1*n(vCkeJYQNfk)RC^`s@laBD;`g*N$B zN71*02R%U`&oDQHa+&0HLkKZN^;(&GySl_D69kh-3-AP;=(|eD0nA4KKw$08mUZeE^!p(L)Ukv62`4ok+!^N*2I>{d zOdYjhXL)0VnHMQ_YOlfi`gXky4OSUOWL|CT*0!}%LuvtkcC%`N;x@$0=O8q;7^WJs z5?R3L-%H&MNocD;CpwhcHFN4GPtlq07_Jo`FxUMmHT?3!NK7t(N#|ktdWaglSZy-f znON{5jSbC^gKs3T8~M&~L}Q?l+js%rH@Z4I$T4R`85MUnl9{K{#VBYn zW?6M+*V|oy##0rOlT7RaWEa&e7xs?{Ssnvt9|5c2acoIXBq$kPOam(sK`0tfP$}p{ zk|>^HdrmBid`8eaz%6O6+~d%&9ffmzBA1z>oe94=5@zCT^=zWpJ^D$5_CP(wvmu+2 zlFGEGZg1pA@8-xVVK4uic1MrGALMacSxq%Dk|^P{S6qUS<5{$@%wu`}RbbI{6aPet z>qdk^ex8ufP=NnA_x*R%_}yza*i;`~OAP`AmOBSd#^P+J%gRT6WCY~Je_sAs;D5Wy zO+hJnpe<07{~jk(b5DfN2>XvNfL3?fafaV)B86TE=|_4p6ZWOTiDpx-9>Gn_dO5r2 zbwGn?EBcEiJI+0;V*$DKR$DFl=HJHx>o`74dM=1WdkCjr2o+I-r!p^H5?J9UWC4l# z1n(Je9AV55hOY5KQcjJ#VR{UlZ@mucc85=QlN^~w)_TnTdv++7_hf=t$xtbRoTXKnQ-(Mobegwey$-h%GUqC>lo-j{AwFegy4^7^l1;o{=r z;qiGqVCe5E<5Xx z{3^hR_Ic0PdJO>ZOD*#q6WSiY}T@&CCEB9OtvFiCl+B zLJ_93STGDSIZ@%k2i*1kkf9Q!3T2OGr>)63bjc2yKs~D**mXIaM!}P9OXL3aN*((Ve@@-#d$pJ9ZEOt2 zg^-h1+@Qayb)jqMi*BFhebz6vfburMbND|X1BzY3Wjn|nF5Ez5GnEKkp-NEJD!e=d z0WS7-G*#+^@#5cifZ=#$RJoox1vexxq0lc$MXMCVO!dwSTRYy=ra#NM)K{#)2WtX~U}Fk(sQn>fni*DX zyr8?GXNIM{6iX|y`-l@?C8@&Hjin*}cVuH%qN1?D-{FCP_et1RJ!4(s2-!l?YI#f= zh0sZ0d}xb4;#OJm$gI#rI(cT}3|qS)qo%2Br9k10WnlQr_aAluQVRy`kmhP5tO<|x z{VyHU|E~p*!x(!6k^kQ1NAJ1H3?WwQ|{=YK@t(fD93? z0U=R@D~E~{)J->EfhsjKCwWu+)T7DjT(pu83)CT=(9Muql1ei@2O|X;zfhs{+x^); zo-P3jzPG-bcKJt~V!WD}oaKfY%46@wLC6_jGZk!sp?1n`+%6U1!|4JU&Z!CEk&YV< zE*+8IQ;n3Fiz}iF_=g^jmm3vJiiaMFoz#sv21IF6VEuL)kpGySogEw$Ez>Sv%5X<# zz&t=Y5Otz-(GL0VTmJ>*rk=y3A*<>iCs9}^lbrnkn$XhAutf`~tD9?>7C4_|odL5@ z@xXJ_PEW4Bp()ZEDjAIl9_AU9$RjNgq{*cn z(T2P0hy#;oSSnpE8tQD^it|OQS1SO zI%xn26yoQ_Z3Pn~({a&dlDSyDLa$rACpqzM=!S$Z2AXr79CjV3^`jkCs4!cwxtlKP zFq0AZF40CObmXnj1j~1ps(O3$FSCt;t3M}d6>Bo=(SWdWfTBy429$N-KKwj@At<4$ zR)z3%=s_xY4%vN9DqRehB|iCDBfCBd)&IZo1KR)ZK^jmwf`aq`=jL@m!*n+q59>Pz z8!~Zm)dKR%0A31LfWZR`NuQJH)Z_4dyS|bsrIqXI`C)q^a%noIHDuXRCnq87(~|oU z7Co?Vt$cxBh)`fBHDa39i3G4*4}OsR4&TWNV|fdXeOqZazf677Wy-qqEE)0!CJq;y%;r z2?$%h!Y5qQ*uP&T}FDzO8z4+l$T7;&0A!hfv+KaXKqvx% z!Lk<8`h4J7pJ8I)6W+W(8(M$PwGm$-KP{{bdpticx8~HpCDuS=FCeO#)=#)(&b$|g z1PVm`sCVBKu#O2Bbg0l+uhtp>gq}u5M!n{e(R?9++6Vb3QKlOP!L!F~rwd;MVpZY| ziWLn^!W<&UJH&kxBF@@D!vtssi?kSN*HT&@+fPcjqLp7_J(AT1wiW!e&0H8blY1%< zdJX%D^%?a%X_7CaNU2nZH;Ez>pl82oK@Px)c)>EPptj`6YmkG@J<$zR6kjZXM*7@g z-AA=G%`OE9Az?Wl)1UiCYsMJh1U~_!=e&8MnBkm-Q7;j)tQCjyseEt^A4`o`B$+&} z|0Pnh?9ly%PLp<;wX|Ii=|CD*>$iPq;f(8SGgJEc?X+Az~`D$jxVx3I{EAdN&)k zg`AA>`=$dM=d9pqrE^UmL+QV!Q6|Bp8k-x_i;^eFQ@piG_&;C&xbc8MeTo?;<}=WE z_ff@5_qvTCO0!^;F&Rr{k`w2MgelaT4#w@?9AYIKNAyA!#muR*o*s!grMuB#yRSEFjhni+DR0p#Dmgv|5l^azG?;$r{h^CGPMSE!5@4_ssiyuU)I zL#~KgW_I?b|5QXAm@oo%gp-re)hG$D*H-JmhUHp~|xY^VE# zyImd?$P!KxTXX{ASK_r1ajL&rp~C?K&LNIVeTKJx*-4Ab=JW1?t5OIb)ji~P(kbYo*elmXduD? zl#FK;!<3v9Pii!QCwgvkg+7;30L4lRr`3Ar_oO4iSLp}+ulJh>2jx!g(3x${79Ck|2GU(FP&COOsKt4#aN` z!vEqWC5>w zdS(VU5SE9wA4`XhR_-SzYV8&Hp+tx5xNPCp#Sp}Adv!A)7Q>Nh{zngb?^7+kp8e%a zpJuZie}Is(?&(X~U4NjYEY|x*hjYt~{~Q?cb!p`DWBA-Pr_S~ItAXMBVMKo>X3KMi z`}!mJ_0?AURg4DoVt66vv*7l2j8fontr2pWKlkP4z5Ew;NsD5|)oR-b>9wzCRlD6D zC}E@L3~*_5g!i+S-uvrJwGrS0TY~<(GXv4XR~iYY)2og$;rT;u*(Td8cTOL|89Fj5W~0r%tu`DDV-26TZ+r&27eQ3B+=CFmF5oN&qL zRH-Enz6WNkn2jnkhZRBG+EW@HtH(J}M=fFFGT@p4xMYpQ656ui42J}WLyD2HKf=H-`*j?w>{Tpt1Wwg`u4l4qMMnEtI>O;nzqNwU8I4g_Jn$-{lRQS=N@3e$b8E3 z+;EryC=@C`v%6(Y|pFCKtO0fCHB8d zrh&db(lSbISl?r`lbAm__~cz}YkI~y9}7!>jD^!EGhd-mWfJqtkwQkXiA4}bpj%1jFz9L(Cyb%gsbNXImwrqp>!s)0WWO;963cgFx08Jdo`+k7t zePpI{+n-NyVnmV?(N(_JA7n$kUg! z!_11>R%|@H&35Pe!9gyCX=HIUQg}pq??VeqAN!4!fH4D&ULn46&>qF;0);PT#>HIW8I8Da<$i$3$${$?0f0`a+S1=V(4$>Ou9E->0-eQ71{{KTzGQa!Hbw@ zipxQQ#zbS)(%`f9j%PT1B!N*h6-^g)KW+uiFc&^%F(ZcnNTMC5g`WUgav_vOw5IzT-f3%6~$6YfNClJ6_G)>p({65|Cac+564p86{G??wA z4+j_>58eWvd>}0BJ*Mu%tV6M|iNH-YC+FX2%CP+uIs|COP znE-$`txfqCE(QTy$?BCNBLWAFfW#*uA<<&JA;8BMB{bBZM?@Okl<~8Hr}-C;OuI{q zv>~`sMf&zS5r(UZ@l_-*R}mmm&^&_{ZXs@aM`Zgqll(@C#xk1h1yCLAG7fT_iQ~Ou z#Tp7dHH&*A)$n|d3QWUjVjpj3FRf~)5_m8}`k6lv;Nw1<6;y#x2IFRfczF z0PZD4-S3{m=q8gh zkwsprZp2=p5Wq8T$+>6E{^r zG=3Rt^ZrvO4t;M_qOHO;QI8(6bn&)w1}_K|CjBy;tY^i6_Qx*wo{*`{9@Q$#^(*-K zJy}N9Adif}du(EIg;4La3%BNbtnIl?|6q8k=X?D7KK|S5{RzKg=X;uy*&^Hi7%y00&7{or@m^X@+Z79Ou}-W|ynV;7h9wcqf7r+nIFi>hc$$n)NLFGfaiAN0rT zLgc#p8&)c(Iz*n`H&|XC2pB{ddEt za4X|cxekkAWRP~)xLrOAX+%2*CJ{|=au~iKGr#wZ?kq@v3i@5?y45d`9a!vIqaoq^ zwJ5JyG6{NBy3`QLfbmhUreAp2K}W8U_fTMThg`xi^gzzySbr6C1~#2Okd3!8f1@3Q zf0CS3TQ`T?4?SG^IEP=}84Ox2Bv>6;T5U{lXVhw#xu;IA?W{fK1&YZ+(&-Kk$qhSM zPMJidq@LwlpGHwt{L0=o{~rDST7XgEAmiu)czcg3Idb^|3M4OAc9>|43C4LaexVQ4 zI8@~4#=w4j5!)iR?yfSVe;=wli(|t|6pWBX6gotP^9*FVU#ckD_$rq++mP^JA(ET; zS+NpY0fFiX7zjvmxviumz5vI@#)SdtfGXg>v4wyTa;wbj)PpZhYT5K0q~60e&?O~;Lj0v6 z=@0^q9~t{K5Y$u__i}V74MK_iN*}?+Jycr$@EXA#X;@ABH1LqM7Xk_h_|vyBh2Y3} z&g?M@7>$dBvC@7%Anebj{+Jhh>lxI;(!QIOxY}$o^LTsl+-}efLF|67Dl|I4)OMSz=6@d^rU1y0d77Lz zV0Ax~s5p3dzLpq$pWZBe!8Safvz=G6dp?G$1a|=2`7!+l0)A2mZXO`qQ${F%R`{L@ za5QcL`p0NlJkQHQPL|dPE7i1y1D@b#sRuY_9NMfT^yCff4L^SOmcgsR6;v_FJHu1k z^pUi#8BS4{GckdL@<^4$lKa|*v{94J+{5~`p(N4NM%e_jbikBjnbeOf{EbfDiE!Tf z(oFwqcj2)NiS>x39OiUusi<|qr{iPU}Nun&lW^)h2udu z_g5-AeD5X$gr~RB%ncMJVf&F@mj;TMMMcR?PEU6N<}#2W2%7VZ*H@(A%~cDa`M;{!G<)qk4>NA0o7GlC4T>?}*9ejSkVM}L1m!0e{~#rHrdCQ(64*dHLSThku)u2^J@N+}<$EFF#j#+j zP=kC)9>qjkEz1y-YQ#uv%5;t%;y^?((#*}C)j=;H4CB3{Tz{-Mro&|CH}A{ITw+*| zV$uzHOS$yAMY~1mDw{DK@`+;ghgsPhayItqGIISi?cuA;&L#6Sil^TnmoYIk^wQ4* zkqtd@7e4u6fIUpTCK&d#f}nPObLtUZ{MXKdgbMAD#Xnqz;B6FA`$2zQgC;rmcJmt? z5AvMQb?Y_qwXH3Z@UBAd)r%J$R>Q8B5b$gD@;c+Szh(R0l4<*a{V*K-+V;<{(BJQ+ zo6VG}`}t|R@WlPxw$mi|*3IqooZW&L8^!YPf(QEVf`ZS9FF_8`>BfggV~dU% zEM*an!4Vd)`~$LgLHg!yRI(Hs;6|g!$Nb%uz!mLV6ya&yV}?=z=^DefKN4BZ}{CI2_MQRQT~)XC0c!`cU-g2 zps%I!B5jWeOB+v9ViwJ-GUxaKd=c+~(@+>E1wo!Um_@DRdGRh#s`t^0=u9H=Gr3Vj z)a9?=IaK7?k3VvMv|#YpI^0rb(eDXJY_h!UP|1nIlg*j5C!S&Aqssc&mBZ^V0palREHu;2K}X|vt_$sFrD z{%hyspY0s+-8H-JT1wvYs-?$$sETt-lAZ54xJQc$O$=>nX6j>^u(MsU1|AO1`%0NG z-{W`F`g6@2M3&9(ZU6@e^Y|Dg71RYZg4h)0UC*K@Zd_{mim30wCb+30vP_$OP}3I{2ivGNib=EW<|qxm?k|TsB=OA{1GYs z_iSPte$lh^uBQ-@m%OwV0VV}Xr8%Szw~oqX)}z_zoX8r2ZCtL3hw)nIq~@)X2AC@C zRziU6q_!bn7@4#GT6EGYgk21B$S5&neJT2~RuvCz$h?j$zgRprhmsSqz-qxe&dG%Q zCtdY40XAfv*DWcAYizhA#)P3HNHSooN5#ha9~=4DblV`~-HOAk=il){A0?(~YRK=!+^MUlbj1NDWf}4)x3@^23WEc&Qk!5RsoA97hcn$k) zmHw(wTut&64f3H;(PYAxNXi!&WJLQA2C7i8fUI$>Ly{3zyIIEUt8;-XmnW`BD{d9z z0=ESbj%i1E{sDakZQ>**!3PI=fsEfLw)h)FLgP7@j4LgwiuBIGj*Kzh>-f(KtvZ98 zfjxVXHF;&W74i?tzEVIDcD>QU!NI}Qv{|#NVLC9TiJl8Zo^*kjeC{}CPyCo1Wts4Q z6~75^On~;#z{jUrw+0=i@AP!ET0PAN;dWB)76gH4yf_q^xGLQA_3~&txos*o7uR@ zVid7#d2`<wVubbM%&rGbrqYnft{Xe%aPiJy4-hKNPe|qb`r+$4MD{3L~Mx=J!2WK()hbt$ou*%%fZ1mA_dbmDS?8&AoqBFu=^5Zj7o4c)yi7a2zwJ)wQ zOH@9Ne>!d7Yb`b=Bq&t*+kSN zx%1zm0=r?Sk))2ueEGl%4D`}yJPNJFEe`OhEW&kZ=Zt7I<2olSXwY%pM|opuGfS`a z;ny<}t3z3t@;at{C&A!nPoGY&lV6&z!45jb+Ai4WS|~N3*#aBT@0oo5P$Kg>E7%`H z#^|dR>sBoBz#vZ5rD3Yf+(cdat8E}UjE%0~7donsvs||j$YReqDQ-GeT_Q`nUg&{X z5x<&f$2Q?f+Qlirs33L%0tjRHWx{x5BnAN)>bL8M9&i%B^ZN}``H>S}keY*=k^r-o zEIj4$i$|2^N7;W(oB@Y_@J^Uo1t1*2Dmu}PA)$kgsydBM<^{Dg$t9HZe{+aTB3B%kCNQCS>TV|5a|S3eQ(*{) z3uxGhCj3Q8lK~%Q$gmsr)gnL~P)!b}06e64y!HMDP*_(R z3jmFw`m6$_af-Zai zKgEXK=zq#Xz8zd_g6(ea4L|@Nkyk*(Fye=9im1#2^HY#)#p3i(ibMEej`+2rgUk4(83%eNZ6KWPDq!MKt+iv6(+epj^nno>U-gfx~ql3 z-~}nQ`b8mM9k1prlCa_cM| z9E2gHkXqj^eRY~TE20zYUC!EnoBiS1F{ovemh|yx>>Y&4FQN;nnG)|>GPhIG%aqYq zLkWOm<022HM@W(ZO%A)4OnI^rh;Cr(TEzk7ucn(0Z!wUc~_h5{+`}mE~k3!?Hqx0H`6_NqQOds0uiAvCR z4l88WIU{RtcB8fUwFDpdJQTsE^q$VH4?I_6_>@6=pgv|cHZ}nPWKfgM+XmXNQgNtp_IZ)BO_MP50$(02Ew^l>{S40J9QviGmK%^T@vUHUyEu6@V zaveuPduroB6+=q3i+dG~Dh{>_9)A+|QJi{$O9V}vd&Bqu4m>ko*?V$6mpU(|6~G9) zj7xBf-^dglR5z+XKyMDIN0$s3|AS4OP`~1l1gheS$-~B_&ol-@wh-Na&#(k+Ox!W_DD}Ig;uf2*@VtCY@NO(r35GFPe96@#fv#mp0|~yv)Fy#fasy%TQTm{8nJv^3oR*8=)%nEp~>>&l+3V&kiqBVgc-LGX|w{HLHi3j8O? z_1j&oH<1BE*#8Dqg`+^!UUr~qvoS#UgBqm3K$F#iFVkvBV$9bs-nIODnO^}z#vdvS zHi~tedNThnV_F_16&g$)R^>zyjq0H$O|P8b14Z*{@#d=IDQcEGIF`o?>=m^b@5wqU z3<#h-Bpt;(m@|E(RX9Wd?7W(xN)k^~u`{oLCV8tZQ z-a-&le1LYt)Vh@7GjNdHFR7f59MmAJfOFVXXEKrGleVbwZC2))|ptvKaBspH=kwTZo-ph$x-j@%P zozw-6unSLg?q-iB^>clXM)o?fA$Y%(=rBT&^Pp}&+p13@Hb*zXsc^ijGoDa z_!MkaG$&?T_NrOd%(_ydLfT{Iq|4HI>Yd=%Gd0)3?9x?cl|$dW*P+6}&DeJ+7Yu z7|Wo+bA?*V(vvC=I8i+`R(@pKG-G8Qx;4++4)N&5N$vZa!UwbY1E$|&Ij?dOy&Zln z?x(x?Wo18;rHeJI>Xng_^Xu~C0UcBnFtLBdfo1?F9jEE<=EL0I59sFMS_-I`+wgGV zI;X=^=G`y(0;qO`muunt+22jZ0iKQPOu96;Vq;S39$|Qo&c4aU;TR-lymbw(T}RaZ zi(PankVw+ULV6StlrVaLKUHi%$R)ViOIVLEQoE;v#l`(~m+$Z!@FuIu~Z(5TPnY2Gbl5r<_t zQ9;L`=o;sYxl$+xliVyymtly+pJox}BgzTBds9vJ;0>0@p-;H49c!S8NRtq`xG_;+*K zxCsz$;{z%(F5`3CKh?3K-#bLbJ8}SDh1bts8yg0U%A)`hlpIErDOD_cd=w!@iBsG! zqeyqU0hR*F&)=%RHZft^cLvlKAx!gp)$_r+4;#V)V`R+=KWR$SK4zP#khZQYq)u(w zX+=Bt_^ydzY2C@Xb&P`iQ4nkeDbtCR!ug@sbYDbm6%eG@D*TVcdfgjzEf*~n(cZE= zXA5`bgMer8k}$<^EN|i>Gs1>LAYeezuioc%{a@*#B)3s-Da3Prkoy8fE7$xb+z3vDPD6EopZKUz5F9R$<;yMWmgT|o+w7_A zQi^eS6q$W@&nx0dPNgO^pFzo+Gg$JtVZJ5Vsxha1(v?_NjU}3yj$RFa9JY(-A%d9J ztsl`2(JaJ6vExMGMsm*`>eRPEpJ6vH3&1QBgc0gsX!IZn3p z0FI*9*(^_nEXFjQ%_jg#M+@ZfkUjG ztYQ4VnQwc0O^RaB|I{6OYR`p08ksO6h^o)Ji5guF*{+RVe}mi+I16M($wir;V8+La z4F;}i7Q@_vM>#3^HOk5LjApV4+L+*kMKoc(cEhkefdb!2E#XzsP3Yw)h3amUkpvz) zToX)${YHcvjNc9W22Qw-Pme9*UTlu+H>eLp?E#tDBU?v&XQ&Ncs5Oz1fTN_p)q6&_ z>Fyx8o19YY4FUJ*bI_7ueIG9c z$n9shZ^Y0FEntodTuB5Y1NELk6hZItp>2BtlW2~8^WbBH}tVGsTjQFVxsK`v_lOoK60&lK1hRIf>^Y%p6<}``K zFpj*HVE(K!%yN>xStq_w<{Whh%U8~=G#AjLzA!8(^{fKB&QlLz@-2H_R};7Q;gyFLBk1(tHnB-l(MVqGYP$>c_c622k^{`;2t$%XsDKq zzi^pC{$goryH*CUIbcivf3Su^?-{WsXOMGxe`7sPBv-oXoOfB6jk zj7=7H{^2=~MRtLebW?7;-4wz7!Dg!nXn15Tm?3HFk=7|~2TAPP@wK0Cm9i84wO(+;Eb)B!|WV8V6XE7&m`ps|xEj`QCen5L~2WlALoXpjU&Q_{XTzED|4IVy;1b|nNA5cw&;-T-q8+FmY9atLWd*-ao_mpiY(@7ezWjsy3cV2iwx8P2|AtJC49$jcst zl%E5Ju@IuhASoh&64cToUv;Wa^7x_Ck(>GUsL5SX5m}dqK$FwkZq(x!Xk0KepTHcpVk4HWs{0x61k8!_Hb6nH4 z7WTeJz*Pn3ML5cYa{Y2OmOzAL`E#+vkNnCIlz1Qa}F zyg_T)ih~z<%XK~}hXjojo#F$X`e0-R}mR39}7MjrZ}V#w>sFw zaL4C8+hii;>+QUzHajVe%i_*BT9wyrb{7C)2;pp?dOTmY z-DytNPJFR4;(8CMO#NA(tCHa=zP#ncJ!-6RO_1aWc`&28rWQL?I5+k(7PjI)B!4oxRRm4~z67w3_EclK; zkKoTqkx|M}&KqM;Iu6Sg1&4OPq9&+4%kZ-mWPm`Vz$YqqgI)=Cs!i~nE)Aa&7^8-< zVx^8D8&1dVqT;BiT4`FRW@{K9g503o)q!+}-qNl`tNL{K%FR)1*IFfBBhr2=0adT` zZkeQ`6+?@=(BpW6z7it8k3B~A5M`gu9JU#c?b!VvGmN7m0g3^vF%yXK`rBfOt~k6% zHSa(jMZ(3>M{q(-{&wZmtpmky-@Od^NTrWpC+`7_NL<;n6pk8nZ48OL!F~E@_4HIA zPM`_MIvGq&LGPAFn``$RJRH0yl8f#S(t7WiC=IA?3DLv^dF-dU5#15Jk&g9v#&oJMDfGkw;R5)-?;H(bhXfH-$N z&R-o*mMcHweY_(eIaIygr_}Vk#j2^j)}`JrzF!b*O=ZQc-0&vd1En^9UfD(VBBl03 z=_5Qu9Z<$S_M}2P=#SG<=HQF@ZmGl$3{oOWqY~TPLPIY$Jj%u8BS^j;DdwRwHi%q@ zN?KR8DI(GvQunBUJ~r-SF}Wr|KKDes)}5Mv-d~Yw_*CllbMX`3VGgDTakU2rtO1cS zz^zYt9~1n4EkM#`TVYsd!63+PUKj-Lv}FUmbyZ_of~LQ>!krvPKlMEQxe?;o zT80?x?ZcnT#(xy7UpXQF%2do0G$7u8gA8r?-YZHFygwAf(A61|T#5Q%UM=gpT`8`e z{4e8AS26<5Id`;A=+z6kNEC77vHew39r7R9em6tAr{%ntyXKs6@d~(z{q?dybxCh} zXwPcF-ECX$6r8+Zii|pok*?+>S*xRI8f_XR4YfpiNFB+%G52l8d+ks+q{pTa_DnaH zrj?b!b!W*>;{8EPQ6S>C8b0k23nDZVJV{0`N7B&Qa>rSG15-Q)leQO)winy4uE(L* z4=^n0_o2N!oQPg=jBeT!fB=K5}5Y_2xy_XP-k+ycP6ivr;Be(_8Ry(q(YKoM6ziUw7n9sRT_-dBR* zs}5{T>XnDNWp$@~2{7BNQp);20Ng+$zrAL$3>O@MV){@?oZUZ<{T?a$u=sHOg50y} z+!hVRdg>{j&obj4%J=ZxCnT_rue879VV&XH7ACm430t&N=K9rpTt-tcZoddt=za+@ z10!6chYO+i>B^NOI}+D;HGi<((E)X)%!(`{Twg_xHw|Im+uag_YYFP^lD7BNUOmL0Ff*Lp$r7ZeXL`b(`HL5(b?6oxU5y8`RdrxtExXCf3vS zh}%6BA1I{bp`&;luGi}Y?La&o!CJkGVH|pw7K10C|MgzgzFcH zUQ~ObngNwdXavU{XQ=s)Lzv+I<9`NJG_l$o2Rib_I=y^4;u4X&Dwc?U0E}ma=_~_Y zP$r_%V1%a-v<9g}Dx^h3zXk3OXMVvg9`BGd>}*6tL}X+nm98Y5OIqiY7N<=UUv9*; z2(qYFqYtrE?l_k4h8>JppA#J;k~_Tdl)p^GOs)uU;oCN^A30(eY(Mejb02;3ISudG zv&Xw{z1FX9ADK*g;^gV4o_XQK$y35n|M#cg-G1B6i3#zgW#ub>SpB;{{FxvKf*|dW z_AYQd^5|nqrQ(g(U-!8;F)2x-)xPk;i#6!yQY-_^UAB>*c8QxNZ@T;8IxQY5U zqlLBD=(fxzk=gmtLx!>PJq0_ohCvL0kw1k}q1H&`a;nVO9lbs74=0__PNB2o;OOi6 z$T#c5(THae1QaR)C^$_#+^o>&_7T`5-uKCQ#ln8KJ*|ZEw1IULmjWVqw{+|9JESE( z$08#X;wC^a#I5HQ$RVpRbnkwk$B)Z3h|4e%k6AJz8waP_;(iK`3k+-vCF&is-iJ6l z;jFV~ZKwTE=$?x^%tRz zb@q_?VR@$wg^*K*dUtj7j5%k$M!7`KnrmvxE9|CPh14vSSYV{xXl1SZIiw8qgZZr3 zm21y&Cvs2bT+58f=$?E z(3@Dt)d}}P&+1R#8?0Ut{AF|NQTFPaZw^zc>B?U;p#&m)ejJA@bjT z|5R~dK52@7|KqR2h7M-k2O(l&qPK2Z2Y>hNKVUYSCr%i*Wz&y?hK?>SDTTd%e);z= zJpXiEUH!@**36zYT`)#)i$L`y^hjd~*$M##QRWQCriN(DppNQL}j7Kjg(79ZPct`f=RkGd#>%KC; z2BCno0G720S@p<;|MdtR4OVdvImpZ~Zn_044s|bT;xRmFCA=2^>P&zi8iw{PC>fmJ zN2{Hi!EfiR&!sG#pzb&i^rhg#$`vxTTB!+CYZ?vc|=p??KS2hn|?69*ife{Q;h z)yiutR%g$j{fNj-o$cfw5f0v(gK0GQ$tZwufIMQr3ieDT)Q|MF- z95UwE;k_Qu*vN6y&YnD6UsnU$e6F-Fv46lolekx#d?vaD!;SU8Y~PvsWb zC*r|mVaU!`G`Dsz>sucoqU$1FFOO-0QMk7_oA&~Ei z@hx)JHC{{jxhve$#}aCNPy6Gnd%ss)AdD{;7_4KqKk-$_EE`Drk^0-^Cl8)`J+i?t z9+o)n(OAz1%liqT-4xH1Jw+4NI|X0YKm?J)8#-RidO>)vD`GsR8}K9KjyyXY3OExI z5)`Wd^e^9OcP`oB9Ec~P$bgxLDg}ZxAT}1@lm@5V0J}kc5HdsDJGz1E9NLj{B+F|F z;^)Nf+$Sgw&Jy5OI(E)+uz7UXMNT4I?8hr^5P^!`iLifI;(t%v78B& z-D*-vt#Sr_o4iv|nZ&|Mtt>UO0a4N>XCyGfZZdK6%s5vZb`zAXT*7sAiCB_jT!rS| zd*}W@KNdQ?HmC!vcFyOB6JZHbxCHlw-WlFRfF>cO>V_{Rg0VaOkJ8kHzfR%69-uIM z6&mCEZHl_=)aj;pxo9Dd_u;Ha$nZF?IP>B>NF;nCit+I}bIgblB_Lj%jbQ`_L%xL@ z;(j%>nsS=Gu-=*XiI8W`A+(fRv6y?dt(@%!{IY6mYdu~l++F9*GT^>*h8qfnno~B^ zRNWA6IwQKHd>|v%Tzr(S^99 zIg5S3uyKd>Y{|<$D@34#goFFP$<(fdZ(uM43CVOI_t)wg4~$61f^0H85K8jEZ0NXw`+?*&z&N@N1ujETk);O6qSb_enhX=!$x6Yq0waeCT(y!qCf5aHWzznMLIc0znY_tfssKmQz^pF)|xU_n}18oAbRv_R<>+A8OV z&g~NFSBlm_qyPXA07*naRNo;2c=oZqypuA5X$C(+#rN}FBV^){F>yD~HE2nKfRQ*y zgY7oEbM^#XsDS!9B(6rIa9*+Q!VuAXW7f-$nc(@4B|h{TClc`q_;xT$MD7LHfXxVT zQ48~y`9Rtbvg6kt-V|5Cu#VAex3U#? zfSoBwoL}8~8qQe0Z!YVaE6<90zjnK8K7u1zi~9A*o&S8exF z7Wnq6GlX{|AQ~fgyLX~l03ZjLjYcEuu22XU1=(=8J*2Tk69?=zq03L#ToB9(9OrjF zA>Jj!dOk!h4;uc;lXd<&-0zd~7CP>ZNUUW2=#hb|6_2i(N4WSezt>Y9b^h=reiG-z zGXLF3BI@Az5X(IJ#>9EyjGsArqI~*D6VweW=qapPM$9tS!n+*>u~Ndh8f7H>>MnH2 zBP&#r3XnjE)WXQP?yXWGW9|MHJMze(tFC8Q8MpMTt4aa5<&y}3C*RJwos=@7bx#<_ z>bLZwrDEBFi0UGt`V#*YMPumGNGm$%Zqsz=L(BiRq+-!QFd!0NoFJUhUmqMIR#k*J z+ygH%0Abzx=)zPghWO~Xi)Prl_1-x<2&q3~Jx2u5ToNMj^=x_G5wDE^k=+lo^S4jJ zNnsM1L=Jb8D_DcUK#el^d%?;iQl(OYHS6fVEcVx+2uxCDvgeeJV^IOfh@ z2p`xu^y{wgmwhDO)vM2-lg=(5+~wh;Cj9IFp0`-cfBgGfE57^)Hs~&cURPX@*N30# zQj~YeV9<-LM}PO%|2pzwvps(AEyO=?Y2PgSOm8sUx%fWsy$K2N@VBAS(G#w#=Z4c0 z6BFQXN=g?RSX)yIe=900=~p3KplkP@#RZpq-i%xCeljjGrM9YKO}l>K_Kho`iuWi`?-V)@ATAVu z5;1P_tipmE*v{zD?_w60RM&19JGX9VY^Ws&g0vDHa*E0a@Bc3*Hnx#}2`S}ry-v4q z;qBrGUw{1#j8V{S+LZa=gZFR0?e?L=h6!eb{L!)xq|b#IjtH&w!M2*VFU z2@yQHN#0W`L=78Pzmai{_%Uu{fSv=$#Jv~l$e3u5N)*A-TB1EkUTh$uaX8|k0C3$o zQRCL_bd0On<LZ5f`xX>X&^uRZ{@#Qbvhj@n)pCO zzX&?rz~ixZFF;4Jd&mcj2-Z2-fp(&(MnL~E&Jk%=tW~(C#Jqn6ZgIH&lxMe-6+0L7gZK~+gv5Q~ow5)=Kn+9OpI*P&@{;hpyVIu~VP z-)?%C-EUs~&z>W^qX&mQ0>c|$c$7Bv%ODP+6{Fbqq`#|Xg%^RVO5{9dLI>MThTvCM z@Kc!1#|WC5(;nlGtd?CYQ^(R|yClGKoU2kn_bQmZdy?L~fVQiv0@nCBgdmp}I{` z;fqS6)&AqHF9^RT&U@Tk6^?!bhZg2vswn3iUgYLn95!<7zTMk}ME367I&$npI2r=O zBO=A=2R}t+@Sp+x`t>O&Ec)(S&KLx)9sWM`H5wW@JS+_U zR#tL00%gv5D4}7Yq=>}E#L%y$rDYzcNXzV(bK#WFv#6#ksi_nqq-ON$(X+o?t^{QvEIjhq;l198z%M>7KB>C0%%{N7EiDtCtCVhOAhgv~ zSHvYIhlFtDpO)FDvEDIrBsD#=q`07WzadqXW&H*YSE;q|*i$VdKR5eQ_Sx~1=bSrz z{NnkO;*=ATyCo)dAqaw8K{~1rj=S%^yS~2R!w=pk{EU@9{pn@#USST4zzH7Mc;ST? zN=r+>_~Hw4ZNfOZOU|Yv4P?9~0*Ko}tjo(i~L5PvMX$Sf}$s);VnM z-fbkJE`-ovatyNzsYK2TG>)wLl72RWE|P$8{$HN5tq8+YUi^qi+AFFYXqPCtZVSm zxeg+)y71mCZ2W)Fm*(sStR}Mwy7kn#9m~lG4%hl_OJX=IcZ8CJ{)*jfw{nX^N~GKu zgPAp1N_N$0c#mUZv%i*6e0=1)-6>mW{B9lA&XT)K_gB5-PkjUL6F z+UdoBTMUsmo_Dx{WL?d1dN%}fYiA}{!J@!%D_F3%;M`5IBhA zf#IjYJI>;~EA-5el#ICBRf&F{u5kCBvY=hs332y-$ZxnFaiM&h0kjXXPXr;DRum%tjsa2@r$&g=e%B2zoJXO?U!?_wTEgo7YuwwHZalP$%TL~de3o_hq5_FBfy!2>DSoqui6CkKhStE8czid>b>;D(StWFya&F1{?S|GC(j_4K}wfo_y}Lq z)4CV*%!G|IXU`2EHe}$y{`4}33?2x9r%!Y9M^2qO4Q~?=M8=LDb@uG}abri*0z7`g ztt$(cmXy-{mrX1Eo!V?NojSU2(yW`*nh=A&QP)tPn|;n`(7~s;;8I*-3J4tOJqM(8 z?=f_gV`}!}FTJ|-fB&eft@4=y=^XUxH`v1Kl3h~MPaXX=FDGl*sEOhTxfjpbESAu) zh%+Y-Po1@(p}sC5$ukjnq&KWy?(YTekhQ#%gug86_ljt*)xb?A^a>YC7Q_U>Rg%|E|rT zv0AK-K`)cZK+QMA(Sl?4yYY^j~MCEWQ2F{@Zlp~fBiN3wIDzLFMs*7 z_^95!dy`HH?{R?Y5v=Gh-Nm>I4gOk6;zp9Ii%#5wgl`@#e$jxO5W&OL!V$t-f4DVI zX`9K}N@JUu+)h~6&?zXI;bMAn1?41tu@oCx|lP5N*4NvlauZ>#(D#tb%{CAetn@MnE2skuVFkN*mUX|KJ{!s&;k3a`J23Qd zaw<4(-B9-@(=#c@4)<;V>;9|ojvovjY4VST)8>UwiOLFeju`EQxnCL)`wC$gznS+s zokLIaxmU0cVtfolyYvw8iRe!+aHlSq9t`o3Veeo;m}qW-ED=o#V0^n!ah=>fCnPZ9 zexR4e#QM)F%LH)|-hE)VB!XR4no}+apA{*QNV#8^gihD9vl5}km9Y{Tr-1Me9_tdZ zBmtF{fEtI`n+7d*$D`~NI0XHOA%qeCvS>%yc`S!el}WjkrsZ^z4mu}7Fdy)k+alDT z)RBZxe~K$r-e)oqo`1pZ3#~=z>4a5LJ$IeEzKHFNXwGB6Y%sy78{#HIV}0G~@4kHgkN+M#e9Xmjr@N)~eCFXh zjRvl#2LliPdi&GJsOXw%N5>xWy5_s3bc3{%{l6Ymg~SM7>#L#Hst}fm{o5=5nmcP_`4{ZEl$-bZoA3Vixu;(F>&s(CkDNV=Q&`^r;FI&`v&88q zbxEzNDD}>vN~6_=gjJQ7z}5F1II^y$ifR*CXODL6o_X=yamWVrq7Ur)83NaQ^8r0d zs|~yB@juX2RQ#pTNdOs$oA@bVK!Di2{ilei7(ww7h>cImyTsiNiBw`Sn@BT($FFO* zv@<7xi*a^Dkw==^#Q>Y=TCk?K^0=gsXSi3+GNiT_mU*KxuJ4 zeBu&r+qQ`_4-X5E>@$Ef8&@inR^F;tqX|jt*@s59^QNW-oldRQ5^m+4f#_ruL0Xaa zE^uu8>8FVkC#_zy)+2oX{(bxR?W5cM`uAV6*3HS2Pv;uYgj%}ntJLn@|NQcw$Tf;y zZ*X>B39nYuVRXJ9&X4)C4zGbWI-*VpajtK{di01Lom&2nb0I^=4#DXl5oQ|;-FoL7 z5|*WF^3l$%V^)IWD`ACBJ3TRD{~#Q*>h07*naRF16kIt9NWQWoe~Keu8Cp(Vr#9Q2HIugPR_I!xe>5Kl&M zmWG{LxQP3l&K5Iqx@ff99i#Mo|B0WT?rFy0C_jew9SHS<=x~JSFyY_~&87T*+~}<1 zG=e7u_XokHCJ8siOH$y@CXrI_`uyANL3N6U32cnqijp)MyADdKF0_#O_)(m+{wAVnogqJYbu$ zFCam~2HTnUeWJ@bys5N(7-=v8wGA4@?U@kYDAx6r^%22UhX%ePp@->wqFv8EBd)7Ha49;{; zSZ$QLv=o_d5JBVcuAg0G0~_Kn!@^CNA8-3kGz|VhAkUbO__0hRCMgcbEl}f0az^NlgAECm^y3CcP>xxhxYIG znXcf219;~@e>rpN7-lpZ@P}$4{KR z@1DEn&7FPp*zr$4U;6C|PfLyT%>Fs&PkW~fg*JWO9Wn8Vu-(v5w{_h&H2m=X?GtA# zxZ}R(pnY-n#6f>^TLPreNdUzG9!0rAnRWK$m`O8Ddfn+0hoj>X#Bmamx?DJWA|WZI zy0W4Lr>H0`>e0LZprNBBGPzOT`0MU1{fCT(7FS4Uc(?AEg}K?4WyNd0`@Czn3~gxG zxzk4@BV*ukoig)Ag;E98*Sa6RqQ3~JspzB?YQX^;nvhU^Bj@^*f4NE`qhl+}i}NmB zNKNaRec`lPt&NI_gWsO6MT@^62;vhaBqD*dHYw=?a?TzIGIIB{^h=j=Njsx^_tZ-{ z*`%ZK=|^unW(0F^ZA*A-vi$r8806;`FR(lRb1d1(wN_w1v zoxN>tb^tZ6piLcK3*cu)u#RpwoEvlkgWl+vF2TA++o<7&(a8B?@Q01Xqj7sZ`Vkq+Vmr=7XW%Z5kJ8K>E{(1j3h`y17VpS?+oZUOI(@|Q!IJo{4C>wmY$}S~#nM7*k9xg^ApucQ1 zd0>E~h_8pC$wQim>8pzvoIY?2N6ANI-EW)rPTgHP|1Z#8$yg`6X*Ao-=;Qw9- zK2q?$lRj4iXMEX!w93y@@bberUfR?okgFxaBf=C4xlln#oOUiWpYU=J8`OyZ66ei2 zbG|q?FQ15&b&r#a{$<>?9PvLueSOEaN?7#Z3oE{O!)$7{`9SN^y*Vw|uqYqBfSCBM z1=%M^^MUq%Wm$eR4+hFL0dR{f6io>htq1YPBI#=iIdRyDxh59$?Vxl2f|V zWn#l4qDGIO&T_KRPP5tAXTUJQ$>_hRwpE;loaJdob1yz-0eFqLJ%D+@wU1>I7XbKwYK90NDv?xA3?L5QD5 zIp2hpxl0Nzoc{BPTT_Qmm_JZh`NC6@aGx*jmKm1y9NTj4;+k=Ep1*ix+M_EgG~9(2 zFg=zQfVfR5ZrHJ`NB$k~)+=^#dtH1LLNcOWD?>+F?ap3=XqWGfoY0pbe0!AfvX>U( zOFU*@J1rXcQBoPdu8D*W&)GSlN@61tlZ}-)yq!3QjHyLD%df@56^j+b5njoWLDQJ? z<00Wr3Y`$nKFB$0$|o!3ZwxoOEa9C1GOXRB+~}2xeG0nYL6&;i;ETe;dYz$Db({w> z!q~4+N;sc6WZIL}KiA11g|Se6YwvZoT{b(BYhz$rADHL;)>{Nb$o8piH5 z8{@RH@exQ|YIM`LfMtcjb545X9yL2}wn0Y6^Vv|{z!cN*qK)OVFbYyJCXBb zE}cQBlpF=()LHnMn5<0VclMl6fX=VN9@dA9#<=#0{_(TbVOjR!zX;OCq-ONaI?I{5 zWS!+YMIaW?@VeUSGbaxZ896bdXP+}CefP2guc8ta6UWs&DwRT^I(B$(RYfTX9Q$_u zoXIcOLYIk!mbArWI(_^gJkt}V&8w@fve+zwy5RxTR}-dP$IrO7$HXO?j0TM+1df7A z=jV0bL4+|AXAT}Qe*f;x)fHs~L6Fu11&(WqvNFyvkMU)&zz>&6#NA#(L<)i{4cg$7 z3F$zqk+?dRL}=M^$J$xqtG`E^M||UzHnxcnE@aGSw?xzv@X>FfQ)#?=fUN()b_$(d zB5HCm!e5EYX({Wp&vo@D;Z?_9i`yxLel=ZwMRe7Kjsb&IqEILtr7UqQXGjYKaP&Sz z%Z@UFAqKbBfv4$*>xPM}7=#u%wDsX!T%(kAb<9}S%>#%#f)cW>cghXHe9UJN@%0$IcchHNsi~_P=RBq3B!u|= z&0Pbc2682dOGxh4J@cm@zjg&Nj;bZOYg%USf$M)*I&jz+rBaoU*o96N2@7|uxf6I5 z6~g<-ptfRAg9u(ctWN)+ITz2uhfYyxY~bde?EU4Zv6E&M737K&fgu-dNT`mVlLEEP zcHSRH|G^^-dVNVzo=(@eXZueRrq1o!JrnAvy*qy%JZ#*M;p4K-ohr)D5zd7m$Yp{8 z$2A3Qt_cWZokNjVURWEwI)x(+*ZXiR5WvkXhxZ)cJ~vWJ8Y*@`tDSY)k8p=Dygw9CE8O1&3fc!vN-C90 zylhNZKMQAyd*dwUK!kBjO6L=l@H649AYE_~WeoLm!5MDwAKqsnTmg)l+9YgT~x+LWZ_37 z_;FyJQ&b3FY191$jMyjcea z#1wQ2h{M%{G7w)K?;X1c;T1!~Z#~hv$P$Sp_$S-D#_*^v*cPaOzLE+iJg78@$Io13 zG~t9A+VJy4?NsdGtZCefOPnSxSjXc{2{zihPgEe)+UWaPMM zAaph~)I~+dbxY0IvE@hjdT8G-unQ!E(w63W=j7HA4OW;*hfWrKoBL{X7&X-DG zo7FOE{B)>Qx~67?MMPc7KJOhL8gVV%q=12`7>M1 zCZn#gJ|QW^Y&M04hUVsEL7t8t+yxR$T94kkhWg5i5`rK|YjE`i4(Om=W`m&BVYu;m zE9rnp9Fsd7e|YElTsUn&GPIG+h}t5q?u}ntT%m#M@J~kEd@S-!c#-8i-;8zyN*L-! zl(w8$;_$M_@O~Xx$2X)XhIMj`*w|$6UVtkk%;f<1_AU7pD=zB^pOJN_i(qI&{2Ha32;0(uB&9J zpynL;r$bb{ZNJdfWceG)_*5~1=$U)2`r(jETHc2WWa;3(OKis>+zx*S`UqqgHo#Y( zh~or&)v%14l}E6}!3eT~<3Qm-6n>1n%;1(){>^Dn2bE zl(XgFrVv7P2%%OBbSppsF@tohQmdsBi^*t&gOn;5kyWVFN@)XQFz8ik6?AZ*lLJ+7 zXjqs+sT2uzqOxLzR~FZ|v0AJ)yF*;Ixx80%DmLe^hU?t(6FA*e4~L>eL=USVEi!}& z@Ox)`&3vKTQr>z63%&^4()`lYNv* zg~Vw*$loJ6!Hr8_!i&_-_c*QmiC}_63&3@Q-4gbjHR97E2tfjJ_7pekBtP$>5TesH z()CJ!>ZK}L_`D#9BQ>1 z_EM8$yPaA_5T_bq6XpZ64$}-qX!LOnG9qefIU_K1RS*6>B)SeqBI5N$xTsQw&he&G z!QpVZOs3JOX~tA4?vGjxBZ^Ai&_lQ!o5%{93J*3===oJ-t_E|{tYj{XZnh`0$ZRsnoWxpz+YXNk~~b~IP1lEL?UB#wr> zsEO>zIrnVIgoZV32lKZ5j3BRxny*f6i5}=UbMOL(BhW{5QS+H`3klFMWrLnnA7nJI z!-(tTY~Z9=XK_1rGon6$BgEMgb=(Jsw+qUV@l8K34G-u-sEi*ecvIx9)m?E?8qi>uKkPT7;68^_I2vD&!Rjrf(h zWl}kB4acj#P%X>2Stn5OhJ=PxRaL=wEPaV6SfzxkSQ&?B55`fY2~r2+E_<+|6}Ux) za)jXp4(jYf?61mqjlnurIA<6iMd49U9w`XwJiHkQwRYj``}iVq*u4d+qgXb06_O!- zZm(mf^RG|{v~W1<8{VA?|JXRzTXULtIJIoX{nx>YOTb%m_0EO69Ma+%2SE@7L40xb z1&+G9+K9-A^0IQ$ZitMEtgWdb9T3)Qj)r#x@9{ShxMxD!GXCt|8PBtOSrbe=gqa|W$YrItK)67@$w%9NkQuF3Lo-9|r6F9%3Cqu9rcUf2rcE%* z5H*y!Awzx#zZ9)wHixrI>CkbgSATIa3SU|AWES@V574Feb3Ywhu0W4CJ`NL3!}Mqx&bBX3d}uZ=h+333iT z_z5ylQxm~_&b3-PU1i*17(xVh+d`=Gq8yTvQ~2lA8cv}WTtqXxJ115&eTD>Ofz>KF za%N`*za*XyFzdXDLbxDcxv4f%M)YIx6xBWKmVD3}zZXP+>997)2!bH3#nl%$V4xm` z#pB}R>T2tt2g_c)R~@_+pgSEN5pFgb=!CtFj>|y9T?Ggb(+6JKgy<+j-1Q5O&N*!i ziNy%Qj~8)cZoHEYjYbn085tE71?mRhNVP+w78EEsu= zOmgKcQaG4^#u2Sf3sPph1h}^E5Vk&-eL6RFNiehWt0cDQ?Eaz!d6s5zb;XD_q6Hd9 z3zBNZvs@;ko5(t5l=zCnHZ>hdq;c2@ahup8)V<=Gjf%lS6%F;Vov+xTB9+NF#~Pgf zOMa0icq2eP3?ewU>N#B<*2aS9Olwz0{Ni^r+}~wXPT%`s|oMJoA*5JkgXlBX{q3pml0o! zN3mb#k5@R_?p$+Ma3)3fb03yr9X36#o5+f0B++ar!0g?f1KzKb5JPgoyW%vjHJQ&oMK5W3%B_t$p3j^}j92Oqfpzi1( z5L#(eR}i0$I`3p8)E1aCxwY{vU3xg*s&g$*NSu}(;?^6>FG&d6N@HUqWDm5M(9lr& z7cvIN(xh1DoO8$&BcXDJTT|FE_UC8^3(a4l3G29f;`rjP!mSkAmqPJ6rf~!(Z3L+- zi2EeL!1$8D{H4kGG9wC*W^*51pZx*xF7NFtgX@fbMl}OuDaaewylk`sn1BZPjI)k=->&K-GUi&R~dJP37;A4hvC5M1+@@S9qM>YYVv&bX*%` z1VND2qP?u50`LCHiVA{U9SqiOR5JD^_vxPIw zV7X-nr2K+l{4{RPPK28w&b5zW7c{GFEa$b4o&pktWFa$kb#DW7$-OZ~oH74Oo#0G%M;Mc$TeLC?Uc$9MZCT+rIq! zc68i@C(_oFPui7Efm}&ieB)c?k@Pk$5WLI3Fp@cZ_(%=@ga`^mVM2WjzU!ZLJyCNBXKGtqCo2>i+ z44tpRFs>PlqCknTT9ZN7NH0&cke0&^g)@lYV{?_il)EA`f%gdaQh=jXDiypz;8gioe?8(f-M?j7;;VV;+~?!ejL0gO70)i_$Izfj7Z)2x)dA{U z1B3}^twJLY@HXXwG3V{l`Gd}e5Vh{!hw>ohZ_4L+7yN;`EKp}E`G*a+!>C=(SG1sB5424{uY z)Ixh*tWs@*x!erLgWMDnX{! zPR+Db-(Z998&hk6fImi|awfhmlh$DjHlRu%D=NnsathDyXn8ctZcTDV&<>#+n4Sks0eRBYRw^$Ns04G2&RH<)(_U_;rzr&hpQ=GbGN&y zCgRgV)0Hv09TOE27OJXmh%y+>R-3)aTnPVqB;BaPo_Pzo25gWK1VLI06gaLaJlY%{ z&1l9~$Fk}6w6mofOe;#%?{b@uK%218c!DOKR;x`&OwekybaDsi5zw$JD=UL{Gej1A zTL|-|g(f2aal}JpRH2}=ra>#A;gCMGeLchlZXHK{^p8$Ofek?&5L|Kym%z9L8k=A{ zO$|nbywRyKkRKBdoYArH3LldW8#+IXM*-7t@#Jpe-WvA)>d|Nxs~bW~)aQ)|aF2q& zo2?$xyQk!H+h3euPC@*SGhaUbeq7Dy6??5P>HSM&f}SjJIW#-xSCAH8FA=?4n5exKz-E<=m-^@?yE z&9GERvfHe7-hDRfGJ6rV5EysW#-jp+uL`*=Hd^kPy8E`H*d%m;W7qpj;JcNZ2-5km zTFnxf+-^l_20=P1&>6E@+L8Ak6gaLa{56z4+9-_3BBN>L!dJ)J>kPj0Kob{ckX!op(0(*OVv07*naRP8+>;3~IttUKrJY8+qZSJ_VVzDqC!O18QX%MVP zcqOD6=0(KKD8ntOU}`6!vZXRM;W|GyTD?QX3w10_jXG)4>GYrxK&@-BaPBR*sVxj4 zzElhnNVMXsXvXdlhxI=~Ammo;9Mt1PjKem>kt~gA=N1v?J|>f~va*sI{ei{-@4m3G zFy6q!s?+Oz78n23w@lVFUU6MVJ3X+%n~N2V3($fiUa$TDlAI7OSVa1$xZDRn+TyK* zbU4gLgG!~*H`WmZK{_TXb%@!dC+!9lIIc|sclIu)W5ak1kBiRZ-V<|~7!arTIFJB9 zT!3aA!#SZ~_%%}ub#?X7NEN)xiKGO_Bn}!=d<9~yL4fKSawpt;Y!TGjgE#e%f)*hx zZUXH|gF#O%hs|b-(P)4O27>{bnSv3Dcx74Y1cX905js%?4yX1hyo>@(ZI@GhK`3!f zb#b*Iy>5z+8{u!%PRIrqf$_XM-tme5G31uPG+)|qz`Fgzx3n3b{SIn7p zWfT)IHfbi_x%Zt2<26FwBrU#KZP#t?xN68{>RfCPqv-xBRiEJ=ig1&O2b1AIaTuSQ zo`(yScz~2y(Zmf`noea_SPM27QNtqu%S*S9D3rlfw?^#)APrIN5tXGx4`f+2))E09YV{@qar(gs{eSXih+ zA&0*pHCU_;uhqn1?#I%X7(Zvz?L;kxVLYoR`Zly!WSoB}LFHis4QU7(mMD(f+sWgM zPJx5*er8BVu2PW8r?I9gBs3ZY4g~4Az)!fbu8Oo9P~fg=U_?U;=dmJ|>fyE=NM0%=3p6Aywk&0cG6y1Wu*O8R9#Eg1&SuP z1P|`+5L|);32woi5Zv7@!CgaeclY4#?(Qty-C>=XoxQ*R-1~TMYcw8Mja74wsv6oy zADTafGVZF3)Z8Bm+G1&;%JW-tJH9$>>7;Pf6Ynq=VVQK08%NhUBA(lwoa%+_^O*3< z5^0ZNZ&ai;JfxzAUj}=aq;pFrYC_qan zK}lsNN$8aJ(=$_%lJ8%#>UlZYxPoYf^z(G)pF+!5d<;AD+DwHTW4?hIUD_XyW;!xsev2`MZ?#w>n#D$1J@&P{(;!hfv1JS}rZ?Mk<7wn}deA4!wpAqw{f! z=F3Kdv3_g*0E}Cv32_I{6evfh_a1%^67YF9QwN9H@sQG3WdmT~W7s)4=gudxc$U6l zr8=Uo<2s})l3KTirqrAU;=cnq&TdlKet+&vafL(d>Ef(&gMkmU8)PS#D4siTyt%oF zHaWaNCQEUHO7bhO>hidO?SH~T1mPAm*MC)y==mEEKOOzAo1mR*%VGk;e@;XP`3 z&jMC6p(XQ&w1!#f49=;nueMUsT2rDIoGRz<&eVRqI(;nuET2S9Eu_wcIxouCtW-rw zWbi6bN7u)rp{QIo$xt&B`Q7?UQljej>rseSyRhC3z61$umq7&456Kt-4LhrQ`G|q8 zOozv45gu$Y#v8Ga4bL6;mF4Bgx-sG1$Y1aX36Xd|w%F|tOGnQ@Yjyqp9*XOWI^K>| z1dA!3LO8ct|K-i#0uZ}fBY+O3d-kd%OmfPrY4~_)nP_F_6|C6VmsSes$l<1te-QnY zBFsjL@J;a1QDHHY&+Pb7OAvVPq)k;wH$erd1F7r>crS+`7+pedGGpJWa257^>V??S z3)@^)A=1N*iXq$LaNbkkPhAR1@q(?7%;-*oMaue3B1uA^|3WMdx=Kcg%a0QcW9HGP}o)!!z4cfM5 zfH}U}F7|1aCRSSW{PveMP0pj8t(mOtONgLMv}vabxc1U`%{*(kmf0>70sb8W^M-xgtx%$d2~v(?e)xH0=FY=IbJyVxL|>)>Oe`#z z3h6`N876zNRbviLMtYi06DymJs;>dGJ8QGydt?`u9S>!trn<%4cozo6erF`2?;N9* zlPd@IMFXVBUT^pUK+o|7jM|a+btEJ>q&g5y_z%36#rT4oeVbx0fC}X&n=Xn zgvBD8K3in%Itp(Ax#;IKmPjs#h&)+myLVMNL*jjmpcBx+>j+^2mEZ_5%b+p{-~Ne# zW}s&?UUEyOJI@-9=c01eHdWM+amz8};0&SFn*9ir62vRw$xr0lB*gqnZiF4U?{#(0 z?=g{DEG(VzX$Vn~yMoePxTTmiYz01RVt)yA!-T&y*%^+DmnzCotFK4zzHi97ev7)Q zARo~k?+^nGJ2xH-E9S-T35Xn+wE`htKw^8OQR?Rki{4M6MN9^lmUpr$f%!X2BVVr5QV2j_W>N_;g$$>Q>^^*6FYx z9kydiz6|wh2edFEW)E@$yZe_3?%L(!5AVcQ)27^*u-LfX0)q%GEw^71lZ#|SM*A); zU=h}6w8dtLppRG}uw)>=th9+UWpq#u{?-mP6X|J1n{vM|b@F|{3u{I_`2=GMOm zh#N!UROq~o{U*9bkF(|pnTu2mF(D`5czhs=k`j++WlL_~)l^w(n(uh7Vbt0Z5uB{y zDc8a(iQn@L&3_0hT`Fi=9u_IrK^;ogpR?lNCzknj=P;IPU`m5!0T%HGn!Ut$gF4@i zw|lb($}NJPC5x8T4zK48&J?8ShcFi?R{I!VqOu3Yvc+8>mVcfgpg6eMEX}@rgD_7f zOzVh`I>WnUU5WX?$hrRU02yBb-+^YQ_Tjb=sH)rmV2xOEbT z@3le{?{pji2gLtVFbUj|mB0E%CLS!cZO8EEFD5Up{g08h6JqyDe9W+Y<0L#&n%c(jPm{6`$uzEL+l#GF{3sK@;yNW^~x*$fY9Mk_>oN*-C`M32^@ra%vwWhI48R;58` z|2>ncw&AY#vEJRbcDcp5s!h1Z7>%7Rv+Y9*!2bQY*FZIFL&HxKZ`#Ou2cc23EEid3 zQ}f}dPSFlFxp*ZNJ4tlZ3~1v;k&V!F{%8{WG?04-m*_H2=88kWZXpiE<-X#XIQN_T zQ{wHWANhSxz{!TvUX;0*vp?Qi4*R_>FWTdS(A;_W^o;*JKvxWp*0`Cu`;MJedC


N+qmu<5bsd;sn- zKa(o@9J?*{_tQ=&!OscoXobWT{86z&WzrVA9%k)6GYDD=F}&IvAvMg|uZYe^kA@fr&)dqkxFKNwY+kS7u^f)QNAm0B3-LG&^LNT8|t&&a?g@ zrKmw$(GAf-LFF2-+Tmru@w2Mv$| zYHpbs{SR^mx*sHJsVmveJ6=sO(e_iSmAWFaG99gNO~X_T^lc|MU~z;;9JSu6eZ9@V zj_WTzXJ9#mpVZP>2g1K{2oe^W2&$$sGW2+I^-}6e#^UU7fA&PbU~#pd@6}?kUca)u zV^NoA_Oru^BM{)FFKSeFdY#gp(&)@52d=*TLv7YL9q=gs{Y2A4A`tvhumAoyx+-jZ z`roIhx!rycu>bcd2_eyY?*Be*G=GP~>e;g&QkkV{fwd0ieqm@iB0Wn)5xCIGpk49o zZ)I)NzIO*3jH8K)O3k0L?T!>IzrZ4;uzN+eC&uNE9cgSG0vr`3ke&NkQ^YnLsV zWeu-2S$|ELE7N?u_~)8}oz(BfqMYKTiwKbG702Y3NchY5+t%hQs@fVO7BGdZMcP$t$17@IH~K z^$`y#@T~38-_hM#$1y_{-@Hzv)rC-?a&IIgM>WN&Z{$VdhzMIu7tK?!ObMKB? z*WvT&d+NA|GcmqB^!V9s_epGAAg5nGYuP@DkCl$9Rh;>eLy*g){}&R}rqGXnE)F2+ z=#o!PD$v32Ap_s3(Q?4vo|v+Fds#&U6}|=L<5XKL74TQC9is*1ui{GTDy~6QPnziJQzx5W|R_$vfiG z^EPF1uy7^jX>b9bo#iX9+m0z0c zZ3Nt&QH5NAH`UQot8v*=T{vBJ`6T=eTWBLuV6RVQ-8<>$iqUZ{E_-fyzVXRWMGMAc zVBqT0hEf1@Lfhjtf64491x1lGmun@^#5*b$*IAe#LBYauLGm??-|2bcRp7+HkL~rZ`Yb89A)-afEQ)4pd11n} zTG*=vll}|GoC|W2viQ8;O0}s4F|x=RaCWvo@>jE}+mZrlZ!!TQ^l* z15sX@Ocn~^&x74XC(OG6svqb5`PE9VaS{0k8PRzK@IjDGhhCIzAZagjcmB}_jFQJQIc%Kx}|Lx&`%0^ENBx@17;Mvpw3&woKD=bCm4_VaKB{h6XBQ3B5<6lyS z6I^TU9v;qfZZw~RA717&I6`@6Nl0ar+|thDnT^|0HwA^W4M6DVu{Z97LMs~W#=XG7 zX#!80v_EeHw8H4yU$XSkxYnA2^>Gf3V4!G=lr{DA#P56)tM(E`(Thg5*!L4zz1H?G zlqk@(KlH%T*ftC_m`P1F2GU(bR!01LTnFN_!5f<-wz+rv z&@WATE?_c$iw1%8(yTVl^UVorL%8RD35pdg%7zf^v;g3Oi+%oF;CPh2LixFD&T_5N z{eB-l2<3RN)zNO?e3EAo+jTG-{{GwUFzzczMBO*<7}6DWv$3p{0iWNxcpR6eTRaLF zvyHkrFM|fL$pfo{B7Ev~Z?*u37}c^9>B09?AWw0zLz-xrtHKXO=y`$&I|quJ@4$d1?mVV`3cfulI~JH#v50 znEsYeWYSS8DWON=)$!JKHnSr&!BbFk*jtVA1mEmw`DjX$tNe_&i{x1u93Q=q87?9< zV<&|f^YkMPhO5)`c#%b{8OsoGHwAY!6D6}6to_d5j}P{Ha)wUNhUh?|V`L=aG;dj` zfsyczxVbqOk&uz1rDe+e5ra0_ANkzmuq+Q{H`NA(TqKj87^^=4jK4j{FRfWV$<33X zKu?k(RV!@Dy=9^usD99_3CflXCjy^Qt4wY(&DJYrPZVoJm~W+wpL(7miD=Zdo2u;) z0WIl&_IjQOxHKQk`8~JQNqa2W6&XcJkQS?Bj=WPC^V76^=;#?$OE@@YU|vs` zUazuVJi@8`P>UL9Z0?u^#ryl>v9JM@AkELA;-Sk^vm{3Vyv_5hQZyr#SDc?u_wnd? zA_jDOvhuBF*-B`PKEKD^@=5r2g{7E|xBPnzJu^+o zQXkLOc3FQ!)-PI8XqTCUOUuzS4_(arq@S-B%ub>~ZGP5})qekeApf(^HSVVQD*0b8 zVta|4wsCk<&!r&WWXbCHi0+G&bm%Xy+jQ~NkT{baZ9Z%wA{P7O?K=AQLqStl9a$v( zMUDCO+QVgLq08?m1kFBo&LzFPU(YKC+4f%UySZx0n|YY2!yMhWSs%h1ugJ5rvp%M! zIM}V1mnF46$tCmFHeMY6`BQrnlIoKOd1zI)wenph1n2-9?vb8^4cE=b{ z&(0~wC&V8wG@-fAi{IM0?}unzskdY6T%aO(Bj_IsgzAQU_rtk;+bP8xxe2{zgbIh>z64~ z6hcK~twRsp>AI9%Yc3z+7yj#5l}k1`pT#)6&XntU2bSNQvtaP0Ql;}c7go2tgIO3$ z7XZAi{WXUId5ZID39t}3&^Y!J3mf4gC;|_g5CfLqVPB>E`H>B0|1^E=O7YfhDx^xC zQ5^Zi@066HMpjc{vbxPlggi~ExH#ls%s|J8$G%58 z5d*7X#o&-L&1f%Q_6)3A@Y?{z1D2i$5((<8Kg~HyVbDQT6s0HbV{a_#Erf+zKaMZ; z;P|>`$%fw`w`l&WDYp?&ahH3?mP&>ZvpT1R_z@+H>~UBSkfKzJYwO}a=dSu4!T>?G zw8sT!sX}3?Dw`P_iCsWf|5ins!{Se>3k?RfqknZyEk~xc`bhp|w)A6PQ{)jBnmzIZ zGyPDNCLU>K@T{O}MT&_%+87R&*rxs@s*4K0xZJQbfyy{*yBub*-^9H{Qb6cFXPtCU- zd>e2SiCT)sua$=h$h(OX6^pr-qgk%MT2bOjENiN^D5t8#cd$^Y(Dstqmn1DL(!4Wf zuJtLceqF2{5h3jLu9G?!Qqlf!Whx-5Q!h?V&;3z&@ZbvD<@jTpefQ~I+rs^&PrJ{x zO9ZBh@OG*WtSaBNZ}}A3H%rUs+aK#hx^2m|)B<``2<_o>=rMr+13 zFE1Gt$s<<#&zA@tsMQZPHVAs%aTr}n=y|M38YU!TH!@wZAlaLU2Uz**bbj2nxn1lO zd?!|;C(Z6~qs8{u7r_kAbL17mBIU=eEBSS|`4_CP_`g0u?RzmYTmfTYVZ{Utag?#N zGn}m{Q;|PG(CS*9m6EmG9MHM>h(bg~fJunq0eDy%R*}vF>4aJi)nk9&YI`Y91cAVX zsin>)Bi+&CW(i3=DuaTMAV8IEr?=m^?O_n9qP68RCP7gg1#`N1VE^3})nhPx=1e?cQ2?KLU zc`-@CsMp=9L$}lX&3;UkcX!`jy3ap;{7HBuwJilD<k0Ruq(fY4btD;uZIIX$OU_Ebps|PXqUF1(-mh0Eu zuYOs59Mc;hvkDX?b9*i_5sxIWV#o7YYItq7Y0C!Cl4tDW-CCq;%PrkZ4{Weu*gM0Re%};Ew1AB5Lidw6N7=?>>m)I^dm=n zyv@0}RRt*R`12Z4Z*W2T?hbbJ#zdh1ylT8V)U7UGP%XN~@@<+% z{mxXn2znAb8KopyJWS;Hl16~4PMjJ}+T83n_H>2j|x% zi@9wgrM^IF%57q9-jh3E*3NbL53X^fkSLz}2baU(^R}yediLVSt?FyZoaT3gnUQeh zWj&2X_RLK{1SPw-d^rynl(&`kX~y!RE<`f*}#^(KFGI2&VIdE zz4H}QP#$2sx@{JGX>4kGjBkIg%F5b|p-x|UyrNS2WJSiwY5fvq;eLG#4WG*A9S$QV zE>6tvzMpfqzFco|x6cB4M8Sh%75^>@cnqd zUF301j?kmR1r|6g?p=Yi>#t2F{Tuo@g5Gztsih9&eKOq6u7aY{vPWnciR_-&P&jK6 z5^@K#E*%}D|)BRI!zwYCPO+)(L7$e0$QG*igmvaX`8oIBs_nbX(Pw_<-K+aHyI zDx3{cIxLowaG3D-pP&C3vpem3E4aCI1=Z+@(|5+iusKKeiIki%=bv`eMCg7e2NM*0 z&P3~q^@f9vtnS>jNp)SfjcFVC^4G@k722ffzTx-YUWU2yI7J}{<7-tu=Uw>dPdC*X z49)8geBd|p6~Ab^|C1fi0r}s zJw3qb;By_{NQDaIpaGm_@dl+T4A`+t2r?Yj6QZ zCn1u96OJVnHqM|oe?p>iE1*lb$!Yve=eHY|VTfJ>30>>k7)TEwRRvu!leWwyw6KD2 z^U>#a8wipmQmTwcmkI|4Py}7i)}Dw``7L&5ij>aU_H=Q!Nv!$^5p6sUde^(wKVmT! z6&6ygaLbNd8tFM7&5eHjPsL9X=%B}78pQ4 z!N{ba%RdnV6QuKO#vo(Cb=40b(&atNZob$0ui**ymg~ z?gD$=YyCOJ`UjHVWaZ7Ctnj_y>lJV6d(O}>L=-}95`XYHNZ!%lEPb`m`i~L~<^r-> z8yE>4jYRa!^hxz!^-WDh)m?kYPUOtYHp|TI(UE~v z?7l`CI$ja8=7HsuLJ(vzQbw!+QiNoPUk>pt{P++Md0b85>1RokcYl7ElG~Sn=x37` zz`{ins?VxeD73h8nKcqNik!CKx=ZIT0@8v+tm1RGUX30oU1G04a?VLxFvY<5CaQ)y zp%QX$j12d*G61E^=ya`s%Dq2j#HQ!X3+||L5IiGrJmL#b_1dVx)L3>_(;bi_TxQa? zasA4g0)8D=TX(xsDN-(0E%63#IiT?0zBjW1c$zmVCQbJRZ52dCw>L##ySlnbDfq~` z4B5?Rl1W8^hy~rDej9*50XYblS6JFy^aDx+Jf8cz_`BCfg0}YqU#R5?Y*=5X#swcv zk17Hqagu(ee^K8sUvIg7o(7nf4f^GkX@0tYkw&zK*c!fzpe zN|-JHSuPXB5?nER)Ag~M0>U^}_9X=|GPGZ+Xo;+#Ca03NqM~vNtB^rMvK3LCIZN}a z*%vx`y0Zp{iz#M7*e{^voz%OkpFf%0+#XyQG!-=_erHchxJ8Tp-v zI3HKJE!Nt7u#-_ej+g7Cu?1j$c&zI^zH~FkXAB-Xtj@rBr@fDhtci-Xm~k$VEsnlx z_IBT$v4i(wySj52lDK0Lk!;>ZU_g~N@ii6cN_X3enb&(Uk&p;(idgD z)bbDrD5^~O$-{d$e#UO=df*~mpLaqsKc7Z*_HZSKdpMtb&pB;86S8xal^t!WD+xj- zX0f|ewsW<%9#hlvzP)x}7VGc%>j6Jw5TVtbmj`)2+T{%zK3~Sw1DWu(k_rPU8$(A& zA?vZJsC5?n2yx`&0Hpg=9=jRCnfkgW7MHPO*t2$<$*t!33N7$OKe4~5*7{%?Y@U&k z^l4K;W+p07Gqq;WwRia}uEe!1#bKiQo$w4R%c9U2zC@uILR{%9Y5jW|_Fv4Od>sjR zV>>REQqJaPc(2)T-l0sIOM3X;>~xEltK8{*cnX}?J>@X)?Jp;F86$pLSgnkis24Ys zs^+Akqhj-VBu=MLlst1&>gVgyz&Fp%Hr)_(N@k?rUNYL#QFC2 z&)BVDolMbA>Ow_mt$rbxDOOg`TynMRf$Qs?tnV|Jp(7(yo}Pb{h(y8XB87keacwT} zEyp?|J=0K7bl>CSPR5JPMl@RL6H=F`sF)I|!RQe1ZUAS@Rnrj`!|`GP7~qv6vl(+DKe0U=EydPPt0eihf00fTh;MZpR#?)FC%Dg-mCQS$*M8vR9t8WJV9NCqNQRAvw z@r~+DM#NebSN)S=&}3j3wy_{S&l|#%pM*wI*ea{dx7@WENCG-+pE|=evDUy4z`q^k zuir0$k(!LsH$+8b5`?^F(XzIphY%N2z_(OYi{s_XO?N(ep)T?1ZJ7~ail#HFagzDU zQyrm*Jm>S@KFnfqcQS{3xMt2w754ipKJA3{Rfr4lC!g0_6$rO&6Nk9 z9EWKX$fqUpcraPs9dMB_33zumadeTM-w=b{yoPEIWU1P=)1NkNlkXYp*X(GimuW6J z9rt2AUZ02B{VRYBeArnVGieN4lccO{3b)(gB3psJ$NqT?kX!zgvq{l7sFYuDw{Q1I zVIuSKt_iYpeN)dfiQYYUr??3!r>Q!4Gn^)zQh)_8`fxgR&D|5ls zXXdPYv%13NpZ|arCP}9R6-gdXU#dR9*lW%qxvX1yEv}qV3va^*30hxkp4yWqdK&MC z-}(%z^?9fN2H!+3MHpK#iOXbem$ALJJ3yHN!3|IwUKJ_%A>pQTKUk6u{A|+dABb%L zgTTW|sWHJ_Y>#*tHtar{{%$$238qnMu=@DoY|hAPIp-ZB%PiNaCn!g9TtJk%rWS+M z!*IoWSl00){n1u}o|o)WIN+>)zJoWqNn3^(z4)x5i^h4&yH<$uhewEvfao z9!~4JOmPer?Ps0h>29x|bqqic4kqBs{&f`<)xW{#ilpR;u6lI6*A;KPCYxi=!ZMa% z$2sFGP)eJE1G1E=C3Plp1i0Le%JruQB<2LXmDJQnzS>krY_*0*=}f8?VdW?QTiEGJhB?dl?%(vOicWZ-RSd3Vg2qP!#;Bu5vMtu~3(9bdU! z?39G5zqG$*_D;XW1i=&|huk9{y$A7sJV$iHmnlvXcOybOc|wG-KYun{Povbf$cjpu zEp}1f;Ue{YMGlwVcR%Q2iRySrStBj$iJTW?;`U@U*cy1fGBG#EP6^j@J$77M>*i_p zPJ6gyvumB=YQMyC8YDHI@e6a8A!>a%v*|8}mtWOWQ?P&RA7$ZJ)E{ZTzfT&l9*pyO z+E2RC8n;^LlQ3d?y-#UKKgf+&=a^PPd-qDj^6qM<;OuT-gVmr){mkb+idA z{BIm$B~?E{iY<3aK2MPJBEF}?Jyk05ZL1l}FoR^Y0Q$s-gs$8|;|r${li&UJ^`U(` z<#;s(FA?HH^>_4gy5T#Jb(NJQ&aBUM2C*jpo}>+-ZAT7Q!TJYD0{h1+VrI|Q5Jv({ z?PaKZKMh-e@?##-txPO1&xbtg`dX3r(nP-_mnn-jcjqMvf zuhwrvxglOpwx>9I>mOW>exLkFrl_R=!73-c_u+olIa-?>_{D~gJe)v=%U#Zen?|SQ z*s2LFE{5**XwqTH^*;Jm=x$~P2uE_3AD{z>eI6Sj}12?}yKttWdmb?sb2Rn}xC~0Ma@g^kC1MF!@*NgyCGpgEL)E z>q#Ay#F4Z@n7f0z+O+fj^%vEmZBN2Q%ilh)dpB}UdjDLKgJ4_fFNN%neZ2NaS;}Uk zWaImAzK$efFFX41?KqSl9+_8`@SGAtfAc>_6$-5GXj)5(JIPRx>u&{p#G1CGp_xrk z#mEIA5bz{0dEJcG1KB-{-%3psK&G$NrtL~VsGQSQ{PEhEdsVyH8WfY$4>UUeF^Et3 zRpqP{47+$3+`{D9_=gj2Jid;~-fovFMxrz~os%z37ezH08%-?@ zeFM;U98$^|r(S2xF?MnWLQsFwyX94K4EBcI;A(zbli=u3@FmFQgB_R?u1o{%3mVjJ` zr7rz|v;9(7$E3Pi(n?vv?6=>`T3}?!T-G`O2b4YFq2{G)=6H4r9FI}$FP8*C64R-UkwYOO4s^{MjBqa$Zug_#3-bh;!I(?U^Q30B^{#Uj&qIVjMbKuvg0rV z86rv+Pm2?xTs9{>zNi=UymhBFU7qo^W;quyQM}St;$@skc>gm8J8%%R5fhTbm9mvbU5WauoZLwmUDlq zPu_={I?#t_ndLn7JZyivr5*pjhYuwC6y4m$C|Y7z9qb-mOtA)67y4kubU98M^dg zboa{>Y6o#uB%9+W^KB!n|6AGu@@%(~vmp#q#{PL!P8lChS1vPpeoT5zThvT?WuYwP zblB~%sLWewJ{Md=E5-I9y_1)XynvuQ*}=j-;r&%({_(Fa`SZJ+=m~u7Eh1FJ zO7{MDm6Q2KR4$DLV2k`^MzKN8p?&v&5LqN6DvfwUM@8&$oL+WcP*KwHeF8>8J(Du zoAu=fE2x76nj657h?Z2tdBiR;a(|wNN^T=M=YK#DLTKHM2@EKqCD>heo!Lc#UwRnX zrh74oKX4c6>*sasuNcjXi`So;T|5}ml`3CQg_g#AMT*RCS$X~%1As}KF_*PbNKG0Q z%8Xb!V{q|d#=>#gn3dSgDXBei?-yV=qWo*$!sw)Fmm4N>N)r6L7148_$=-U2=ugZ7 zt4AYCYnh?O8@Xj|PX6^pfxkJUu&@1jeg7 z{_cZ$y%uiKPZGw@WbsQWn)iLy^^%p*S_o=g&f1!VVen)>!Fo1d^n|vqx~wOM-?Y+6gm{E9rhqK zE`wI7{*EjA42E#1D=&KoK7=&!Uh@!eK)yu~M}rhJ-MP6Pp{Z z-P}@gO(Pz_Bh1jmo4YFHPSM-x{{@<^;mHLw(-i!Gt3$(epfsO6u@`tJ^Z8EBO_z+w_Xus|=+|9v?xAC2u_Rr=5Gss9(! z3)qJLKbCGxzCNA?2$WqORyf@HRsg>mLC;3Zv7M70Xs7kP7cN!^_`A=4midKFgjq2^ z8-o?|D~88nZPa;Az;*!cUM89MBNY?N(F$!-eSWd|$bFGwQfW&qPMZ|v)X3TklaU@t zLWCH8>{5=r(`Q=CyJN20-%a0+e>Ohwltr`plE`Mm!&g+$l9wT2pvQbFhcmZo&@NJ? zn>|;fiwCfa|D7|=l-X~ri83(v7N$iMo3c`m--oI_rz{hzFba@}1coxFUt08?gmmTR zPY>AknA9H!o5lQe379#``tNDk^hK!Rs~D;l%PLz9hWh~8n>R%+lCtun`yjN!*}&w{ z{4f*w_uIxu1f1nrZB2z6Q`c_w^K;69LE^KsDk7;bA2~G`je&hhLBm+DBYA={vnu!R z0KBanP_rh3aiLt9Dx0TzbJ~A}M*+K91iR1Rb#5`e>a62eVO_1vX{WWW7ixJ zyYJxamLM}|N(_&vi|^e&>^NO(qs{XWJ8OfaRO~bT(9=g5p1}utUX0^VQE_rQFYsL1 z#-&}I@_xMVm?Dc+Y$~`p7OyWEmL5;#-gkCyZY{1J0a!DgkbpxGDUXjpz%blL;Rpa} zZt5`q+py?csCS>{=6{vUkbbe#DH*!N2_#kf1z;;=6Itm6x%Or*MDXXz-!uv_#i8J^ zejBI+XmBL~pdxRB&7N9D+dKzHTJ8IIF^`Ltkf|=d07VYdd)Esl-XD1+)bw;r0u&T{ z>q!mZ_K5LfnNz**tNv!byT?W0(>a?Ys&g!6ATn~4PvPvvl8ATf8T7~S58 z$L)9`@3{2y9bdKr9RNOyOH5Q$RD82(2QNw7@*uCDEhXYr#!2D07Ew>DTs$#hiz9Zj zSl_;A`s}>ezs*WBGCIe_*+-p6u-YzykC#4K<0E9PDx6?#5=jUw=_s=#wV^=6AytQE z#r5yy$xSn0XZQcyGZHHDET9aB`mJJaC9EiA&8cZiBGI2y1{Im; zZ(TQA*1nJUpP?y}6Y(;V5jF>us(r7d#BufgYrj4@1x2{Es8~Oohlogb9ck7NHEH;f zBn((-#489<95yrOFRyt^#Rw6c>^zi=Oj*)n4{?9giv(bSfm=EIO{^mOKbew#J#kLy zEu9O*b-dXgLSOJTxD0Rn_}iL-k#g^uw&Fuj3ED48+9LjMptws1W`!txcb)EC;&uQs z<*2?S3J7RcwItwwJ0w)O#9*KIID|;x3oR}pk`@FVBwX}UD(^G2TU^2T(G;T!cP@a_ zJ-_Q(RBAtc2!N8kBl3!C&I|AlG`WJoTb*J(DfJo-JbOgK<1y*n3Vh`KRf$@zSQL86 zl*aD{bU92-eibG|m$1M{`Dx60cyOrn?|b;x`fMX$hAXN5(k2_Fru6z`q#*wqgPf>93+Q481ALP1Q+`Z zObRiuc@`-DL9e5KbryS>KTBZHPhX4p=+;#lQIP&d3k-_K~s2f_gCI|wM%!WB$>5^@L z^l5u3-wU1q5*As#Wq+*0ZY*Ow8=}9892OWOHqK2gpXPpeU@|J>GLp*TwbUE7 z2y!|ku`tB*(_@bH2*OC$pGgf2`M3A@>!T~^%sg~NK? zFMC43uXln1ppo{2xrM~U3s65Yu>b=Z!}H)e-xn>dyG?Q8zusQ7bBJ=esR}bnsWMZ0 z7eo**#fT+)w5qSs37%|XeP-sAzxsU=`C8#jF$;N+&65$fr8qEDI~PD*%+*=U1GrDc zraZ^R>XQVQ^*c)>WTFLdd9MrK&5Cqx-ap&-+bl|e`_yM+QxApUd&I&*;va;>_I=1+ z`cv5*YUiL*L7rkgEzmtd#m|VunVp@jLKp8cG-E4rPq+U*O5k<7T&LOlDy4sef!Oof z&b;1wa8S~PQCC|b8pR{S2K~iOnRu?aNp7&TUOB@_tK+<3E^}Nn3 zda~%hTmY&*(9|$Hi-|p+k)>zT;5J#|;X}*F^qx#6#Y^OyeW-Xf{=BD}#uqVPaw;bM zsal?OW-z;LZNq&J57)}=Ly(Q@S(9~V7o>;F&v>i?Bg4ovc0U*_>}{*n>m2csW}@fO zp$b78%fdUjcB>1&b*7hSYjLs9@)-`GP@{~$hQ^aLUc#!ZK)lj9MKqs64J%K$%%+sQ z%zxG#87b`24l6J8D*F&INz{Frk*kwtzJ9%;>FRfih_HwZ)$}p$bemqC^G-1bz*>JP zu)PqX1U_D6qou8CXz;J45j?>PP|Q)V0P2~dWPc{B*&JuuYK>4C@mN`#-2Xul|IEHX0oSI5tYC%M zqt9jCgP8O77#LyobQVRx!)tg>vgrumcsx#Sxcb6zk&*`z^gKCTCqCI3|MGr#c)Y>) z0rv~?KaD5_Arn4~#>wMHP)T%xSU0tmH{S4&fBg6qF%Nz^zG8lUzA}c>sQl_J=ANuM zI>_GYe)o8CHQv=XzwW7}pdg_cm2`zgLE#h<5~Ak`2C<74eUAx5y>D^?Ep26cW{8hA zZmNd$ruxu&^v^~3&lAph-l=%$M4;*eJ~r)7I&^jrwe0w)05P|mr6)uxcD3WGEFyrT zj39Bn!TsR6A6ymlg(VuPA!S&=i+OJg8V&^>Da8W>Ae1Rw4#b8HPws|~%uD`e;K*>` zr$gn0NNGcA4=9a2?g&2sU|Ij%ZJl<@wX0!cdp8NAYE|t~s90a9)@~#Ffc6^pLr3Q4 zSFpg-c^V)P?>or;fjNIWgSH1}Bb`=hjE8RNaU>${%sE^|b>(M39<7ykAo;e1yQ9vH zt7(Brf6-6Va`Ot1I#kUk2t4!v%CJ`=SFvzK`4BZp+sw3-@+9O|A5aqm^NVU*O1BpGyq~Ui8HMjJK-8Zm8%~=VD_=lS zN9|A6J9#VGT`Bh8J1#EzGlIQrj~A+HDhmfJfB#HKa5-D!XQdM^Qxv(cfJO1;j6o#^ zKbT~EFRU_)I|>*=g&7ze{544M&EcCTUob0nb@e6UC8_O2_D2+A0f@)mz8gX3gIERv zbv->n=Vyuw%5WSO2+?SGz--|1UxHts>uKYt9T|MOkKRdr!A7==bM4KgX{%n!rZTKv}ET2wQgAB{`m? zuhn^oNvOM?z4`@ooSzQ>EhrfRc2K`bugJyZkJDq^PDetfsUwButaYwC)(BG&FPS^n z*r+4B#49(>-4Ggu(Qt8fbay>Ub6nbmy2T>GB6gGkKp^Mtp0UW+FZx7)2lyvoY!2sL#@$%hS?)3+A-J#2RUr%ToqWejZaWGjnrmUf>*HBD-$O z01F)kA>qqo3D^+6Amq97kF5~w?3<@o^#*t2b?NEUm(w=7t)ME4iu!9)l@*qf5fV5_ zK(Q+ct^VIE&$9HIy~1-&)6`*!vi$f>%>Waet$9t z7_o@u11shqlp?c>kF^5SEoqR~>pW0AKXH6N@d59(i@=~X?O=_)Cu6IDeQpK6K7Abl zKQ|&;cyCj&0ua80O?_nQwcbyXW~QcamSCW01x1X9+?b6o68P@V9TgK5+rQ=om^Lxc z3sr^F!e4JHDhNJ$3l}K|vpGEXgvblN5OAJ0Sx5p`yPxtM5&H&k@^qS=F@+Hj!>rqG zZeKiHeuzOfx^32v$Lu^E@A= zA0CFj^$SA5=dm6LW`XhZeoV#BG1wCDzEk0yg!K2rx4yVe{lXy-R7ESdTw z$jUS-SG()KUH0aHRs0}6fPYiIBRBi}gPfF8F3&7b$Nb#zc0eLLss`gWA+32+1k&f; z`CxjymFD+G=1R@>Sltnj9xC>EKG_KfJM)Xym@eqBUDX4>+Dyg7(9V>meRhHr;-xtU zAw>ko&?~>DBI&p-C3KL+4Q^HpcErogm(%)g2Ex`Dk7`#hl!>RUP--wGc-?n;Y<1YQ z=~g+f@-_j4hp&$o6u;jH zMSnILhDn(P>c*T-b56n=zalIoq;WqO-oNsHJZ(72LRixmmyp=6@NgmEsZ{H8!vjG@ z0*GF1J{R|0C$5|Q*#si{mr)m6o(DHCSy{|N{OP?Bc?qdNNC5peeS|-rA;4pPoSVEv zS0rlw;XWPR`Krsq@>$VJAk9%(O~t$YInLWJ5TGLee^`6Vs4Bm%dsqZPKtj3^kW@ec z>5xXcOB(6ULm%lb5$Td{knZm8=8)1Hy5amU{Ka#R`+2^-uj71h4B>cPd#}CriaF;B zCt>|7jUA8hejf{vy}(;dk@Nc(XJM&C;TL<%{I5v8zccmS-<~J%S=LYa8F# z=md7k>wE}=Qpd}{@TqG`0(^c`JuxvUJXVazFklj8h34BbH1OWu9wjAZ)3b=108M$OZT6I( zl*WW%A(gzYc-4C6V{P`G6Vz}{0P}T0<@h8<0W~m?>6hXriV*aAL^uL%F3Pxflsa| zdSTZNv>e5^-d9_XG{*%`+qfQ?TO)nnRb~~+*bDM^I0)!}vH^+H=z+~;?|1SxR!LKe z76pYrz!wkqk$h?@8V_CKHglRC!5x9nCa!d_?&@AK$53F4h6X-90%cmEG#6O$}L>X`Wh?0&vLHL9mR)By>_5QWAY>R3M=b=`#4%hm*B5+KdpJ1Uh8+7HCkvQCy0KeH>WnRN`-#C zdW);ASRY2f)lEgyONoq(oUS`LHN*GNskAkc23;tHG+iz_b)3)G%+?+U_mFvW>Uxzc z2c>|;|2`a@9x0XoGqGz#!+M|QPdc`6h$3t81fvcZpX~9`&)8p9jRZ` zpidcu>3Af5tgnOF2+o#+>2M?oHm_EbnEk4yL|gy_0pSOgSVJ0FVm6P-G2dIi-uLCl zRUcGTEwHWV4^L`+^!0q5bch!!b6NlRSPxd?v*+QO>o3(66kK};R^19y?Aj**f=y>P z?b#vE(MGxO#xK{SG1{H(OpN8+@jM;(b?xzP%cX~bN8W_>ND7}ruU;Vh?NQli;(UEa zVC3CH*H(K+`*1Q3qy?O|k(D*HP#qHyDMD#DR!HaL_yxcyh|EVQ`(IGozV4=-o|^I| zJFZcCNcc{G!QAd&17SCx>@`UP3j)(Tkj`eyv4}A_IL5C!(ozbP% z&1Mgw%)A_S<5xOOZugBwd*$mTY!3EJOYdMghxz#yGquuBQr&->Sp`*RRXQF-?)iNz z$3VZTH=i;`rkZOs8kmsdTV-a2%gHmXnkU;KtS}jtmIy)`dT0Vc0+2ROJZ&eZCQaEW zhHYN-A{o$TW_S6&`Qd2~+^1eTExWyKo&XKa_B22xJp3GxVA`MIYtiD|tL7xsB3+WV z5IQ@Iko~5lNK8zO5R4EMDR;BBUc=BY*)?|NwsbNA;keecZO$wEsQxqd2)%L*3+owG zC{ScJ9@dcmyw%H0UQcXIsKFEyR( zKo=WLO^{7Rbh1?4zz=Yh>#6E)urK9Bo1n1j1E#4a0y3xQW^^EVqeY*=>YK)k^o+)Y1_EP4+fK5khN9|L!{-e?++L;Hp!mD}nf{Z-J&?JR3c=?2Ex zD-POFwr9zp{=vQBG_JW!5Co{Fi#4YYC-YaI=T-@k0AUFb3{M=4mX&>){*0+nF37!ym=Com!9~$d zn#XoxOoI&s?y69kAzecPAMOqkm^QA>p~Z1Ixl^$mApw+%qk#T)7}*D4D!Wc?)CY&( zVLVXv_*f^qrJHc!z!(17uN*7^%EXT(8@G#u+%BoY%CD7&OC}@9pK7XzhO$0kgRk^s z%Vs)|kT!40z0%wJbJP0Y58V7y3-G9QJD+F#T#c2Bfet;(Pg9nk=4*$Y%&1M8$DvwR zbXhHT=Ex)w6A=Le*zqBCWf=-v$v^Gu_&L7<^m4l;A0DyDq%Y;_GbMMDjZ%*s>)TR7 zw42PpS~Jo~pUnD#r`4AS%O$@Vr9hTToCar|jZ*l`sRA*R*$V*ik;6ud50FIg7qUC7 zw6P%aat`)JbJKVA6~xCQV~QEbFOB-?!@InWSh4ZY^QK>tD9O#WhxWA%%yntQ%pqHm zPy#o-y%Cm^WctDC0!>?K0^mO8?Y_juX67V(HeGsyH%Y#A!SU`lHZC?XA!&upOttH& z&BFZv&>wlsL~d7ly_=4XpZi^OlpF}4>N?eT>t5aoaX&Q=zscT8<@eZ_<*~Wjx+S0H zzu0gP^EEtsKA#7Fa|sN~oUTzDDJt4uu+ezud4sds%GCGi!&H?rn)mcc!H2!a^VL2P zfe?|eM2jt3vV1v?%B=id%_r)(b3O86Kh~^DQ~&FI)eMB>y~pelh;g}o>O=mQi+zny zd^-#CtgI{ok0HZ+mDE7L+xh$^{Kk&%%TJLghyo=zKi zNg3jbisLurY6D-iT#3SSeA(JK(cuakBpHXgHvjxwF<4N-Jk4hRLzKlGrrP zz|U!3Q0T#gCbcuqXth!MsePvI)FzX7RQ2fSBN=`gr`h~|6@tWZd_X7E6t^ zq?8Zl`x^kw+^rUM4uD%%;JLy*%dM=9s@Kvk-(TOeX|8B2>f9b6{;M#a;(f_~=U0h_ z-4o8|ytaW+2$q%(YB6g=gEl_ch)uS`HQ(90T6r!p^MFdVsxFeCUawMc2@DGh-ew+d z!AFf%=zg+muHo=}50vKj_RP(%j(O7@<)F_%8Sj{T2APpFrUG8S1`RdaijmMVa8Db& z=a`%syV^ABs_%9)O}QX^x4+-l!R|;8Ko(+wlUj7AUnN~K`^#ye?sRK?ZH-B1(Cad# zhO@!9Nw@3I*Sy_M*hb#_BS{x_O3x(4s5!aB}Z#Z`$VT z7c1wzAB>;XJX%g#y?x6rd&p-Xc0KLL*xf41$|!-8GaQ3yU@FIw8tVnGRS#9!;p}ol zu?{5XsmbXQt-6Bd<_8Guy0j-aBs7%cq4HC{qLFT6t<&B`Vq(rrgQT34)WNjXWoi?U z`SMfY;NcCW?x}~XGzLk(S>tQyOoErKGPvG?qIz@TGq?6o{r-(sC=oi_;PA%d?tpQy zp>n_C@}v4oq1TFL9zs}4Iu~=d#xeQx%H{e8DE5T%w6FG5>ryG@r@gLz|JCUU!hbt( zy>9$zuhrIiSMyD|UbzP2D18-@l>-rh1Pay}dX47b!|=(($meY_0GY@G^_0-W1z+z3 z5rhY%dLnKbrOEBLGltQr*|U$~ZE5YCnndv2Z?m=uL`RR6A0zj*tI5kT89C@Ipqwz# zWIM_oPTwm|YqS$1e0r@I&Ms2w;F@k>nzsk^z!ciDfeB;d-+U%3Q%&SZgTtkO&^&t= z@kJ;)3Ab43I!(Zv?*-W67Z{{ma3Yv)wVK5CzdQBQtapIQy+?dh5W@I;Ru)Fy+X33Y zEU>pKNRkx5NE}>D6BY&VzxD_V5VL<4EGNsF6y})zaH2q`jFMenM7Kc2%S1n9U|m(A z<>i(5nx@%8(D^j^MoDw}Au?o-NSa2_;0aoLH#2K8@Q|V!FN09qmyde4JF{?NHBmP$ z0k#aDgMybeFyd)jhu{~tn=QwZ^|+64x*9}q+55TQczQ45KExdQ82}Cg6;N#$fuUIm#vtDbI!YhYS5w}a?Zw|+AQAm2LL_F->oLnow z4~!*NPbZpH75;8$&kL|8CFsA-iq%6lY+Cz`h^XD9ob$T)S;sc-h;Tm)5rG9&nq6H;orc@}`!mdLuDgXS(3{KGR_%2Jl zo=OeI@bjuDJx?ncyJ#w!FP}0jn`lu4qwu`{QSuq*1$aeb#Mb2mDHZUV=+CL`6QoNU z#>q-|E#AZ^xi9R#on%_EkCHGlmoc>yjV`TFuAJ7;q!u8Jh>9wkJWHh(b?x%6>+)~B z7M`9<+zmP0X(A%t17bVBc&vN`6v8inO3N1+6;TD2_D|ItL)uhyH+|!BjywXB0hZEZurSy(6 z2ZD^AI4CNDI?m7s;UQxs?Qp}A5NU=QdoPtNXMqtzQ`2~51{vvHO>K?nIrpz3?OzX{ z9LvyHS3ftU24*?t%92P+@8Ye7){I)Prwr0t;TK#ye!V@BpAS zX#6kkgN^*hhh5^y#CJ(uLIgjoB9i0-i01+xD5zrkIGuc(#>vF!os+7HIB6-UJJM~w z95@$Fku;7d0C2n*yCp-wma)X1fL_Nr{8&Ne7a&D1>{8VH4&lqJ5z23LIaHpRd@+hj zM7(=q&*V6=tq~8-FYx&cii~bm>XppGnjWbFLghrx(lG8K^4Ac_BVjif|QkBxwXmv)M@{Zh;#E4xU= zF*|Ah-3dk?BeZ(%Z8-ZBiv&eVBOhM$PRyhw^9s$9yFnZby`pBiOD$88Ffn30B0T!o zK3~33bMNI}I%;YvI+$k~2Cn3oL!44aY;PXzs4QqVMA4=Hrvc}fV1}}2p)#P`l5Rbz z>Odxpi$d)b)Arf+N*;RyMO5A8i>J1XwpG=T`GmQ!aC`f~-A`KNPuemxzALUcSs_<` z8_A(215$Nd{P#%VwJxjQ12NDgWt^UQAfl5x9G;bX3yY(&6Oqv=nJ{5cIZ-J^HN|WMSAyjB zpT0~S#HETB>7m&s+8d%FA8A?f?a&(y3I+EA+Ik9RfLdbLM+uQH(xj!5z)>h<#vl@XR#!EJj)ppkWF?^v z%&fRKXCZpU7%M5`A{7-iTGk)(t5xHhWcJd8I|FY2Y;9+KA_E19qG|!>nBUFH*O9K% zT)?Niam7UhD8*0yDndW{?Qt@F0vd|^z3Yh{fV$I>tx3_CMVDsuN#TK-UV;ckMcqYY znc7+dJ66+>#hwNiD0+5?%lV)c#!m2bdI`sxt5BRbwAotVz~gRaN4#GK658)bFy8=3i{W`Iz~3J@yPJK| z7d#XViW#d&6Hjzpjpe@&WSH75a>&(dsQ<<~r}r8Q7%Uj)z|YQ4|AA2{NWgTPpk6@} zXf(zFJaQloH>*-#$NDys$}g53QxNhCKvnq~d~-i;v`rUb2oIO)Z?+BbI!GPkp% z$e6Zd2hx#w#Q!7~Jchb|KBR&FOtUr9xE1&tm5-T4g`JkHqrJSJ1`f4IYE8%ZPjJEi ze^@a!xQ3;EiR?!NNT>)y)oCbo4fg-6=>~&dmHLJ~prDY|@dty*YR2)eGkevm(wb$X zOfL+)1}R>)qy@{iM1!E%-*A`>B2iK-LLxkBwF0A=`s}X&*g5_Mno}&IHtEhQ^|y!# ziuq`vK(Ptq&5H`I2{lu+L;)}9oqG!W)66c17hHe#=VJUV9X;vkWmo*M=Y%l5zeW@fJH?O z=i4wG9VY;!zY^(k3J&^oC_xlhBAc^PNeE~IMH^fKyTs$1H1#NyRPSumAlOFqHyEUD zTHb<6y`kwekJzAK0CoM__~iT%I-6RTIXin|IPgOoCm(4u|Ju>sDlrKWM?=pKJ4LNk zfqp%}J4VSO{(FFCh*12yzf8sqhXA4(bW_0ZX2k$Xust7hRQz+Dkfmh&4sI5|)h7!T zq8DHNSxz&MA25E4eKMvnT$-uncT))Blal*H-4BR@n4QEpobxhh-$sjyPyBfzEk1xjbpR3&sZsO1rIsM(U~F`Ulvp7n_wm%&VEtS=U$JSv zI$~VivkE1@bG`3R+%0Gg9;thnmf2IAznUH%0xdQGvTv%P!WFf!@~y|-#~;(BlwUEO z@yqJO03RO)h2ccnV)C%qB;fJ9J6(YYOvNhyt`nk$r2jD~#a~ZEKr!4sK z0pdgFPE~xjFSZ3-)GaOB*KWyMY%i1nIY9GbcK-LS%Rc^?Cg&kW=Hbr28?RrkD&Q50 z=30Dh6}|;U60pJ{L|=8j7NewS4sgIhp?>|G80qqm7xeqD?E=<>HB6fQ4s;88MyKrU%&%Zh7NCxm9HcpIysiPa+$q`J z029TSc?QyFZ0^m5g5G1}FM6q&s@L{?@xz&97z%{mC9$Om+^uF)iAVHq*i#@(hBr1g z(x$qclr-SA-2dLgrj@Ihvt%`qZn->cAOo-E14M9)WiWKv{qW(eV&u^&j_a63J{2#` zr=kfbG7m#9m5%u3XC`6dPwj_Hsc9rRnOegyO7)jFOq7z4kT5VNPH+!j=?|4>A z->w$Z2PiRmecS|+aw{~;*SU_U$f)E3cHJxbhOcs@nbhbu44mNAPw#)}vfK3TF9FyB z|1^#w24+Sqs1aO=QRnlx&(qta1Y9{SX)h`uN9+oCV9l~w29O(w_3Z-<~*Is$OS zN~O`h?W(UL6o2QPYREsTnD4Jrs4oTVY+NgfnKMp4Mk@feG6Sh?rD`l`)+ts% z)fE;J`glgs7>PBT(@9qc+mr2DWqY79bTH3!kS{gv-eRYV^N+m8<}kC_LH$WD#O}3itsA>@*w8} zGy(e7x+fns%#i((`J4u+Qh=cDyR{h*Rtc2qQX?b|{hqH+tnJ)rhDQ`$W2xh%ps35K z7b3=TJK3ffC&`gb`j(pNe3P};2;?#c9D24oIy!M1Op@8{Us#0oG%pW7*I4_9udS^C z&V$XkREH1XE~}rpJ*G4fk2tP~L$0ENJX8TSy3(1LVHE7j`nLIl03XFaSHtshX(1sY z78Vu*1A~ZEPH{~U$b;|%tD7A=QM3Y4IO0YyP|p;6ruX_AkU1te;FIt)f&qXmiGW+# zD}kE&Ur(Kj?`}e`{Ic48coTh80oBkmKY(ui0yP{UUb}^}cbbmI;oxF=*`riCm>0Z} zBjI|YdkJ0kTHQBGP-b`i-I%ffPIX!uV$nG5-sQhLS|8N$+}ZNQ;OBPP#>^weY__;e zO_t&UE!!bkjwHX=W{NA-xuvoMpHIyIt?U;Vs4PyF)rb#>hEr#iv)J z5g0-IQb~R;AuhQ15K>IRm(gdcAJ@Mno%VOypZG~wSXfYhEFcY3?UFFNA8I0-#`&d6 zQ!ql^_V(L(At!QrW9660**@)tTQn;!6i1o`N?eG&}Tk^MNqThiB(Cf(d zuJ2gpHb7p)%Y5`7lj|4P4kGW9HZ^ho3Tc9TtrLWqk{89f-~YuX6-xXh?J?uCkDYa` zcS|l9WjwYvPxJ1;E(ApgZf0ug~b+_?Di+@wlCIa z-}%mfwo^o>qFK@R2&U&I{5PMSAqsa=q0k@8s=y+8xupL2(|`Xn5XE+sjoHy>oxUVZ zYyai!+Z^%d`~MA{O9X)!)yUpe%RZ6jwGcy0j0I_}Sgm5MPJ*(kn#QB<7B~6!mp`ua zQ5++`_^qNv$4>(+T!wJzl22aaTwasc1=2*3Z(4Xr@$|NWm&ow@PSWR}^c$!MBjqdO#l>r?DVfzQJRgUHb8WX|Z%4cf|06t! zTx9pP-W${?Otoki3REa2n~JU!8^$>S|C*s}t~_s>AOv?#`j5$F(OWJm!&T_$n!Mlr z)I;$;TU?v;!m0Gx3JoBI~5I+@jZ2Bb2SR|>cbOI5Aohg1pc$eb+vfs+*f{n zJ@GQ#1t&t&*0e8#F<;J)KF|B`d@U`ithf7y$ma@fAotUe+VtxyoDazSqB1^m3VLN8 zi3nh$W_=AYNBi-||5rmG}7k91&xi)*FM7WDB*(L@7p3Qbopfer3*wOhBjw3i$T;e!p+BD$_M(bVuQosGA9$GZk zl5v#K?{}7r`F(R120EGF8H%S2*oo0*+0HPUpCmpZPi@t^W-xS}TILlO7K;hslM*$q zGv)w7oQsL{FObn9lUdxGwMqtuxWJ|(>^`;LGGs{WxkGzPV7jGhjpDS%dr5;hYhUrO zNSSR##rOt`fk?zAS;|`@@WVXr6`U z@9$iwEBbVA=of1Yl~i2mkCnWE>GDgzHllUei~IP&{p)m@ZFE+0h6TNwPnxniv#i}b z4bALQ=PXf3G&1CcN(t8DaJKX#YkYk`clM^B!o09lw)I)>C08!|-^?DbF4yMze_{F5 zwl3$xZws{|u>-{(D61{Os_HVk4Ww2wLm31ea~RB;`^@110j<27w$?q~gaJL?umrZf ztt!YjS&WD#cmQ>Gn={2)s!N9+79pi7%QZ7u9T zpqQa|039{*bqcXZNXSXWC3F#o2#@PBM|6`~v}y`B#$@SgJ!Hfw{JaAHEfe#b=?{|# zB}LqPk4xBCX*+jYT3%Fa0->Plky$wEU;*Ry<{`5(B+D2@W;$DI$VKJEv$(eBz3g~+ zD4o&c;o{n1W`;!sr18100wM`O`t5vrXVan^i%EVty$5SPk7)3ui3J2SLq`P8S2E?k zy>dCe&Slo>z0n`y#v(bg_ina!Qqg!9ogpyT^QD98r!Q~T>1YV(jw6y!OI=;UW^IsA zDOWb}Y5{WJ2XQ@)(ET)@(h5gKkTm0kMEuMZzrhI_{4|hfmghW6JPi(i#I9+m!2VDn zx)bm~E|#liZuTY)@GCcbzLM z%_ylTeG($40My^E7UXr6l(Y#+iEY-jgy#3IkurZ~Va^MQy3Wa{sHP-a)QP?(*q_~3 z4Hl8SQ+`FCPpE>MtuSWCAK9*}@y>Fg*$9HS+)ua82H*6#3_voyAT+}35Ljzf5KPl3N{ahBZA=Gu{{j4ej&gF8I zM238;qx-VNKed3Rz)~RkI$P~Yb0-H5MaUSyF|X6ouTY<0amOI!Ivzasjec*3U`RXE6x>F5B1xQ=i+ooM+fn}|nHmxPf={OVCn$n9DvcN{sA({+?!7v)~t%7hfB+UAqJU!!4vo%f#DtG)2Nn z%3dw#O=2T>+Fk(^DdP*$#@`EanLiDK9q>k`&)=Nibob`P=R4Sxyk%yjAIy$sy?t?a z+%WQK3z3{s(o*P`{DR99IR#~D4;iPf`dF#V%=K=$qz3yrB+MdtIyELw0XAOy8>gDL zx7+8#S{ds~B3I})v-A}pD{Z7Dg)0=6o@8R^WI%ue*=G;jC~>2yow{!D$GI)JOa zCUFRvC*qG-95=OB@PQHh?ssKF#;2gkF?jdqSP0?VOB|5CP z?}D|?(Ws%E!1Yzfv0eEQ>)iqbb{ZI$3f~8)m)R`Wr&PiLO3TdI=+Ep0Fm9vt5(X4_KXOmRR*;nU_zFx7|Dp`g`q^z;_Q0 z^K5>*#<$(%=@ANflQoR4FNQdt->e*o0s`tniv7ZJEz&K+&HAe#DG2K}G2!s@ykA-G zm}!{2QAm!y5;}69sfJ>_TqO@ESK6|?($61>pks?=xqU(RhUpzK8qkpTlcS@x+lNS#=1!iT9Zsjl%o>$P7owLaxfV-2 z%QmgAGW#k|@nx;9&F&)sHC#EA>~TvWUw-mR9&(bdXL0pT(C9b^3hFAP7Z#X*c1*so zYOEaj`i{-`6)%hN$v!c7?kscE1ZW34H^}5GMkcVaa%}Mm+D=a5kLIVNi%?}6<_-_9k z80ZG0WcORL2^oEGYkx8PM%)F#5lZn?)7H_rTP9&)Rvxq#Rfx(E)5oYPH8U9&mU_=# zax;I68QEXpP)NKnZ~d~1+A+U`!FNV}rc#3B7)|9(iN(BR zpDtN=LO!cres_9H<^k{xT0nh=OSk;DCvOOxbnUkegiwq&H`#pR(F)ZtH$p}=K^|TG zQ-!Lqt2NXSkK;}I(5?dI*)-SPa)1CQBVcnqkf!4XXxVA^4`tI=u93<~A1bQt9knRw zs4~P1#9+lZI5<3C6K%leytqF({XTaZ7@3lo+?H=$x35ogBMxU9 zr#%JU#Pt8*z56`Mxoch!Nj~4PfaqWp_hZbYD-H_8VDoTe^S>|LQ(q0wBeEa5orz{E zA`>^=V%)D5M#_=iIxRDTmRw;I>E&dL6|qJKDx1{QUDq7UUO}u(w`NYX%pyghH!oIH zxuF7n4#KrIJvlMr7m)-U#gLVkRa3F<)i&==CK!?minzIen2L^p#`$naYQw&6?#p7c z-G^0AVh#;&7ZU%LU&e5rGcp1tgZ<8q_?eVc0A2HXtL0va!~_6g0(bVfAqJ$z?6i95 zbBzTXQD)nFuV(YEUrG4On<;M>nRmnB&1|%WNtqcKyjGeU0JbH!JHN&6uzWebZ;i@#nN&{CDOhV@hM*#cR%$Y) zHQ$G9S^vnsS4SYH)Ld{u@H$Q{R6&r7?lV=dJM)a>47{`s#1K59rxa0l)zpJB6R!%_ zRAnr3r+dG7Gd@@MwiV9)8fp;O(J~mZB!RRexux+MO+|qXi)9 zTtOB}Q+U$OhhhAh70$kr2eub;A)rN57CPmKFPkh}9**JMK36}NkVzyy-mxQ7+iR5a zn55k9(qf)T&?5HyfcM>;c%26aq|YLc=`)aX{*xGDuMoFr_yziVUTmI#@9$y5$$T6h z+fm=f$CYxmL%-yI*zF;AJsJYly6OW3c!*aE0}~TFFK=aYbE;TSLmJ>M}{=1|7C9d!qMTK#J58oZj;=v6uY(`OP--tlZ7_!GA05+W<-U5x}D* zdY4ZAVyVMqKR)_1cn;YcyF1eL!pz!weT%#W=>!`Ph7KKRpV>-tBdJEV&#t}W@%p`g z&3EfX%I!F616(utv}AVYf%dDbMaL6<5{&}T?_ckUAW^hni^-8jMMF*EHgq2D zDng8I^a&F5&NN(IH+r_)eFcWm=&O^@2DGHxeSE%Cv|=j?)INNkB>Bkz2|gxJJ3#oi zb};?;S7FJx4ZqsjWU07iuS?&tOtCPc_b<9nH-RBSs5Zgj!8vvhSK|(23JNgS1I9>0 zq%$adw0@@A$RL%tT&uIP@?;kB`m2qzxko|UNg$p?PXpq977h+MhS+8B$Txtqg6N)& z^{M7N6TpNZ3;%y3yxxdd#4Kj>qku)6we66|(kj5bY$F1GCW4*DPx^~yAs#|zdZ z<#o5Z&;?oqrfx=%3b<|`jXCXs>o;?Vmpu;BfMfNuTdCNq2MzH)F3pn-080wopX;u^ zd^r&3C59=mf)g9zzcmn2KwN%5?vFXj`RdDz4dfRQ&w9O}DIpn9<)9HvTBd3H?`jQT z2kS__n#B)$5Lkt0PYLX9y%Z>^9Mp#k+&5Cc=Ra(AkMp={sabY8T|&jLI7eKM8EG=D znA@fZMm{cVUcU=S<#qbqNpG##5wzL?&_mFC@3`VltP{Zi$�VoSYUvK^c5BX0lcB z%YkW=jMI^znM}Y=Gg}pqE|mFP-P6@&tUb6~XCWu%Ht!m}6+x3R**ihTFvp$9S(=`9d?TVfJ$~RN^memUxD<9nRi)JRiq_jVyb%_9!f0 zucsY&R_!)j?otkDAEFtyB8JiD#N!D`Nic1C#lfKVpuT&1t*4(Po?{9Hj<)I9LPe`L zDF?nyxkEkOufd=mooQgM4P|C5zs=s}%s%kv5a0|Ao+g+WyGY7BcW4srHgT$+VpXG~ z<898zyF791IY&tp$@o~%d(ZbG3~sZ-Y*rJIDg)0>h;hwN zbAzlu5POBi3K6j zicD(f=g#j}Qp!3@pOv(&YBw*ar+8jti;9e@^KlaNXfUv$v39M@{&J@ymo;s25TXXw ztrNMbm+yNdQrQo?Ry;6Ex%wdEkP|-(>d%uF zO8d@t2*6Iu-h^?>%vso`Yp*^o|HX5z46J{*qX%AnfKxA2pih3Gs-ZCafgqj;crYSa zg=3~Tt3tY#bR6GbVfxrIbB?}k3d~G&3DU4C&y2wS{~d$&Y-iI?5?I3^Y~b4; zUx9Ibh4Hh;fjl|o$ftmwEwjLz9=9KK$@P321jN5){P z6W^QbU(DkITg^!UzxIE8+nx8aY@Jru7AiqJ&ehWl>6~5Y0f~jr!!mlEVmLLw*=$^` z)`!uVK52sIoQ(tV3k>}qTq0lz`DjBZm-75%A3J>DHKAl58P~o_0;lB9wzw!Tnwu3V z7qC5rv-Vs$M|c`7nVs=$*X!4h0F<}Kecsw@R$5Ybh{CO_+GPVBaN*xecY$rJOJ zNXnFt-`uL%-4<#dJ!N2RQKFE4pN&-~@wN+>t_huyMbj25Unu{=%A%m!eyM2E-Yi(E ztg{Kbp|dOQ$CXF}ALBPxsE;-oI%oC%qvdEqljVas|D0uejLmhpA|sMUi%>b4wVZma zDjInjwcx2{p^P9ns6KmAm|KdoX?jCbTSuH;u=|HCESn3LV2A|p?x@bLoPV1l%PZMD zkz~M-#z$xOXB*S;;94iy zs!DdS;z^clFP@qJw-Qm}w;U6iAH?7_`ci zvU#Gw>in~-md9{<=82j!E0~gFA5Ga`E!7$*n_^#dtC@oA z+Q}&X@85416>CDRR=hdSP*P$o_%eazHak}{rw@((xHb7G-VHe zIO%{}(8Kb~L5`mY4@;zDr!RPf_W{#^dONR+w5F)3zQKHJFrEfWc76pT(BUtS$gmAJ zNr|2(^2bG1g~6M@d?GEg-KKvT8lE)%)36G0sqDD_NlRn`vXPKwk=G>{oIeriwcD=? zJ61JO8wqf-_%*Jm-E(_ww!tz7Z~|aE+xrH*wlM7)aI;mgM(RqhS20ux2^v^;9nW}fd_xi$>e5rTb z7O=%v>7AfX(_g(SkP40_b-5`Il*Pp%8xdH)*~edS9wp})+&Y~p)@e5G`lhz*5+}n! zRu3{XS2S>tvbZ=KbShLGB@oa}+aWX?aKMi*{sDP-7?E<=0>%e~O@4{~XnV`Z(e!XA zV3)wyQi1`O5ts8IA}I&N=gbAn2iIIF}g`Kau@~4WO*SM}7Flq1jCpg$32J z46&*+(@-9du+;kX=_J`yCOz8Fkmv4qZMP36H=m2GNGow$KWjA&;Dt6>wEEuwn92T#Fcg$8Pp1YuStz^?ts)}z-x0hiut=ur z!W!$PS3wcgg*|zOW;nE4M@z2qMrD|c#2;U>dA7Ra3w!D3CZCkAYXQXzAU;QEw=V32 zmc3A>SlOe>Q%+0zHJ=J(R$8Mk((9TT)C}l6qZL z(L<~4+Q3+`LqQNjFE`|WUF$l^p+NPQL5(mRvD?_Fm3Zt}pSUV357#I|G=BoUHZl)^ z<&%Ugw-L!z{;4^R?3~|IlmbmTyw7LYc`%-*=seN#S)kW)HV5-_rKASp%iN%cWa`uS zU?ZVBboC-2uWI)8#^Apn=Q;^yOq%mXVqbbmOxgtiVcS0f^T+_c_i*1fwPp$v6(jW< zMryiu>FH&?Gc#KirPUKkNznSH+}t=s00Auyx*RBNDOTo+)T%Iz4C@D9^<_PTd7<^Ce){4|o^y5a29TtL+S{X6V| z&RJD1_5?w=hmt-~y|20t2@Y&4L~J7BeoB_d3-~(rp67C@9%0`?*Tzo(LzH`qbq^MR z9RqHn-)Ow(Y%;R>DlvtIk1N`EczpqU^VGxBfOwMJ|SrVoQz0nYfeUq1ft8`~(98A}SlvW$m+ z^3_x{4h_cJQ=eIv?*Y{QJ??sCXm~Te;#q*5uvy=~S}8-FG}WwcNOReVRPYw``uSB2pFH3o%0gm(v6)$)(#UByErZBq42a<5W1^ZAXRsnZ00XEd>Sn3-z9HdqsG| z303BEYmk;ZO%KOOrAWTByR?Vt$?;uoZ`DGF)SvCwZJG-_?Lt1XtTBX%+GMTlasNAh zl48VqE~+~4x(T%O-KYZD|7ZDAbl0-9k3o3bS?%~JrscB9U*Tuyj5o0IyCjDk_I?1< z^+t4CBb56#T9)s=im{DFY=L3Kg;(*W7c9wgmURLj=)Fll!F9$&^0sd2$~eu}dtSD4 zaXH4foxjGZM(SzdJw>>c&z*r?qJlHNWK|V+a1Ak9sJ_);gLTEhmKPR4H;dOcpo7fx zFSCPlWF%)bw!@aga`x3_rqf|>w>lCgSa9qK(XMXIwol8b)(tZ|goO!8{WE?3#D_O< ze}WevVfiP9t&(_!sXpduXrJ+{A3w}>M?P9QqzZJB(bfMLX2w4=+sQ^d=NL&%N{YU| zJb?V7YCyaJ!*+U-+G7%eH*X>8Mmf}K%BlhvtxZdwp2Nh6ICyc%TxyOZq%~+s@jX*; zj5G{uv~v8;bWp!3xc1H-b{bt^Uf3E`*UjIK#Ah40kQTJ%o+?#{CfTu)Z?3ekdUQ+1=!`_{^?R=v?3kZ!{9DFowUCmd3e~})u@;rOw@*0Lo?);CtWJtf1Xv=ZQMeh#4}D~ z=ME<_;z@E;TP}Z3j0ff|^Aojftwe8z;&akUg@tO@7}u#57M2tfvm>FJkSg0LB0(oQ}`ga3q4)mPZqRd-zm!Sb*1o|k;~9W`jFD;YCon;peBZqM}3 zCMRHfVaAvP-5PxBEkUQyo6WH4ROF3~CYN9LaDPCxUctSvTY4laE$`x?WHr~dy6<$5 zTl0QqLHkyx&wUSdBscAHG$1mLuT7c&B^El`8eemrZUgcZI9Nn_sH(r>g;!%@;ybERHAi{D8ID&9 z{d|p;f#KyEVKf=)gNwG`tud@P7M*dJ5nV# zGu?b=Kqkzo12=>47?3{3pZ}G(=83Kg(&1$1ypB~UaImBER4Cc7w3`5sx)M6=mpIm;S`WCXv%p-_w|jUsypJ zQB*kE(<@d?Z_71`Ub3hvs1Z^w2;Wq_0ucH2&w% zbMna;qxU|(F;0M*A);Xs5%=DL6mJhIH>NbZ3>f=2`zi5kP%$H4X9c~=@o5`Ap8jy( z_d+STq-J@m0*+N=g>6($m9%y;P@qt$4+P?*Hot0m)E{E~%rX3lStUDi!o0zpK+>&4 z_Up_~(}(rUhs^fGa{PeG{d5>3*hq@a$D`C&>^dJ>tC35Q<~37(e(gny1;F^PRsAgkDa5Ux9 z**ENNJ=P>tIOt$6J3CqQoV$#y^cSyqDT69`GKNEcP*};Z?#iwWGP?;s_FKG^Xny>- z)A2bvk&&&%&rja(J2|v#eq~yB zn$RPwo$@JbGqb6Y^YeS9%$SB@oBW2ns@e~qDV2zcOseMQC8$l;L=242i3aMo1nLIA zl27DU(Sdkw1bh?=bV_{~Tb2K)(8m0Jfe-@(wtufj_LSe?-$rrt-}$a09eoQRPQCP! zvDQ{m!9z3gn6!`y%kF#`C90^TWy+eoJ(4WgbBp{V z!W_iF4xO*c?y9lc4(tttf-QewIWebDUPgpK9bbN+N$mcLzqz6%Z_1D`DB|a7L*e08 z&(z0wiT0{X?}yTihU~+Vga@cSFNa}rVaG%6Z zjwc10amem->vz1mhdX2?G~T=L7++=5Ni<&JGs_} zcqRonSo9O$Fl-IX|G|-2oRuvBHb;YQ;{v|fvw0`@5N-ZExPLc4|FSa1-XD68nnq7` zytilgN&3$SOyn~44k}L8(ww@2?Akw*rr#FC5aYkb!+J}gq^!=vK6ElY71vJtJ;rDU(TIA zzRNug#fp;L#v~{{+GE*6+mGab7crtWS_!HDvci{sdR?Vj{78G`@owc4^Pj1HQ+gSp zevHIFl&m4r86+=%EvE&l+K>MuUs2ZnQ$bNwMn*>5ZoReDyUXLu7vAye$}ygfEGaR{ zs!FSown&J^%FIf)G)3`aDwEfHuEm8V1tl{X`5V|~QJQs>mqtIvxJ~rHIUZRhRdqEz zp7&GGY9$1`0JVF+@DNaSY|Mo}tXUsMGXI-hlBBfkSEp4gUzKRgsOfNrMdNSt-3gPb zmYUX>nC=MFu{{Z9;zvamUAeHu@psqq%@vNyCl0*_uhr~QG9-e67& z{uMU5aVYeKom^{s96GYo2Dc4c zA;W6O5XvFkFobVrs!xw<{-d+{MpX?KA1J$_kMl77_-oiF~yZS&LCbDCAnkIX33;0&eyuzwJ0?$X;jbB+1J{wcE0 z;K|c-Cxq_vOaWp~{@uOXp3H~Xt}b#R;hq?+pW607OtF_U@y$Eu@;df5-z$g z#Vut>2%{vKv2;E|7Z$0e-VdMnvXHz8jQD1b~_G-RgbX6+~(#M(ZSh~;F2>{i#oP{ zHj4PD^(b3VztS>pxjI3+5qt|fpsE*faQrkY$=urZ^`~G}rYMNdXT?+1@YMR>i zMO4fQUbi2)*vZF?%KtRL^&@k%`2#J&4Idqf14LFF!dv4k>=aTLNJw@}6lz{3`G=|R*C3(f0u2FN~F!JEI#M8!H)qf>#bQ(F~eliAwdP4)gY4(%jtKH$@NK+U9hW-jftKe@&>z4PQ|*OU_B%&bp$s%*Ra=s zF>6dk-XksXoW=-+%+^%$xIvCV|%!HgHBIYUFu&D8%$#@xW|%_Hz17*~fd4;F%^ zW&S+`G%&BiXCCSaBS^o&dEq|6kG%?F{R~-lE@K?<2sI_GeyQwNcf!XoB5ga}%hSxJ z68|SWn{|eguC2GR(e$gy4tMD>FEfM|zkSO)I?_McEPQo}HY7TBjV8OX!uMVTo9GX$ zIhli3;563BC8o)#tfT#JT*Kew;1T9AW zkZBRF7#7i|pZEcnn%FFPU@vf@iq&p+;oa`^Ec`*co}T{)2Dmhamvio(+O|?_fFd1%|V|zAV_Gt9K5NRUo`OI)p z@ilytp>fug2_NH?kN~8w^CQg){T-ffpZnt&(p?wl$dN&}PifUfugp`YOF2NXd4q$U1o6MS7a^yN) zQR+#J0~W4QVi4-x=RvYAvY_YO+JLEyH?MK;I8 z1AanlHdT&X9?AX@lb2V`eNIg-20w`(FDjGR8XeUyAB%3HRQtD7_uthCDSp=EZ+XO6 zA>_ab?>}m-@B_O5vNx33IEfDq6OY!xQP$g2=VwdPgx;O!F+eyVkt1_}LJw5nJ`wz7 z&(Ya60A=xSt>fSI6=nuG1l`ZO-Fr0H2{#l7?G_KQYVf-c3D#ei|7%$OT?bjEt?IR4{V<+v;ld3u z91JlrTd0?G(?E5UXE;EcueLI3=l@%FOBVeH!aREac4x8wNVTVNY6h$X-(lZiq_TfR zd#C-!>%_3T@-&is%Z%p|75`5t_qOzeOtLfWl6W^PXLPe$Yu@?;zZAxdtKt7P7$Ma+ zYL+dsWFp-RNTUK+L7i@vE%$wDGFYl;D+?g`HO@?J{E)Uuw{>o&sMWmA2A%joxc^zN zlv4LdmdYc39YhA}6FGvb1AoekWpN7c0k8$pyH#*0gkQwj)Xs;KuMk2ue6v&%o6s93 zP0q#yEI71GSUmY^8i`S?Fb_0=td$pk>6d`Y2>9{eaNu8p6@8H~YqC?m3|SoV%MVeq z-pfSGT}F`kg?fNn<=b47C>Bwe$bb~?$^x963gr$GJgN&u@|+&gBrw(p`)Y|nh%W-2 z+vqS5zd!v$eEd6pUsegp2U+M{qDtb!InrY8&!c_j_@#dT8q2g_!aVyL&Ab05aLlg5 zwN8Hb?r8Ym>^BdXgd{)+#ghPP{N(>g zrLrL~I%e%G!A&6zVN*Q=X*#*wMCwi$TOCLx&j0NR**rt6%M)BZDrsO^NiXfc@DU8M zw~j>q9*s(BKoPVNKonxFUYx_=uPkt3SW8&#KwB0t3nX z>(akc6F&TYI@*7pt{7(?!pguF+4j!nPdpeH40c#_S|;?5vaPzT1h=p+;EsQ-r*OA_ z1pj}tBDJS@kh$JP{*Qjr-rjI9+=6_#xne7Rf<;I1)RU;Set?F&i+}t}LmpmV`!{XP zK9KeLLxu|5?8qX2eFkroX#2+6gEP!(>yAS6`)RbW0qe<^cW8`zfkJP~RX!N%NdB5z zC;wkYqt~1L-%=SCqxxgGUwEa|do_&BDdnA?8N&u4zIyT*!ecy7ZF`DgC<50?I~z;3PpqtUy2Bz9T_oKs!A~OB1#rJE_BNkf zQYt&cP%!sJxZ|=^qK1Q6tvwFt@6j$JK0Gor>YflnW&iam9l;dBbu&Ta)VR4Cq}_6CfyA}q}y@iG6tiOr+jkL`deGZ>hJASYbx-=6^wS@dh> z|9UmPgZ=B@hJiVC`};P)!yUX|{rd+nWr6?OA4?5;ZvbZ)-W?Pmz5{(U-?ngKC#N-^ zPJthE5to$4MD}O??7TLf2-%tN7BA67RADP?Q7@2(bbN@qc?tyEfjD>^7gCew)_{6! zDpWrp9nGFEIPC3B2m7lI_TEQ~v{(;d-~*52eYUg6w}$fan)mYieUP6(yYXPsff1}W z-!ssUegu6&TFq@*(hccopzf)QJhuYVV)oMe!vDpW_iSY=%yt21K?N6f@nJ+|(6gp3 zsQaw$a0Cb>mR<4l&s}orX{s11r$M0b$L?Yo=>gla%*y--pUo_8UR2uPimJE4Uccj| z7Aj0@44J`4)RkJ(D2P?Gsc(wDL(>sjUiGyi*giG1`n9vAr@^c_3$Ld-pLP@^lY;{z4(%R>@{gtmBwzDUIYP&a+xp^jBM|4Ob|J(w%_^!we>&$n%IrdaO_+7 zmz>VR^XkdYR`)4M$HPY4N1(nfJ8_&8dRM6N^D@AdW}Q7=`mw1e69x|bJRKAZ{v3=l zQqf>ptZI)#^-bJtzf7y<{pZi$m+78oGsaQ;JG3OY9QA?{5KOLvph<6|CuYy`!bvD8 zsnUJ$IWDLMBxa-LGu}tZ8Zag&5Pslb5u}M4Z%uM^R{R!Ywk!Kh=hU~0n#SnD|H61- zLG2vALKzH_RsLpc@+?iIa_5URA(|t<-M{2;kSdbp1AQAZoC;Xb+_!1+nC%+Y zQD>^DmREz={q+a&rF#e=jN$DOUThZGWnQbJtKMVj&f%(6PM|&i3oIL1k4RoxE((gu zrlze9XRWMPI+NFGje3@fl0@K<>uZetU6UM!uNcQAr4+iQaT-)Nt1I!v&ofP=V7JQx zFW37jKI0Kpe)S}v!ZG)G2e|pRKcwtp#A#G?{Lm^;Fe$A!>8m#+_n0qEm5@Gr+o zMKuH}zLZ(#7ro;+8;?C${~=EXz+`P}ebCU8=E~yzC?-kc6>=CGp2L|K)DvI8N4DfF zr>T~n*DX+Od!gmdVOEl2^>Y%czw!CrDTb8m{*7*qxD>}Lax7+kF22LD)FJgWkCUPJ zz$R96qj1F!e2LDofcA2+ifP^Qw9`Q1vgqjB*l|xaK=*k=GH1HDc27xMtB_j4y}kkH zVHz#ZM6s9dy)nB6I^?yq5ygubkr9_>OO!O7Fp{Uxho~hJ><19H5`HA3(c7B8s+z)?AqH z6ywmSijkVavGTZEK^OhSbN15^y+#$aCGh#&QNM?4{;>+ok@uOPEzFT4jW&pyrO{NG zii1VrL)6{P&Bu>1nd~K1x3o`-9Zd3!q>r7wA0UOWDQM zg?5GB5iUu~!0d=Il0H$MwXB^kWX=QhxZ=Li@L2hMbMkzA-L-B&JY=Mbg>9RJkK6r8 zFA2%L(`01042Qmjl8SZtN)!EfX?fz?7dSTSS%WIoz0sLrOKla}?cs3NyHkh}P?~i~ zR8+i(=9(EXNJcW_wA~SP3mrJ4i4qT#Iib#V=(s9KsD0@cj4-d3Oj(ld(@1&`>+2gq zyvVd4#M;qoaPIrCp~zT5E0wYK27?zWsjV#|dGOV(wY{KDyQA% zPc%!wTEEa{XP5q`AvF%fTIH044`8Ya=_3&W`8Q1B|(b<_`~2BEjgW zl@sI3)91$rUZB%6<7LmIk;9lXu>yxy5sJ6TQbyflq*s9Nf{xw9Vc}XVpLfvny3QC? zg{M3tS~uyjCR*9`34IsPl{YgoafuQ7khN47MV~S2&$EpA8mmHQEVu;RyyFq<)P3XO z(`RrpOC2RnN~%w>dU}C!ih-)f{n>qay*Gh<1TK7Vk0WMslX!WDh{<}5k#q3l+aL<0 z{o}**jc*qDy5$KCobx+0D$!9akn#-ZV&V_If|&I*pC{fsZHl*Z+Q|(xJoel9){08< zvntvLoB`<3-@e76L9a_R+zEmCxN!GnZ?AW0L=-i*)xM9f1ZX{<7hv(nljm=A3jabjI`91 zr>!G7MfgXawhE6eX^Y+>!})TY-S8%+A=3(xl;+w#a+6J`20f~BKfe2Eir2Xs{@(2;ZLMNc1N zat$}fc~#|B$e*DZFUgp>+fA4q9V^R8@xRJMhKEmkd#~yTUT@QBPAyLIk0FFvTbd%H zqY1aZ__b4n$go&A(HW0ct{lzo91=C!KH|dqjGC2yhBjCBvBVnkXcFtL$z6W_Op#rc z!vK_TtU94R5LlZl-s6B>qX=|GCz&;6O*F8pI}oi#K^U_6+0s&+Z8J+r75aGh<~uV%?9zMJ09nw;0jwM5A%)ERj zubB|$%N;=MOvY)lWVvv1!we1-A;E?^t)2u`P9`?Rx4i~-ep(U@{$XfUU*2b+IkKSQ zzAJz3ksWNafBnke!WbXMo6-apo~V8qDo4-Q?-CGBMuAm$TpHegj)G+~Y83kP{LoQD zs=pqJ(7~^*4Y;hWOW>Ry+~Z?qpFwF@guo=5!<=OEgV(@UErZJHdfz9F9+z1^QhhaO zXo~CMXUhQ72%jc3L{KW^*7d#an_JhwxtVC7QO1!r$ZcXPuNoE@S`Ft)HWp@Pyf@h9 zKn0KaFUUZuc+e*th;Z4=n2ny9o1^d?$R6kJ76E8IIcK3X9~RYb$a(bI{+5#J@)U?g z01rWOOGoN%b909XgbGdKB{;~!Sz2EdJ}QWI%5$D7*xU(Oe)B(|r0O`m_2@3YcQ|Z4 z)|I)szCtC;nnG{7kBG;m*Y0o$<=t=a^6IOKI6ABgF)Qq+!I4lElQU2p?2Vtr)6&s- zWyscpt~qBq88agbZm~{$M_72*5h#3e zm>-I)^l;l_ul@88Y~9XSjELyYgRV4IH!;lRK9pADVv`RJw_^v>Qc^|MY9#xynnJ_h z=|-`9!~Pu=q3P-$&a_jogHk{)J53l~&^2`s&yqvyp270_ux>W@VP6zsD9ZQSn;YoW z6=S@#oGqU^Cw~|E&jvADq`uI{;6u=g$C(U?_Ve!SO<}M+-*c;Onc7c-u4;PTY-9WQ zQltCm(5=g$tkA3zzrdOxmc&qZCyy={i<3Wt+}-NU3I~2DFqz84!iJ;lpooVYb-g7b zXp??T^p@Z?(fGuYs z6ELD_`}UVReKDUd(1%rj8f-LSpnS*WYZ=pF*S6`unhcH0MYwA?8ou`xY_Af8GYprA zESyZLp2nGXvGBYROCxRkWPt$`Sc%pTgfgaas4s8Lt7bJ@noJUQMx!OalP#Pq)z^@b z(!7yq+n+RLI*` zFP-^r;DS^H!k){Q=@>X@jh8d6t38DRt9D0vuJxQFHYba9dU(ECH9e-)&r|ht-EeE! zEATwZnY{)lNOJj@k!T0>OwK5Blm-2Yxwb=9nIoCJ-fYYw9hzA`LyOFLF1v!T&|Yc( z!v&yAl{<^tvY1b$Me}n$5Cf^0#TlxW5Bf-1__mv56>QTw>75AMiPIQ+7%l>f` z)sV4kbQGp)&ApxzuXY*MH;bSU8Ni|^%wlwm`@$3nEl>wPp!aw-Auybuqcm3>{8Ha@ z$^3fz`7QXDq5btbwcB`Wqe5e}gH8AQz0P_TvCDiIFb9p(wVkg4Dux1!>GwCQ`ucZZ1G7p_^dd0s1w*@c=WS%)@OJkz_gl&DI@%gWdZJ_)1IYy4KOXhSIJ?@t-AlklK@|~ zz7*j);yi*W{frE=Y(+HkVeiOd`^;6IgYh?2;Eg2yQTf6}Qd=1XU^lk9uNm=|EU#EY zgiDat&un@A{>}Fh+?yK&Rod-iC<1Vo`?Hv&*G4HpWEI|#cp-BJtb{`V=V1*|`vvS_ zU~0>EYqMl2aDqDpGDQEta;~0C3T0=4hxr&p_{QdEHgc89hj>wg80S|n1rUB;U`2yM z?8v-}ijX>%KDCiX3G=ybvG!j?g-*)qI6#g4eyv`;w)y){?-{}0&hOV-EsPh4e}C-4 z^OETAKmR}T3=+_Gyw7`GyPSRwqYfSniEKtEm%04~c_yP#egW|S zjZ%Zs@8TtM01OR#iit3~ARVdbFrQh$d_&A;;hPA-9n`K2M%b0w!>sTE%5qLbLEN@+ zpIPj(C39VF`oLTyMusDrt{23EESSNa5zo6Tv$|p0t@@xPG_WF&QFW>Fdv+R{Vm^jbSVd|aB1IL{xYKM9` z)MuK|+I#tLvVtlYzNC$i+ZX zUz6qj^Bq=zP+baJehKl~7)pmsOZsni^Y}P}NiwhY%m05Z>m_ zIOA)Qz60Z8f;WqWW8_VlJwZI0r4un)>}*WNZOy6bf9>kB53$s_hV+Ac)ogT>ntaSy zKVn4g>*W(44ukxRenfrjO^Q`lms?yhwoagF+8g$3Xs%Qn97K!Z)z?yXmjIGBdh_!A z*AH-Kw@gYTiEPF2KmI7s$mO=2xA2ke44*ux!tRQ%GFexsaN}GcIG{>Acu&UN%|}sr zWAU!)!H8Z>P(ObRbUr$q(e^}ce!Plum?${5mvvMU7NTUuUU70@tNG^!o=xM~%+A+v zhi%v{|GZu6S%~6mApF&R!@$kEVpKKUr|EQe^FBy%URGUrh?^6-Jgx|ijJAC^)e1rM z|9}+~hS0%P*2I2J*$?|t;NBTmf^2WXsOggLc|}m>L6M?f1#TAZ-n!)+mc?%mL@6FA zj@^TPwC>TfoK~uv(Y+{4U12nc^a{D|d5i>Tudb~1fChTnzEd+x92wFqJs+!;=Jpz+ zG8^?g-PUGb`O`Op110WpV{1o${RdDpVxppg&*R=2J7)*DwyXe;17vF3rVMV};0#bZ ze=}CDS1$BXS%r3*P0Q2MZTP|0!=2IYI_^`~hqIp?gIYfwrWb{cAc7XyQPkiDb*!hSRnIMDERw_23S!}iKkO-bDuu)*)D_?~O}&IG%W(ISrRztdnv zS-7nRW%@o~EV~4OsLZz)vnM54m(rgBYA%W6Q+$BGfSiR#KiKQqdgNif#UzpU^u}Ha zt?E!>IJKXU8oqYi&|%_EYB;cKM@C_oao+;6vpKC4>*!d`r0h#Y2C@*(I`(?#X{ZJj zj@^{0xO3fO30q&%;<=C8^5yWAmIX_Q8zQO^9JJi7!~J#*s{}VU@RB2{WC|3NRwji- z!+h;aSoLO;i#fSvW9bo6!|Yz$vxojggYg9Ln#4oRK5bYwvlb4g5huDG3G)k-iF1ff z9;_A*g$u_^kRwuK>HzdonE_S2w>Xwns=G@v#7kW4qioaMJG83Q4U-2xjPd*D9|%Mx zlj$hvb44sfpk40{6g9Sljj1F?7_3fRocSJ7F66$cj^%d|;$VLT=RKciC8u3rAaga| zOE@LSBOtT;c4ST%4;uFTDU{heo1GnM0o7eA=t#93vf7SI;3ef|lrB|Nj!&iMdDf&( zOU)Ca$;frEQBl;lbhS2Ks8jf=v}ji&otJ||gM&i*=l2*>T$zDYhRH$W?g@4TqDl5_ zgq^5Iu0=_Hdy*?=J64}UY*GX*6OnPGeP&^t(_&vR)Mqj^91I| zV{kW(Hsq;k=DAZNdC`#VTMTwr*C)U|ESpTWJOpZ6H$ITlVJ~SCrwy)!V63DY zd}1ei==C8^Cb^9gB^%as!z8o`WwsvczYS4-+9(^jc5;@VS_FI>RZM>wuWS5;CIxzL zTD*j9kU&OZS<(K|yrJzLYuhZn z2dC#gRUW$9<%n~!^CzND5d{m+QqGm~TYS)ANH(Vuryug0nP22T>E%seaQ9(?w*d}} zM^S5vkPE{P=XasqwLXHHlnHEP$-d&_8xnyV?~|6(2^|TlHx-zSdNM>i)G9lcYxaj`mG>S~l<^zPf&4WJ_1BN?_h0yAILv~} zueZY!c#WeZO9^&Q!eoXsC%x5X2CSB|r*E7pS%{Y|SnU0QGLzCpjyxg~7uRDqClC7| zGrJygLmGAWnQEC8n?&!L!|etvWJ5(sy>E5e%RbhDLy@_Id=;NKdHvhN0T9#~9x5h@ zi(H#LJNfXiS;E%<{z$`ET2l8E3>#i)3fY7EnMWggPkkYO_`!jw2{?qB#h0UoiJk1Q zwkX#9X7tjb+PRv|ez>j%($u(HERdt+e(x8X<3IeeFb`6aRTYJkDOsAmHNG&Fp?F7@ zGeSW*ND?Ok?=So@cjWTyO1;`CD-4|v%*`&Hs@=`7x++9l$@UC&Cz!3hJ+n6sS7-=F z-|%{zbDvz#i2@u?q5S>Brg954HLvqUfV-kuSPE5DcUe{4)0na67sR6i&3(@&!0ntg zLe~o&3A6Mz@PWf;Qejo08x(8{BOf?G1mqtNJD+^o9P%HsYMw7&L{k&$M6<Ds zBh>Uy0pTMPE2FD0Jlvix^!8QJ;}yHlbtrL7Pw^`((=($sf|icMja^> zdFVN|1RppEhN=V3V*lU(9TlDD=fS~XqCG{q{Qc}eSOr|m!6zT%5_?%UcUsly=>k|3b*Q>MZ9)b!d@Md7(y?T5Rr5xyHl@JL=X26~9j;Q)DI zRZdiDz{pN2ujfsx)$vMOk-CUtb;(c|Rz+|2`NS=>(_p^X_5E6uxmA<-S%31i$Azn< z<87lbwb5|3Gt=I&+4KH{b0ET9K{Gt3mVoQdiuRosDMXUs;{N&WxSheh`Eikho7DKl z&}mpaeJ*zl!m{lODK7zEO3^Q(1ZD;(3t7WDEnojdEqQ_Kq-MLN|=;wsi5KsYd5 zliuy5ZM{=@=r45@%d|}w)mLy^e1?I1baI3fB*gsO__*+c2_i#W#dcS*LvL+fIKo$9 zzBeDH9Ay_&uhW7~C>&{9Qrx3;tIFNqNn0eL0jCSn9=%S83~zo@7F^ zfT6H{-7|uFmV0CwO|W37`N`t=xa<+`Yk|3*GeNE#MpYL@4;@h#z4_3Gj)u^Wg3!hs zvnueM9ydnHsy2&)HitX@;9%wnnLE!*S*}SaF zJ_RkxHAzG}N+D{@-YB9CPrPq<6_^ji%+0yn!?r5WhD6fS^`c08WJImDu`XlYg7Nka zQJ#i8wr>Q?DR3fj;;GYPjMe!{_^pvgsC6rYd)s(S=w7M*18j4HC>cnmdW!fpo}JDJ zYoesCzD11rPqf31dneg|vr!DsdgFWC$^6{hRUpQEdhnY8O5As71O?3Z|M+@+4Rv_h zRDNbUe0cLN;%iSI+wwEJ{>jti@eYa0ncy9KCCdUN2r!AS{M(cE&jHdHGGu)x(qSaf zFFt}{o?;Ee47wAt(RTMd5jQtC@<-X$M+byDpLb=piC05SMXu(-o?JY}p0YOVIfQS< zY%lDTO%M@DO;h0i;Q|06-~axdE70ogQ5gQ#;`)c6do#WhPk5de6x?AXEVTt-r}r5^ znkMaKn?QReAago_9+9g0=8*EMWL$-$av;VpAMse^E597w){jXC)*;I=oY^8#md4ZT z66L+BbIRO1ImlrVDv(v6V5LFnO<)}`t14ZxxpsecF@^dXF7;rH_AW0Uv(#8paf4EH zWt4!>s^`oGKgUS&xeWKI4gF;07CV_zw)%W~;Mp89m-g6YO_#z2I#$$z9xis=S1YsA zokXX^#j@DbNu^Jr535(6SBGI#fcRly{j?u9tSR&fZgl}IOo`&#nV4xQse3~MFX;Ki z9*1rqg*RFL?HwQA35-6UvGFkAhYRAy2QSc%05KGJmA3h}|G0=>UR;@3nv~U4wrw-1 zJSiI=+fLPPmY`kiDq?#XD6CLaTOA$F`VMFk*^XU<;4e~L!%U}QE75l57v_jP8JF9n zA&#~?`!rl~a&`;L>9Df2+^!|tA@&EBe6T>t%HZLxgGqHkX>>RpZYock5%pS>EgO8$rW?$o~BXjA)nF>^-t(>E~yTpdb~RjK+`T_Bd7Ij1XM; zjz|H#>cE>eVzAspR8lNy)cBcr*F(<1Z2Zr>d>cZ=0^UAggL`w%JMBH!oioUwOvou3RhN#-CXTP63^UYxTl2Tx2F@a@7y(_k*wUdK1G zqFC7A!m|sK-2)Wnpn%XK^w>9?3TVgnevEc|$?ao5@v=Jwx46Sk0rCyw1h}okikZ@MbM{94S zB6L|?7gEnTzfG?y>=kp=bhDuCc7sn5eRBwgZs#+}ya&v@lJQq+;%gjF=W-;%5=SV+ z7cW_e$`CkkGV5+ko9<1@LH~Lpo19h<=Pc9;}a<%$&7z=C1%|d*Z@adelZw+(;kf_{~+!!b9A| zg=_)t&$lE%71Mzk3r2Z=Q)}~PfPs)>Kyqt}1EJitWsYtF0j5LF0MJ29QG6Jjk%yQ&C-b*trHOkpjvb z8iQSqmCG7$2zQu`FCxa1gil0BZWa@{lc%a(w(6C&Zptv0ABr?eb^h8@QuwBzV<6+IhGn5$0n9CNYpDTK-a`kPq*WE|P zNx*&^lY0a}r~wgI{UJQdY)yL8`k%Wm%`83{4#tRMe}#Hr6X2^We-rR&Pw6U>c{ksO zd3s>hPnW_YZEZ_sZGBhCA!jQ_dLkX!pa%j;B}$Bh$eqH&CTa|)J*>BP*J8g*9i5nx zJ`tsbHo~pQo<}Do0*$-m9&>WnyXf0-cLNb3h;IVQ*|cDZ!}?D~XEGMao_h^@{`krx zoKrBooZ)F=!(Vbn2vw97t(D)x#5Zq=@>R@@?hT7TyChs<-k?vy{d|L$N@CGYUd3E|W%48anpauO+50R#(HBMT0Iu?j}(*BC)mH+On zkXpW*pL78@xMD@*i$+~lgWrE<6$ z0HOMvDUh{pf(}qtcA$)lcSJy`WT0XL^CQ5U#244+l+*?FQ?Y!s0AokJbF50o#GrB~%SI$JzQpz*#FDFtMt`iszY3 z(bamNbqtR4EWO5JWJ28Dyi~XA-caZs2Nv@KS)K5>SKl*k7!_S(7F@EhXN%2Y&Ep<5!1uc((+^?{{g81&&K__WdbJn*#;FiPf4a92PO%my$ z{x=-QvpNus>I<OH%`lLj+(ss`B#(#9_oKv;@8oW6rVa2lx?BbLakX$o7CSTJ zBg>Fv4EE;FKz8u2oG@BUR`Z3$Z-r%MPAR+ODZBUsofndUec#7~Ubo3VdnawzrWN zz96ANr>s`Iy_@jek?`1%zM)sk*RlI>G3(x0wjn%xFaq1gU^roEQKiA-C?KhyaTsHI zVo8kFpiW@UW{v1!k{tmt3fVw@};KXr^Uc=Quu((3^4m)cq=;F&Jslz;AEiuOIyrkjwGmbZj~eH~h6YJcHOMB!g3j9OxSH=0ntCbF zNM^4b-KL8J!4LS@S6p7t8@)AL6<#|46eK(_C&I;#Ep0i-cxZ&P2mz8MX*3a5>e~P3i_Vg8>0w0f)aR(8)ySI1{@E;|M>I^A-`|+DRK#uhI|B zKQE%E^AnT)&x9eYB-z%sS($@gN*fJ_A5)6T8?}hY5qju_2y>Fh`S!K@hMQ?xWk0Ct z>CnZUSS{T0^G~tJKVHJ&;$xn;^^bfOgu~6+n*FXA+)YWCdF3@}5q|zAOxs(9rjDC) z4IVzw`u>4OEol9%e2ULsGD@Jp2fchWnZSMzW=|}^-2j9wWJeQ&V#QKs{Ht;#Q~29J z6K-OLBMa;5xrCp+v)gxtZ9bkEDn#$FR}{6oHqQ36%@s7&l%&-an)*y+@9zjJ)jDzx zc59d?&{%T|2rrTgsazNE>EnJFZTOOxvuV;*-$03;<-}}A zcSflS`@$=~1$;CvK$0LH(N0q!nL&I=Z<`V-I5zHBm(i0CFb?TXju!2`)W_#~nDS0u z7p^V_hvgEuDCsyfRgQb2T(KzNwF%#BsF}L?1R$A{>a9D@f2lreJPN@8(rO$WE4x4C z$yMm!Z#0sciP5Rng%jN3PZ;yu(mzXY^)3ha;dk6JeX4KZV$75KXm|F3`i&gJJ7%kUF=PU=*5$&$7MJ8n zU*(;{;V2!X%i$z0FQq0ZNAf$U-{iB!%AK67=xjb1l3AI5I*7!oaR?+tMhJye04(tE zPy{yiCKzyLlgdLWO{1>v%AcS#?{@@&{GD#k_Fm^2hg_MhLy+L?fx_mX`r@QYPa2=y zgrzeEm$2bxgFPMT3~++$4@RY$^hQBE zBl+wYj|9WqixT9Xd>5r?@4TrhGt2KwbKqW{>72l67;Q-6Ve##!nJs6%Cms#lAX z$*DZwtA%pViX<39p});-R_O8AM_3h$8)G(fp3HW3>oNa+<^HHAtC^Vi1&`NVet%E= z(@2uzS-ydd%A~*j?PMC;zT>3maKBchm&3Jh4jakh)vXF{8lHr#iO||!R)+{q3Oh5c z@mkUXtAct2H2)fOA7l=GCH{@#s}5ZF&E_D`w^R1Q$JW7jRh!daJ7Ze*HigUePu}bc ze+Hz2j^PcwzAFNvFvnORSY)wFO6o^ZWx^Hd`nW?fr)jgkI#?%RRsZ-RNN9d+nOmuK z4GaH=C<-~b{WQOIyg3HC8TmNSg@hAuL-~WsOVx=uvQk~}N2foWMWRwTit-wo&IQ_f z^M;A|MwjJftn((&r@qCGW{xBIr)&`w9jI-bYO6iy{}B^X<=gAVNSvilCyUGs!Q}sMyC52U4*! z*;Bb&`?m3va*eK&wqA%4g2)2q5eRjvuHk-|99g+JxKL%<=_Jp^N#j^n)V|+^ zv{)1u<@%2=O}%+T{FSm*-Rr(7FoU3~(t6?M4{}P7)rpP}MAPwtSljFADThhb#d-Of zf_7_oz#6^s7&`QR?ak%D`7PgyLsO`zsEVSV_}v<4#jzlP#rX|rem^+m)8x&+O_SE#2jhlHe_o2+POUx%^{o=mCi(6FW>z-0}_N`M*4 z20WWRXme;M?Cc25sb>j`E8fcIeT%~xZ++-8&#_Q;ORmb?Byu+#C8TGk;dFnh@cn^^ z8J$PG+`wYL{g7}I^W|%Dd2_CXNje+ou5r41{yZ8M3X3ma_u)N9NI!Hi$S+3rvoiU6 z@}i=wlDwiYG+;ng3rU!!$jU oz`#5|hjk0z1M3svmobKeajACQy3VV*{7i6!DAOHXW literal 0 HcmV?d00001 diff --git a/docs/architecture/teleop-trajectory-annotation-design.md b/docs/architecture/teleop-trajectory-annotation-design.md new file mode 100644 index 0000000000..0b80cd638b --- /dev/null +++ b/docs/architecture/teleop-trajectory-annotation-design.md @@ -0,0 +1,403 @@ +# 遥操轨迹图像序列原子技能标注工具设计 + +状态:方案已确认,首版已实现。日期:2026-11-09。 + +实现位置:`embodichain/toolkits/trajectory_annotator/`。工具接收一段本地遥操视频、HDF5 相机图像或已有 RGB 图像序列,让用户逐帧查看、标注原子技能区间、检查和删除标注,并保存为可继续编辑的 JSON。 + +本文记录已确认的设计约定;启动方式和快捷键见工具的 [README](../../embodichain/toolkits/trajectory_annotator/README.md)。稳定使用入口包括 CLI、版本化 JSON,以及 `frames.py` 的帧输入协议和 `app.py` 的启动接口。 + +## 1. 目标与首版范围 + +首版完成一个可独立运行的本地工具: + +- 打开一个视频,显示当前画面、frame index、时间和总帧数。 +- 支持播放、暂停、前后一帧、跨多帧、时间轴拖动和输入帧号跳转。 +- 为每段原子技能填写名称,例如“翻转包裹”,并标注起止 frame index。 +- 同时通过时间轴和列表展示全部已完成标注;支持定位、修改、删除、撤销和重做。 +- 自动保存、手动保存,以及重新打开视频后继续标注。 + +初始支持本地 MP4、MOV、MKV、AVI 等容器,实际能否读取取决于所安装 PyAV/FFmpeg 的解码能力;以 MP4/H.264 为主要验收格式。每次运行打开一个视频,默认选第一个视频流,也可显式指定流。支持具有可靠时间戳的恒定帧率和可变帧率视频;首版播放不处理音频。 + +本轮不包含自动识别动作、多相机同步、机器人关节数据切片、多人协作和视频片段导出。这里的“切分”首先形成带原子技能名称的帧区间,后续可以由其他工具据此裁剪视频或切分轨迹数据。 + +## 2. 与现有项目的衔接 + +已核对的现有基础: + +| 当前代码 | 对本设计的影响 | +| --- | --- | +| [`toolkits/__init__.py`](../../embodichain/toolkits/__init__.py) 将 toolkits 定位为独立于仿真循环的工具 | 标注工具归属 toolkits,运行时不创建仿真环境 | +| [`cli/main.py`](../../embodichain/cli/main.py) 通过 `Command` 和延迟导入分发命令,已有 `annotate-grasp` | 增加同类入口 `embodichain annotate-trajectory` | +| [`pyproject.toml`](../../pyproject.toml) 已声明 OpenCV;[`depth_video/reader.py`](../../embodichain/data_pipeline/depth_video/reader.py) 已使用 PyAV | 使用 PyAV 解码,复用 OpenCV 做预览图像编码 | +| [`setup.py`](../../setup.py) 显式列举非 Python 包资源 | 新界面的 HTML、JS、CSS 必须加入 wheel 的 `package_data` | +| [`recording/segments.py`](../../embodichain/data_pipeline/recording/segments.py) 使用半开区间 `start_step/end_step` | 本工具的视频帧区间单独声明语义;将来接入该格式时显式转换端点与时钟 | + +新工具只把原子技能名称当作用户标签,不自动将其绑定到 AtomicActionEngine 或 Task Program 的技能注册项。视频帧也不自动等同于机器人控制 step;两者需要独立的同步关系。 + +## 3. 推荐技术方案 + +采用 **本地 Python 服务 + 浏览器界面**: + +```text +统一 CLI + │ 选择视频/HDF5、指定标注文件 + ▼ +本地 FastAPI / Uvicorn 服务 + ├── FrameSource:统一按索引读取 RGB 图像 + │ ├── PyAV:视频 PTS 索引、按需解码 + │ ├── HDF5:只读单相机 JPEG/PNG 或原始数组 + │ └── ArrayFrameSource:已有内存图像 + ├── OpenCV:缩放并编码预览图像 + ├── 标注校验、JSON 读取与原子保存 + └── 同源静态界面:HTML / CSS / JavaScript + │ + ▼ + 画面、时间轴、标注列表、快捷键 +``` + +前端使用原生 JavaScript 模块,随 Python 包分发,无需 Node 构建流程或 CDN。界面使用中文,原子技能名称支持 Unicode。首版在桌面 Chromium 和 Firefox 上验收。 + +后端只监听 `127.0.0.1`,默认选择空闲端口并打开浏览器;提供 `--no-browser` 供手动访问。视频直接由 Python 进程读取,不通过浏览器上传。服务只操作启动时指定的输入、标注文件和随包静态资源。 + +新增可选依赖组 `trajectory-annotation`,显式声明 `av>=15.1,<18`、`fastapi>=0.115,<1`、`uvicorn>=0.30,<1`,不依赖其他包的传递依赖。OpenCV 和 h5py 沿用现有依赖。已在 conda `py312` 的 Python 3.12 环境验证;Python 3.11 尚未实测。CLI 帮助和普通包导入不加载解码器或启动服务。 + +FastAPI 提供静态资源挂载和自定义响应,足以承载这个单页界面及帧图像接口。参考:[静态文件](https://fastapi.tiangolo.com/tutorial/static-files/)、[自定义响应](https://fastapi.tiangolo.com/advanced/custom-response/)。 + +### 网页界面选型:与 Viser 的取舍 + +当前推荐由 FastAPI 提供服务,自行编写 HTML/CSS/JavaScript 标注界面。用户的使用方式与 Viser 类似:启动一个 Python 命令,在浏览器打开本地地址。 + +Viser 是可行的备选:它提供 `gui.add_image()`、滑块、按钮和文本控件,可以组合出帧预览、帧号选择和区间增删。上游 `main` 文档还提供带 `hotkey` 的 `gui.add_command()`,因此不能笼统地说 Viser 不支持快捷键。参考:[Viser GUI API](https://viser.studio/main/api/core/gui_api/)。 + +需要区分项目实际版本:当前 `pyproject.toml` 固定 `viser==1.0.21`,该标签的 [GUI API 源码](https://github.com/viser-project/viser/blob/v1.0.21/src/viser/_gui_api.py) 尚未包含 `add_command()`。采用上游命令接口需要先评估版本升级及现有仿真可视化的兼容性;保持当前版本则需要额外实现快捷键接入。 + +本工具的主要界面是二维视频、可缩放区间时间轴和标注列表。基于这些需求,目前选择自定义网页,直接控制布局、键盘焦点以及“画面显示完成后才能标记”的前端状态;这是本设计的工程取舍,不是 Viser 无法完成这些功能。若后续需要同步显示机器人、位姿或点云,Viser 的三维能力会更有价值。视频索引、区间模型和 JSON 保存保持独立,以便在界面方案调整时复用。 + +### 帧号与实际画面保持一致 + +核心状态为 `displayed_frame_index`,表示**已经完成显示的帧**。用户设置起点、终点时只读取这个值。正在请求的目标帧号与已经显示的帧号分开维护;寻帧期间保留旧画面和旧帧号,显示加载状态,暂时禁用标记操作。 + +浏览器按指定帧号请求预览图像,响应携带实际帧号和时间;PTS 仅对视频存在。图像完成加载后,前端一次性更新画面、帧号、时间和游标。快速拖动、暂停或逐帧操作会使旧请求失效;迟到的响应不能覆盖新的画面。 + +播放时按索引中的相邻帧时间差调度。按 `I` 或 `O` 时立即暂停在当前已显示帧,并使后续播放请求失效。性能不足时允许实际播放速度降低,不积压请求或为了追赶时钟跳过帧;界面显示目标速度和实际预览帧率。 + +## 4. 帧与区间约定 + +| 项目 | 首版约定 | +| --- | --- | +| 帧号基准 | 从 0 开始,合法范围为 `0 ... frame_count - 1` | +| 帧号定义 | 图像序列的索引;视频按完整解码的显示顺序,HDF5 按选定 dataset 的行号,内存图像按数组/列表顺序 | +| 区间 | 闭区间 `[start_frame, end_frame]`,两端均包含 | +| 长度 | `end_frame - start_frame + 1`,允许单帧动作 | +| 排序 | 按起点、终点排序;稳定 ID 不随显示顺序改变 | +| 完整性 | 允许空隙,不要求覆盖整个视频 | +| 重叠 | 单轨道,不允许已完成区间重叠;相邻区间为 `[a, b]` 和 `[b + 1, c]` | +| 原子技能名称 | 必填自由文本,去掉首尾空白;从本项目已有名称提供补全 | +| 时间 | 视频以 PTS 计算相对首帧时间;HDF5/数组按明确 FPS 推算显示时间,不冒充实测时间戳 | + +例如“翻转包裹:120–185”包含 66 帧;Python 切片对应 `frames[120:186]`。与现有 recorder 对接时,只有在已确认一帧对应一个控制 step 的情况下,才能转换为 `start_step=120, end_step=186`。 + +前端提供即时校验,后端在保存前独立校验:端点必须为整数,满足 `0 <= start <= end < frame_count`,名称非空,ID 唯一,区间不重叠。修改一个区间时,重叠检查排除其原版本。错误信息明确指出冲突的标注。 + +## 5. 界面与操作流程 + +```text +标题栏:原子技能区间标注 · ← 前一帧 · → 后一帧 · 空格 播放/暂停 · I/O 起止帧 +┌────────────────────────────────────┬──────────────────────────┐ +│ 视频画面、当前帧、播放控制 │ 已标注原子技能区间 │ +│ │ 名称、起止帧、帧数 │ +│ │ 定位 / 编辑 / 删除 │ +├────────────────────────────────────┤ │ +│ 原子技能时间轴(与视频等宽) ├──────────────────────────┤ +│ 连续拖动 / 缩放 / 按帧号跳转 │ 新建/编辑原子技能区间 │ +│ 区间覆盖各帧单元,游标在帧中心 │ 名称、起止帧、备注 │ +│ │ I/O 标记、提交、取消 │ +└────────────────────────────────────┴──────────────────────────┘ +``` + +- 视频区显示当前帧、总帧数、相对时间、播放状态和寻帧状态,支持输入帧号跳转。 +- 时间轴以帧号为坐标,绘制全部区间、当前帧和未提交草稿。拖动从已标注色块内部开始时也按鼠标位置寻帧,不跳转或吸附到区间起点。支持拖动游标、缩放和横向滚动;精确边界通过逐帧键或整数输入调整。首版不提供拖拽修改区间边界。 +- 列表展示序号、原子技能名称、起止帧和帧数,标注较多时滚动查看;选中行与时间轴高亮联动。起点、终点可分别点击跳转。 +- 原子技能区间编辑区位于右侧已标注列表下方,展示当前草稿和错误原因;新建草稿与已完成标注使用不同视觉样式。 +- 保存状态区分“未保存”“保存中”“已保存”“保存失败”。未提交草稿单独提示,不能因其他标注保存成功而显示为已保存。 + +一次常用流程: + +1. 打开视频,在目标动作开始处暂停,用左右键精调到第一帧。 +2. 按 `I` 设置起点,选择或输入原子技能名称。 +3. 播放或跳转到结束位置,用左右键精调到最后一帧,按 `O`。 +4. 检查名称、起止帧和帧数,按 `Enter` 提交;新标注进入列表和时间轴。 +5. 对下一段重复操作;需要返修时选中标注,进入编辑,重新设置端点后提交。 + +### 快捷键 + +| 快捷键 | 行为 | +| --- | --- | +| `Space` | 播放 / 暂停;到最后一帧后自动暂停,再次播放从第 0 帧开始 | +| `←` / `→` | 暂停并向前 / 向后一帧;在视频边界保持当前位置 | +| `Shift + ←` / `Shift + →` | 暂停并向前 / 向后 10 帧 | +| `Home` / `End` | 暂停并跳到首帧 / 末帧 | +| `G` | 聚焦“跳转到帧”输入框,输入后按 Enter 跳转 | +| `I` | 暂停并把当前显示帧设为草稿起点 | +| `O` | 暂停并把当前显示帧设为草稿终点;没有起点时提示先设置起点 | +| `L` | 聚焦原子技能名称输入框 | +| `Enter` | 提交有效草稿;没有草稿时不操作 | +| `E` | 将选中的已完成标注载入编辑草稿 | +| `Esc` | 取消草稿并退出编辑,不修改原标注 | +| `Delete` / `Backspace` | 删除选中的已完成标注 | +| `Ctrl/Cmd + Z` | 撤销最近一次新增、修改或删除 | +| `Ctrl/Cmd + Shift + Z` | 重做 | +| `Ctrl/Cmd + S` | 立即保存已完成标注,阻止浏览器“保存网页”动作 | +| `?` | 显示 / 隐藏快捷键说明 | + +输入框、下拉框或文本编辑区域获得焦点时,不触发全局播放、标记、删除或撤销快捷键;保留文本编辑的原生行为。此时 `Ctrl/Cmd + S` 仍保存已完成标注。名称输入框中的 Enter 只确认输入并把焦点返回视频区,不直接提交区间。 + +方向键允许长按重复;提交、删除、起止标记等操作忽略按键重复事件。页面失去焦点时暂停播放。 + +### 草稿、编辑与删除 + +同一时刻只有一个草稿,状态为“新建”或“编辑已有 ID”。没有草稿时按 `I` 创建新段,默认沿用上次原子技能名称;仅选中一行不会让 `I` 覆盖该行,必须先按 `E` 或点击“编辑”。草稿中再次按 `I/O` 只更新对应端点。 + +编辑期间,原标注保留到提交成功;取消即可完整恢复。存在草稿时禁止删除已完成标注或切换到另一份编辑草稿,提示先提交或取消;浏览画面和定位其他区间仍可进行。 + +删除同时更新列表和时间轴,并显示“已删除,可撤销”。首版不增加逐条删除确认弹窗;撤销栈记录完整标注数据,支持恢复删除和修改,最多保留本次页面会话的最近 100 次操作。撤销后的标注也参与保存。 + +## 6. 解码、索引与资源预算 + +PyAV 的 `seek()` 以时间戳定位到附近关键帧,之后仍需解码到目标位置,不能把传入参数直接当作 frame index。参考:[PyAV InputContainer.seek](https://pyav.org/docs/stable/api/container.html#av.container.InputContainer.seek)。 + +因此拟采用以下流程: + +1. 启动后在后台检查视频、计算文件 SHA-256,并顺序完整解码选定流,建立 `frame_index → PTS / 关键帧锚点` 索引。总帧数以实际输出为准,并包含解码器 flush 后的尾帧。 +2. 界面在初始化期间显示阶段、已扫描帧数和可取消状态。容器总帧数仅作为可能不准确的进度参考;索引完成前不允许标注。 +3. 连续播放和向后逐帧尽量沿用解码状态。向前逐帧优先读取缓存,缓存未命中则定位到目标之前的关键帧再顺序解码。 +4. 随机跳转后用索引中的 PTS 对齐输出,确认目标帧。无法可靠对齐时退回从头解码计数;仍不一致则报告错误,不返回带有猜测帧号的图像。 +5. 首版要求帧 PTS 存在且严格递增;缺失、重复、非单调时间戳或扫描解码报错时,停止初始化并报告原因,不生成部分可编辑项目。可变帧率本身不构成错误。 + +一个专用工作线程独占解码容器。HTTP 处理、播放和拖动不得同时对同一个容器执行 seek/decode;初始化与交互解码也不并行使用它。前端最多维持一个活动帧请求,合并快速连续的寻帧目标;后端只保留当前任务和最新待处理目标,避免队列随拖动增长。预览解码不能阻塞保存与状态接口。 + +设计工作负载为单用户、单视频,常见输入为 720p/1080p、30 FPS、1–10 分钟;另以 1 小时 1080p 视频检查启动耗时和内存。性能实测记录见第 11 节。 + +资源策略: + +- 索引使用两组 int64 数组,分别保存 PTS 和关键帧锚点,空间复杂度 `O(N)`。每帧有效数据 16 字节,1 小时 30 FPS 的索引约 1.65 MiB;不含数组预留容量、Python、解码器和容器开销。配置有效索引数据的字节预算,增长前检查。 +- 解码像素与预览图像按需生成,不把完整视频预加载为 RGB 数组。预览保持宽高比,默认最长边不超过 1280 像素,缩放不改变帧号。 +- 编码预览使用按字节淘汰的 LRU,默认预算 64 MiB;仅保留少量在用原始帧,解码器内部缓冲单独计入实际内存测量。浏览器释放被替换的图像 URL。 +- 完整扫描的时间成本为 `O(N)`,首次打开长视频可能需要等待;首版不增加持久化索引缓存。一次随机跳转通常解码一个关键帧间隔,最坏回退 `O(N)`。 +- 退出、初始化取消或失败时关闭容器并回收工作线程,不遗留后台解码任务。 + +实现阶段测量初始化耗时、缓存命中/未命中的逐帧延迟、随机跳转 P50/P95、实际播放帧率和峰值 RSS,并记录 CPU、视频编码、分辨率、帧率和关键帧间隔。只有实测支持时才承诺具体延迟或播放速度。 + +### 图像帧输入与 HDF5 适配 + +核心 `FrameSource` 协议定义 `prepare(cancelled, progress) -> FrameSourceInfo`、 +`get_frame(index) -> ImageFrame`、`check_unchanged()` 和 `close()`。 +工厂创建与资源操作都由同一个工作线程拥有;只有基于不可变元数据或 stat 的来源检查可在保存线程执行。 +`ImageFrame` 包含原始尺寸 RGB uint8 图像、实际索引、相对时间、下一帧间隔和可选 PTS;输入缓冲区可借用到下一次读帧,预览结果则拥有独立 JPEG 字节。 +共享核心负责缩放、编码、64 MiB LRU 与请求合并,不感知输入容器。 + +`Hdf5FrameSource` 只读选定 dataset,支持逐帧 JPEG/PNG 字节数组和 N×H×W×3 uint8 原始图像; +原始图像显式指定 RGB/BGR。默认解析复数或单数 observations/observation 下的 images/cam_high;显式错误路径不会自动改写。 +初始化仅解码首帧并分块计算文件摘要,交互按需读一帧,不预载整段或其他相机,也不生成视频。 +FPS 来自参数、图像 dataset/root 的 fps 或 metadata_json.record_fps;缺少时要求提供参数。 +FPS 推算时间在页面和 JSON 中显式标明;HDF5 帧号始终是行索引。 +其准备复杂度为 O(文件字节数) 的摘要扫描,单帧读取与解码只占一帧临时内存;不存在视频的 PTS 索引。 + +`ArrayFrameSource` 接收已有 RGB 数组/列表,按帧哈希而不复制整个批次;调用方在运行期间保持输入不变。 +大型自定义数据通过实现协议按需读取。来源身份必须覆盖图像内容、顺序、相机选择和显示时间定义。 + +## 7. 标注文件与保存恢复 + +默认标注文件与输入同目录:视频为 `episode_0001.segments.json`,HDF5 为 `episode.cam_high.segments.json`;可用参数指定其他位置。文件存在时加载并校验,不存在时创建内存中的空项目,首次保存时才创建 JSON 文件。文件路径相对于标注文件解析,来源身份绑定内容、相机/流、帧率和帧信息;重新打开时校验来源一致。摘要不匹配时拒绝覆盖原标注,要求选择匹配的视频或新的标注文件。 + +以下为结构示意,摘要与身份用零占位,区间不代表真实 demo 的人工标签: + +```json +{ + "schema_version": 2, + "frame_index_base": 0, + "interval": "closed", + "source": { + "kind": "hdf5", + "name": "episode.hdf5 / observations/images/cam_high", + "path": "episode.hdf5", + "identity": "0000000000000000000000000000000000000000000000000000000000000000", + "frame_count": 3644, + "width": 640, + "height": 480, + "fps": 30.0, + "timing": "fixed_fps", + "details": { + "sha256": "0000000000000000000000000000000000000000000000000000000000000000", + "image_key": "observations/images/cam_high", + "encoding": "encoded", + "color_order": "encoded", + "fps": 30.0 + } + }, + "revision": 3, + "segments": [ + { + "id": "flip-001", + "label": "翻转包裹", + "start_frame": 120, + "end_frame": 185, + "note": "包裹翻转完成后结束" + } + ] +} +``` + +`source` 与容器无关;适配器信息放在 `details`。视频详情保留实际 `stream_index`、time_base 和可空的 average_fps; +图像序列使用 `fixed_fps`,不保存伪造 PTS。整张视频时间索引不复制进标注 JSON。 +旧版 v1 的 `source_video` 文档仍可读取、校验;读取不改写文件,下一次成功保存才升级为 v2。 +仅保存已完成标注;草稿和撤销历史保留在页面会话中。 + +新增、修改、删除、撤销和重做后立即标为未保存,1 秒无新修改时自动保存。`Ctrl/Cmd + S` 和“保存”立即触发写入;存在草稿时明确提示“草稿未提交”,保存不会隐式创建标注。 + +保存完整快照时,后端重新校验并在同目录写入临时文件,flush/fsync 后通过 `os.replace` 替换目标文件;只有成功后才返回确认。保存失败时保留原文件和页面修改,显示原因并支持重试。“导出 JSON”下载当前已完成标注的副本,不将本地保存失败显示为成功。 + +写入携带预期修订号,后端拒绝旧页面覆盖新修订。前端串行提交保存请求,只有最新修改已经确认后才显示“已保存”;旧请求的完成不能清除新修改的未保存状态。不同标签页的冲突显示明确错误,不自动合并或覆盖。 + +刷新或关闭页面时,如果存在未保存修改或草稿,触发浏览器离开提示;恢复能力以已经收到保存确认的 JSON 为准。非法 JSON、未知 schema 版本或无效区间需报告具体问题,不用空项目覆盖原文件。 + +## 8. 入口与模块划分 + +使用方式: + +```bash +# 安装项目及本工具的可选依赖。 +python -m pip install -e '.[trajectory-annotation]' + +# 创建标注,或恢复同目录已有标注。 +embodichain annotate-trajectory --video /data/episode_0001.mp4 + +# 指定标注文件;不自动打开浏览器。 +embodichain annotate-trajectory \ + --video /data/episode_0001.mp4 \ + --annotations /data/labels/episode_0001.segments.json \ + --port 8765 \ + --no-browser +``` + +输入参数为 `--video` 或 `--hdf5` 二选一;HDF5 可指定 `--image-key`、`--fps`、`--color-order`。其余可选参数为 `--annotations`、`--video-stream`(仅视频)、`--port` 和 `--no-browser`。`--video-stream` 使用容器实际流索引,`--port` 默认 0。预览和缓存预算集中于工具自己的 `@configclass` 配置,首版不增加通用插件或任务配置系统。 + +```text +embodichain/toolkits/trajectory_annotator/ +├── __init__.py # 包说明;不启动服务或导入仿真 +├── __main__.py # python -m 入口 +├── _cli.py # 参数解析、依赖检查、输入选择 +├── app.py # 公共 FastAPI 工厂与本地启动入口 +├── frames.py # 公共帧协议、结果对象与内存图像适配器 +├── video.py # PyAV 视频输入适配器 +├── hdf5.py # 只读 HDF5 图像输入适配器 +├── _file.py # 分块文件摘要和来源身份 +├── _cfg.py # 内部 @configclass 工具配置 +├── _models.py # 区间校验 +├── _frames.py # 与容器无关的工作线程与预览缓存 +├── _storage.py # 来源校验、版本解析、原子保存 +├── _server.py # HTTP 接口、静态资源与错误映射 +├── web/ +│ ├── index.html +│ ├── app.js # 显示状态、草稿、快捷键、撤销与保存调度 +│ └── styles.css +└── README.md # 安装、启动、快捷键、文件格式 +``` + +最小 HTTP 边界: + +| 接口 | 职责 | +| --- | --- | +| `GET /api/status` | 初始化阶段、进度、就绪或错误信息 | +| `GET /api/project` | 帧源元信息、已保存标注、修订号和界面所需配置 | +| `GET /api/frames/{index}` | 指定帧的 JPEG;响应头带实际帧号、时间、可选 PTS 和请求标识 | +| `PUT /api/annotations` | 接收完整标注快照与预期修订号,校验并落盘 | +| `POST /api/cancel` | 取消尚未完成的初始化 | + +Python 模型和存储独立于 HTTP,可用 CPU 测试。前端管理临时编辑状态,后端负责文件格式与持久化校验。公共帧协议和启动 API 允许从已有图像输入复用整个标注界面,视频与 HDF5 均为协议适配器。可运行示例位于 `examples/toolkits/trajectory_annotation/`。 + +## 9. 验收与实施顺序 + +现有 grasp 标注和 recorder 区间测试无法覆盖新工具的视频寻帧、浏览器快捷键或保存恢复,新增针对这些生产行为的覆盖位于 `tests/toolkits/trajectory_annotator/`;沿用 `tests/test_main.py` 检查统一 CLI 注册。按 `add-test` 的必要性审查合并重复场景。 + +| 验收项 | 验证方式 | +| --- | --- | +| 帧号准确 | 用具有独立可识别帧内容的小视频,将随机跳转和前后逐帧结果与从头顺序解码结果逐帧比较;包含 B 帧、跨关键帧、尾部 flush、非零起始 PTS 和可变帧率 | +| 区间规则 | 验证首末帧、单帧段、相邻段、允许空隙、越界和重叠拒绝;修改失败保留原标注 | +| 核心用户流程 | 浏览器自动化完成定位 → I → 名称 → O → 提交 → 编辑 → 删除 → 撤销 → 保存 → 刷新恢复,并检查列表、时间轴与 JSON 一致 | +| 快捷键与异步显示 | 验证文本焦点、长按、视频边界、播放中标记、快速拖动后迟到响应不会导致画面与标注帧号错位 | +| 保存可靠性 | 验证中文标签往返、摘要不匹配、保存失败保留旧文件、旧修订拒绝和保存期间继续编辑的状态 | +| 独立运行与打包 | CLI 帮助不导入仿真;从构建后的 wheel 读取静态界面并启动本地工具 | +| 资源与体验 | 用短视频和 1 小时视频记录初始化、交互延迟、播放帧率和内存;人工检查界面与键盘可操作性 | + +实施分三步完成同一个首版: + +1. 建立模型、文件格式、视频索引与准确取帧,先验证端点语义和帧号正确性。 +2. 实现浏览器画面、时间轴、列表、草稿、快捷键及编辑删除,完成可操作流程。 +3. 接入自动保存、撤销重做、异常处理和 CLI/wheel 打包,完成针对性验证与使用说明。 + +使用 `pre-commit-check` 选择检查范围;新增公共帧输入和启动 API,并配套接口说明。按项目要求运行上下文影响分析并核对 CLI 相关指导,结果为 **no update**:现有主题关于命令分发、仿真、资产、GenSim 和训练的指导仍然准确;新工具自己的 README 负责其独立使用约定,无需为一次独立工具新增上下文主题。未处理工作区中与本工具无关的已有文件。 + +## 10. 已确认的产品选择 + +首版采用以下约定: + +1. **运行方式**:本地服务加浏览器界面,入口为 `embodichain annotate-trajectory`。 +2. **区间语义**:0 基帧号,起止帧均包含;单轨道、允许空隙、不允许重叠。 +3. **标注内容**:自由输入原子技能名称,可选备注;支持修改、删除、撤销和重做。 +4. **输出范围**:首版保存/恢复 JSON 区间标注;实际裁剪视频或机器人轨迹作为后续扩展。 + +用户已确认技术方案后开始实现;当前版本按以上约定提供工具。 + +## 11. 首版验证记录 + +验证环境为 conda `py312`:Python 3.12.15、PyAV 15.1.0、OpenCV 4.11.0.86、FastAPI 0.162.2、Uvicorn 0.54.0。视频首版通过 16 项后端/解码测试、8 项 Chromium/Firefox 浏览器测试,以及 3 项已有 CLI 注册与分发检查。HDF5 扩展复用这些测试并补充图像输入覆盖,最终结果见下文。实际构建 wheel 后,在源码目录外使用其中的静态资源和解码接口完成启动检查。帧协议和文件适配器扩展后新增公共 Python API,配套 API 文档覆盖检查。 + +性能检查使用合成的静态图形 H.264 视频(1920×1080、30 FPS、关键帧间隔 30、含 B 帧),硬件为 AMD Ryzen 9 9950X,解码线程数 2,预览最长边 1280、JPEG 质量 85。它用于验证长视频资源行为,不代表真实遥操视频的编码复杂度。下面保留视频首版、抽象帧输入前的基线记录,为冷启动完整扫描与每种路径 50 次取帧的结果;取帧计时不含 HTTP 和浏览器渲染: + +| 视频 | 完整索引时间 | 随机取帧中位数 / P95 | 顺序取帧中位数 | 有效索引数据 | 采样进程峰值 RSS | +| --- | --- | --- | --- | --- | --- | +| 1 分钟,1800 帧 | 0.91 秒 | 11.83 / 18.02 毫秒 | 2.79 毫秒 | 0.027 MiB | 535.20 MiB | +| 1 小时,108000 帧 | 51.79 秒 | 11.58 / 17.07 毫秒 | 2.80 毫秒 | 1.648 MiB | 540.74 MiB | + +RSS 包含 Python、项目配置依赖和原生解码库;10 毫秒采样可能漏过瞬时峰值。两组独立进程相对索引前运行时的峰值增量分别为 38.78 和 44.18 MiB。合成素材上的 150 次取帧均通过索引/PTS 一致性检查;独立像素一致性由小型真实编码夹具的顺序解码对照测试验证。真实用户视频、Python 3.11 及浏览器端持续播放吞吐尚未单独测量,不据此承诺固定帧率。 + +### 图像帧接口扩展验证 + +真实 HDF5 demo 为 `/home/oem/piper-sim-data/label_down/episode_20261022T073344_323022Z/episode.hdf5`, +实际 dataset 为 `observations/images/cam_high`(复数),3644 个变长 uint8 JPEG、640×480、metadata_json.record_fps=30。 +自动化覆盖压缩/原始图像、RGB/BGR、单复数路径、按需读取、帧时间、相机/帧率/内容身份、保存恢复和线程所有权; +已有视频 PTS/像素对照、浏览器异步定位与保存失败测试继续复用。 + +相同硬件/参数、每种取帧路径 50 次的实测(不含 HTTP/浏览器): + +| 输入 | 准备耗时 | 随机预览中位数 / P95 | 顺序预览中位数 | 采样进程峰值 RSS | +| --- | --- | --- | --- | --- | +| 真实 HDF5,3644 帧,640×480 | 0.215 秒 | 0.993 / 1.101 毫秒 | 0.918 毫秒 | 525.70 MiB | +| 1 分钟 1080p 视频,1800 帧,帧接口抽象后 | 0.891 秒 | 12.148 / 18.500 毫秒 | 2.783 毫秒 | 542.76 MiB | + +HDF5 准备耗时包含全文件 SHA-256,操作系统文件缓存未主动清除;不是磁盘冷缓存性能承诺。 +HDF5 不分配 PTS 表,采样峰值相对准备前运行时增加 29.07 MiB,未保留完整 RGB 序列。 +视频随机预览中位数相较基线约增加 0.32 毫秒(约 3%),顺序读取接近;统一 RGB 输入多一次颜色/缩放处理,保留该取舍以简化协议。 +每组 150 次请求均通过索引/时间一致性检查;独立像素正确性依赖生产测试的对照验证,不将 benchmark 自身的索引检查当作像素验证。 +运行示例和正式 CLI 见工具 README;示例默认将标注写入 outputs/trajectory_annotation,源文件只读。 + +HDF5 接入阶段在 py312 中完成:31 项工具测试(含 Chromium/Firefox)、43 项统一 CLI 测试和 11 项 API 文档检查测试,共 85 项通过。 +真实 demo 另经两种浏览器验证首尾跳转、逐帧、区间创建、保存、刷新恢复和删除;对第 0、1、1822、3643 帧做独立 JPEG 像素对照,预览与原帧平均绝对误差为 0.794–1.013/255。 +构建最终 wheel 后,在源码目录外验证帧适配器、网页资源和两个 example 均已打包,并用真实 HDF5 完成 ASGI 启动/末帧请求检查。正式 --hdf5 CLI 与 Ctrl+C 正常退出也通过。 +Black 26.10.1 全仓检查和 4442/4442 公共 API 文档覆盖检查通过。 + +按用户要求在 py312 安装 docs/requirements.txt 的文档依赖(含 Sphinx 7.4.7)。新增 API 页在包含 autodocsumm 的独立 Sphinx 严格构建中无警告通过。 +仓库的 architecture_sphinx 扩展要求相关源码已提交,因此在只包含本次改动的临时独立工作副本中运行全量 dummy 构建;构建成功,日志报告其他文档的 1194 条警告,新增帧标注 API 无诊断。临时工作副本已清理,原工作区保留未提交改动。 + +## 12. 原子技能时间轴交互修正 + +按原子技能区间统一界面、前端状态/函数/DOM 和后端校验命名;JSON/API 的 segments 字段保留现有兼容格式。 +时间轴从按钮色块改为可选中的可视区域,所有指针事件由时间轴统一捕获,不在点击或拖动后跳到技能起点。 +删除色块的最小宽度、边框占位与外层内边距,标签在独立子元素中裁切;区间几何按 [start/N, (end+1)/N] 绘制,游标和刻度按 (index+0.5)/N 对齐。 +新增浏览器回归检查覆盖从区间内部拖动、穿过其他区间与空隙、单帧/首尾帧色块、缩放及横向滚动;已有键盘编辑测试同时检查新布局与标题栏提示。 +上下文审查为 no update:改动位于独立标注工具内,命令分发和其他已路由子系统的所有权与行为约定保持准确,使用约定由本工具 README 维护。 + +本轮验证:33 项工具测试(含 10 项 Chromium/Firefox 浏览器用例)和 43 项 CLI 测试共 76 项通过;Black 26.10.1 与公共 API 覆盖检查通过。 +真实 HDF5 demo 在 Chromium/Firefox 中验证了自由拖动及小于 1 像素的单帧色块几何,并检查 1440×1100、1024×900、390×844 布局无水平溢出。 +长序列的指针定位测试放大至每帧覆盖多个像素,避免浏览器鼠标坐标像素取整影响断言;颜色区间几何在原始缩略比例下独立核对。 diff --git a/docs/source/api_reference/trajectory_annotator.rst b/docs/source/api_reference/trajectory_annotator.rst new file mode 100644 index 0000000000..f88ec956b7 --- /dev/null +++ b/docs/source/api_reference/trajectory_annotator.rst @@ -0,0 +1,86 @@ +:orphan: + +Trajectory image annotation +=========================== + +The annotation application consumes indexed RGB images through a common frame +source. Video and HDF5 adapters own their input resources; the application owns +one worker thread, a bounded JPEG preview cache and revision-checked JSON output. +Frame indices are zero-based and action intervals include both endpoints. + +Frame input contract +-------------------- + +.. currentmodule:: embodichain.toolkits.trajectory_annotator.frames + +.. autosummary:: + + ImageFrame + FrameSourceInfo + FrameSource + ArrayFrameSource + +.. autoclass:: ImageFrame + :members: + +.. autoclass:: FrameSourceInfo + :members: + +.. autoclass:: FrameSource + :members: + +.. autoclass:: ArrayFrameSource + :members: + +File adapters +------------- + +Video uses presentation timestamps and a full presentation-order index. HDF5 +reads a selected camera dataset on demand; its display time is derived from an +explicit or recorded FPS. Neither adapter preloads the full RGB sequence. + +.. currentmodule:: embodichain.toolkits.trajectory_annotator.hdf5 + +.. autosummary:: + + Hdf5FrameSource + +.. autoclass:: Hdf5FrameSource + :members: + +.. currentmodule:: embodichain.toolkits.trajectory_annotator.video + +.. autosummary:: + + VideoFrameSource + +.. autoclass:: VideoFrameSource + :members: + +Application entry points +------------------------ + +Pass a factory returning a fresh source. Construction, preparation, reads and +cleanup run on the same worker thread. Caller-owned image arrays must remain +unchanged for the application's lifetime. + +.. currentmodule:: embodichain.toolkits.trajectory_annotator.app + +.. autosummary:: + + create_annotation_app + launch_annotation + +.. autofunction:: create_annotation_app + +.. autofunction:: launch_annotation + +For example:: + + from embodichain.toolkits.trajectory_annotator.app import launch_annotation + from embodichain.toolkits.trajectory_annotator.hdf5 import Hdf5FrameSource + + launch_annotation( + lambda: Hdf5FrameSource("episode.hdf5"), + "episode.cam_high.segments.json", + ) diff --git a/embodichain/cli/main.py b/embodichain/cli/main.py index 2c4e4a5cc1..fe6c746471 100644 --- a/embodichain/cli/main.py +++ b/embodichain/cli/main.py @@ -157,6 +157,11 @@ class Command: target="embodichain.toolkits.graspkit.scripts.annotate_grasp:cli", help="Interactively annotate a grasp region on a mesh.", ), + Command( + name="annotate-trajectory", + target="embodichain.toolkits.trajectory_annotator._cli:cli", + help="Annotate atomic skill frame intervals in an image sequence.", + ), Command( name="decompose-urdf", target="embodichain.toolkits.acd.cli:main", diff --git a/embodichain/toolkits/trajectory_annotator/README.md b/embodichain/toolkits/trajectory_annotator/README.md new file mode 100644 index 0000000000..ff3e26cf1a --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/README.md @@ -0,0 +1,162 @@ +# 遥操轨迹图像序列标注 + +在浏览器逐帧查看遥操视频、HDF5 相机图像或内存图像序列,为原子技能标注起止帧。支持时间轴定位、动作列表、区间编辑和删除、撤销重做、JSON 自动保存与继续编辑。运行时使用 CPU,不启动仿真。 + +![原子技能区间标注界面](../../../docs/architecture/images/trajectory-annotation.png) + +## 启动 + +在已经安装 EmbodiChain 的环境中,安装本工具的依赖即可: + +```bash +conda activate py312 +python -m pip install 'av>=15.1,<18' 'fastapi>=0.115,<1' 'uvicorn>=0.30,<1' +embodichain annotate-trajectory --video /data/episode_0001.mp4 +``` + +新安装项目时也可以选择 `pip install 'embodichain[trajectory-annotation]'`。源码安装使用 `pip install -e '.[trajectory-annotation]'`,它会同时解析项目的基础依赖;已有开发环境可以使用上面的单独依赖安装命令。 + +命令会打印本地网址,并默认打开浏览器。首次打开时完整扫描选定视频流、校验来源并建立准确的帧索引;长视频需要等待。按 Ctrl+C 退出服务。 + +```bash +embodichain annotate-trajectory \ + --video /data/episode_0001.mp4 \ + --annotations /data/labels/episode_0001.segments.json \ + --video-stream 0 \ + --port 8765 \ + --no-browser +``` + +- `--annotations`:读写同一标注文件,默认视频同目录的 `.segments.json`。 +- `--video-stream`:容器中的实际流索引,默认第一个视频流。 +- `--port`:默认 0,自动选择空闲端口;服务只监听 `127.0.0.1`。 +- `--no-browser`:手动打开打印的网址。 + +也可使用 `python -m embodichain.toolkits.trajectory_annotator --video ...` 启动。 + +## HDF5 输入 + +```bash +embodichain annotate-trajectory \ + --hdf5 /home/oem/piper-sim-data/label_down/episode_20261022T073344_323022Z/episode.hdf5 \ + --annotations outputs/trajectory_annotation/episode.cam_high.segments.json + +# 指定其他相机或缺少元数据时提供帧率 +embodichain annotate-trajectory --hdf5 /data/episode.hdf5 \ + --image-key observations/images/cam_left_wrist --fps 30 +``` + +- `--video` 和 `--hdf5` 二选一。HDF5 以只读方式打开,不转成临时视频。 +- `--image-key` 默认自动定位 `observations/images/cam_high`,同时兼容单数 `observation/images/cam_high`;两者均存在时要求显式选择。实际 demo 使用复数路径。 +- 支持逐帧 JPEG/PNG 字节数组(含变长 uint8 和定长填充行)及 `N×H×W×3 uint8` 原始图像。只按需读选中相机的单帧,初始化时仅解码首帧确认尺寸;文件摘要按 1 MiB 分块计算。 +- 原始数组默认 RGB,可用 `--color-order bgr` 指定 BGR;压缩图像按编码解码。 +- `--fps` 优先级最高,否则依次读取图像 dataset 的 `fps`、根属性 `fps`、根 `metadata_json.record_fps`。缺少有效值时提示提供 `--fps`,不会猜测。页面明确显示时间由 FPS 推算;帧号始终来自 dataset 行索引。 +- 默认标注路径为 `..segments.json`,可用 `--annotations` 覆盖。 + +[可运行示例](../../../examples/toolkits/trajectory_annotation/README.md)默认使用上述 demo: + +```bash +embodichain run-example toolkits/trajectory_annotation +``` + +## 图像帧输入接口 + +核心只依赖 [`FrameSource`](frames.py) 协议,不依赖视频或 HDF5。已有 RGB 图像可直接传入: + +```python +from embodichain.toolkits.trajectory_annotator.app import launch_annotation +from embodichain.toolkits.trajectory_annotator.frames import ArrayFrameSource + +# images: N×H×W×3 uint8 RGB 数组,或同尺寸图像列表 +launch_annotation( + lambda: ArrayFrameSource(images, fps=30, name="episode"), + "outputs/episode.segments.json", +) +``` + +调用方保留数组所有权,在应用关闭前保持内容和顺序不变。适配器逐张校验并计算摘要,不额外复制整个序列。 +大规模数据可自定义 `FrameSource`,实现 `prepare`、`get_frame`、`check_unchanged` 和 `close`,返回 `FrameSourceInfo` 与单张 `ImageFrame`。 +图像统一为 `H×W×3 uint8 RGB`,帧号从 0 开始;借用图像缓冲区至少有效到下一次读帧。 +工厂创建、准备、读帧和关闭均在一个工作线程执行;保存线程只调用不访问解码/HDF5 句柄的 `check_unchanged`。 +应用负责预览缩放、JPEG 编码、有界缓存和标注保存。程序化服务入口为 `create_annotation_app(factory, annotations)`,启动入口为 `launch_annotation`。 +文件适配器分别位于 [`video.py`](video.py) 与 [`hdf5.py`](hdf5.py),公共接口参考[API 说明](../../../docs/source/api_reference/trajectory_annotator.rst)。 + +## 标注 + +1. 用时间轴、帧号输入框或播放按钮定位原子技能起点,左右键逐帧精调。 +2. 按 **I** 设置起点,输入原子技能名称,例如“翻转包裹”。名称输入框中按 Enter 返回视频区。 +3. 定位原子技能最后一帧,按 **O** 设置终点。 +4. 检查原子技能名称、起止帧和帧数,按 **Enter** 提交。已完成区间显示在时间轴和右侧列表中。 +5. 选中列表中的标注,按 **E** 修改、Delete 删除,或点击起止帧跳转。 + +帧号从 **0** 开始,起止帧**均包含**。`[120, 185]` 包含 66 帧,对应 Python 切片 `frames[120:186]`。允许空隙和单帧原子技能,同一轨道的已完成区间不能重叠。 + +桌面布局左侧为视频和与视频等宽的原子技能时间轴,右侧依次为“已标注原子技能区间”和“新建/编辑原子技能区间”。标题栏常驻提示 ←/→ 逐帧、空格播放/暂停、I/O 标注起止帧。 + +整条时间轴均可按住拖动,包括已标注色块内部;游标按鼠标位置定位,不吸附到区间边界。点击色块可同时选中该技能,列表中的起止帧按钮仍用于精确跳转。 +每帧占一个等宽单元,闭区间覆盖从起点单元左沿到终点单元右沿;游标和刻度位于对应帧单元中心。色块不会因标签长度、内边距或最小宽度被撑宽,短标签可悬停查看。 +长序列在缩略比例下一个屏幕像素可能对应多帧,精确定位可放大时间轴、使用 ←/→ 或输入帧号。 + +每次只有一个草稿。编辑已有标注时,原版本保留到提交;Esc 取消。删除和撤销只修改标注。存在草稿时先提交或取消,再切换编辑、删除或撤销。 + +| 快捷键 | 操作 | +| --- | --- | +| Space | 播放 / 暂停,末帧重新播放时从头开始 | +| ← / → | 前 / 后一帧;Shift 跨 10 帧 | +| Home / End | 首帧 / 末帧 | +| G | 输入帧号跳转 | +| I / O | 将当前显示帧设为起点 / 终点 | +| L | 输入原子技能名称 | +| Enter / Esc | 提交 / 取消草稿 | +| E | 编辑选中的标注 | +| Delete / Backspace | 删除选中的标注 | +| Ctrl/Cmd + Z | 撤销;加 Shift 重做,最多保留 100 次 | +| Ctrl/Cmd + S | 保存已完成标注 | +| ? | 快捷键帮助 | + +输入文本时保留正常的文字编辑快捷键。播放中设置端点会暂停在当前已经显示的帧;手动寻帧期间需等画面显示后再标记。页面失去焦点后自动暂停。 + +## 保存与输出 + +提交后的修改在停止编辑 1 秒后自动保存。也可点击“保存”或按 Ctrl/Cmd+S。新项目在第一次保存前不创建文件;未提交草稿不写入 JSON。状态栏明确显示未保存、保存中、保存成功或失败。 + +重新用同一命令打开数据会加载原标注。JSON 使用 `schema_version: 2`,包含 0 基/闭区间声明、与容器无关的 `source` 信息(类型、身份、帧数、尺寸、帧率、时间来源和适配器详情)、修订号,以及 `segments` 数组。界面与内部模型使用原子技能区间命名;保留现有 JSON/API 的 `segments` 字段和文件名后缀,以继续读取已有标注。数组元素示例: + +```json +{ + "id": "seg_2e841dc2-0e4c-4b37-8d97-1d064da7e8a1", + "label": "翻转包裹", + "start_frame": 120, + "end_frame": 185, + "note": "可选备注" +} +``` + +写入前校验,临时文件 fsync 后原子替换;失败时保留旧文件与页面修改。多标签页的旧修订和磁盘外部修改会被拒绝覆盖,可先“导出 JSON”保留当前已完成标注,再重新打开。导出副本不代表本地保存成功。 + +文件路径相对于标注 JSON 保存。文件适配器用 SHA-256 绑定源内容,并将相机路径、流索引、帧率或颜色解释等选择纳入身份检查;移动文件后可显式传入新路径,相同内容仍可恢复。内存图像按像素、顺序和 FPS 计算身份。不同内容或选择不能套用已有标注。兼容旧版 `schema_version: 1` 的 `source_video` 文档,读取时只在内存中转换,下次成功保存才写入 v2。 + +本工具生成区间标注文件。将视频帧映射到机器人控制 step、裁剪视频或切分关节轨迹需要后续消费程序显式处理;现有 recorder 的半开区间不能直接复用本工具的闭区间端点。 + +## 视频与资源约定 + +以 MP4/H.264 为主要验收输入,其他容器/编码取决于 PyAV/FFmpeg。支持恒定与可变帧率,以及非零起始 PTS。首版要求帧 PTS 存在且严格递增、视频分辨率固定;原始无时间戳 H.264 流等输入会报告原因。播放不处理音频。 + +帧号按完整解码的显示顺序定义,包含 B 帧和解码器尾帧,不能由 `time × FPS` 推算。随机跳转按关键帧定位并校验 PTS,必要时从头解码。预览最长边默认 1280 像素;JPEG 缓存默认上限 64 MiB,紧凑帧索引另设 64 MiB 预算,不预加载完整 RGB 视频。性能不足时播放降低实际速度,不跳过索引帧或积压请求。 + +## 开发验证 + +```bash +python -m pytest tests/toolkits/trajectory_annotator/test_annotation.py tests/toolkits/trajectory_annotator/test_frames.py -q + +# 可选:实际浏览器键盘、删除恢复和异步帧/保存检查。 +python -m pip install playwright +python -m playwright install chromium firefox +python -m pytest tests/toolkits/trajectory_annotator/test_browser.py -q +``` + +浏览器检查也可使用系统 Chrome,或通过 `ANNOTATION_CHROMIUM_EXECUTABLE` 指定可执行文件。缺少可选浏览器时明确跳过。 + +性能测量入口支持 `--video` 或 `--hdf5`,例如:`python -m scripts.benchmark.toolkits.trajectory_annotation --video /data/episode.mp4`。报告初始化时间、随机寻帧、连续取帧及缓存延迟、RSS 和索引/缓存占用;具体数值取决于分辨率、编码、关键帧间隔和硬件。 + +实际 HDF5 demo(3644 帧、640×480、30 FPS)在 AMD Ryzen 9 9950X、py312、JPEG 质量 85 上,含文件摘要的初始化为 0.215 秒;50 次随机预览中位数 0.993 毫秒、P95 1.101 毫秒。测量不含 HTTP 和浏览器渲染,不据此保证固定播放帧率。 diff --git a/embodichain/toolkits/trajectory_annotator/__init__.py b/embodichain/toolkits/trajectory_annotator/__init__.py new file mode 100644 index 0000000000..bf80248686 --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/__init__.py @@ -0,0 +1,26 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Browser annotation of indexed RGB frame sequences. + +Use :mod:`.frames` for the input protocol and in-memory arrays, :mod:`.video` +and :mod:`.hdf5` for file adapters, and :mod:`.app` to create or launch the app. +Optional server and codec dependencies are loaded only by those modules. +""" + +from __future__ import annotations + +__all__ = [] diff --git a/embodichain/toolkits/trajectory_annotator/__main__.py b/embodichain/toolkits/trajectory_annotator/__main__.py new file mode 100644 index 0000000000..79a023cdbf --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/__main__.py @@ -0,0 +1,26 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Allow ``python -m embodichain.toolkits.trajectory_annotator``.""" + +from __future__ import annotations + +from ._cli import cli + +__all__ = [] + +if __name__ == "__main__": + cli() diff --git a/embodichain/toolkits/trajectory_annotator/_cfg.py b/embodichain/toolkits/trajectory_annotator/_cfg.py new file mode 100644 index 0000000000..2c31ce90c8 --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/_cfg.py @@ -0,0 +1,62 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Resource limits for the local trajectory annotation tool.""" + +from __future__ import annotations + +from embodichain.utils import configclass + +__all__ = ["AnnotationCfg"] + + +@configclass +class AnnotationCfg: + """Configure CPU decoding, preview quality and bounded editing resources. + + Attributes: + preview_edge: Maximum preview width or height in pixels. + jpeg_quality: OpenCV JPEG quality, between 1 and 100. + cache_bytes: Maximum retained JPEG payload bytes; zero disables caching. + index_bytes: Maximum packed timestamp and keyframe-index bytes. + decoder_threads: FFmpeg threads per decoder; the owner is one worker. + autosave_ms: Debounce interval for completed annotation edits. + document_bytes: Maximum JSON request or annotation file size. + """ + + preview_edge: int = 1280 + jpeg_quality: int = 85 + cache_bytes: int = 64 * 1024**2 + index_bytes: int = 64 * 1024**2 + decoder_threads: int = 2 + autosave_ms: int = 1000 + document_bytes: int = 8 * 1024**2 + + def __post_init__(self) -> None: + for name in ( + "preview_edge", + "index_bytes", + "decoder_threads", + "autosave_ms", + "document_bytes", + ): + value = getattr(self, name) + if type(value) is not int or value <= 0: + raise ValueError(f"{name} must be a positive integer.") + if type(self.cache_bytes) is not int or self.cache_bytes < 0: + raise ValueError("cache_bytes must be a non-negative integer.") + if type(self.jpeg_quality) is not int or not 1 <= self.jpeg_quality <= 100: + raise ValueError("jpeg_quality must be an integer between 1 and 100.") diff --git a/embodichain/toolkits/trajectory_annotator/_cli.py b/embodichain/toolkits/trajectory_annotator/_cli.py new file mode 100644 index 0000000000..e5a8623b4e --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/_cli.py @@ -0,0 +1,128 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Lightweight command entry point; optional dependencies load after parsing.""" + +from __future__ import annotations + +import argparse +from collections.abc import Sequence +from pathlib import Path +import math + +__all__ = ["cli"] + + +def cli(argv: Sequence[str] | None = None) -> None: + """Launch the local annotation website for a video or HDF5 image sequence. + + Args: + argv: Command arguments, or process arguments when omitted. + """ + parser = argparse.ArgumentParser( + prog="embodichain annotate-trajectory", + description="在浏览器逐帧标注遥操图像序列的原子技能区间。", + ) + inputs = parser.add_mutually_exclusive_group(required=True) + inputs.add_argument("--video", type=Path, help="本地视频文件(PyAV)。") + inputs.add_argument("--hdf5", type=Path, help="本地 HDF5 图像序列。") + parser.add_argument( + "--image-key", + help="HDF5 图像 dataset,默认自动定位 observations/images/cam_high(也支持 observation)。", + ) + parser.add_argument( + "--fps", + type=float, + help="HDF5 播放帧率,默认读取 fps 或 metadata_json.record_fps。", + ) + parser.add_argument( + "--color-order", + choices=("rgb", "bgr"), + default=None, + help="HDF5 原始数组的颜色顺序,默认 rgb;JPEG/PNG 按编码解码。", + ) + parser.add_argument( + "--annotations", + type=Path, + help="标注 JSON,默认视频同名或 ..segments.json。", + ) + parser.add_argument( + "--video-stream", type=int, help="容器实际视频流索引,默认第一个视频流。" + ) + parser.add_argument( + "--port", type=int, default=0, help="本地端口,0 自动选择(默认)。" + ) + parser.add_argument( + "--no-browser", action="store_true", help="只打印网址,不自动打开浏览器。" + ) + args = parser.parse_args(argv) + source_path = (args.video or args.hdf5).resolve() + if not source_path.is_file(): + parser.error(f"输入文件不存在:{source_path}") + if not 0 <= args.port <= 65535: + parser.error("端口必须在 0–65535 之间。") + if args.video_stream is not None and (args.hdf5 or args.video_stream < 0): + parser.error("--video-stream 仅适用于视频,且不能为负数。") + if args.video and any( + value is not None for value in (args.image_key, args.fps, args.color_order) + ): + parser.error("--image-key、--fps 和 --color-order 仅适用于 --hdf5。") + if args.image_key is not None and not args.image_key.strip("/"): + parser.error("--image-key 不能为空。") + if args.fps is not None and (not math.isfinite(args.fps) or args.fps <= 0): + parser.error("--fps 必须为有限正数。") + camera = (args.image_key or "cam_high").strip("/").rsplit("/", 1)[-1] + default_name = ( + f"{source_path.stem}.{camera}.segments.json" + if args.hdf5 + else f"{source_path.stem}.segments.json" + ) + annotations = (args.annotations or source_path.with_name(default_name)).resolve() + if annotations == source_path or ( + annotations.exists() and annotations.samefile(source_path) + ): + parser.error("标注文件不能与输入文件相同。") + try: + from .app import launch_annotation + + if args.hdf5: + from .hdf5 import Hdf5FrameSource + + def factory() -> Hdf5FrameSource: + return Hdf5FrameSource( + source_path, + image_key=args.image_key, + fps=args.fps, + color_order=args.color_order or "rgb", + ) + + else: + from .video import VideoFrameSource + + def factory() -> VideoFrameSource: + return VideoFrameSource(source_path, stream_index=args.video_stream) + + print(f"输入:{source_path}", flush=True) + launch_annotation( + factory, annotations, port=args.port, open_browser=not args.no_browser + ) + except ImportError as error: + parser.exit( + 2, + f"缺少标注工具依赖:{error}\n请安装:python -m pip install -e '.[trajectory-annotation]'\n", + ) + except (OSError, ValueError) as error: + parser.exit(2, f"无法启动标注工具:{error}\n") diff --git a/embodichain/toolkits/trajectory_annotator/_file.py b/embodichain/toolkits/trajectory_annotator/_file.py new file mode 100644 index 0000000000..5155f574f1 --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/_file.py @@ -0,0 +1,61 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Bounded file fingerprinting shared by file-backed frame adapters.""" + +from __future__ import annotations + +from collections.abc import Callable +import hashlib +import json +from pathlib import Path +import threading +from typing import Any + +__all__ = ["file_signature", "hash_file", "source_identity"] + + +def file_signature(path: Path) -> tuple[int, int, int, int]: + """Return immutable file identity fields without reading a source handle.""" + stat = path.stat() + return stat.st_dev, stat.st_ino, stat.st_size, stat.st_mtime_ns + + +def hash_file( + path: Path, cancelled: threading.Event, progress: Callable[..., None] +) -> str: + """Hash in 1 MiB chunks with progress and cooperative cancellation.""" + if not path.is_file(): + raise ValueError("输入路径必须为普通文件。") + total = path.stat().st_size + digest = hashlib.sha256() + completed = 0 + progress(phase="hashing", completed=completed, total=total) + with path.open("rb") as handle: + while chunk := handle.read(1024**2): + if cancelled.is_set(): + raise InterruptedError("初始化已取消。") + digest.update(chunk) + completed += len(chunk) + progress(phase="hashing", completed=completed, total=total) + return digest.hexdigest() + + +def source_identity(details: dict[str, Any]) -> str: + """Bind content and adapter selections to a reproducible identity.""" + return hashlib.sha256( + json.dumps(details, sort_keys=True, separators=(",", ":")).encode() + ).hexdigest() diff --git a/embodichain/toolkits/trajectory_annotator/_frames.py b/embodichain/toolkits/trajectory_annotator/_frames.py new file mode 100644 index 0000000000..b731368704 --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/_frames.py @@ -0,0 +1,259 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Source-independent preview cache and bounded, single-owner frame worker.""" + +from __future__ import annotations + +from collections import OrderedDict +from collections.abc import Callable +from concurrent.futures import Future +from dataclasses import dataclass +import json +import math +import threading +from typing import Any + +import cv2 + +from ._cfg import AnnotationCfg +from .frames import FrameSource, FrameSourceInfo, _rgb_image + +__all__ = ["FramePreview", "FrameReader", "FrameWorker", "SupersededError"] + + +class SupersededError(RuntimeError): + """A newer pending seek replaced this request before reading started.""" + + +@dataclass(frozen=True) +class FramePreview: + """Owned JPEG bytes and timing for exactly one displayed image.""" + + index: int + pts: int | None + time_seconds: float + delay_seconds: float + jpeg: bytes + + +class FrameReader: + """Consume RGB images and share resizing/JPEG/LRU behavior across adapters.""" + + def __init__( + self, + source: FrameSource, + cfg: AnnotationCfg, + cancelled: threading.Event | None = None, + ) -> None: + self.source = source + self.cfg = cfg + self.cancelled = cancelled or threading.Event() + self.info: FrameSourceInfo | None = None + self.cache: OrderedDict[int, FramePreview] = OrderedDict() + self.cache_bytes = 0 + + def prepare(self, progress: Callable[..., None] | None = None) -> FrameSourceInfo: + """Prepare and validate metadata before accepting requests.""" + info = self.source.prepare(self.cancelled, progress or (lambda **_: None)) + if any( + type(value) is not int or value <= 0 + for value in (info.frame_count, info.width, info.height) + ): + raise ValueError("帧源需要正整数帧数和固定图像尺寸。") + if not info.identity or not info.kind or not info.name: + raise ValueError("帧源需要非空 identity、kind 和 name。") + if info.fps is not None and (not math.isfinite(info.fps) or info.fps <= 0): + raise ValueError("帧源 FPS 必须为有限正数。") + json.dumps(info.details, allow_nan=False) + self.info = info + return info + + def check_unchanged(self) -> None: + """Run the adapter's thread-safe source identity check.""" + self.source.check_unchanged() + + def preview(self, index: int) -> FramePreview: + """Return one exact indexed frame with owned immutable JPEG bytes.""" + if self.info is None: + raise RuntimeError("帧源尚未准备。") + if type(index) is not int or not 0 <= index < self.info.frame_count: + raise ValueError(f"帧号必须在 0–{self.info.frame_count - 1} 之间。") + if self.cancelled.is_set(): + raise InterruptedError("读取已取消。") + self.check_unchanged() + if index in self.cache: + self.cache.move_to_end(index) + return self.cache[index] + frame = self.source.get_frame(index) + image = _rgb_image(frame.image) + if frame.index != index or image.shape[:2] != ( + self.info.height, + self.info.width, + ): + raise ValueError("帧源返回的帧号或图像尺寸与请求不一致。") + if ( + not math.isfinite(frame.time_seconds) + or frame.time_seconds < 0 + or not math.isfinite(frame.delay_seconds) + or frame.delay_seconds <= 0 + ): + raise ValueError("帧源返回的显示时间或帧间隔无效。") + scale = min(1.0, self.cfg.preview_edge / max(self.info.width, self.info.height)) + if scale < 1: + image = cv2.resize( + image, + ( + max(1, round(self.info.width * scale)), + max(1, round(self.info.height * scale)), + ), + interpolation=cv2.INTER_AREA, + ) + success, encoded = cv2.imencode( + ".jpg", + cv2.cvtColor(image, cv2.COLOR_RGB2BGR), + [cv2.IMWRITE_JPEG_QUALITY, self.cfg.jpeg_quality], + ) + if not success: + raise RuntimeError("无法编码预览图像。") + result = FramePreview( + index, frame.pts, frame.time_seconds, frame.delay_seconds, encoded.tobytes() + ) + if len(result.jpeg) <= self.cfg.cache_bytes: + while ( + self.cache + and self.cache_bytes + len(result.jpeg) > self.cfg.cache_bytes + ): + self.cache_bytes -= len(self.cache.popitem(last=False)[1].jpeg) + self.cache[index] = result + self.cache_bytes += len(result.jpeg) + return result + + def close(self) -> None: + """Release the source and all cached preview bytes on the owner thread.""" + self.source.close() + self.cache.clear() + self.cache_bytes = 0 + + +class FrameWorker: + """Own a frame source on one thread with one active and one pending request.""" + + def __init__( + self, + source_factory: Callable[[], FrameSource], + cfg: AnnotationCfg, + on_ready: Callable[[FrameSourceInfo], None], + ) -> None: + self._cancelled = threading.Event() + self._source_factory = source_factory + self._cfg = cfg + self.reader: FrameReader | None = None + self._on_ready = on_ready + self._condition = threading.Condition() + self._status: dict[str, Any] = dict( + phase="starting", completed=0, total=None, error=None + ) + self._pending: tuple[int, Future[FramePreview]] | None = None + self._thread: threading.Thread | None = None + + def status(self) -> dict[str, Any]: + """Return an owned initialization/lifecycle status snapshot.""" + with self._condition: + return self._status.copy() + + def _report(self, **updates: Any) -> None: + with self._condition: + self._status.update(updates) + + def start(self) -> None: + """Prepare the source on its sole owning worker thread.""" + self._thread = threading.Thread( + target=self._run, name="trajectory-frames", daemon=True + ) + self._thread.start() + + def request(self, index: int) -> Future[FramePreview]: + """Replace a queued seek, bounding work even after HTTP cancellation.""" + future: Future[FramePreview] = Future() + with self._condition: + if self._status["phase"] != "ready" or self._cancelled.is_set(): + raise RuntimeError("帧源尚未就绪或已关闭。") + if self._pending is not None: + previous = self._pending[1] + if not previous.done(): + previous.set_exception(SupersededError("已被更新的寻帧请求替代。")) + self._pending = (index, future) + self._condition.notify() + return future + + def _run(self) -> None: + try: + self.reader = FrameReader( + self._source_factory(), self._cfg, self._cancelled + ) + metadata = self.reader.prepare(self._report) + self._check_running() + self._on_ready(metadata) + self._report( + phase="ready", + completed=metadata.frame_count, + total=metadata.frame_count, + ) + while not self._cancelled.is_set(): + with self._condition: + self._condition.wait_for( + lambda: self._pending is not None or self._cancelled.is_set() + ) + pending, self._pending = self._pending, None + if pending is None: + continue + index, future = pending + if not future.set_running_or_notify_cancel(): + continue + try: + future.set_result(self.reader.preview(index)) + except Exception as error: + future.set_exception(error) + except InterruptedError: + self._report(phase="cancelled") + except Exception as error: + self._report(phase="error", error=str(error)) + finally: + try: + if self.reader is not None: + self.reader.close() + except Exception as error: + self._report(phase="error", error=f"关闭帧源失败:{error}") + finally: + with self._condition: + if self._pending is not None and not self._pending[1].done(): + self._pending[1].set_exception(RuntimeError("帧源已关闭。")) + self._pending = None + + def _check_running(self) -> None: + if self._cancelled.is_set(): + raise InterruptedError("初始化已取消。") + + def close(self) -> None: + """Cancel initialization/decoding and join its sole owning thread.""" + self._cancelled.set() + with self._condition: + self._condition.notify_all() + if self._thread is not None: + self._thread.join() + if self.status()["phase"] not in {"error", "cancelled"}: + self._report(phase="closed") diff --git a/embodichain/toolkits/trajectory_annotator/_models.py b/embodichain/toolkits/trajectory_annotator/_models.py new file mode 100644 index 0000000000..2e07356ebd --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/_models.py @@ -0,0 +1,98 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Validation for zero-based, inclusive, single-track atomic skill intervals.""" + +from __future__ import annotations + +from typing import Any + +__all__ = ["ConflictError", "validate_skill_intervals"] + + +class ConflictError(ValueError): + """A stale edit would overwrite a newer annotation document.""" + + +def validate_skill_intervals(value: object, frame_count: int) -> list[dict[str, Any]]: + """Normalize intervals, rejecting invalid boundaries, IDs and overlaps. + + Args: + value: JSON array of atomic skill interval objects. + frame_count: Number of images in the selected frame sequence. + + Returns: + New dictionaries sorted by inclusive start and end frame. + + Raises: + ValueError: A field, boundary, ID or overlap violates the format. + """ + if not isinstance(value, list) or len(value) > 100_000: + raise ValueError("原子技能区间必须为数组,且最多包含 100000 段。") + result: list[dict[str, Any]] = [] + identifiers: set[str] = set() + required = {"id", "label", "start_frame", "end_frame"} + for position, item in enumerate(value, start=1): + if not isinstance(item, dict) or not required <= item.keys(): + raise ValueError( + f"第 {position} 段缺少 id、label、start_frame 或 end_frame。" + ) + if item.keys() - required - {"note"}: + raise ValueError(f"第 {position} 段包含未知字段。") + identifier, label, note = item["id"], item["label"], item.get("note", "") + if ( + not isinstance(identifier, str) + or not identifier.strip() + or len(identifier) > 128 + ): + raise ValueError(f"第 {position} 段的 id 必须是非空字符串,最长 128 字符。") + if identifier in identifiers: + raise ValueError(f"重复的标注 id:{identifier}") + if not isinstance(label, str) or not label.strip() or len(label) > 512: + raise ValueError(f"第 {position} 段的原子技能名称必须非空,最长 512 字符。") + if not isinstance(note, str) or len(note) > 8192: + raise ValueError(f"第 {position} 段的备注必须为字符串,最长 8192 字符。") + start, end = item["start_frame"], item["end_frame"] + if type(start) is not int or type(end) is not int: + raise ValueError(f"第 {position} 段的起止帧必须为整数。") + if not 0 <= start <= end < frame_count: + raise ValueError( + f"第 {position} 段 [{start}, {end}] 超出 0–{frame_count - 1},或终点早于起点。" + ) + identifiers.add(identifier) + result.append( + dict( + id=identifier, + label=label.strip(), + start_frame=start, + end_frame=end, + note=note, + ) + ) + result.sort( + key=lambda skill_interval: ( + skill_interval["start_frame"], + skill_interval["end_frame"], + ) + ) + for previous, current in zip(result, result[1:]): + if current["start_frame"] <= previous["end_frame"]: + raise ValueError( + f"区间重叠:{previous['label']} [{previous['start_frame']}, " + f"{previous['end_frame']}] 与 {current['label']} " + f"[{current['start_frame']}, {current['end_frame']}]。" + ) + return result diff --git a/embodichain/toolkits/trajectory_annotator/_server.py b/embodichain/toolkits/trajectory_annotator/_server.py new file mode 100644 index 0000000000..987b4f4085 --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/_server.py @@ -0,0 +1,185 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Same-origin HTTP boundary for the local image-frame annotation application.""" + +from __future__ import annotations + +import asyncio +from contextlib import asynccontextmanager +from collections.abc import AsyncIterator, Callable +import json +from pathlib import Path +import secrets +from typing import Any + +from fastapi import FastAPI, HTTPException, Request +from fastapi.responses import FileResponse, Response +from fastapi.staticfiles import StaticFiles +from starlette.concurrency import run_in_threadpool +from starlette.middleware.trustedhost import TrustedHostMiddleware + +from ._cfg import AnnotationCfg +from ._models import ConflictError +from ._storage import AnnotationStore +from ._frames import SupersededError, FrameWorker +from .frames import FrameSource + +__all__ = ["create_app"] + + +def create_app( + source_factory: Callable[[], FrameSource], + annotations: Path, + cfg: AnnotationCfg | None = None, +) -> FastAPI: + """Create one local app with a lifespan-owned frame source and annotation file. + + Args: + source_factory: Create a fresh frame source on its owning worker thread. + annotations: Annotation JSON destination. + cfg: Optional tool resource configuration. + + Returns: + An ASGI app ready for a single Uvicorn process. + """ + cfg = cfg or AnnotationCfg() + cfg.validate() + store = AnnotationStore(annotations, cfg.document_bytes) + worker = FrameWorker(source_factory, cfg, store.initialize) + token = secrets.token_urlsafe(32) + + @asynccontextmanager + async def lifespan(app: FastAPI) -> AsyncIterator[None]: + nonlocal worker, store + store = AnnotationStore(annotations, cfg.document_bytes) + worker = FrameWorker(source_factory, cfg, store.initialize) + worker.start() + try: + yield + finally: + await run_in_threadpool(worker.close) + + app = FastAPI( + title="Trajectory Annotator", lifespan=lifespan, docs_url=None, redoc_url=None + ) + app.add_middleware( + TrustedHostMiddleware, allowed_hosts=["127.0.0.1", "localhost", "[::1]"] + ) + + def ready() -> None: + status = worker.status() + if status["phase"] != "ready": + raise HTTPException(503, status["error"] or "帧源仍在初始化,或已经关闭。") + + def authorize(request: Request) -> None: + if not secrets.compare_digest( + request.headers.get("X-Annotation-Token", ""), token + ): + raise HTTPException(403, "请通过本工具页面提交修改。") + + @app.get("/api/status") + async def status() -> dict[str, Any]: + return {**worker.status(), "session_token": token} + + @app.get("/api/project") + async def project() -> dict[str, Any]: + ready() + snapshot = store.snapshot() + return { + **snapshot, + "source_name": snapshot["document"]["source"]["name"], + "annotation_path": str(store.path), + "session_token": token, + "autosave_ms": cfg.autosave_ms, + } + + @app.get("/api/frames/{index}") + async def frame(index: int, request_id: int = 0) -> Response: + ready() + try: + preview = await asyncio.wrap_future(worker.request(index)) + except SupersededError as error: + raise HTTPException(409, str(error)) from error + except (ValueError, IndexError) as error: + raise HTTPException(422, str(error)) from error + except (RuntimeError, OSError) as error: + raise HTTPException(500, str(error)) from error + return Response( + preview.jpeg, + media_type="image/jpeg", + headers={ + "Cache-Control": "no-store", + "X-Frame-Index": str(preview.index), + "X-Frame-PTS": str(preview.pts) if preview.pts is not None else "", + "X-Frame-Time": str(preview.time_seconds), + "X-Frame-Delay": str(preview.delay_seconds), + "X-Request-ID": str(request_id), + }, + ) + + @app.put("/api/annotations") + async def save(request: Request) -> dict[str, Any]: + authorize(request) + ready() + if ( + request.headers.get("content-type", "").split(";", 1)[0] + != "application/json" + ): + raise HTTPException(415, "请求必须使用 application/json。") + content = bytearray() + async for chunk in request.stream(): + if len(content) + len(chunk) > cfg.document_bytes: + raise HTTPException(413, "标注内容超过大小上限。") + content.extend(chunk) + try: + payload = json.loads(content) + if not isinstance(payload, dict) or payload.keys() != { + "segments", + "expected_revision", + }: + raise ValueError("请求需要 segments 和 expected_revision 字段。") + + def persist() -> dict[str, Any]: + if worker.reader is None: + raise RuntimeError("帧源尚未准备。") + worker.reader.check_unchanged() + return store.save(payload["segments"], payload["expected_revision"]) + + return await run_in_threadpool(persist) + except ConflictError as error: + raise HTTPException(409, str(error)) from error + except (ValueError, UnicodeError) as error: + raise HTTPException(422, str(error)) from error + except OSError as error: + raise HTTPException(507, f"保存失败:{error}") from error + + @app.post("/api/cancel") + async def cancel(request: Request) -> dict[str, Any]: + authorize(request) + if worker.status()["phase"] == "ready": + raise HTTPException(409, "帧源已就绪,无需取消初始化。") + await run_in_threadpool(worker.close) + return worker.status() + + web = Path(__file__).with_name("web") + + @app.get("/") + async def index() -> FileResponse: + return FileResponse(web / "index.html", headers={"Cache-Control": "no-cache"}) + + app.mount("/static", StaticFiles(directory=web), name="static") + return app diff --git a/embodichain/toolkits/trajectory_annotator/_storage.py b/embodichain/toolkits/trajectory_annotator/_storage.py new file mode 100644 index 0000000000..51434bb0a1 --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/_storage.py @@ -0,0 +1,222 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Identity-bound JSON loading and revision-checked atomic persistence.""" + +from __future__ import annotations + +from copy import deepcopy +import hashlib +import json +import os +from pathlib import Path +import tempfile +import threading +from typing import Any + +from ._models import ConflictError, validate_skill_intervals +from .frames import FrameSourceInfo + +__all__ = ["AnnotationStore"] + + +class AnnotationStore: + """Own one file; publish new revisions only after atomic replacement succeeds. + + Args: + path: Destination JSON file, distinct from any backing source file. + document_bytes: Maximum encoded file size. + """ + + def __init__(self, path: Path, document_bytes: int) -> None: + self.path = path.resolve() + self.limit = document_bytes + self._lock = threading.RLock() + self._document: dict[str, Any] | None = None + self._digest: str | None = None + + def _read(self) -> bytes | None: + try: + with self.path.open("rb") as handle: + content = handle.read(self.limit + 1) + except FileNotFoundError: + return None + if len(content) > self.limit: + raise ValueError("标注文件超过大小上限。") + return content + + def initialize(self, info: FrameSourceInfo) -> None: + """Load matching annotations; preserve legacy video JSON until next save.""" + if info.path is not None and ( + self.path == info.path.resolve() + or (self.path.exists() and os.path.samefile(self.path, info.path)) + ): + raise ValueError("标注文件不能与输入文件相同。") + source: dict[str, Any] = dict( + kind=info.kind, + name=info.name, + identity=info.identity, + frame_count=info.frame_count, + width=info.width, + height=info.height, + fps=info.fps, + timing=info.timing, + details=deepcopy(info.details), + ) + if info.path is not None: + source["path"] = os.path.relpath(info.path.resolve(), self.path.parent) + # The versioned JSON key `segments` stores atomic skill intervals. + document = dict( + schema_version=2, + frame_index_base=0, + interval="closed", + source=source, + revision=0, + segments=[], + ) + with self._lock: + content = self._read() + if content is not None: + try: + loaded = json.loads(content) + except (ValueError, UnicodeError) as error: + raise ValueError(f"无法解析标注 JSON:{error}") from error + if not isinstance(loaded, dict): + raise ValueError("标注文件必须为 JSON 对象。") + version = loaded.get("schema_version") + source_key = "source_video" if version == 1 else "source" + expected_keys = { + "schema_version", + "frame_index_base", + "interval", + source_key, + "revision", + "segments", + } + if loaded.keys() != expected_keys: + raise ValueError("标注文件字段不符合格式,不能覆盖原文件。") + if ( + type(version) is not int + or version not in {1, 2} + or type(loaded["frame_index_base"]) is not int + or loaded["frame_index_base"] != 0 + or loaded["interval"] != "closed" + ): + raise ValueError("只支持 schema_version=1/2、0 基帧号和闭区间。") + original = loaded[source_key] + if version == 1: + matching = ( + info.kind == "video" + and isinstance(original, dict) + and all( + original.get(key) == info.details.get(key) + for key in ( + "sha256", + "stream_index", + "frame_count", + "width", + "height", + "time_base", + ) + ) + ) + else: + matching = isinstance(original, dict) and all( + original.get(key) == source[key] + for key in ( + "kind", + "identity", + "frame_count", + "width", + "height", + "fps", + "timing", + ) + ) + if not matching: + raise ValueError( + "标注来源与图像序列不匹配(内容、相机、帧率或帧信息不同)。" + ) + revision = loaded["revision"] + if type(revision) is not int or revision < 0: + raise ValueError("标注 revision 必须为非负整数。") + document["revision"] = revision + document["segments"] = validate_skill_intervals( + loaded["segments"], info.frame_count + ) + self._digest = hashlib.sha256(content).hexdigest() + self._document = document + + def snapshot(self) -> dict[str, Any]: + """Return an owned document snapshot and whether it has been persisted.""" + with self._lock: + if self._document is None: + raise RuntimeError("帧源仍在初始化。") + return { + "document": deepcopy(self._document), + "persisted": self._digest is not None, + } + + def save( + self, skill_intervals: object, expected_revision: object + ) -> dict[str, Any]: + """Validate and persist atomic skill intervals without overwriting stale edits.""" + with self._lock: + if self._document is None: + raise RuntimeError("帧源仍在初始化。") + if ( + type(expected_revision) is not int + or expected_revision != self._document["revision"] + ): + raise ConflictError("标注已有新修订,请先导出当前修改,再刷新页面。") + content = self._read() + digest = ( + hashlib.sha256(content).hexdigest() if content is not None else None + ) + if digest != self._digest: + raise ConflictError( + "磁盘上的标注文件已被其他程序修改,请导出当前修改后重新打开。" + ) + candidate = deepcopy(self._document) + candidate["segments"] = validate_skill_intervals( + skill_intervals, candidate["source"]["frame_count"] + ) + candidate["revision"] += 1 + encoded = ( + json.dumps(candidate, ensure_ascii=False, indent=2) + "\n" + ).encode("utf-8") + if len(encoded) > self.limit: + raise ValueError("标注内容超过大小上限。") + self.path.parent.mkdir(parents=True, exist_ok=True) + temporary: Path | None = None + try: + with tempfile.NamedTemporaryFile( + mode="wb", + dir=self.path.parent, + prefix=f".{self.path.name}.", + delete=False, + ) as handle: + temporary = Path(handle.name) + handle.write(encoded) + handle.flush() + os.fsync(handle.fileno()) + os.replace(temporary, self.path) + finally: + if temporary is not None: + temporary.unlink(missing_ok=True) + self._document = candidate + self._digest = hashlib.sha256(encoded).hexdigest() + return deepcopy(candidate) diff --git a/embodichain/toolkits/trajectory_annotator/app.py b/embodichain/toolkits/trajectory_annotator/app.py new file mode 100644 index 0000000000..7d2c0bb4b6 --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/app.py @@ -0,0 +1,110 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Launch the same annotation application with any image frame source.""" + +from __future__ import annotations + +from collections.abc import Callable +from pathlib import Path +import socket +import threading +from typing import TYPE_CHECKING +import webbrowser + +if TYPE_CHECKING: + from fastapi import FastAPI + from .frames import FrameSource + +__all__ = ["create_annotation_app", "launch_annotation"] + + +def create_annotation_app( + source_factory: Callable[[], FrameSource], annotations: str | Path +) -> FastAPI: + """Create a source-independent FastAPI application for a single process. + + Args: + source_factory: Create a fresh FrameSource on the app's worker thread. + The app closes it on shutdown or failed preparation. + annotations: Destination JSON path; persisted only after an edit/save. + + Returns: + ASGI application with lifespan-managed frame input and preview cache. + """ + from ._server import create_app + + return create_app(source_factory, Path(annotations)) + + +def launch_annotation( + source_factory: Callable[[], FrameSource], + annotations: str | Path, + *, + port: int = 0, + open_browser: bool = True, +) -> None: + """Run the local website until Ctrl+C, accepting any frame source factory. + + Args: + source_factory: Create a fresh frame source on its owning worker thread. + annotations: JSON output file, distinct from any source file. + port: Loopback port; zero asks the OS to select an available port. + open_browser: Open the website after the server starts. + + Raises: + ValueError: Port is outside 0–65535. + OSError: The requested loopback port cannot be bound. + """ + import uvicorn + + if type(port) is not int or not 0 <= port <= 65535: + raise ValueError("端口必须在 0–65535 之间。") + annotations = Path(annotations).resolve() + server = uvicorn.Server( + uvicorn.Config( + create_annotation_app(source_factory, annotations), + host="127.0.0.1", + port=port, + access_log=False, + log_level="warning", + ) + ) + stopped = threading.Event() + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as listener: + listener.bind(("127.0.0.1", port)) + url = f"http://127.0.0.1:{listener.getsockname()[1]}" + print( + f"原子技能区间标注:{url}\n标注:{annotations}\n按 Ctrl+C 退出。", + flush=True, + ) + + def browse() -> None: + while not stopped.wait(0.05): + if server.started: + webbrowser.open(url) + return + + if open_browser: + threading.Thread( + target=browse, name="annotation-browser", daemon=True + ).start() + try: + server.run(sockets=[listener]) + except KeyboardInterrupt: + pass + finally: + stopped.set() diff --git a/embodichain/toolkits/trajectory_annotator/frames.py b/embodichain/toolkits/trajectory_annotator/frames.py new file mode 100644 index 0000000000..c3ecce3b1c --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/frames.py @@ -0,0 +1,244 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Container-independent RGB frame input for trajectory annotation.""" + +from __future__ import annotations + +from collections.abc import Callable, Sequence +from dataclasses import dataclass, field +import hashlib +import math +from pathlib import Path +import threading +from typing import Any, Protocol + +import numpy as np +from numpy.typing import NDArray + +__all__ = ["ImageFrame", "FrameSourceInfo", "FrameSource", "ArrayFrameSource"] + + +@dataclass(frozen=True) +class ImageFrame: + """One indexed RGB uint8 image and its display timing. + + Attributes: + index: Zero-based position in the source sequence. + image: H×W×3 RGB pixels, valid until the next source read or close. + time_seconds: Nonnegative display time relative to the first image. + delay_seconds: Positive duration before displaying the next image. + pts: Optional original presentation timestamp; never synthesized. + """ + + index: int + image: NDArray[np.uint8] + time_seconds: float + delay_seconds: float + pts: int | None = None + + +@dataclass(frozen=True) +class FrameSourceInfo: + """Prepared source identity and fixed image geometry. + + Attributes: + kind: Adapter name, e.g. ``video``, ``hdf5`` or ``images``. + name: Human-readable source name. + identity: Stable identifier covering content, selection and timing. + frame_count: Number of images, greater than zero. + width: Original image width. + height: Original image height. + fps: Nominal playback rate, or None when unavailable. + timing: ``pts`` for measured video timing or ``fixed_fps`` for derived time. + details: JSON-serializable provenance such as image key or stream index. + path: Optional backing file, protected against annotation overwrite. + + Names and paths are descriptive; changing image order, pixels, timing or + camera selection must change ``identity``. Adapters must not retain all + decoded images merely to compute this information. + """ + + kind: str + name: str + identity: str + frame_count: int + width: int + height: int + fps: float | None + timing: str + details: dict[str, Any] = field(default_factory=dict) + path: Path | None = None + + +class FrameSource(Protocol): + """Random-access image input owned by the application's single worker. + + The app calls the source factory, prepare, get_frame and close on the same + worker thread. A factory must return a fresh instance for each application + lifespan. Only ``check_unchanged`` may also run on a persistence thread; it + must use immutable metadata/stat checks, never an open decoder/HDF5 handle. + """ + + def prepare( + self, + cancelled: threading.Event, + progress: Callable[..., None], + ) -> FrameSourceInfo: + """Prepare identity and geometry, reporting phase/completed/total keywords. + + Args: + cancelled: Cooperative cancellation signal, checked during scans. + progress: Callback accepting phase, completed and total keyword arguments. + + Returns: + Complete source metadata before any frame requests are accepted. + """ + ... + + def get_frame(self, index: int) -> ImageFrame: + """Read exactly one indexed RGB image with its display timing. + + Args: + index: Integer in ``[0, frame_count - 1]``. + + Returns: + Pixels and timing belonging to the requested index. + """ + ... + + def check_unchanged(self) -> None: + """Raise ValueError when the prepared source has changed.""" + ... + + def close(self) -> None: + """Release source resources, including after failed preparation.""" + ... + + +def _rgb_image(image: object) -> NDArray[np.uint8]: + if ( + not isinstance(image, np.ndarray) + or image.dtype != np.uint8 + or image.ndim != 3 + or image.shape[2] != 3 + or min(image.shape[:2]) <= 0 + ): + raise ValueError("图像必须为 H×W×3 的 RGB uint8 数组。") + return image + + +def _positive_fps(fps: float) -> float: + if isinstance(fps, bool) or not math.isfinite(fps) or fps <= 0: + raise ValueError("帧率必须为有限的正数。") + return float(fps) + + +class ArrayFrameSource: + """Adapt an existing RGB image sequence without stacking or copying the batch. + + Args: + frames: Sequence of H×W×3 uint8 RGB arrays, or an N×H×W×3 array. + fps: Positive rate used to derive display timing, not measured timestamps. + name: Display name stored in annotation provenance. + + .. attention:: + The caller owns the input and must keep pixels, order and length + unchanged until the app closes. Preparation hashes one image at a time; + this adapter does not take a second full copy of the sequence. + """ + + def __init__( + self, + frames: Sequence[NDArray[np.uint8]] | NDArray[np.uint8], + *, + fps: float, + name: str = "images", + ) -> None: + self._frames = frames + self._fps = _positive_fps(fps) + self._name = name + self._count = 0 + self._cancelled = threading.Event() + + def prepare( + self, cancelled: threading.Event, progress: Callable[..., None] + ) -> FrameSourceInfo: + """Validate fixed geometry and hash pixels with bounded temporary storage. + + Args: + cancelled: Cooperative cancellation signal during hashing. + progress: Callback accepting phase/completed/total keywords. + + Returns: + Pixel identity, geometry and FPS-derived timing metadata. + """ + self._cancelled = cancelled + self._count = len(self._frames) + if self._count == 0: + raise ValueError("图像序列不能为空。") + first = _rgb_image(self._frames[0]) + height, width = first.shape[:2] + digest = hashlib.sha256( + f"RGB:{self._count}:{width}:{height}:{self._fps}".encode() + ) + for index in range(self._count): + if cancelled.is_set(): + raise InterruptedError("初始化已取消。") + image = _rgb_image(self._frames[index]) + if image.shape != first.shape: + raise ValueError("图像序列的分辨率必须一致。") + digest.update(memoryview(np.ascontiguousarray(image))) + if index % 64 == 0 or index + 1 == self._count: + progress(phase="indexing", completed=index + 1, total=self._count) + self.check_unchanged() + return FrameSourceInfo( + "images", + self._name, + digest.hexdigest(), + self._count, + width, + height, + self._fps, + "fixed_fps", + {"color_order": "rgb"}, + ) + + def get_frame(self, index: int) -> ImageFrame: + """Return borrowed RGB pixels and FPS-derived time for one image. + + Args: + index: Zero-based position in the prepared sequence. + + Returns: + Caller-owned RGB image and timing for the requested index. + """ + if self._cancelled.is_set(): + raise InterruptedError("读取已取消。") + self.check_unchanged() + if type(index) is not int or not 0 <= index < self._count: + raise ValueError(f"帧号必须在 0–{self._count - 1} 之间。") + return ImageFrame( + index, _rgb_image(self._frames[index]), index / self._fps, 1 / self._fps + ) + + def check_unchanged(self) -> None: + """Check sequence length; pixel immutability remains the caller's contract.""" + if len(self._frames) != self._count: + raise ValueError("图像序列长度在准备后发生变化,请重新打开。") + + def close(self) -> None: + """Leave the caller-owned image sequence intact.""" diff --git a/embodichain/toolkits/trajectory_annotator/hdf5.py b/embodichain/toolkits/trajectory_annotator/hdf5.py new file mode 100644 index 0000000000..b50c815341 --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/hdf5.py @@ -0,0 +1,233 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Read-only, on-demand HDF5 image sequence adapter.""" + +from __future__ import annotations + +from collections.abc import Callable +import json +from pathlib import Path +import threading +from typing import Any + +import cv2 +import h5py +import numpy as np +from numpy.typing import NDArray + +from ._file import file_signature, hash_file, source_identity +from .frames import FrameSourceInfo, ImageFrame, _positive_fps, _rgb_image + +__all__ = ["Hdf5FrameSource"] + + +class Hdf5FrameSource: + """Adapt compressed image bytes or an N×H×W×3 uint8 image dataset. + + Args: + path: Local HDF5 file, opened read-only on the annotation worker. + image_key: Dataset path. When omitted, resolve cam_high under either + ``observations/images`` or ``observation/images``. Ambiguity is an error. + fps: Explicit playback rate. Otherwise consult the image dataset's fps, + root fps, then root ``metadata_json.record_fps``. No rate is guessed. + color_order: ``rgb`` or ``bgr`` for raw arrays. Compressed JPEG/PNG images + always use their encoded color definition. + + Time is derived as ``index / fps``; this adapter does not claim recorded + per-image timestamps. Preparation hashes the file and decodes only its first + selected image. Subsequent reads load a single selected-camera image. + """ + + def __init__( + self, + path: str | Path, + *, + image_key: str | None = None, + fps: float | None = None, + color_order: str = "rgb", + ) -> None: + self.path = Path(path).resolve() + self.image_key = image_key.strip("/") if image_key is not None else None + self._requested_fps = _positive_fps(fps) if fps is not None else None + if color_order not in {"rgb", "bgr"}: + raise ValueError("color_order 必须为 rgb 或 bgr。") + self._color_order = color_order + self._handle: h5py.File | None = None + self._dataset: h5py.Dataset | None = None + self._stat: tuple[int, int, int, int] | None = None + self._cancelled = threading.Event() + self._info: FrameSourceInfo | None = None + self._raw = False + + def _read_image(self, index: int) -> NDArray[np.uint8]: + if self._cancelled.is_set(): + raise InterruptedError("读取已取消。") + if self._dataset is None: + raise RuntimeError("HDF5 帧源尚未准备。") + item = self._dataset[index] + if self._raw: + image = _rgb_image(item) + return image if self._color_order == "rgb" else image[:, :, ::-1].copy() + if isinstance(item, (bytes, np.bytes_)): + encoded = np.frombuffer(item, dtype=np.uint8) + elif isinstance(item, np.ndarray) and item.dtype == np.uint8 and item.ndim == 1: + encoded = item + else: + raise ValueError("压缩图像必须为逐帧 uint8 字节数组或 bytes。") + if not encoded.size: + raise ValueError(f"HDF5 第 {index} 帧为空。") + image = cv2.imdecode(encoded, cv2.IMREAD_COLOR | cv2.IMREAD_IGNORE_ORIENTATION) + if image is None: + raise ValueError(f"HDF5 第 {index} 帧无法解码为 JPEG/PNG 图像。") + return cv2.cvtColor(image, cv2.COLOR_BGR2RGB) + + def _read_fps(self) -> float: + if self._requested_fps is not None: + return self._requested_fps + assert self._dataset is not None and self._handle is not None + rate: Any = self._dataset.attrs.get("fps", self._handle.attrs.get("fps")) + if rate is None and "metadata_json" in self._handle.attrs: + try: + metadata = json.loads(self._handle.attrs["metadata_json"]) + rate = ( + metadata.get("record_fps") if isinstance(metadata, dict) else None + ) + except (ValueError, TypeError, UnicodeError) as error: + raise ValueError( + "无法读取 HDF5 帧率元数据,请显式指定 --fps。" + ) from error + try: + return _positive_fps(float(rate)) + except (TypeError, ValueError, OverflowError) as error: + raise ValueError("缺少有效的图像帧率,请显式指定 --fps。") from error + + def prepare( + self, cancelled: threading.Event, progress: Callable[..., None] + ) -> FrameSourceInfo: + """Open one image dataset and hash the file without loading the sequence. + + Args: + cancelled: Cooperative cancellation signal for preparation and reads. + progress: Callback accepting phase/completed/total keywords. + + Returns: + Selected-camera identity, geometry and FPS-derived timing metadata. + + Raises: + ValueError: Dataset selection, image format or FPS is unsupported. + """ + self.close() + self._cancelled = cancelled + self._stat = file_signature(self.path) + self._handle = h5py.File(self.path, "r") + if self.image_key is None: + candidates = [ + key + for key in ( + "observations/images/cam_high", + "observation/images/cam_high", + ) + if key in self._handle + ] + if len(candidates) != 1: + raise ValueError( + "无法唯一定位 cam_high,请用 --image-key 指定图像 dataset。" + ) + self.image_key = candidates[0] + if self.image_key not in self._handle: + raise ValueError(f"HDF5 中找不到图像路径:{self.image_key}。") + dataset = self._handle[self.image_key] + if ( + not isinstance(dataset, h5py.Dataset) + or dataset.ndim == 0 + or not dataset.shape[0] + ): + raise ValueError("图像路径必须指向非空的逐帧 dataset。") + if ( + not Path(dataset.file.filename).samefile(self.path) + or dataset.is_virtual + or dataset.external + ): + raise ValueError( + "图像 dataset 必须存储在输入 HDF5 内,不支持外部存储或虚拟 dataset。" + ) + self._dataset = dataset + self._raw = ( + dataset.ndim == 4 and dataset.shape[-1] == 3 and dataset.dtype == np.uint8 + ) + if not self._raw and dataset.ndim not in {1, 2}: + raise ValueError("支持 N×H×W×3 uint8 图像或 N 个 JPEG/PNG 字节数组。") + fps = self._read_fps() + image = self._read_image(0) + height, width = image.shape[:2] + sha256 = hash_file(self.path, cancelled, progress) + details = dict( + sha256=sha256, + image_key=self.image_key, + encoding="raw" if self._raw else "encoded", + color_order=self._color_order if self._raw else "encoded", + fps=fps, + ) + self.check_unchanged() + self._info = FrameSourceInfo( + "hdf5", + f"{self.path.name} / {self.image_key}", + source_identity(details), + dataset.shape[0], + width, + height, + fps, + "fixed_fps", + details, + self.path, + ) + return self._info + + def get_frame(self, index: int) -> ImageFrame: + """Decode one selected-camera image with zero-based FPS-derived timing. + + Args: + index: Zero-based row in the selected image dataset. + + Returns: + RGB pixels and FPS-derived time, with no synthesized PTS. + + Raises: + ValueError: Index is out of bounds or the frame cannot be decoded. + """ + if self._info is None: + raise RuntimeError("HDF5 帧源尚未准备。") + if type(index) is not int or not 0 <= index < self._info.frame_count: + raise ValueError(f"帧号必须在 0–{self._info.frame_count - 1} 之间。") + self.check_unchanged() + image = self._read_image(index) + if image.shape[:2] != (self._info.height, self._info.width): + raise ValueError(f"HDF5 第 {index} 帧的分辨率与首帧不同。") + assert self._info.fps is not None + return ImageFrame(index, image, index / self._info.fps, 1 / self._info.fps) + + def check_unchanged(self) -> None: + """Check only file stat fields, safe on the persistence thread.""" + if self._stat != file_signature(self.path): + raise ValueError("HDF5 文件在打开后发生变化,请重新启动标注工具。") + + def close(self) -> None: + """Close the worker-owned read-only HDF5 handle.""" + if self._handle is not None: + self._handle.close() + self._handle = self._dataset = None + self._info = None diff --git a/embodichain/toolkits/trajectory_annotator/video.py b/embodichain/toolkits/trajectory_annotator/video.py new file mode 100644 index 0000000000..d487cb99ac --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/video.py @@ -0,0 +1,272 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""PyAV adapter preserving exact presentation-order frame indices.""" + +from __future__ import annotations + +from array import array +from bisect import bisect_left +from collections.abc import Callable, Iterator +from fractions import Fraction +import hashlib +from pathlib import Path +import threading +from typing import Any + +import av + +from ._cfg import AnnotationCfg +from ._file import source_identity +from .frames import FrameSourceInfo, ImageFrame + +__all__ = ["VideoFrameSource"] + + +class VideoFrameSource: + """Index and decode on one thread; stat-only identity checks are thread-safe. + + Args: + path: Local video file. + decoder_threads: FFmpeg threads for this decoder. + index_bytes: Maximum packed PTS and keyframe-index bytes. + stream_index: Actual container stream index, or the first video stream. + """ + + def __init__( + self, + path: str | Path, + *, + stream_index: int | None = None, + decoder_threads: int = 2, + index_bytes: int = 64 * 1024**2, + ) -> None: + self.path = Path(path).resolve() + self.cfg = AnnotationCfg( + decoder_threads=decoder_threads, index_bytes=index_bytes + ) + self.stream_index = stream_index + self.cancelled = threading.Event() + self.pts = array("q") + self.anchors = array("q") + self.metadata: dict[str, Any] = {} + self._container: Any = None + self._stream: Any = None + self._iterator: Iterator[Any] | None = None + self._cursor = -1 + self._stat: tuple[int, int, int] | None = None + self._time_base = Fraction(1, 1) + + def _check_cancelled(self) -> None: + if self.cancelled.is_set(): + raise InterruptedError("视频初始化或解码已取消。") + + def _signature(self) -> tuple[int, int, int]: + stat = self.path.stat() + return stat.st_ino, stat.st_size, stat.st_mtime_ns + + def check_unchanged(self) -> None: + """Reject use of an index after its source file was replaced or modified.""" + if self._stat != self._signature(): + raise ValueError("视频文件在打开后发生变化,请重新启动标注工具。") + + def _open(self) -> None: + if self._container is not None: + self._container.close() + self._container = av.open(str(self.path), options={"err_detect": "explode"}) + streams = list(self._container.streams.video) + if not streams: + raise ValueError("输入文件中没有视频流。") + self._stream = next( + (stream for stream in streams if stream.index == self.stream_index), None + ) + if self.stream_index is None: + self._stream = streams[0] + self.stream_index = self._stream.index + if self._stream is None: + raise ValueError( + f"找不到视频流 {self.stream_index},可选:{[stream.index for stream in streams]}。" + ) + self._stream.codec_context.thread_count = self.cfg.decoder_threads + self._iterator = None + self._cursor = -1 + + def prepare( + self, cancelled: threading.Event, progress: Callable[..., None] + ) -> FrameSourceInfo: + """Hash and fully index PTS/anchors with bounded storage and cancellation. + + Args: + cancelled: Cooperative cancellation event. + progress: Callback receiving phase/completed/total keyword arguments. + + Returns: + Identity, geometry and timing of the selected video stream. + """ + self.cancelled = cancelled + report = progress + self._stat = self._signature() + if not self.path.is_file(): + raise ValueError("视频路径必须为普通文件。") + digest = hashlib.sha256() + completed = 0 + report(phase="hashing", completed=0, total=self._stat[1]) + with self.path.open("rb") as handle: + while chunk := handle.read(1024**2): + self._check_cancelled() + digest.update(chunk) + completed += len(chunk) + report(phase="hashing", completed=completed, total=self._stat[1]) + self._open() + report(phase="indexing", completed=0, total=self._stream.frames or None) + self.pts = array("q") + self.anchors = array("q") + anchor = 0 + dimensions: tuple[int, int] | None = None + for frame in self._container.decode(self._stream): + self._check_cancelled() + if frame.pts is None or (self.pts and frame.pts <= self.pts[-1]): + raise ValueError("视频帧时间戳缺失、重复或非递增;首版需要可靠的 PTS。") + if dimensions is None: + dimensions = (frame.width, frame.height) + if dimensions != (frame.width, frame.height): + raise ValueError("首版不支持中途改变分辨率的视频。") + if (len(self.pts) + 1) * ( + self.pts.itemsize + self.anchors.itemsize + ) > self.cfg.index_bytes: + raise ValueError("帧索引超过配置的内存预算。") + if frame.key_frame: + anchor = len(self.pts) + self.pts.append(frame.pts) + self.anchors.append(anchor) + if len(self.pts) % 64 == 0: + report( + phase="indexing", + completed=len(self.pts), + total=self._stream.frames or None, + ) + if not self.pts or dimensions is None: + raise ValueError("视频没有可解码的画面。") + self._time_base = Fraction(self._stream.time_base) + fps = self._stream.average_rate + if fps is not None and fps <= 0: + fps = None + self.metadata = dict( + sha256=digest.hexdigest(), + stream_index=self.stream_index, + frame_count=len(self.pts), + width=dimensions[0], + height=dimensions[1], + time_base=dict( + numerator=self._time_base.numerator, + denominator=self._time_base.denominator, + ), + average_fps=( + dict(numerator=fps.numerator, denominator=fps.denominator) + if fps + else None + ), + ) + self.check_unchanged() + self._open() + return FrameSourceInfo( + "video", + self.path.name, + source_identity(self.metadata), + len(self.pts), + dimensions[0], + dimensions[1], + float(fps) if fps else None, + "pts", + self.metadata.copy(), + self.path, + ) + + def _walk(self, index: int, expected: int | None = None) -> Any: + if self._iterator is None: + self._iterator = iter(self._container.decode(self._stream)) + for frame in self._iterator: + self._check_cancelled() + found = ( + bisect_left(self.pts, frame.pts) + if frame.pts is not None + else len(self.pts) + ) + if ( + found >= len(self.pts) + or self.pts[found] != frame.pts + or (expected is not None and found != expected) + or found > index + ): + raise ValueError("关键帧定位后的画面无法与完整索引对齐。") + self._cursor = found + expected = found + 1 + if found == index: + return frame + raise ValueError(f"无法解码目标帧 {index}。") + + def _frame(self, index: int) -> Any: + try: + if self._iterator is not None and self._cursor < index <= self._cursor + 64: + return self._walk(index, self._cursor + 1) + self._container.seek( + self.pts[self.anchors[index]], + stream=self._stream, + backward=True, + any_frame=False, + ) + self._iterator = None + return self._walk(index) + except (ValueError, av.error.FFmpegError): + # Reopen instead of seek(0): the first source PTS need not be zero, + # and broken container seek tables must not alter frame numbering. + self._open() + return self._walk(index, 0) + + def get_frame(self, index: int) -> ImageFrame: + """Read exactly one RGB image using indexed PTS, with sequential fallback. + + Args: + index: Zero-based presentation-order frame position. + + Returns: + Requested RGB pixels and original PTS-based timing. + """ + if type(index) is not int or not 0 <= index < len(self.pts): + raise ValueError(f"帧号必须在 0–{len(self.pts) - 1} 之间。") + self._check_cancelled() + self.check_unchanged() + frame = self._frame(index) + if len(self.pts) > 1: + offset = min(index, len(self.pts) - 2) + delay = float((self.pts[offset + 1] - self.pts[offset]) * self._time_base) + else: + fps = self._stream.average_rate + delay = float(1 / fps) if fps and fps > 0 else 1 / 30 + return ImageFrame( + index, + frame.to_ndarray(format="rgb24"), + float((self.pts[index] - self.pts[0]) * self._time_base), + delay, + self.pts[index], + ) + + def close(self) -> None: + """Release decoder buffers on the owning thread.""" + if self._container is not None: + self._container.close() + self._container = None + self._iterator = None diff --git a/embodichain/toolkits/trajectory_annotator/web/app.js b/embodichain/toolkits/trajectory_annotator/web/app.js new file mode 100644 index 0000000000..aa3ee41a52 --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/web/app.js @@ -0,0 +1,600 @@ +// Copyright (c) 2021-2026 DexForce Technology Co., Ltd. Licensed under Apache-2.0. +// Completed atomic skill intervals are immutable snapshots; draft and displayed-frame state +// stay separate so neither an in-flight seek nor autosave can move an endpoint. +const $ = (id) => document.getElementById(id); +const state = { + project: null, skillIntervals: [], selected: null, draft: null, lastLabel: "", + displayed: null, target: 0, delay: 1 / 30, playing: false, loading: false, + serial: 0, pending: null, inFlight: false, timer: null, imageUrl: null, + speed: 1, zoom: 1, displayTimes: [], undo: [], redo: [], + version: 0, savedVersion: 0, persisted: false, saving: false, + saveTimer: null, saveError: "", conflict: false, token: "", autosave: 1000, +}; +const clone = (value) => structuredClone(value); +const maximum = () => state.project.document.source.frame_count - 1; +const selected = () => state.skillIntervals.find((skillInterval) => skillInterval.id === state.selected); +const dirty = () => state.version !== state.savedVersion; + +function message(text, error = false) { + $("message").textContent = text; + $("message").classList.toggle("error", error); +} + +async function checked(response) { + if (response.ok) return response; + let detail = `${response.status} ${response.statusText}`; + try { + const body = await response.json(); + detail = typeof body.detail === "string" ? body.detail : JSON.stringify(body.detail || body); + } catch { /* Keep the HTTP error if the server did not send JSON. */ } + const error = new Error(detail); + error.status = response.status; + throw error; +} + +function timecode(seconds) { + const total = Math.round(seconds * 1000); + const hours = Math.floor(total / 3600000); + const minutes = Math.floor(total / 60000) % 60; + const rest = (Math.floor(total / 1000) % 60).toString().padStart(2, "0"); + return `${hours ? `${hours.toString().padStart(2, "0")}:` : ""}${minutes.toString().padStart(2, "0")}:${rest}.${(total % 1000).toString().padStart(3, "0")}`; +} + +function renderTransport() { + $("play").textContent = state.playing ? "Ⅱ 暂停" : "▶ 播放"; + $("play").setAttribute("aria-label", state.playing ? "暂停" : "播放"); + $("playback-state").textContent = state.playing ? "播放中" : "已暂停"; + $("loading-frame").hidden = !state.loading; + $("mark-start").disabled = state.displayed === null || (state.loading && !state.playing); + $("mark-end").disabled = $("mark-start").disabled; +} + +function pause(freeze = true) { + state.playing = false; + clearTimeout(state.timer); + if (freeze) { + state.serial += 1; + state.pending = null; + state.loading = false; + state.target = state.displayed ?? 0; + } + renderTransport(); +} + +function requestFrame(index, playback = false) { + if (!state.project) return; + if (!playback) pause(false); + if (!Number.isInteger(index) || index < 0 || index > maximum()) { + message(`帧号必须是 0–${maximum()} 之间的整数。`, true); + return; + } + state.serial += 1; + state.target = index; + state.pending = { index, serial: state.serial }; + state.loading = true; + renderTransport(); + void pumpFrames(); +} + +async function pumpFrames() { + if (state.inFlight) return; + state.inFlight = true; + try { + while (state.pending) { + const task = state.pending; + state.pending = null; + const started = performance.now(); + let url = null; + try { + const response = await checked(await fetch(`/api/frames/${task.index}?request_id=${task.serial}`)); + if (task.serial !== state.serial) continue; + const index = Number(response.headers.get("X-Frame-Index")); + const requestId = Number(response.headers.get("X-Request-ID")); + if (index !== task.index || requestId !== task.serial) throw new Error("返回的画面与请求帧号不一致。请重新定位。"); + const blob = await response.blob(); + if (task.serial !== state.serial) continue; + url = URL.createObjectURL(blob); + const image = new Image(); + image.src = url; + await image.decode(); + if (task.serial !== state.serial) continue; + image.id = "video-frame"; + image.alt = `视频第 ${index} 帧`; + $("video-frame").replaceWith(image); + if (state.imageUrl) URL.revokeObjectURL(state.imageUrl); + state.imageUrl = url; + url = null; + state.displayed = index; + state.delay = Number(response.headers.get("X-Frame-Delay")); + state.loading = false; + $("frame-current").textContent = index; + $("frame-time").textContent = timecode(Number(response.headers.get("X-Frame-Time"))); + $("scrubber").value = index; + // Do not change a number the user is still typing. + if (document.activeElement !== $("goto-frame")) $("goto-frame").value = index; + $("playhead").hidden = false; + $("playhead").style.left = `${frameCenterPercent(index)}%`; + $("timeline").setAttribute("aria-valuenow", String(index)); + $("timeline").setAttribute("aria-valuetext", `第 ${index} 帧`); + renderTransport(); + if (state.playing) { + state.displayTimes.push(performance.now()); + state.displayTimes = state.displayTimes.slice(-20); + if (state.displayTimes.length > 1) { + const fps = (state.displayTimes.length - 1) * 1000 / (state.displayTimes.at(-1) - state.displayTimes[0]); + $("actual-fps").textContent = `实际 ${fps.toFixed(1)} fps`; + } + if (index === maximum()) pause(false); + else { + const wait = Math.max(0, state.delay * 1000 / state.speed - (performance.now() - started)); + state.timer = setTimeout(() => { + if (state.playing) requestFrame(state.displayed + 1, true); + }, wait); + } + } + } catch (error) { + if (task.serial === state.serial) { + pause(); + message(`读取画面失败:${error.message}`, true); + } + } finally { + if (url) URL.revokeObjectURL(url); + } + } + } finally { + state.inFlight = false; + } +} + +function step(delta) { + if (!state.project) return; + const base = state.playing ? state.displayed : (state.loading ? state.target : state.displayed); + requestFrame(Math.max(0, Math.min(maximum(), (base ?? 0) + delta))); +} + +function play() { + if (!state.project || state.displayed === null) return; + if (state.playing) { pause(); return; } + pause(); + state.playing = true; + state.displayTimes = []; + requestFrame(state.displayed === maximum() ? 0 : state.displayed + 1, true); + renderTransport(); +} + +function skillIntervalError(draft) { + if (!draft) return ""; + if (!draft.label.trim()) return "请输入原子技能名称。"; + if (!Number.isInteger(draft.start_frame) || !Number.isInteger(draft.end_frame)) return "请设置整数起点和终点。"; + if (!(0 <= draft.start_frame && draft.start_frame <= draft.end_frame && draft.end_frame <= maximum())) return `起止帧需满足 0 ≤ 起点 ≤ 终点 ≤ ${maximum()}。`; + const conflict = state.skillIntervals.find((skillInterval) => skillInterval.id !== draft.id && draft.start_frame <= skillInterval.end_frame && skillInterval.start_frame <= draft.end_frame); + return conflict ? `与“${conflict.label}” [${conflict.start_frame}, ${conflict.end_frame}] 重叠。` : ""; +} + +function renderDraft(syncInputs = true) { + const draft = state.draft; + $("editor-title").textContent = draft?.id ? "编辑原子技能区间" : "新建原子技能区间"; + $("draft-status").textContent = draft ? "草稿未提交" : "在起点按 I 开始"; + $("start-frame").disabled = !draft; + $("end-frame").disabled = !draft; + $("note").disabled = !draft; + $("cancel-draft").disabled = !draft; + if (syncInputs) { + $("label").value = draft?.label ?? state.lastLabel; + $("start-frame").value = draft?.start_frame ?? ""; + $("end-frame").value = draft?.end_frame ?? ""; + $("note").value = draft?.note ?? ""; + } + const error = skillIntervalError(draft); + $("draft-error").textContent = error; + $("commit").disabled = !draft || !!error; + const length = draft && Number.isInteger(draft.start_frame) && Number.isInteger(draft.end_frame) && draft.end_frame >= draft.start_frame ? draft.end_frame - draft.start_frame + 1 : null; + $("skill-interval-length").textContent = length === null ? "— 帧" : `${length} 帧`; + $("draft-range").hidden = !draft || !Number.isInteger(draft.start_frame); + if (!$("draft-range").hidden) { + positionSkillInterval($("draft-range"), draft.start_frame, draft.start_frame + Math.max(1, length ?? 1) - 1); + $("draft-range").firstElementChild.textContent = draft.label || "草稿"; + } + renderSelection(); + renderSave(); +} + +function mark(endpoint) { + if (state.displayed === null || (state.loading && !state.playing)) return; + pause(); + if (!state.draft) { + if (endpoint === "end_frame") { message("请先按 I 设置起点。", true); return; } + state.draft = { id: null, label: $("label").value || state.lastLabel, start_frame: null, end_frame: null, note: "" }; + } + state.draft[endpoint] = state.displayed; + renderDraft(); + message(`已将第 ${state.displayed} 帧设为${endpoint === "start_frame" ? "起点" : "终点"}。`); +} + +function historySnapshot() { return { skillIntervals: clone(state.skillIntervals), selected: state.selected }; } +function beforeMutation() { + state.undo.push(historySnapshot()); + state.undo = state.undo.slice(-100); + state.redo = []; +} +function afterMutation() { + state.skillIntervals.sort((a, b) => a.start_frame - b.start_frame || a.end_frame - b.end_frame); + state.version += 1; + state.saveError = ""; + renderSkillIntervals(); + renderDraft(); + clearTimeout(state.saveTimer); + if (!state.conflict) state.saveTimer = setTimeout(() => void save(), state.autosave); +} + +function commit() { + const draft = state.draft; + if (!draft || skillIntervalError(draft)) return; + beforeMutation(); + const skillInterval = { ...draft, id: draft.id || `skill_${crypto.randomUUID()}`, label: draft.label.trim() }; + state.skillIntervals = state.skillIntervals.filter((item) => item.id !== skillInterval.id); + state.skillIntervals.push(skillInterval); + state.selected = skillInterval.id; + state.lastLabel = skillInterval.label; + state.draft = null; + afterMutation(); + $("video-stage").focus({ preventScroll: true }); + message(`已提交“${skillInterval.label}” [${skillInterval.start_frame}, ${skillInterval.end_frame}]。`); +} + +function edit() { + const skillInterval = selected(); + if (!skillInterval) return; + if (state.draft) { message("请先提交或取消当前草稿。", true); return; } + pause(); + state.draft = clone(skillInterval); + renderDraft(); + message("编辑中;提交后替换原标注,Esc 取消。 "); +} +function cancelDraft() { + state.draft = null; + renderDraft(); + $("video-stage").focus({ preventScroll: true }); +} +function removeSelected() { + if (state.draft) { message("请先提交或取消当前草稿。", true); return; } + const skillInterval = selected(); + if (!skillInterval) return; + beforeMutation(); + state.skillIntervals = state.skillIntervals.filter((item) => item.id !== skillInterval.id); + state.selected = null; + afterMutation(); + message(`已删除“${skillInterval.label}”,可撤销。`); +} +function undo(redo = false) { + if (state.draft) { message("请先提交或取消当前草稿。", true); return; } + const source = redo ? state.redo : state.undo; + const destination = redo ? state.undo : state.redo; + if (!source.length) return; + destination.push(historySnapshot()); + const snapshot = source.pop(); + state.skillIntervals = snapshot.skillIntervals; + state.selected = snapshot.selected; + afterMutation(); + message(redo ? "已重做。" : "已撤销。 "); +} + +function selectSkillInterval(id) { + state.selected = id; + renderSkillIntervals(); +} +function renderSelection() { + const skillInterval = selected(); + $("selection-info").textContent = skillInterval ? `${skillInterval.label} · [${skillInterval.start_frame}, ${skillInterval.end_frame}]${skillInterval.note ? ` · ${skillInterval.note}` : ""}` : "选择一段标注以定位或编辑"; + $("to-start").disabled = !skillInterval; + $("to-end").disabled = !skillInterval; + $("edit").disabled = !skillInterval || !!state.draft; + $("delete").disabled = !skillInterval || !!state.draft; + $("undo").disabled = !state.undo.length || !!state.draft; + $("redo").disabled = !state.redo.length || !!state.draft; +} +function renderSkillIntervals() { + $("skill-interval-list").replaceChildren(); + $("timeline-skill-intervals").replaceChildren(); + $("label-suggestions").replaceChildren(); + state.skillIntervals.forEach((skillInterval, index) => { + const row = document.createElement("tr"); + row.className = `annotation-row${state.selected === skillInterval.id ? " selected" : ""}`; + row.tabIndex = 0; + row.setAttribute("aria-label", `${skillInterval.label},${skillInterval.start_frame} 至 ${skillInterval.end_frame} 帧`); + const label = row.insertCell(); + const number = document.createElement("span"); + number.className = "row-number"; + number.textContent = String(index + 1).padStart(2, "0"); + label.append(number, document.createTextNode(skillInterval.label)); + label.title = skillInterval.label; + for (const endpoint of ["start_frame", "end_frame"]) { + const button = document.createElement("button"); + button.textContent = skillInterval[endpoint]; + button.title = `跳转到第 ${skillInterval[endpoint]} 帧`; + button.addEventListener("click", (event) => { + event.stopPropagation(); selectSkillInterval(skillInterval.id); requestFrame(skillInterval[endpoint]); + }); + row.insertCell().append(button); + } + row.insertCell().textContent = skillInterval.end_frame - skillInterval.start_frame + 1; + row.addEventListener("click", () => selectSkillInterval(skillInterval.id)); + row.addEventListener("keydown", (event) => { + if (!state.draft && (event.key === " " || event.key === "Enter")) { event.preventDefault(); event.stopPropagation(); selectSkillInterval(skillInterval.id); } + }); + $("skill-interval-list").append(row); + const block = document.createElement("div"); + block.className = `timeline-skill-interval${state.selected === skillInterval.id ? " selected" : ""}`; + block.dataset.skillId = skillInterval.id; + block.setAttribute("role", "img"); + block.setAttribute("aria-label", `${skillInterval.label},${skillInterval.start_frame} 至 ${skillInterval.end_frame} 帧`); + positionSkillInterval(block, skillInterval.start_frame, skillInterval.end_frame); + const caption = document.createElement("span"); + caption.className = "skill-interval-caption"; + caption.textContent = skillInterval.label; + block.append(caption); + block.title = `${skillInterval.label} [${skillInterval.start_frame}, ${skillInterval.end_frame}]`; + $("timeline-skill-intervals").append(block); + }); + [...new Set(state.skillIntervals.map((skillInterval) => skillInterval.label))].forEach((label) => { + const option = document.createElement("option"); option.value = label; $("label-suggestions").append(option); + }); + $("empty-list").hidden = state.skillIntervals.length > 0; + $("skill-interval-count").textContent = state.skillIntervals.length; + const count = state.skillIntervals.reduce((total, skillInterval) => total + skillInterval.end_frame - skillInterval.start_frame + 1, 0); + const percent = count / (maximum() + 1) * 100; + $("coverage-count").textContent = `${count.toLocaleString()} 帧已标注`; + $("coverage-percent").textContent = `${percent.toFixed(1)}%`; + $("coverage-bar").style.width = `${percent}%`; + renderSelection(); +} + +function renderSave() { + let text = state.persisted ? "已保存" : "尚未保存"; + let kind = state.persisted ? "saved" : ""; + if (dirty()) { text = "未保存"; kind = "dirty"; } + if (state.saving) { text = "保存中…"; kind = "dirty"; } + if (state.saveError) { text = "保存失败"; kind = "error"; } + if (state.draft) text += " · 有草稿"; + $("save-status").textContent = text; + $("save-status").className = `status-pill ${kind}`; + $("save-status").title = state.saveError || "只保存已提交的区间,草稿需先提交。"; +} +async function save() { + if (!state.project) return; + clearTimeout(state.saveTimer); + if (state.saving) return; + if (!dirty() && state.persisted) { + message(state.draft ? "已完成标注均已保存;当前草稿尚未提交。" : "已完成标注均已保存。 "); + return; + } + state.saving = true; + state.saveError = ""; + renderSave(); + try { + do { + const version = state.version; + const skillIntervals = clone(state.skillIntervals); + const response = await checked(await fetch("/api/annotations", { + method: "PUT", headers: { "Content-Type": "application/json", "X-Annotation-Token": state.token }, + body: JSON.stringify({ segments: skillIntervals, expected_revision: state.project.document.revision }), + })); + const document = await response.json(); + state.project.document.revision = document.revision; + state.savedVersion = version; + state.persisted = true; + message(state.draft ? "已保存完成标注;当前草稿尚未提交。" : `已保存 ${skillIntervals.length} 段标注。`); + } while (dirty()); + } catch (error) { + state.saveError = error.message; + if (error.status === 409) state.conflict = true; + message(`保存失败:${error.message} 可使用“导出 JSON”保留当前标注。`, true); + } finally { + state.saving = false; + renderSave(); + } +} +function exportJson() { + if (!state.project) return; + const document = { ...state.project.document, segments: clone(state.skillIntervals) }; + const blob = new Blob([`${JSON.stringify(document, null, 2)}\n`], { type: "application/json" }); + const url = URL.createObjectURL(blob); + const link = window.document.createElement("a"); + link.href = url; + link.download = state.project.annotation_path.split(/[\\/]/).at(-1); + link.click(); + setTimeout(() => URL.revokeObjectURL(url), 1000); + message(state.draft ? "已导出完成标注;草稿未包含在导出中。" : "已导出当前标注副本。 "); +} + +function frameCenterPercent(index) { + return (index + 0.5) / (maximum() + 1) * 100; +} +function positionSkillInterval(element, start, end) { + const first = Math.max(0, Math.min(maximum(), start)); + const last = Math.max(first, Math.min(maximum(), end)); + element.style.left = `${first / (maximum() + 1) * 100}%`; + element.style.width = `${(last - first + 1) / (maximum() + 1) * 100}%`; +} + +function zoom(factor) { + state.zoom = Math.max(1, Math.min(32, factor)); + $("timeline").style.width = `${state.zoom * 100}%`; + $("zoom-value").textContent = `${state.zoom}×`; + $("zoom-out").disabled = state.zoom === 1; + $("zoom-in").disabled = state.zoom === 32; +} +function bindControls() { + $("play").onclick = play; + $("previous").onclick = () => step(-1); + $("next").onclick = () => step(1); + $("mark-start").onclick = () => mark("start_frame"); + $("mark-end").onclick = () => mark("end_frame"); + $("commit").onclick = commit; + $("cancel-draft").onclick = cancelDraft; + $("edit").onclick = edit; + $("delete").onclick = removeSelected; + $("undo").onclick = () => undo(); + $("redo").onclick = () => undo(true); + $("save").onclick = () => void save(); + $("export").onclick = exportJson; + $("to-start").onclick = () => selected() && requestFrame(selected().start_frame); + $("to-end").onclick = () => selected() && requestFrame(selected().end_frame); + $("speed").onchange = () => { state.speed = Number($("speed").value); }; + $("scrubber").oninput = () => requestFrame(Number($("scrubber").value)); + $("goto-frame").addEventListener("keydown", (event) => { + if (event.key === "Enter" && !event.isComposing) { + event.preventDefault(); + requestFrame($("goto-frame").value === "" ? NaN : Number($("goto-frame").value)); + $("video-stage").focus({ preventScroll: true }); + } + }); + for (const [id, key] of [["label", "label"], ["note", "note"], ["start-frame", "start_frame"], ["end-frame", "end_frame"]]) { + $(id).addEventListener("input", () => { + if (state.draft) state.draft[key] = key.endsWith("_frame") ? ($(id).value === "" ? null : Number($(id).value)) : $(id).value; + else if (key === "label") state.lastLabel = $(id).value; + renderDraft(false); + }); + $(id).addEventListener("keydown", (event) => { + if (event.key === "Enter" && !event.isComposing) { event.preventDefault(); $("video-stage").focus({ preventScroll: true }); } + }); + } + $("zoom-in").onclick = () => zoom(state.zoom * 2); + $("zoom-out").onclick = () => zoom(state.zoom / 2); + $("zoom-reset").onclick = () => zoom(1); + const timeline = $("timeline"); + let timelinePointer = null; + const seekTimeline = (event) => { + const bounds = timeline.getBoundingClientRect(); + const index = Math.floor((event.clientX - bounds.left) / bounds.width * (maximum() + 1)); + requestFrame(Math.max(0, Math.min(maximum(), index))); + }; + timeline.addEventListener("pointerdown", (event) => { + if (event.button !== 0 || timelinePointer !== null) return; + event.preventDefault(); + timelinePointer = event.pointerId; + timeline.focus({ preventScroll: true }); + timeline.setPointerCapture(event.pointerId); + const interval = event.target.closest("[data-skill-id]"); + if (interval) selectSkillInterval(interval.dataset.skillId); + seekTimeline(event); + }); + timeline.addEventListener("pointermove", (event) => { + if (event.pointerId === timelinePointer) seekTimeline(event); + }); + const stopTimeline = (event) => { + if (event.pointerId !== timelinePointer) return; + timelinePointer = null; + if (timeline.hasPointerCapture(event.pointerId)) timeline.releasePointerCapture(event.pointerId); + }; + timeline.addEventListener("pointerup", stopTimeline); + timeline.addEventListener("pointercancel", stopTimeline); + timeline.addEventListener("lostpointercapture", stopTimeline); + window.addEventListener("keydown", keyboard); + window.addEventListener("blur", () => pause()); + window.addEventListener("beforeunload", (event) => { + if (dirty() || state.draft) { event.preventDefault(); event.returnValue = ""; } + }); + document.addEventListener("visibilitychange", () => { if (document.hidden) pause(); }); +} + +function keyboard(event) { + if (!state.project || event.isComposing) return; + if ($("help-dialog").open) { + if (event.key === "?") { event.preventDefault(); $("help-dialog").close(); } + return; + } + const key = event.key.toLowerCase(); + const modifier = event.ctrlKey || event.metaKey; + if (modifier && key === "s") { event.preventDefault(); if (!event.repeat) void save(); return; } + if (event.target.closest("textarea, select, [contenteditable='true'], input:not([type='range'])")) return; + if (modifier) { + if (key === "z") { event.preventDefault(); if (!event.repeat) undo(event.shiftKey); } + return; + } + if (event.altKey) return; + if (event.repeat && !["arrowleft", "arrowright"].includes(key)) { event.preventDefault(); return; } + const actions = { + " ": play, + arrowleft: () => step(event.shiftKey ? -10 : -1), + arrowright: () => step(event.shiftKey ? 10 : 1), + home: () => requestFrame(0), end: () => requestFrame(maximum()), + g: () => { $("goto-frame").focus(); $("goto-frame").select(); }, + l: () => $("label").focus(), i: () => mark("start_frame"), o: () => mark("end_frame"), + enter: commit, escape: cancelDraft, e: edit, + delete: removeSelected, backspace: removeSelected, + "?": () => { pause(); $("help-dialog").showModal(); }, + }; + if (actions[key]) { event.preventDefault(); actions[key](); } +} + +$("help").onclick = () => { pause(); $("help-dialog").showModal(); }; +$("close-help").onclick = () => $("help-dialog").close(); +$("cancel-init").onclick = async () => { + $("cancel-init").disabled = true; + try { + await checked(await fetch("/api/cancel", { method: "POST", headers: { "X-Annotation-Token": state.token } })); + } catch (error) { message(error.message, true); } +}; + +async function initialize() { + try { + const status = await (await checked(await fetch("/api/status"))).json(); + state.token = status.session_token; + if (status.phase === "error" || status.phase === "cancelled" || status.phase === "closed") { + $("init-title").textContent = status.phase === "error" ? "无法打开标注项目" : "初始化已取消"; + $("init-message").textContent = status.error || "关闭此页后,可从命令行重新打开图像序列。"; + $("init-progress").hidden = true; + $("cancel-init").hidden = true; + message(status.error || "请从命令行重新打开图像序列。", true); + return; + } + if (status.phase !== "ready") { + $("init-title").textContent = status.phase === "hashing" ? "正在读取图像来源" : "正在建立帧索引"; + $("init-message").textContent = status.phase === "indexing" ? `已扫描 ${status.completed.toLocaleString()} 帧;完成后可准确逐帧标注。` : "正在校验图像来源,请稍候。"; + if (status.total > 0) { $("init-progress").max = status.total; $("init-progress").value = status.completed; } + else $("init-progress").removeAttribute("value"); + setTimeout(() => void initialize(), 350); + return; + } + const project = await (await checked(await fetch("/api/project"))).json(); + state.project = project; + state.token = project.session_token; + state.autosave = project.autosave_ms; + state.skillIntervals = clone(project.document.segments); + state.persisted = project.persisted; + const source = project.document.source; + $("video-name").textContent = project.source_name; + const fps = source.fps ? `${source.fps.toFixed(2)} fps` : ""; + const timing = source.timing === "fixed_fps" ? " · 时间按 FPS 推算" : " · 视频 PTS"; + $("video-meta").textContent = `${source.width} × ${source.height} · ${source.frame_count.toLocaleString()} 帧${fps ? ` · ${fps}` : ""}${timing}`; + $("annotation-path").textContent = project.annotation_path; + $("frame-last").textContent = maximum(); + for (const id of ["scrubber", "goto-frame", "start-frame", "end-frame"]) $(id).max = maximum(); + $("save").disabled = false; + $("export").disabled = false; + $("initialization").hidden = true; + $("workspace").hidden = false; + document.title = `${project.source_name} · 原子技能区间标注`; + $("timeline").setAttribute("aria-valuemax", String(maximum())); + const tickFrames = [...new Set(Array.from({ length: 6 }, (_, index) => Math.round(index / 5 * maximum())))]; + $("timeline-ticks").replaceChildren(...tickFrames.map((frame) => { + const tick = document.createElement("span"); + tick.className = "timeline-tick"; + tick.style.left = `${frameCenterPercent(frame)}%`; + const caption = document.createElement("span"); + caption.textContent = frame; + tick.append(caption); + return tick; + })); + bindControls(); zoom(1); renderSkillIntervals(); renderDraft(); requestFrame(0); + message("已就绪。定位起点按 I,终点按 O,填写名称后按 Enter 提交。 "); + $("video-stage").focus({ preventScroll: true }); + } catch (error) { + $("init-title").textContent = "连接标注服务失败"; + $("init-message").textContent = `${error.message}。确认命令行中的服务仍在运行后刷新页面。`; + $("init-progress").hidden = true; + message(error.message, true); + } +} +void initialize(); diff --git a/embodichain/toolkits/trajectory_annotator/web/index.html b/embodichain/toolkits/trajectory_annotator/web/index.html new file mode 100644 index 0000000000..a7d3e9929a --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/web/index.html @@ -0,0 +1,85 @@ + + + + + + + + 原子技能区间标注 · EmbodiChain + + + + +
+ +
EMBODICHAIN · TOOLKITS

原子技能区间标注

+
+ ←前一帧→后一帧空格播放 / 暂停I标注起始帧O标注结束帧 +
+
等待图像
+
+ +
+
+

正在打开图像序列

+

建立逐帧索引后即可开始标注。

+ + +
+ + + +
正在连接本地标注服务…Space 播放 · I / O 标记 · Enter 提交 · ? 帮助
+
+ +

键盘快捷键

Space
播放 / 暂停
← →
前一帧 / 后一帧;Shift 跨 10 帧
Home End
首帧 / 末帧
G
输入帧号跳转
I O
设置当前显示帧为起点 / 终点
L
输入原子技能名称
Enter Esc
提交 / 取消草稿
E
编辑选中的标注
Delete
删除选中的标注(Backspace 也可)
Ctrl/Cmd Z
撤销;加 Shift 重做
Ctrl/Cmd S
保存已完成标注

在文本框中输入时,保留文字编辑快捷键。名称输入后按 Enter 返回视频区,再按 Enter 提交区间。起止帧都包含在标注中。

+ + diff --git a/embodichain/toolkits/trajectory_annotator/web/styles.css b/embodichain/toolkits/trajectory_annotator/web/styles.css new file mode 100644 index 0000000000..5865d70e92 --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/web/styles.css @@ -0,0 +1,156 @@ +/* Copyright (c) 2021-2026 DexForce Technology Co., Ltd. Licensed under Apache-2.0. */ +:root { color-scheme: dark; --bg:#0c1018; --panel:#151c28; --border:#293346; --text:#e8edf6; --muted:#8f9eb5; --accent:#66d5bc; --blue:#70a7ff; --danger:#ff8998; font:14px/1.5 system-ui,-apple-system,BlinkMacSystemFont,"Segoe UI","Noto Sans CJK SC",sans-serif; } +* { box-sizing:border-box; } +body { margin:0; background:var(--bg); color:var(--text); } +[hidden] { display:none !important; } +button,input,select { font:inherit; } +button { color:var(--text); background:#202a3a; border:1px solid #364258; border-radius:7px; padding:7px 12px; cursor:pointer; transition:background .15s,border-color .15s; white-space:nowrap; } +button:hover:not(:disabled) { background:#2b3a50; border-color:#65809d; } +button:disabled { opacity:.38; cursor:default; } +button.primary { background:var(--accent); border-color:var(--accent); color:#092e27; font-weight:650; } +button.primary:hover:not(:disabled) { background:#91e7d3; border-color:#91e7d3; } +button.danger { color:var(--danger); } +button.danger:hover:not(:disabled) { background:#492736; border-color:#a65368; } +:focus-visible { outline:2px solid var(--accent); outline-offset:3px; } +input,select { color:var(--text); background:#0e1521; border:1px solid #34425a; border-radius:6px; padding:8px 10px; min-width:0; } +input:disabled { color:var(--muted); opacity:.6; } +input::placeholder { color:#65738b; } +input[type=number] { font-variant-numeric:tabular-nums; } +input[type=range] { padding:0; accent-color:var(--accent); cursor:pointer; } +kbd { font:11px/1.4 inherit; color:#b1c1d7; border:1px solid #455168; border-radius:4px; padding:1px 5px; margin-left:5px; white-space:nowrap; } +.primary kbd { color:#174e43; border-color:#31816f; } +h1,h2,p { margin:0; } +h1 { font-size:19px; letter-spacing:.02em; } +h2 { font-size:14px; font-weight:650; } +.muted { color:var(--muted); font-size:12px; } +.small { font-weight:400; font-size:11px; margin-left:10px; } +.eyebrow { color:var(--muted); font-size:10px; font-weight:650; letter-spacing:.13em; } +.app-header { min-height:90px; padding:18px 30px; display:grid; grid-template-columns:auto auto minmax(0,1fr) auto; gap:14px; align-items:center; border-bottom:1px solid var(--border); background:#111824; } +.brand-mark { width:42px; height:42px; color:var(--accent); background:#1a3936; border:1px solid #285a51; border-radius:11px; padding:7px; } +.brand-mark svg { width:100%; height:100%; } +.brand .eyebrow { font-size:9px; } +.header-shortcuts { display:flex; flex-wrap:wrap; align-items:center; justify-content:center; gap:8px 15px; font-size:11px; color:var(--muted); } +.header-shortcuts span { display:flex; align-items:center; gap:6px; white-space:nowrap; } +.header-shortcuts kbd { margin:0; color:#d4e7f6; font-size:11px; } +.header-actions { display:flex; align-items:center; gap:9px; margin-left:auto; } +.icon-button { width:34px; height:34px; padding:3px; font-size:18px; line-height:1; } +.status-pill { color:var(--muted); border:1px solid var(--border); background:#101825; border-radius:20px; padding:4px 11px; font-size:11px; margin-right:5px; } +.status-pill.saved { color:var(--accent); border-color:#2f594e; } +.status-pill.error { color:var(--danger); border-color:#77404c; } +.status-pill.dirty { color:#edc58a; border-color:#655136; } +main { max-width:1680px; padding:22px 28px 12px; margin:auto; } +.workspace { display:grid; grid-template-columns:minmax(0,1fr) 360px; gap:18px; align-items:start; } +.media-column,.annotation-column { display:grid; gap:16px; min-width:0; } +.card { background:var(--panel); border:1px solid var(--border); border-radius:11px; overflow:hidden; } +.card-heading { min-height:50px; padding:11px 17px; display:flex; align-items:center; justify-content:space-between; gap:10px; } +.file-heading { display:flex; align-items:center; gap:8px; min-width:0; } +.file-heading h2 { white-space:nowrap; overflow:hidden; text-overflow:ellipsis; } +.live-dot { width:7px; height:7px; background:var(--accent); border-radius:50%; flex:none; } +.video-stage { background:#070a10; position:relative; aspect-ratio:16/9; max-height:51vh; min-height:200px; overflow:hidden; display:flex; align-items:center; justify-content:center; } +.video-stage img { display:block; width:100%; height:100%; object-fit:contain; } +.frame-overlay { position:absolute; inset:auto 14px 12px; display:flex; justify-content:space-between; align-items:center; pointer-events:none; font-size:11px; } +.frame-overlay>span { border:1px solid #ffffff23; border-radius:5px; background:#070c14d9; padding:4px 8px; } +.frame-badge { letter-spacing:.03em; color:#9dacc1; } +.frame-badge strong { color:#fff; margin-left:5px; font-size:13px; font-variant-numeric:tabular-nums; } +.frame-badge .dim { color:#73849f; } +.loading-frame { position:absolute; inset:12px auto auto 14px; z-index:1; background:#182839ed; color:var(--accent); padding:5px 10px; border-radius:5px; font-size:12px; } +.transport { display:flex; align-items:center; gap:16px; padding:11px 16px; } +.transport-buttons { display:flex; align-items:center; gap:6px; } +.play-button { min-width:94px; } +.timecode { font:13px ui-monospace,SFMono-Regular,Consolas,monospace; color:#c3cfe0; } +.speed-label { display:flex; align-items:center; gap:8px; font-size:12px; color:var(--muted); margin-left:auto; } +.speed-label select { padding:4px 7px; font-size:12px; } +.zoom-controls { display:flex; align-items:center; gap:6px; font-size:11px; } +.zoom-controls button { padding:2px 8px; font-size:12px; } +.zoom-controls span { width:25px; text-align:center; color:var(--muted); } +.scrubber-row { display:flex; align-items:center; gap:15px; padding:0 17px 9px; } +#scrubber { flex:1; min-width:40px; } +.jump-label { display:flex; gap:7px; align-items:center; white-space:nowrap; font-size:11px; color:var(--muted); } +.jump-label input { width:87px; padding:4px 6px; font-size:12px; } +.timeline-scroll { overflow-x:auto; margin:0 17px; border:1px solid #2b374b; border-radius:6px; background:#101724; scrollbar-color:#435771 #111a29; } +.timeline { user-select:none; position:relative; height:84px; width:100%; min-width:100%; cursor:crosshair; touch-action:none; background:repeating-linear-gradient(90deg,transparent 0,transparent calc(10% - 1px),#34405655 calc(10% - 1px),#34405655 10%); } +.timeline-ticks { position:absolute; top:4px; left:0; right:0; height:20px; pointer-events:none; font:10px ui-monospace,SFMono-Regular,Consolas,monospace; color:#7688a3; } +.timeline-tick { position:absolute; width:0; } +.timeline-tick::before { content:""; position:absolute; top:15px; height:5px; border-left:1px solid #65758d; } +.timeline-tick>span { position:absolute; transform:translateX(-50%); } +.timeline-tick:first-child>span { transform:none; } +.timeline-tick:last-child>span { transform:translateX(-100%); } +/* Geometry belongs to the frame cells; captions must never widen a short interval. */ +.timeline-skill-interval,.draft-range { position:absolute; top:31px; height:30px; min-width:0; padding:0; border:0; border-radius:3px; overflow:hidden; background:#264a70; box-shadow:inset 0 0 0 1px #48759d; color:#c8e1ff; font-size:11px; text-align:left; cursor:inherit; } +.skill-interval-caption { position:absolute; inset:0; padding:4px 7px; overflow:hidden; text-overflow:ellipsis; white-space:nowrap; pointer-events:none; } +.timeline-skill-interval.selected { background:#2d756b; box-shadow:inset 0 0 0 1px var(--accent); color:#edfff9; } +.draft-range { background:#edc17320; box-shadow:inset 0 0 0 1px #e6bc78; color:#f0cd94; pointer-events:none; z-index:2; } +.playhead { position:absolute; width:1px; top:20px; bottom:7px; background:var(--accent); pointer-events:none; z-index:3; } +.playhead span { position:absolute; top:-4px; left:-4px; width:9px; height:9px; transform:rotate(45deg); background:var(--accent); } +.timeline-legend { display:flex; align-items:center; gap:14px; padding:10px 17px; font-size:10px; color:var(--muted); } +.timeline-legend span { display:flex; align-items:center; gap:5px; } +.timeline-legend>span:last-child { margin-left:auto; font-size:10px; } +.timeline-legend i { width:8px; height:8px; border-radius:2px; } +.legend-saved { background:#4c82b6; } +.legend-draft { border:1px dashed #e6bc78; } +.editor-grid { display:grid; grid-template-columns:minmax(0,1fr) minmax(0,1fr) minmax(48px,.8fr); gap:10px; padding:0 17px; } +.editor-grid label,.note-field { display:flex; flex-direction:column; gap:6px; font-size:11px; color:var(--muted); } +.editor-grid input { width:100%; } +.label-field { grid-column:1/-1; } +.duration-field { display:flex; flex-direction:column; gap:7px; font-size:11px; color:var(--muted); } +.duration-field strong { font-size:20px; color:var(--accent); font-variant-numeric:tabular-nums; font-weight:500; } +.duration-field small { font-size:11px; } +.note-field { margin:12px 17px 0; position:relative; } +.note-field .muted { position:absolute; left:32px; top:0; font-size:10px; color:#65758d; } +.note-field input { font-size:12px; padding:7px 10px; } +.draft-error { min-height:27px; padding:5px 17px 2px; font-size:11px; color:#eabf85; } +.editor-actions { padding:4px 17px 15px; display:grid; grid-template-columns:repeat(2,minmax(0,1fr)); gap:8px; align-items:center; } +.editor-actions button { font-size:12px; padding:7px 10px; } +.editor-actions .spacer { display:none; } +.annotations-card { display:flex; flex-direction:column; min-height:320px; } +.count-badge { color:var(--accent); background:#21423d; border-radius:5px; padding:1px 6px; margin-left:5px; font-size:11px; } +.history-buttons { display:flex; gap:5px; } +.history-buttons button { width:28px; height:28px; } +.coverage { padding:0 17px 16px; font-size:11px; color:var(--muted); border-bottom:1px solid var(--border); } +.coverage>div:first-child { display:flex; justify-content:space-between; margin-bottom:7px; } +.coverage-track { height:4px; background:#293447; border-radius:3px; overflow:hidden; } +#coverage-bar { height:100%; width:0; background:var(--accent); transition:width .2s; } +.annotation-table-wrap { min-height:150px; max-height:320px; overflow:auto; flex:1; } +table { width:100%; border-collapse:collapse; table-layout:fixed; } +th { position:sticky; top:0; z-index:1; background:#192231; font-weight:450; color:var(--muted); font-size:11px; text-align:right; padding:9px 12px 9px 3px; } +th:first-child { width:47%; padding-left:17px; text-align:left; } +td { padding:11px 12px 11px 3px; border-bottom:1px solid #263146; text-align:right; font-size:12px; font-variant-numeric:tabular-nums; } +td:first-child { padding-left:17px; text-align:left; overflow:hidden; text-overflow:ellipsis; white-space:nowrap; } +tr.annotation-row { cursor:pointer; } +tr.annotation-row:hover { background:#1b2a3c; } +tr.annotation-row.selected { background:#203d3e; box-shadow:inset 3px 0 var(--accent); } +td button { border:0; background:none; color:#a3c7f2; padding:2px 0; font-size:12px; } +td button:hover:not(:disabled) { background:none; text-decoration:underline; } +.row-number { font-size:10px; color:#788fa6; margin-right:8px; } +.empty-list { text-align:center; color:var(--muted); padding:35px 10px; font-size:12px; } +.empty-list>span { font-size:37px; color:#4a687c; display:block; } +.empty-list strong { font-size:13px; color:#bdcadb; font-weight:500; } +.empty-list p { margin-top:9px; line-height:1.9; font-size:11px; } +.selection-info { font-size:11px; color:var(--muted); border-top:1px solid var(--border); padding:12px 17px 8px; min-height:36px; overflow-wrap:anywhere; } +.selection-actions { display:grid; grid-template-columns:1fr 1fr 1.1fr .8fr; gap:6px; padding:0 17px 16px; } +.selection-actions button { font-size:11px; padding:6px 5px; } +.selection-actions kbd { font-size:10px; margin-left:2px; } +.annotation-path { display:flex; flex-direction:column; gap:4px; padding:14px 17px; border-top:1px solid var(--border); background:#121a26; } +#annotation-path { font:10px/1.6 ui-monospace,SFMono-Regular,Consolas,monospace; color:#9cafc7; overflow-wrap:anywhere; } +.annotation-path small { color:#677b98; font-size:10px; margin-top:4px; } +.app-footer { padding:17px 1px 3px; display:flex; gap:20px; justify-content:space-between; font-size:11px; color:var(--muted); } +.app-footer>span:last-child { font-size:10px; color:#64738a; white-space:nowrap; } +#message.error { color:var(--danger); } +.init-panel { text-align:center; padding:75px 20px; margin:0 auto; max-width:660px; } +.init-icon { color:var(--accent); font-size:50px; } +.init-panel h2 { font-size:20px; margin:10px 0; } +.init-panel p { color:var(--muted); margin-bottom:20px; white-space:pre-wrap; overflow-wrap:anywhere; } +.init-panel progress { display:block; width:280px; height:6px; margin:20px auto; accent-color:var(--accent); } +.init-panel button { margin-top:8px; } +dialog { width:min(560px,90vw); color:var(--text); background:var(--panel); border:1px solid #3b4a61; border-radius:12px; padding:22px; } +dialog::backdrop { background:#000a; backdrop-filter:blur(3px); } +.dialog-heading { display:flex; align-items:center; justify-content:space-between; } +.dialog-heading h2 { font-size:17px; } +.shortcut-list { display:grid; grid-template-columns:180px 1fr; gap:12px 10px; font-size:12px; margin:24px 0; } +.shortcut-list dt kbd { margin-left:0; } +.shortcut-list dd { margin:0; color:#bdcce0; } +dialog>p { font-size:11px; line-height:1.8; } +@media (min-width:1500px) { .workspace { grid-template-columns:minmax(0,1fr) 400px; } .video-stage { max-height:55vh; } } +@media (max-width:1100px) { main { padding:16px; } .app-header { padding:15px 18px; } .workspace { grid-template-columns:minmax(0,1fr) 320px; gap:12px; } .small { display:none; } .fps { display:none; } .transport { gap:12px; } .editor-actions kbd { display:none; } } +@media (max-width:850px) { .app-header { grid-template-columns:auto minmax(0,1fr) auto; } .header-actions { grid-column:3; grid-row:1; } .header-shortcuts { grid-column:1/-1; justify-content:flex-start; } .workspace { grid-template-columns:1fr; } .annotations-card { position:static; max-height:none; min-height:350px; } .annotation-table-wrap { max-height:350px; } .app-header { gap:10px; } .header-actions { margin-left:auto; } .eyebrow { font-size:8px; } .brand h1 { font-size:16px; } .brand-mark { width:35px; height:35px; } .status-pill { margin-right:0; } .header-actions button { font-size:11px; } .app-footer>span:last-child { display:none; } .video-stage { max-height:55vh; } } +@media (max-width:480px) { .app-header { grid-template-columns:auto minmax(0,1fr); } .header-actions { grid-column:1/-1; grid-row:auto; width:100%; justify-content:flex-end; } .editor-grid { grid-template-columns:1fr 1fr 1fr; gap:8px; } .label-field { grid-column:1/-1; } .editor-actions .spacer { display:none; } .timeline-legend>span:last-child { display:none; } .shortcut-list { grid-template-columns:140px 1fr; } #video-meta { display:none; } } diff --git a/examples/toolkits/trajectory_annotation/README.md b/examples/toolkits/trajectory_annotation/README.md new file mode 100644 index 0000000000..047124ebfb --- /dev/null +++ b/examples/toolkits/trajectory_annotation/README.md @@ -0,0 +1,76 @@ +# 遥操轨迹原子技能标注示例 + +在仓库根目录运行,默认绑定 `127.0.0.1`,自动选择端口并打开浏览器。服务运行期间保留终端,结束时按 Ctrl+C。 + +```bash +conda activate py312 +# 若尚未安装网页端依赖: +python -m pip install -e '.[trajectory-annotation]' + +# 使用本机提供的 episode.hdf5,无需另外传入参数 +embodichain run-example toolkits/trajectory_annotation +``` + +默认读取 `/home/oem/piper-sim-data/label_down/episode_20261022T073344_323022Z/episode.hdf5`, +自动定位实际的 `observations/images/cam_high`,共 3644 帧、640×480,从 `metadata_json.record_fps` 读取 30 FPS。 +标注保存到 `outputs/trajectory_annotation/episode.cam_high.segments.json`;原始 HDF5 只读。 +首次操作前不会创建标注文件,再次运行会恢复已有标注。 + +通过可选参数指定 HDF5 路径和 JSON 输出文件夹: + +```bash +embodichain run-example toolkits/trajectory_annotation \ + --hdf5 /path/to/episode.hdf5 \ + --output-dir /path/to/annotations +``` + +- `--hdf5`:可选,默认读取上述本机 demo。 +- `--output-dir`:可选,默认 `outputs/trajectory_annotation`;首次保存时自动创建目录。 +- JSON 文件名自动使用 `.<相机名>.segments.json`。例如 `episode.hdf5`、`cam_high` 对应 `episode.cam_high.segments.json`。 +- `--annotations`:也可直接指定完整 JSON 路径,与显式传入 `--output-dir` 二选一。 + +可进一步指定相机和帧率: + +```bash +embodichain run-example toolkits/trajectory_annotation -- \ + --hdf5 /path/to/episode.hdf5 \ + --image-key observations/images/cam_high --fps 30 \ + --output-dir outputs/trajectory_annotation/my_labels +``` + +也可以直接使用正式 CLI: + +```bash +embodichain annotate-trajectory \ + --hdf5 /home/oem/piper-sim-data/label_down/episode_20261022T073344_323022Z/episode.hdf5 \ + --annotations outputs/trajectory_annotation/episode.cam_high.segments.json +``` + +操作流程:←/→ 逐帧,Space 播放/暂停,I 标起点,O 标终点,填写原子技能名称后 Enter 提交。 +右侧显示全部区间,点击帧号跳转,选中后 Delete 删除,Ctrl+Z 撤销,Ctrl+S 保存。 +帧号从 0 开始,起止均包含;例如 `[100, 150]` 包含 51 帧。HDF5 的时间显示按 FPS 推算。 + +## 输入已有图像数组 + +```bash +# 独立演示:90 张 RGB 图像,不依赖本机 HDF5 demo +embodichain run-example toolkits/trajectory_annotation/images +``` + +接入已有代码只需一个帧源工厂: + +```python +from embodichain.toolkits.trajectory_annotator.app import launch_annotation +from embodichain.toolkits.trajectory_annotator.frames import ArrayFrameSource + +# images: N×H×W×3 uint8 RGB 数组,或同尺寸 RGB 图像列表 +launch_annotation( + lambda: ArrayFrameSource(images, fps=30, name="my_episode"), + "outputs/my_episode.segments.json", +) +``` + +调用方须在应用关闭前保持数组不变。大规模或按需读取的数据可实现 +[`FrameSource`](../../../embodichain/toolkits/trajectory_annotator/frames.py) 协议,返回单张 `ImageFrame`; +网页、预览缓存和标注存储均不依赖 HDF5 或视频格式。 +详见[工具说明](../../../embodichain/toolkits/trajectory_annotator/README.md)。 diff --git a/examples/toolkits/trajectory_annotation/__main__.py b/examples/toolkits/trajectory_annotation/__main__.py new file mode 100644 index 0000000000..e58017dbfc --- /dev/null +++ b/examples/toolkits/trajectory_annotation/__main__.py @@ -0,0 +1,90 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Annotate the supplied cam_high HDF5 demo through the common frame interface. + +Run from the repository root in conda environment py312: + embodichain run-example toolkits/trajectory_annotation +Override --hdf5 and --output-dir for another recording and output directory. +Use --image-key or --fps to override camera selection and timing. Ctrl+C stops the app. +""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +from embodichain.toolkits.trajectory_annotator.app import launch_annotation +from embodichain.toolkits.trajectory_annotator.hdf5 import Hdf5FrameSource + + +def main() -> None: + """Launch cam_high annotation without converting or modifying the recording.""" + parser = argparse.ArgumentParser( + prog="embodichain run-example toolkits/trajectory_annotation", + description=__doc__, + formatter_class=argparse.ArgumentDefaultsHelpFormatter, + ) + parser.add_argument( + "--hdf5", + type=Path, + default=Path( + "/home/oem/piper-sim-data/label_down/episode_20261022T073344_323022Z/episode.hdf5" + ), + help="HDF5 recording; omit to use the supplied local demo.", + ) + parser.add_argument( + "--image-key", default=None, help="Auto-detect cam_high when omitted." + ) + parser.add_argument( + "--fps", type=float, default=None, help="Default: recorded FPS metadata." + ) + output = parser.add_mutually_exclusive_group() + output.add_argument( + "--output-dir", + type=Path, + default=Path("outputs/trajectory_annotation"), + help="Directory for ..segments.json; created on save.", + ) + output.add_argument( + "--annotations", + type=Path, + help="Explicit annotation JSON path, as an alternative to --output-dir.", + ) + parser.add_argument("--port", type=int, default=0) + parser.add_argument("--no-browser", action="store_true") + args = parser.parse_args() + if not args.hdf5.is_file(): + parser.error(f"HDF5 file does not exist: {args.hdf5}") + if args.image_key is not None and not args.image_key.strip("/"): + parser.error("--image-key must name an image dataset.") + if args.annotations is None: + if args.output_dir.exists() and not args.output_dir.is_dir(): + parser.error(f"--output-dir must be a directory: {args.output_dir}") + camera = (args.image_key or "cam_high").strip("/").rsplit("/", 1)[-1] + annotations = args.output_dir / f"{args.hdf5.stem}.{camera}.segments.json" + else: + annotations = args.annotations + launch_annotation( + lambda: Hdf5FrameSource(args.hdf5, image_key=args.image_key, fps=args.fps), + annotations, + port=args.port, + open_browser=not args.no_browser, + ) + + +if __name__ == "__main__": + main() diff --git a/examples/toolkits/trajectory_annotation/images.py b/examples/toolkits/trajectory_annotation/images.py new file mode 100644 index 0000000000..2139a9befb --- /dev/null +++ b/examples/toolkits/trajectory_annotation/images.py @@ -0,0 +1,59 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Annotate an existing RGB image array, independent of HDF5 and video codecs. + +Run: embodichain run-example toolkits/trajectory_annotation/images +Replace the generated images with your N×H×W×3 RGB uint8 array or image list. +""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +import numpy as np + +from embodichain.toolkits.trajectory_annotator.app import launch_annotation +from embodichain.toolkits.trajectory_annotator.frames import ArrayFrameSource + + +def main() -> None: + """Build a small moving-square sequence and pass its array to the annotator.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--annotations", + type=Path, + default=Path("outputs/trajectory_annotation/images.segments.json"), + ) + parser.add_argument("--port", type=int, default=0) + parser.add_argument("--no-browser", action="store_true") + args = parser.parse_args() + images = np.full((90, 160, 320, 3), (20, 28, 38), dtype=np.uint8) + for index, image in enumerate(images): + x = 12 + index * 3 + image[55:105, x : x + 32] = (45, 210, 160) + # Caller owns the arrays and keeps them unchanged until launch_annotation returns. + launch_annotation( + lambda: ArrayFrameSource(images, fps=30, name="Moving square RGB frames"), + args.annotations, + port=args.port, + open_browser=not args.no_browser, + ) + + +if __name__ == "__main__": + main() diff --git a/pyproject.toml b/pyproject.toml index 389e6b12df..56820b41e1 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -71,6 +71,11 @@ dependencies = [ ] [project.optional-dependencies] +trajectory-annotation = [ + "av>=15.1,<18", + "fastapi>=0.115,<1", + "uvicorn>=0.30,<1", +] geometry = [ # Optional mesh inputs for pure numerical geometry; no simulation/FCL backend. "trimesh>=4.9", diff --git a/scripts/benchmark/toolkits/trajectory_annotation.py b/scripts/benchmark/toolkits/trajectory_annotation.py new file mode 100644 index 0000000000..92fd4d0836 --- /dev/null +++ b/scripts/benchmark/toolkits/trajectory_annotation.py @@ -0,0 +1,270 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Measure the CPU frame path on a real video or HDF5 recording, writing one Markdown report. + +Run: python -m scripts.benchmark.toolkits.trajectory_annotation --video episode.mp4 +Setup includes hashing and adapter preparation (a full PTS scan for video). Repeated calls measure completed wall latency; +no HTTP/browser work is timed. Inputs are unchanged and no precision variants or +speedup claims are compared. RSS sampling includes native decoder allocations. +""" + +from __future__ import annotations + +import argparse +from collections.abc import Callable +from datetime import datetime +from importlib.metadata import version +from pathlib import Path +import platform +import random +import statistics +import threading +import time +from typing import Any + +import av +import psutil + +from embodichain.toolkits.trajectory_annotator._cfg import AnnotationCfg +from embodichain.toolkits.trajectory_annotator._frames import FrameReader + + +def measure( + operation: Callable[[], object], repeats: int, check: Callable[[object], bool] +) -> tuple[float, float, int]: + """Measure median/P95 completed-call milliseconds after caller warmup.""" + samples: list[float] = [] + succeeded = 0 + for _ in range(repeats): + started = time.perf_counter() + result = operation() + samples.append((time.perf_counter() - started) * 1000) + succeeded += int(check(result)) + del result + return ( + statistics.median(samples), + sorted(samples)[min(repeats - 1, int(repeats * 0.95))], + succeeded, + ) + + +def markdown_table(rows: list[dict[str, Any]]) -> str: + """Render one table with a fixed header derived from its first row.""" + headers = list(rows[0]) + lines = [ + "| " + " | ".join(headers) + " |", + "| " + " | ".join("---" for _ in headers) + " |", + ] + lines.extend( + "| " + " | ".join(str(row[header]) for header in headers) + " |" for row in rows + ) + return "\n".join(lines) + + +def run_all_benchmarks() -> None: + """Measure initialization, uncached/random, cached and sequential previews.""" + parser = argparse.ArgumentParser(description=__doc__) + inputs = parser.add_mutually_exclusive_group(required=True) + inputs.add_argument("--video", type=Path) + inputs.add_argument("--hdf5", type=Path) + parser.add_argument("--image-key") + parser.add_argument("--fps", type=float) + parser.add_argument("--repeats", type=int, default=50) + parser.add_argument("--output", type=Path) + args = parser.parse_args() + if args.repeats < 2: + parser.error("--repeats must be at least 2") + process = psutil.Process() + mib = 1024**2 + baseline = process.memory_info().rss + peak = [baseline] + stopped = threading.Event() + + def sample_rss() -> None: + while not stopped.wait(0.01): + peak[0] = max(peak[0], process.memory_info().rss) + + monitor = threading.Thread(target=sample_rss, daemon=True) + monitor.start() + cfg = AnnotationCfg(cache_bytes=0) + input_path = args.video or args.hdf5 + if args.video: + from embodichain.toolkits.trajectory_annotator.video import VideoFrameSource + + adapter = VideoFrameSource(args.video) + else: + from embodichain.toolkits.trajectory_annotator.hdf5 import Hdf5FrameSource + + adapter = Hdf5FrameSource(args.hdf5, image_key=args.image_key, fps=args.fps) + reader = FrameReader(adapter, cfg) + rows: list[dict[str, Any]] = [] + succeeded = attempts = 0 + try: + started = time.perf_counter() + source = reader.prepare() + index_ms = (time.perf_counter() - started) * 1000 + index_rss = process.memory_info().rss + rows.append( + dict( + phase="cold source preparation", + cost_time_ms=round(index_ms, 3), + p95_ms="N/A", + cpu_delta_mb=round((index_rss - baseline) / mib, 3), + gpu_delta_mb="N/A", + peak_gpu_mb="N/A", + ) + ) + rng = random.Random(1024) + indices = [rng.randrange(source.frame_count) for _ in range(args.repeats)] + + def query(index: int) -> tuple[int, object]: + return index, reader.preview(index) + + def check(result: Any) -> bool: + index, preview = result + if source.kind == "video": + return preview.index == index and preview.pts == adapter.pts[index] + return preview.index == index and preview.time_seconds == index / source.fps + + query(indices[0]) # Warm JPEG/native conversion; cache remains disabled. + selection = iter(indices) + before = process.memory_info().rss + median, p95, passed = measure( + lambda: query(next(selection)), args.repeats, check + ) + succeeded += passed + attempts += args.repeats + rows.append( + dict( + phase="uncached random preview", + cost_time_ms=round(median, 3), + p95_ms=round(p95, 3), + cpu_delta_mb=round((process.memory_info().rss - before) / mib, 3), + gpu_delta_mb="N/A", + peak_gpu_mb="N/A", + ) + ) + cfg.cache_bytes = 64 * mib + query(indices[0]) + before = process.memory_info().rss + median, p95, passed = measure(lambda: query(indices[0]), args.repeats, check) + succeeded += passed + attempts += args.repeats + rows.append( + dict( + phase="cached frame", + cost_time_ms=round(median, 3), + p95_ms=round(p95, 3), + cpu_delta_mb=round((process.memory_info().rss - before) / mib, 3), + gpu_delta_mb="N/A", + peak_gpu_mb="N/A", + ) + ) + cfg.cache_bytes = 0 + reader.cache.clear() + reader.cache_bytes = 0 + count = min(source.frame_count, args.repeats) + query(0) + sequential = iter(range(count)) + before = process.memory_info().rss + median, p95, passed = measure(lambda: query(next(sequential)), count, check) + succeeded += passed + attempts += count + rows.append( + dict( + phase="sequential preview", + cost_time_ms=round(median, 3), + p95_ms=round(p95, 3), + cpu_delta_mb=round((process.memory_info().rss - before) / mib, 3), + gpu_delta_mb="N/A", + peak_gpu_mb="N/A", + ) + ) + packed_bytes = ( + len(adapter.pts) * (adapter.pts.itemsize + adapter.anchors.itemsize) + if source.kind == "video" + else 0 + ) + algorithm = f"{source.kind} CPU" + metrics = [ + dict( + algorithm=algorithm, + success_rate=f"{succeeded / attempts:.2%}", + checked_requests=attempts, + frames=source.frame_count, + packed_index_mib=round(packed_bytes / mib, 3), + jpeg_cache_mib=round(reader.cache_bytes / mib, 3), + ) + ] + finally: + stopped.set() + monitor.join() + reader.close() + cpu = platform.processor() + if Path("/proc/cpuinfo").exists(): + cpu = next( + ( + line.split(":", 1)[1].strip() + for line in Path("/proc/cpuinfo").read_text().splitlines() + if line.startswith("model name") + ), + cpu, + ) + report = ( + args.output + or Path("outputs/benchmarks") + / f"trajectory_annotation_{datetime.now():%Y%m%d_%H%M%S}.md" + ) + report.parent.mkdir(parents=True, exist_ok=True) + report.write_text( + "\n\n".join( + [ + "# Trajectory annotation CPU benchmark", + f"Input: `{input_path.resolve()}`\n\nSource: `{source}`\n\n" + f"CPU: {cpu}; OS: {platform.platform()}; Python: {platform.python_version()}; " + f"PyAV: {av.__version__}; h5py: {version('h5py')}; OpenCV: {version('opencv-python')}; FFmpeg: {av.library_versions}. " + f"Decoder threads: {cfg.decoder_threads}; preview edge: {cfg.preview_edge}; JPEG quality: {cfg.jpeg_quality}. " + f"Seed: 1024; repeats: {args.repeats}; warmup: one call per frame path. " + f"Sampled process peak RSS (10 ms sampling): {peak[0] / mib:.2f} MiB; " + f"increase over pre-index runtime: {(peak[0] - baseline) / mib:.2f} MiB. " + "Sampling can miss transient peaks. CPU deltas below are retained RSS changes, not peak allocations. " + "Initialization includes hashing; video scans all PTS, HDF5 decodes only the selected first image. Frame paths include resize/JPEG encoding on misses. " + "HTTP, browser rendering, input generation and logging are excluded. " + "Returned index/timing agreement is checked; independent pixel identity is covered by production tests. " + "GPU fields are inapplicable. This is a single implementation, without a comparative speedup claim.", + "## Time & Memory\n\n" + markdown_table(rows), + "## Success & Other Metrics\n\n" + markdown_table(metrics), + "## Leaderboard\n\n" + + markdown_table( + [ + dict( + rank=1, + algorithm=algorithm, + overall_success_rate=metrics[0]["success_rate"], + ) + ] + ), + ] + ) + + "\n", + encoding="utf-8", + ) + print(f"Markdown report saved: {report}") + + +if __name__ == "__main__": + run_all_benchmarks() diff --git a/setup.py b/setup.py index 7dcf4d39ea..825ee44c21 100644 --- a/setup.py +++ b/setup.py @@ -195,6 +195,12 @@ def main(): "configs/*.yaml", ], "embodichain.data": ["default_materials.json"], + "embodichain.toolkits.trajectory_annotator": [ + "README.md", + "web/*.html", + "web/*.js", + "web/*.css", + ], "embodichain.compute.kinematics._warp": [ "ssik_LICENSE.txt", "data/*.npz", diff --git a/tests/test_main.py b/tests/test_main.py index 491a8fe51a..f11b226714 100644 --- a/tests/test_main.py +++ b/tests/test_main.py @@ -35,6 +35,7 @@ EXPECTED_COMMANDS = { "analyze-workspace", "annotate-grasp", + "annotate-trajectory", "benchmark", "data", "dataset", diff --git a/tests/toolkits/trajectory_annotator/conftest.py b/tests/toolkits/trajectory_annotator/conftest.py new file mode 100644 index 0000000000..135b4c3190 --- /dev/null +++ b/tests/toolkits/trajectory_annotator/conftest.py @@ -0,0 +1,126 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Small real H.264 inputs and a lifecycle-owned local annotation server.""" + +from __future__ import annotations + +from collections.abc import Callable, Iterator +from fractions import Fraction +from pathlib import Path +import socket +import threading +import time + +import pytest + + +@pytest.fixture +def make_video(tmp_path: Path) -> Callable[..., Path]: + """Encode identifiable frames with B frames and optional VFR/nonzero PTS.""" + av = pytest.importorskip("av") + import numpy as np + import cv2 + + def make( + name: str = "episode.mp4", + count: int = 48, + variable: bool = False, + start: int = 0, + width: int = 160, + height: int = 96, + ) -> Path: + path = tmp_path / name + with av.open(str(path), "w") as output: + stream = output.add_stream("libx264", rate=30) + stream.width, stream.height, stream.pix_fmt = width, height, "yuv420p" + stream.options = {"g": "12", "bf": "2", "crf": "18"} + stream.time_base = stream.codec_context.time_base = Fraction(1, 30) + for index in range(count): + image = np.full( + (height, width, 3), + (index * 31 % 256, index * 13 % 256, index * 7 % 256), + dtype=np.uint8, + ) + cv2.putText( + image, + str(index), + (12, height // 2), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + (255, 255, 255), + 2, + ) + frame = av.VideoFrame.from_ndarray(image, format="rgb24") + frame.pts = start + index + (index // 5 if variable else 0) + frame.time_base = Fraction(1, 30) + for packet in stream.encode(frame): + output.mux(packet) + for packet in stream.encode(): + output.mux(packet) + return path + + return make + + +@pytest.fixture +def annotation_server( + make_video: Callable[..., Path], tmp_path: Path +) -> Iterator[tuple[str, Path]]: + """Serve a real video for browser checks, cleaning up both owning threads.""" + uvicorn = pytest.importorskip("uvicorn") + pytest.importorskip("fastapi") + import httpx + from embodichain.toolkits.trajectory_annotator._cfg import AnnotationCfg + from embodichain.toolkits.trajectory_annotator._server import create_app + + from embodichain.toolkits.trajectory_annotator.video import VideoFrameSource + + video = make_video(count=90, variable=True, start=90) + destination = tmp_path / "annotations.json" + cfg = AnnotationCfg(autosave_ms=150) + with socket.socket() as listener: + listener.bind(("127.0.0.1", 0)) + url = f"http://127.0.0.1:{listener.getsockname()[1]}" + server = uvicorn.Server( + uvicorn.Config( + create_app(lambda: VideoFrameSource(video), destination, cfg=cfg), + log_level="error", + ) + ) + thread = threading.Thread( + target=lambda: server.run(sockets=[listener]), daemon=True + ) + thread.start() + try: + deadline = time.monotonic() + 15 + while not server.started: + if not thread.is_alive() or time.monotonic() > deadline: + pytest.fail("Annotation HTTP server did not start.") + time.sleep(0.01) + with httpx.Client(base_url=url) as client: + while True: + status = client.get("/api/status").json() + if status["phase"] == "ready": + break + if status["phase"] == "error" or time.monotonic() > deadline: + pytest.fail(f"Video initialization failed: {status}") + time.sleep(0.01) + yield url, destination + finally: + server.should_exit = True + thread.join(timeout=15) + assert not thread.is_alive(), "Annotation server leaked its owning thread." diff --git a/tests/toolkits/trajectory_annotator/test_annotation.py b/tests/toolkits/trajectory_annotator/test_annotation.py new file mode 100644 index 0000000000..1a321830db --- /dev/null +++ b/tests/toolkits/trajectory_annotator/test_annotation.py @@ -0,0 +1,342 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Frame identity, inclusive intervals and durable annotation contracts.""" + +from __future__ import annotations + +from collections.abc import Callable +import json +from pathlib import Path +import subprocess +import sys +import threading +import time + +import pytest + +from embodichain.toolkits.trajectory_annotator._models import ( + ConflictError, + validate_skill_intervals, +) +from embodichain.toolkits.trajectory_annotator._storage import AnnotationStore + +pytestmark = pytest.mark.no_sim + + +def skill_interval( + start: int = 2, end: int = 4, identifier: str = "flip" +) -> dict[str, object]: + """A known inclusive span: three frames, independent of storage machinery.""" + return dict( + id=identifier, + label=" 翻转包裹 ", + start_frame=start, + end_frame=end, + note="已翻转", + ) + + +def test_inclusive_ranges_allow_singletons_adjacency_and_gaps() -> None: + result = validate_skill_intervals( + [ + skill_interval(8, 9, "c"), + skill_interval(1, 1, "a"), + skill_interval(2, 4, "b"), + ], + 10, + ) + assert [(item["start_frame"], item["end_frame"]) for item in result] == [ + (1, 1), + (2, 4), + (8, 9), + ] + assert result[0]["label"] == "翻转包裹" + + +@pytest.mark.parametrize( + "skill_intervals", + [ + [skill_interval(0, 2), skill_interval(2, 4, "other")], + [skill_interval(-1, 2)], + [skill_interval(4, 3)], + [skill_interval(0, 10)], + [skill_interval(True, 3)], + [skill_interval(1.5, 3)], + [skill_interval(0, 1), skill_interval(4, 5)], + [{**skill_interval(), "label": " "}], + ], +) +def test_invalid_intervals_cannot_be_saved(skill_intervals: object) -> None: + with pytest.raises(ValueError): + validate_skill_intervals(skill_intervals, 10) + + +@pytest.mark.parametrize("variable,start", [(False, 0), (True, 90)]) +def test_seeks_match_independent_sequential_decode( + make_video: Callable[..., Path], variable: bool, start: int +) -> None: + av = pytest.importorskip("av") + import cv2 + import numpy as np + from embodichain.toolkits.trajectory_annotator._cfg import AnnotationCfg + from embodichain.toolkits.trajectory_annotator._frames import FrameReader + from embodichain.toolkits.trajectory_annotator.video import VideoFrameSource + + path = make_video(variable=variable, start=start) + with av.open(str(path)) as source: + stream = source.streams.video[0] + baseline = [ + (frame.pts, frame.to_ndarray(format="bgr24")) + for frame in source.decode(stream) + ] + time_base = stream.time_base + assert stream.codec_context.has_b_frames + assert len(baseline) == 48 # Includes delayed frames emitted by decoder flush. + if start: + assert baseline[0][0] > 0 + cfg = AnnotationCfg(cache_bytes=4000) + reader = FrameReader(VideoFrameSource(path), cfg) + try: + metadata = reader.prepare() + assert metadata.frame_count == len(baseline) + for index in [0, 1, 12, 47, 46, 24, 11, 10, 0, 30, 31]: + preview = reader.preview(index) + pixels = cv2.imdecode( + np.frombuffer(preview.jpeg, np.uint8), cv2.IMREAD_COLOR + ) + assert np.abs(pixels.astype(float) - baseline[index][1]).mean() < 3 + assert preview.index == index + assert preview.pts == baseline[index][0] + assert preview.time_seconds == pytest.approx( + float((baseline[index][0] - baseline[0][0]) * time_base) + ) + assert reader.cache_bytes <= cfg.cache_bytes + finally: + reader.close() + + +def test_index_budget_and_missing_pts_fail_explicitly( + make_video: Callable[..., Path], +) -> None: + pytest.importorskip("av") + from embodichain.toolkits.trajectory_annotator._cfg import AnnotationCfg + from embodichain.toolkits.trajectory_annotator._frames import FrameReader + from embodichain.toolkits.trajectory_annotator.video import VideoFrameSource + + reader = FrameReader( + VideoFrameSource(make_video(count=4), index_bytes=32), AnnotationCfg() + ) + try: + with pytest.raises(ValueError, match="预算"): + reader.prepare() + finally: + reader.close() + # An elementary H.264 stream has no container presentation timestamps. + reader = FrameReader( + VideoFrameSource(make_video(name="raw.h264", count=4)), AnnotationCfg() + ) + try: + with pytest.raises(ValueError, match="PTS"): + reader.prepare() + finally: + reader.close() + + +def test_atomic_save_resume_conflict_and_write_failure( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + from embodichain.toolkits.trajectory_annotator import _storage + + video = tmp_path / "video.mp4" + video.write_bytes(b"video identity fixture") + details = dict( + sha256="a" * 64, + stream_index=0, + frame_count=10, + width=160, + height=96, + time_base={"numerator": 1, "denominator": 30}, + ) + from dataclasses import replace + from embodichain.toolkits.trajectory_annotator.frames import FrameSourceInfo + + source = FrameSourceInfo( + "video", video.name, "a" * 64, 10, 160, 96, 30.0, "pts", details, video + ) + path = tmp_path / "nested" / "labels.json" + store = AnnotationStore(path, 1024**2) + store.initialize(source) + saved = store.save([skill_interval()], 0) + original = path.read_bytes() + assert saved["revision"] == 1 + assert json.loads(original)["segments"][0]["label"] == "翻转包裹" + restored = AnnotationStore(path, 1024**2) + restored.initialize(source) + assert restored.snapshot()["document"] == saved + with pytest.raises(ConflictError): + store.save([], 0) + with monkeypatch.context() as context: + + def fail_replace(*args: object) -> None: + raise PermissionError("read-only destination") + + context.setattr(_storage.os, "replace", fail_replace) + with pytest.raises(PermissionError): + store.save([], 1) + assert path.read_bytes() == original + assert store.snapshot()["document"] == saved + assert not list(path.parent.glob(f".{path.name}.*")) + store.save([], 1) + with pytest.raises(ConflictError): + restored.save([], 1) # Another server changed the file after this load. + mismatch = AnnotationStore(path, 1024**2) + with pytest.raises(ValueError, match="不匹配"): + mismatch.initialize(replace(source, identity="b" * 64)) + assert json.loads(path.read_text())["revision"] == 2 + # Legacy video annotations remain byte-for-byte untouched until the next save. + legacy = dict( + schema_version=1, + frame_index_base=0, + interval="closed", + source_video=details, + revision=3, + segments=[skill_interval()], + ) + path.write_text(json.dumps(legacy)) + legacy_bytes = path.read_bytes() + migrated = AnnotationStore(path, 1024**2) + migrated.initialize(source) + assert path.read_bytes() == legacy_bytes + assert migrated.snapshot()["document"]["schema_version"] == 2 + assert migrated.save([], 3)["revision"] == 4 + assert "source_video" not in json.loads(path.read_text()) + + +def test_http_video_and_persistence_boundaries( + make_video: Callable[..., Path], tmp_path: Path +) -> None: + pytest.importorskip("fastapi") + pytest.importorskip("av") + from fastapi.testclient import TestClient + from embodichain.toolkits.trajectory_annotator._server import create_app + + video = make_video() + path = tmp_path / "labels.json" + from embodichain.toolkits.trajectory_annotator.video import VideoFrameSource + + with TestClient( + create_app(lambda: VideoFrameSource(video), path), base_url="http://127.0.0.1" + ) as client: + deadline = time.monotonic() + 10 + while client.get("/api/status").json()["phase"] != "ready": + assert time.monotonic() < deadline + time.sleep(0.01) + project = client.get("/api/project").json() + headers = {"X-Annotation-Token": project["session_token"]} + assert client.get("/").status_code == 200 + for asset in ["app.js", "styles.css"]: + assert client.get(f"/static/{asset}").status_code == 200 + frame = client.get("/api/frames/47?request_id=23") + assert frame.status_code == 200 + assert frame.headers["X-Frame-Index"] == "47" + assert frame.headers["X-Request-ID"] == "23" + assert frame.content.startswith(b"\xff\xd8") + assert client.get("/api/frames/48").status_code == 422 + payload = dict(segments=[skill_interval()], expected_revision=0) + assert client.put("/api/annotations", json=payload).status_code == 403 + response = client.put("/api/annotations", json=payload, headers=headers) + assert response.status_code == 200 + assert json.loads(path.read_text())["segments"][0]["end_frame"] == 4 + assert ( + client.put("/api/annotations", json=payload, headers=headers).status_code + == 409 + ) + invalid = dict( + segments=[skill_interval(0, 1), skill_interval(1, 3, "other")], + expected_revision=1, + ) + assert ( + client.put("/api/annotations", json=invalid, headers=headers).status_code + == 422 + ) + assert client.get("/api/project").json()["document"]["revision"] == 1 + + +def test_unified_cli_help_does_not_import_optional_or_simulation_modules() -> None: + code = """ +import sys +from embodichain.cli.main import main +try: + main(['annotate-trajectory', '--help']) +except SystemExit as error: + assert error.code == 0 +assert not any(name in sys.modules for name in ('av', 'cv2', 'fastapi', 'uvicorn', 'torch')) +assert not any(name.startswith('embodichain.lab') for name in sys.modules) +""" + result = subprocess.run( + [sys.executable, "-c", code], capture_output=True, text=True + ) + assert result.returncode == 0, result.stderr + assert "--video" in result.stdout and "--hdf5" in result.stdout + + +def test_worker_keeps_only_the_latest_pending_seek( + make_video: Callable[..., Path], monkeypatch: pytest.MonkeyPatch +) -> None: + pytest.importorskip("av") + from embodichain.toolkits.trajectory_annotator._cfg import AnnotationCfg + from embodichain.toolkits.trajectory_annotator._frames import ( + SupersededError, + FrameReader, + FrameWorker, + ) + + entered, release = threading.Event(), threading.Event() + original = FrameReader.preview + + def held(self: FrameReader, index: int) -> object: + if index == 5: + entered.set() + assert release.wait(timeout=5) + return original(self, index) + + monkeypatch.setattr(FrameReader, "preview", held) + from embodichain.toolkits.trajectory_annotator.video import VideoFrameSource + + path = make_video() + worker = FrameWorker( + lambda: VideoFrameSource(path), AnnotationCfg(), lambda _: None + ) + worker.start() + try: + deadline = time.monotonic() + 5 + while worker.status()["phase"] != "ready": + assert time.monotonic() < deadline + time.sleep(0.01) + active = worker.request(5) + assert entered.wait(timeout=3) + obsolete = worker.request(6) + latest = worker.request(7) + with pytest.raises(SupersededError): + obsolete.result(timeout=1) + release.set() + assert active.result(timeout=3).index == 5 + assert latest.result(timeout=3).index == 7 + finally: + release.set() + worker.close() + assert worker.status()["phase"] == "closed" diff --git a/tests/toolkits/trajectory_annotator/test_browser.py b/tests/toolkits/trajectory_annotator/test_browser.py new file mode 100644 index 0000000000..3d29563fa1 --- /dev/null +++ b/tests/toolkits/trajectory_annotator/test_browser.py @@ -0,0 +1,365 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Real-browser contracts for editing, asynchronous frames and persistence. + +Install optional test tooling with ``pip install playwright`` and +``python -m playwright install chromium firefox``. System Chrome is also used +when available. Missing optional browsers are explicitly skipped. +""" + +from __future__ import annotations + +from collections.abc import Iterator +import json +import os +from pathlib import Path +import shutil +import threading +import time +from typing import TYPE_CHECKING, Any + +import pytest + +if TYPE_CHECKING: + from playwright.sync_api import Page + +pytestmark = pytest.mark.no_sim + + +@pytest.fixture(params=["chromium", "firefox"]) +def page(request: pytest.FixtureRequest) -> Iterator[Page]: + """Launch a clean browser context without touching user browser profiles.""" + playwright = pytest.importorskip("playwright.sync_api") + with playwright.sync_playwright() as runtime: + browser_type = getattr(runtime, request.param) + options: dict[str, Any] = {"headless": True} + if request.param == "chromium": + executable = os.environ.get( + "ANNOTATION_CHROMIUM_EXECUTABLE" + ) or shutil.which("google-chrome") + if executable: + options["executable_path"] = executable + if ( + "executable_path" not in options + and not Path(browser_type.executable_path).is_file() + ): + pytest.skip( + f"Install the optional browser: python -m playwright install {request.param}" + ) + browser = browser_type.launch(**options) + context = browser.new_context( + viewport={"width": 1440, "height": 1100}, accept_downloads=True + ) + tab = context.new_page() + errors: list[str] = [] + tab.on("pageerror", lambda error: errors.append(str(error))) + try: + yield tab + assert not errors, f"Browser JavaScript errors: {errors}" + finally: + context.close() + browser.close() + + +def open_project(page: Page, url: str) -> None: + """Wait for a real decoded frame, not just a loaded HTML document.""" + from playwright.sync_api import expect + + page.goto(url) + expect(page.locator("#workspace")).to_be_visible() + expect(page.locator("#frame-current")).to_have_text("0") + + +def go(page: Page, index: int) -> None: + """Use the user-facing frame jump control.""" + from playwright.sync_api import expect + + page.locator("#goto-frame").fill(str(index)) + page.locator("#goto-frame").press("Enter") + expect(page.locator("#frame-current")).to_have_text(str(index)) + + +def create_skill_interval(page: Page) -> None: + """Create the known inclusive interval [2, 5], containing four frames.""" + from playwright.sync_api import expect + + go(page, 2) + page.keyboard.press("i") + page.locator("#label").fill("翻转包裹") + page.locator("#label").press("Enter") + go(page, 5) + page.keyboard.press("o") + page.keyboard.press("Enter") + expect(page.locator("#skill-interval-count")).to_have_text("1") + + +def test_keyboard_edit_delete_undo_and_resume( + page: Page, annotation_server: tuple[str, Path] +) -> None: + from playwright.sync_api import expect + + url, destination = annotation_server + open_project(page, url) + video = page.locator(".video-card").bounding_box() + timeline = page.locator(".timeline-card").bounding_box() + annotations = page.locator(".annotations-card").bounding_box() + editor = page.locator(".skill-editor-card").bounding_box() + assert timeline["x"] == pytest.approx(video["x"], abs=1) + assert timeline["width"] == pytest.approx(video["width"], abs=1) + assert timeline["y"] >= video["y"] + video["height"] + assert editor["x"] == pytest.approx(annotations["x"], abs=1) + assert editor["width"] == pytest.approx(annotations["width"], abs=1) + assert editor["y"] >= annotations["y"] + annotations["height"] + expect(page.locator("#editor-title")).to_have_text("新建原子技能区间") + expect(page.locator(".header-shortcuts")).to_contain_text("←前一帧") + expect(page.locator(".header-shortcuts")).to_contain_text("→后一帧") + for hint in ("空格播放 / 暂停", "I标注起始帧", "O标注结束帧"): + expect(page.locator(".header-shortcuts")).to_contain_text(hint) + page.keyboard.press("ArrowLeft") + expect(page.locator("#frame-current")).to_have_text("0") + page.keyboard.press("Shift+ArrowRight") + expect(page.locator("#frame-current")).to_have_text("10") + create_skill_interval(page) + expect(page.locator("#skill-interval-list tr")).to_have_count(1) + expect(page.locator("#skill-interval-list td").nth(3)).to_have_text("4") + expect(page.locator(".timeline-skill-interval")).to_have_count(1) + page.locator("#label").focus() + page.locator("#label").dispatch_event( + "keydown", {"key": "Enter", "isComposing": True} + ) + expect(page.locator("#label")).to_be_focused() + page.keyboard.press("i") + page.keyboard.press("Backspace") + expect(page.locator("#start-frame")).to_have_value("") + expect(page.locator("#skill-interval-count")).to_have_text("1") + expect(page.locator("#frame-current")).to_have_text("5") + page.locator("#video-stage").focus() + page.keyboard.press("e") + expect(page.locator("#editor-title")).to_have_text("编辑原子技能区间") + page.locator("#start-frame").fill("3") + page.locator("#start-frame").press("Enter") + go(page, 6) + page.keyboard.press("o") + page.locator("#skill-interval-list tr").focus() + page.keyboard.press("Enter") + expect(page.locator("#skill-interval-list td").nth(1)).to_have_text("3") + expect(page.locator("#skill-interval-list td").nth(2)).to_have_text("6") + page.keyboard.press("Delete") + expect(page.locator("#skill-interval-count")).to_have_text("0") + expect(page.locator(".timeline-skill-interval")).to_have_count(0) + page.keyboard.press("Control+z") + expect(page.locator("#skill-interval-count")).to_have_text("1") + page.keyboard.press("Control+Shift+z") + expect(page.locator("#skill-interval-count")).to_have_text("0") + page.keyboard.press("Control+z") + expect(page.locator("#skill-interval-count")).to_have_text("1") + page.keyboard.press("Control+s") + expect(page.locator("#save-status")).to_have_text("已保存") + saved = json.loads(destination.read_text()) + assert [ + ( + skill_interval["label"], + skill_interval["start_frame"], + skill_interval["end_frame"], + ) + for skill_interval in saved["segments"] + ] == [("翻转包裹", 3, 6)] + page.reload() + expect(page.locator("#skill-interval-count")).to_have_text("1") + expect(page.locator("#skill-interval-list td").nth(2)).to_have_text("6") + expect(page.locator("#frame-current")).to_have_text("0") + screenshot = os.environ.get("ANNOTATION_SCREENSHOT") + if screenshot: + page.screenshot( + path=screenshot.replace( + ".png", f"-{page.context.browser.browser_type.name}.png" + ), + full_page=True, + ) + + +def test_timeline_frame_geometry_and_free_drag_inside_skill_intervals( + page: Page, annotation_server: tuple[str, Path] +) -> None: + from playwright.sync_api import expect + + url, _ = annotation_server + open_project(page, url) + project = page.request.get(f"{url}/api/project").json() + # The fixture has 90 frames. Include one-frame intervals, long labels, and + # both source edges; none of these may widen a bar past its frame cells. + intervals = [ + dict(id="first", label="首帧技能", start_frame=0, end_frame=0), + dict(id="single", label="很长的单帧原子技能名称", start_frame=12, end_frame=12), + dict(id="middle", label="翻转包裹", start_frame=20, end_frame=39), + dict(id="last", label="末帧技能", start_frame=89, end_frame=89), + ] + response = page.request.put( + f"{url}/api/annotations", + headers={"X-Annotation-Token": project["session_token"]}, + data={ + "segments": intervals, + "expected_revision": project["document"]["revision"], + }, + ) + assert response.ok + page.reload() + expect(page.locator("#skill-interval-count")).to_have_text("4") + expect(page.locator("#frame-current")).to_have_text("0") + + def check_geometry() -> dict[str, float]: + bounds = page.locator("#timeline").bounding_box() + cell = bounds["width"] / 90 + for interval in intervals: + bar = page.locator(f'[data-skill-id="{interval["id"]}"]').bounding_box() + assert bar["x"] == pytest.approx( + bounds["x"] + interval["start_frame"] * cell, abs=0.1 + ) + assert bar["width"] == pytest.approx( + (interval["end_frame"] - interval["start_frame"] + 1) * cell, + abs=0.1, + ) + return bounds + + bounds = check_geometry() + y = bounds["y"] + 46 # Inside the colored skill bars, rather than empty track. + + def x_at(frame: int) -> float: + return bounds["x"] + (frame + 0.5) * bounds["width"] / 90 + + page.mouse.move(x_at(30), y) + page.mouse.down() + try: + # Starting inside [20, 39] must seek frame 30, never snap to frame 20. + expect(page.locator("#frame-current")).to_have_text("30") + for frame in (35, 45, 25, 12, 0, 89): + page.mouse.move(x_at(frame), y, steps=4) + expect(page.locator("#frame-current")).to_have_text(str(frame)) + finally: + page.mouse.up() + expect(page.locator("#frame-current")).to_have_text("89") + cursor = page.locator("#playhead").bounding_box() + assert cursor["x"] == pytest.approx(x_at(89), abs=0.1) + + page.locator("#zoom-in").click() + bounds = check_geometry() + page.locator("#timeline-scroll").evaluate( + "element => { element.scrollLeft = element.clientWidth / 2; }" + ) + bounds = check_geometry() + # The same coordinate rule must hold after zooming and horizontal scrolling. + page.mouse.click(x_at(30), bounds["y"] + 46) + expect(page.locator("#frame-current")).to_have_text("30") + + +def test_pause_marks_visible_frame_and_latest_seek_wins( + page: Page, annotation_server: tuple[str, Path], monkeypatch: pytest.MonkeyPatch +) -> None: + from playwright.sync_api import expect + from embodichain.toolkits.trajectory_annotator._frames import FrameReader + + original = FrameReader.preview + + def delayed(self: FrameReader, index: int) -> Any: + if index in {1, 40}: + time.sleep(0.35) + return original(self, index) + + monkeypatch.setattr(FrameReader, "preview", delayed) + open_project(page, annotation_server[0]) + with page.expect_response(lambda response: "/api/frames/1?" in response.url): + page.keyboard.press("Space") + page.keyboard.press("i") + page.evaluate( + "() => new Promise(resolve => requestAnimationFrame(() => requestAnimationFrame(resolve)))" + ) + expect(page.locator("#frame-current")).to_have_text("0") + expect(page.locator("#start-frame")).to_have_value("0") + expect(page.locator("#playback-state")).to_have_text("已暂停") + page.keyboard.press("Escape") + page.locator("#goto-frame").fill("40") + page.locator("#goto-frame").press("Enter") + page.keyboard.press("ArrowRight") + page.keyboard.press("ArrowRight") + page.keyboard.press("ArrowRight") + expect(page.locator("#frame-current")).to_have_text("43") + expect(page.locator("#video-frame")).to_have_attribute("alt", "视频第 43 帧") + page.keyboard.press("End") + expect(page.locator("#frame-current")).to_have_text("89") + page.keyboard.press("ArrowRight") + expect(page.locator("#frame-current")).to_have_text("89") + page.keyboard.press("Space") + expect(page.locator("#frame-current")).not_to_have_text("89") + page.keyboard.press("Space") + + +def test_failed_save_keeps_disk_and_exports_current_edits( + page: Page, annotation_server: tuple[str, Path] +) -> None: + from playwright.sync_api import expect + + url, destination = annotation_server + open_project(page, url) + create_skill_interval(page) + expect(page.locator("#save-status")).to_have_text("已保存") + original = destination.read_bytes() + page.route( + "**/api/annotations", + lambda route: route.fulfill(status=507, json={"detail": "模拟磁盘写入失败"}), + ) + page.keyboard.press("Delete") + expect(page.locator("#save-status")).to_have_text("保存失败") + assert destination.read_bytes() == original + with page.expect_download() as download: + page.locator("#export").click() + assert json.loads(Path(download.value.path()).read_text())["segments"] == [] + page.unroute("**/api/annotations") + page.locator("#save").click() + expect(page.locator("#save-status")).to_have_text("已保存") + assert json.loads(destination.read_text())["segments"] == [] + + +def test_edit_during_save_does_not_clear_newer_dirty_state( + page: Page, annotation_server: tuple[str, Path], monkeypatch: pytest.MonkeyPatch +) -> None: + from playwright.sync_api import expect + from embodichain.toolkits.trajectory_annotator._storage import AnnotationStore + + entered, release = threading.Event(), threading.Event() + original = AnnotationStore.save + + def delayed( + self: AnnotationStore, skill_intervals: object, expected_revision: object + ) -> dict[str, Any]: + if expected_revision == 0: + entered.set() + assert release.wait(timeout=10) + return original(self, skill_intervals, expected_revision) + + monkeypatch.setattr(AnnotationStore, "save", delayed) + open_project(page, annotation_server[0]) + create_skill_interval(page) + try: + assert entered.wait(timeout=3) + page.keyboard.press("Delete") + expect(page.locator("#skill-interval-count")).to_have_text("0") + finally: + release.set() + expect(page.locator("#save-status")).to_have_text("已保存") + saved = json.loads(annotation_server[1].read_text()) + assert saved["revision"] == 2 + assert saved["segments"] == [] diff --git a/tests/toolkits/trajectory_annotator/test_frames.py b/tests/toolkits/trajectory_annotator/test_frames.py new file mode 100644 index 0000000000..4714ae242b --- /dev/null +++ b/tests/toolkits/trajectory_annotator/test_frames.py @@ -0,0 +1,271 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Image-source correctness, lazy HDF5 reads and source-independent annotation.""" + +from __future__ import annotations + +from collections.abc import Callable +from pathlib import Path +import json +import threading +import time +from typing import Any + +import numpy as np +import pytest + +from embodichain.toolkits.trajectory_annotator.frames import ArrayFrameSource + +pytestmark = pytest.mark.no_sim + + +@pytest.fixture +def image_hdf5(tmp_path: Path) -> Callable[..., tuple[Path, np.ndarray]]: + """Write distinctive red/green/blue frames in common recording layouts.""" + h5py = pytest.importorskip("h5py") + cv2 = pytest.importorskip("cv2") + + def make( + layout: str = "jpeg", key: str = "observations/images/cam_high" + ) -> tuple[Path, np.ndarray]: + path = tmp_path / f"{layout}.hdf5" + images = np.zeros((4, 24, 32, 3), dtype=np.uint8) + images[0, :, :, 0] = 230 + images[1, :, :, 1] = 190 + images[2, :, :, 2] = 150 + images[3] = (30, 80, 210) + with h5py.File(path, "w") as handle: + handle.attrs["metadata_json"] = json.dumps({"record_fps": 20}) + if layout in {"raw_rgb", "raw_bgr"}: + data = images if layout == "raw_rgb" else images[..., ::-1] + handle.create_dataset(key, data=data) + else: + encoded = [ + cv2.imencode( + ".jpg" if layout == "jpeg" else ".png", image[:, :, ::-1] + )[1] + for image in images + ] + if layout == "jpeg": + dataset = handle.create_dataset( + key, (4,), dtype=h5py.vlen_dtype(np.uint8) + ) + for index, payload in enumerate(encoded): + dataset[index] = payload + else: + padded = np.zeros((4, max(map(len, encoded)) + 16), dtype=np.uint8) + for index, payload in enumerate(encoded): + padded[index, : len(payload)] = payload + handle.create_dataset(key, data=padded) + return path, images + + return make + + +@pytest.mark.parametrize("layout", ["jpeg", "padded_png", "raw_rgb", "raw_bgr"]) +def test_hdf5_exact_indices_colors_timing_and_lazy_reads( + image_hdf5: Callable[..., tuple[Path, np.ndarray]], + layout: str, + monkeypatch: pytest.MonkeyPatch, +) -> None: + import h5py + from embodichain.toolkits.trajectory_annotator.hdf5 import Hdf5FrameSource + + path, expected = image_hdf5(layout) + reads: list[object] = [] + original = h5py.Dataset.__getitem__ + + def tracked(self: h5py.Dataset, index: object) -> Any: + reads.append(index) + return original(self, index) + + monkeypatch.setattr(h5py.Dataset, "__getitem__", tracked) + source = Hdf5FrameSource(path, color_order="bgr" if layout == "raw_bgr" else "rgb") + try: + info = source.prepare(threading.Event(), lambda **_: None) + assert reads == [0] # Preparation must not decode/materialize the sequence. + assert (info.frame_count, info.width, info.height, info.fps) == (4, 32, 24, 20) + assert info.timing == "fixed_fps" + for index in (3, 0, 2, 1): + frame = source.get_frame(index) + assert reads[-1] == index + assert frame.index == index and frame.pts is None + assert frame.time_seconds == pytest.approx(index / 20) + assert frame.delay_seconds == pytest.approx(0.05) + assert np.abs(frame.image.astype(float) - expected[index]).max() <= ( + 2 if layout == "jpeg" else 0 + ) + for index in (-1, 4): + with pytest.raises(ValueError): + source.get_frame(index) + finally: + source.close() + + +def test_hdf5_camera_fps_identity_resume_and_missing_metadata( + image_hdf5: Callable[..., tuple[Path, np.ndarray]], + tmp_path: Path, +) -> None: + import h5py + from embodichain.toolkits.trajectory_annotator.hdf5 import Hdf5FrameSource + from embodichain.toolkits.trajectory_annotator._storage import AnnotationStore + + # Singular observation is accepted when present, without rewriting an explicit key. + key = "observation/images/cam_high" + path, images = image_hdf5("raw_rgb", key) + with h5py.File(path, "a") as handle: + handle.create_dataset("observation/images/cam_left_wrist", data=images) + + def prepare(**kwargs: Any) -> Any: + source = Hdf5FrameSource(path, **kwargs) + try: + return source.prepare(threading.Event(), lambda **_: None) + finally: + source.close() + + info = prepare() + assert info.details["image_key"] == key + annotations = tmp_path / "labels.json" + store = AnnotationStore(annotations, 1024**2) + store.initialize(info) + saved = store.save( + [dict(id="flip", label="翻转包裹", start_frame=1, end_frame=3)], 0 + ) + restored = AnnotationStore(annotations, 1024**2) + restored.initialize(prepare()) + assert restored.snapshot()["document"] == saved + for options in ({"fps": 30}, {"image_key": "observation/images/cam_left_wrist"}): + with pytest.raises(ValueError, match="不匹配"): + AnnotationStore(annotations, 1024**2).initialize(prepare(**options)) + with pytest.raises(ValueError, match="找不到"): + prepare(image_key="observations/images/cam_high") + with pytest.raises(ValueError, match="相同"): + AnnotationStore(path, 1024**2).initialize(info) + with h5py.File(path, "a") as handle: + del handle.attrs["metadata_json"] + with pytest.raises(ValueError, match="--fps"): + prepare() + assert prepare(fps=25).fps == 25 + with pytest.raises(ValueError, match="不匹配"): + AnnotationStore(annotations, 1024**2).initialize(prepare(fps=20)) + + +def test_array_input_uses_same_http_save_path_and_worker_lifecycle( + tmp_path: Path, +) -> None: + pytest.importorskip("fastapi") + from fastapi.testclient import TestClient + import cv2 + from embodichain.toolkits.trajectory_annotator.app import create_annotation_app + + images = np.zeros((3, 24, 32, 3), dtype=np.uint8) + images[0, :, :, 0] = 240 + images[1, :, :, 1] = 210 + images[2, :, :, 2] = 180 + owners: list[int] = [] + closed = threading.Event() + + class TrackedSource(ArrayFrameSource): + def prepare(self, *args: Any, **kwargs: Any) -> Any: + owners.append(threading.get_ident()) + return super().prepare(*args, **kwargs) + + def get_frame(self, index: int) -> Any: + owners.append(threading.get_ident()) + return super().get_frame(index) + + def close(self) -> None: + owners.append(threading.get_ident()) + closed.set() + + def factory() -> TrackedSource: + owners.append(threading.get_ident()) + return TrackedSource(images, fps=10, name="RGB images") + + destination = tmp_path / "images.json" + app = create_annotation_app(factory, destination) + with TestClient(app, base_url="http://127.0.0.1") as client: + deadline = time.monotonic() + 5 + while (status := client.get("/api/status").json())["phase"] != "ready": + assert status["phase"] != "error", status + assert time.monotonic() < deadline + time.sleep(0.01) + project = client.get("/api/project").json() + assert project["document"]["source"]["kind"] == "images" + assert not destination.exists() + response = client.get("/api/frames/2") + pixels = cv2.imdecode( + np.frombuffer(response.content, np.uint8), cv2.IMREAD_COLOR + ) + assert np.abs(pixels.astype(float) - images[2, :, :, ::-1]).max() <= 2 + assert response.headers["X-Frame-Time"] == "0.2" + assert response.headers["X-Frame-PTS"] == "" + response = client.put( + "/api/annotations", + headers={"X-Annotation-Token": project["session_token"]}, + json={ + "segments": [ + dict(id="one", label="原子技能", start_frame=0, end_frame=2) + ], + "expected_revision": 0, + }, + ) + assert response.status_code == 200 + assert closed.is_set() and len(set(owners)) == 1 + assert owners[0] != threading.get_ident() + assert json.loads(destination.read_text())["segments"][0]["end_frame"] == 2 + owners.clear() + closed.clear() + # A second lifespan must obtain a fresh source, not reuse a cancelled worker. + with TestClient(app, base_url="http://127.0.0.1") as client: + deadline = time.monotonic() + 5 + while (status := client.get("/api/status").json())["phase"] != "ready": + assert status["phase"] != "error", status + assert time.monotonic() < deadline + time.sleep(0.01) + restored = client.get("/api/project").json() + assert restored["persisted"] + assert restored["document"]["segments"][0]["end_frame"] == 2 + assert closed.is_set() and len(set(owners)) == 1 + + +def test_owned_previews_survive_reused_frame_buffers() -> None: + from embodichain.toolkits.trajectory_annotator._cfg import AnnotationCfg + from embodichain.toolkits.trajectory_annotator._frames import FrameReader + from embodichain.toolkits.trajectory_annotator.frames import ImageFrame + + # A reusable source buffer is permitted until the next read; previews must own bytes. + images = np.zeros((2, 24, 32, 3), dtype=np.uint8) + images[1, :, :, 1] = 220 + + class ReusingSource(ArrayFrameSource): + buffer = np.empty_like(images[0]) + + def get_frame(self, index: int) -> ImageFrame: + self.buffer[:] = images[index] + return ImageFrame(index, self.buffer, index / 20, 0.05) + + source = ReusingSource(images, fps=20) + reader = FrameReader(source, AnnotationCfg(cache_bytes=0)) + try: + reader.prepare() + first = reader.preview(0) + previous = first.jpeg + second = reader.preview(1) + assert first.jpeg == previous and first.jpeg != second.jpeg + finally: + reader.close() From c80b36115bb5e0207df3501edb1baa431da52010 Mon Sep 17 00:00:00 2001 From: matafela Date: Wed, 11 Nov 2026 11:46:18 +0800 Subject: [PATCH 02/45] update --- agent_context/MAP.yaml | 8 +- .../topics/atomic-actions/atomic-actions.md | 21 + .../topics/data-pipeline/data-pipeline.md | 6 + .../topics/motion-planning/motion-planning.md | 8 + .../images/trajectory-annotation-fitting.png | Bin 0 -> 75958 bytes docs/source/api_reference/index.rst | 1 + docs/source/api_reference/skill_fitting.rst | 50 ++ embodichain/cli/main.py | 5 + .../compute/trajectory/phase_alignment.py | 92 ++ embodichain/compute/trajectory/splicing.py | 187 ++++ embodichain/data_pipeline/skill_fitting.py | 340 ++++++++ embodichain/lab/sim/atomic_skills/__init__.py | 8 + embodichain/lab/sim/atomic_skills/fitting.py | 803 ++++++++++++++++++ .../sim/atomic_skills/primitives/__init__.py | 11 +- .../sim/atomic_skills/primitives/hand_over.py | 186 +++- .../sim/atomic_skills/primitives/pick_up.py | 97 ++- .../lab/sim/atomic_skills/primitives/slide.py | 133 ++- .../lab/sim/atomic_skills/primitives/twist.py | 114 ++- .../lab/sim/motion/motion_generator.py | 22 +- .../lab/sim/motion/planners/base_planner.py | 22 +- .../toolkits/skill_fitting/__init__.py | 21 + .../toolkits/skill_fitting/__main__.py | 22 + .../toolkits/skill_fitting/_pipeline.py | 222 +++++ embodichain/toolkits/skill_fitting/_robot.py | 264 ++++++ embodichain/toolkits/skill_fitting/cli.py | 217 +++++ .../toolkits/trajectory_annotator/README.md | 50 +- .../toolkits/trajectory_annotator/_cli.py | 61 +- .../toolkits/trajectory_annotator/_fitting.py | 258 ++++++ .../toolkits/trajectory_annotator/_frames.py | 45 +- .../toolkits/trajectory_annotator/_server.py | 133 ++- .../toolkits/trajectory_annotator/_storage.py | 12 + .../toolkits/trajectory_annotator/app.py | 19 +- .../toolkits/trajectory_annotator/web/app.js | 22 + .../trajectory_annotator/web/fitting.js | 315 +++++++ .../trajectory_annotator/web/index.html | 36 + .../trajectory_annotator/web/styles.css | 39 + examples/toolkits/skill_fitting/README.md | 129 +++ examples/toolkits/skill_fitting/__main__.py | 93 ++ .../toolkits/trajectory_annotation/README.md | 31 + .../trajectory_annotation/__main__.py | 47 +- tests/compute/test_trajectory_splicing.py | 100 +++ tests/data_pipeline/test_skill_fitting.py | 94 ++ tests/sim/atomic_skills/test_actions.py | 65 +- tests/sim/atomic_skills/test_fitting.py | 311 +++++++ tests/test_main.py | 1 + .../toolkits/trajectory_annotator/conftest.py | 68 ++ .../trajectory_annotator/test_annotation.py | 114 +++ .../trajectory_annotator/test_browser.py | 59 ++ 48 files changed, 4841 insertions(+), 121 deletions(-) create mode 100644 docs/architecture/images/trajectory-annotation-fitting.png create mode 100644 docs/source/api_reference/skill_fitting.rst create mode 100644 embodichain/compute/trajectory/phase_alignment.py create mode 100644 embodichain/compute/trajectory/splicing.py create mode 100644 embodichain/data_pipeline/skill_fitting.py create mode 100644 embodichain/lab/sim/atomic_skills/fitting.py create mode 100644 embodichain/toolkits/skill_fitting/__init__.py create mode 100644 embodichain/toolkits/skill_fitting/__main__.py create mode 100644 embodichain/toolkits/skill_fitting/_pipeline.py create mode 100644 embodichain/toolkits/skill_fitting/_robot.py create mode 100644 embodichain/toolkits/skill_fitting/cli.py create mode 100644 embodichain/toolkits/trajectory_annotator/_fitting.py create mode 100644 embodichain/toolkits/trajectory_annotator/web/fitting.js create mode 100644 examples/toolkits/skill_fitting/README.md create mode 100644 examples/toolkits/skill_fitting/__main__.py create mode 100644 tests/compute/test_trajectory_splicing.py create mode 100644 tests/data_pipeline/test_skill_fitting.py create mode 100644 tests/sim/atomic_skills/test_fitting.py diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index d521df9466..9af51b0752 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -443,13 +443,18 @@ topics: - id: atomic-actions title: Atomic Skills aliases: [atomic skill, atomic skills, atomic action, 原子技能, 原子动作] - keywords: [PlanningBudget, PlanningRecovery, ExecutionPlanLifecycle, AtomicSkillEngine, run-atomic-skill, PlanningContext, ExecutionSession, ExecutionRunner, SkillInvocation, + keywords: [fit-trajectory, SkillFitCfg, PickUpMotionGoal, SlideMotionGoal, TwistMotionGoal, HandOverMotionGoal, 视频关节轨迹拟合, 轨迹回填, PlanningBudget, PlanningRecovery, ExecutionPlanLifecycle, AtomicSkillEngine, run-atomic-skill, PlanningContext, ExecutionSession, ExecutionRunner, SkillInvocation, hand_geometry_cfgs, GraspOpeningTarget, prepare_grasp_query, scoped grasp preparation, RecoveryPolicy, HeldObjectGuardRequest, PhaseEffectGateRequest, articulation_geometry, control-grid retiming, AffordanceSamplingContext, AffordancePoseCandidates, AffordanceSample, affordance sampling] paths: [topics/atomic-actions/atomic-actions.md] source_of_truth: - embodichain/lab/sim/atomic_skills/engine.py + - embodichain/lab/sim/atomic_skills/fitting.py + - embodichain/toolkits/skill_fitting/ + - embodichain/toolkits/trajectory_annotator/_fitting.py + - embodichain/toolkits/trajectory_annotator/web/fitting.js + - embodichain/data_pipeline/skill_fitting.py - embodichain/lab/sim/atomic_skills/core.py - embodichain/lab/sim/atomic_skills/goals.py - embodichain/lab/sim/atomic_skills/execution.py @@ -462,6 +467,7 @@ topics: - embodichain/cli/run_example.py - examples/sim/atomic_skill/ watch_paths: [embodichain/lab/sim/atomic_skills/, embodichain/lab/sim/atomic_actions/, tests/sim/atomic_skills/, scripts/tutorials/atomic_skill/, + examples/toolkits/trajectory_annotation/, tests/toolkits/trajectory_annotator/, embodichain/cli/run_example.py, examples/sim/atomic_skill/, tests/examples/atomic_skill/] related_topics: [simulation-system, motion-planning, robot-system, ik-solvers, task-programs] status: active diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 174afc8523..b9f19b2484 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -153,6 +153,27 @@ ExecuteDemoSkill executes task-specific demonstration assets with measured runti contracts through the same nominal-source and runner interfaces. Its internal events do not create independent recovery boundaries. +Video-selected reconstruction enters through `embodichain fit-trajectory` +(`toolkits/skill_fitting/cli.py`) or the annotation website's selected-interval +fitting panel (`toolkits/trajectory_annotator/_fitting.py`). Both hosts reuse +`toolkits/skill_fitting/_pipeline.py`; the web host retains one background job +and result, keeps decoding on its existing owner, and invalidates results after +annotation revisions change. Its bounded joint/FK preview follows video time; +the downloadable NPZ retains full samples. See the +[web example](../../../examples/toolkits/trajectory_annotation/README.md). +`atomic_skills/fitting.py` identifies explicit +world-TCP motion goals for PickUp, Slide, Twist and HandOver, then calls their +ordinary registered planners. Motion goals carry no object identity and emit +empty effects; they cannot establish an attachment or physical success. Native +generation remains independent of the source joint samples. A source-derived +monotone phase clock and bounded boundary correction are qualified separately; +rejected bundles retain candidates while leaving the patched reference unchanged. +The host supplies a named robot profile and an offline FK/IK port, without +constructing a simulator. Annotation source identity, frame endpoint conversion, +joint semantics and clock offsets remain explicit. Automatic video contact +recognition is not provided. The [runnable guide](../../../examples/toolkits/skill_fitting/README.md) +owns supported phase partitions, calibration and standalone result arrays. + PickUp, MoveHeldObject and Place can also assemble a pre-planning nominal motion source. The source does not own semantic effects or environment lifecycle; see [Real2Sim motion](../data-pipeline/real2sim.md) for source and replay limits. diff --git a/agent_context/topics/data-pipeline/data-pipeline.md b/agent_context/topics/data-pipeline/data-pipeline.md index e3a9b80cf3..87bafce17a 100644 --- a/agent_context/topics/data-pipeline/data-pipeline.md +++ b/agent_context/topics/data-pipeline/data-pipeline.md @@ -35,6 +35,12 @@ tabletop Cartesian transfer needs source TCP poses. Prototype preparation and planning do not establish collision freedom or physical task success. See [Real2Sim motion](real2sim.md) for ownership and source binding. +`data_pipeline/skill_fitting.py` separately owns named joint recordings, inclusive +video-to-joint clock conversion and standalone numeric/JSON reconstruction +bundles. It does not relax Real2Sim's object-reference requirements. The +[Atomic Skills overview](../atomic-actions/atomic-actions.md) owns the fitting +entry point and the effect-free motion-goal boundary. + Read [online sampling](online-sampling.md) for worker states, refill, shared errors, action schemas, sample provenance, causal histories and example-runner boundaries. Read [persistence](persistence.md) diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index 7a9b832677..eada23644e 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -134,6 +134,14 @@ solver-dependent pose/IK adaptation and compatibility re-exports. Trapezoidal/ Double-S Warp profiles live in `compute/kinematics/_warp/trapezoidal.py`; compute must not import simulation objects. +Offline atomic reconstruction uses `compute.trajectory.phase_alignment` for +bounded monotone native-path clock fitting and `splicing` for fixed-clock joint +replacement. Callers retain required anchors and qualify TCP constraints and +joint derivatives after mapping and correction. The discrete boundary-stencil +contract is not continuous-time smoothness or collision evidence. An explicitly +injected planner/robot port bypasses simulator lookup; ordinary construction +continues to resolve the simulator-owned robot. + The internal Cartesian profile adapter accepts caller-owned endpoint poses and optional Bézier geometry, times distance before batched IK, and derives joint references using the root-frame Jacobian. Its pure derivative and joint-wrap diff --git a/docs/architecture/images/trajectory-annotation-fitting.png b/docs/architecture/images/trajectory-annotation-fitting.png new file mode 100644 index 0000000000000000000000000000000000000000..8c98079dbf4c52466f953307d696de6b4287ff2d GIT binary patch literal 75958 zcmeFZ2UJtrwm*!$7fJvDvdE)rVlP6A|I(72&*~@3oo;h=tQ)6_(LH_PMtb+_ViiyvuD|F z@Nn|n_@|G9_bjZZj~kwNbL@x!%Td-N$5@XX)Uxm$3VHO{kw3`$=i}(HLvfFsI(M_=O9kS@eBbSn;&TQ|w{6JckwjcKo@{!`Aw(+C!QG z>!CzPj-EJr{KWC2M~@ym%yNYF=yjQ6Y`0Cnvv(Y~_on`e$@=hxLqOK-&hiO42OO<) z{9usf{INqm)?=(J8Z5irEGjoSZ?iFl3teysW=oh0f`6S|I#UiZ{r-*wI406Xd&rl; z+FY5(sMM_-3S>Qrkr>UfU3hoW%S<|8^!U$gt#ew7L+@SI1UAs0!AJfz#8Zh-FieZK z2zf7@)*U@k`pX|HS!T5<0mE#aRF7KzCd~QYBsu-df5;T^Z{pDVe>cLRPX1rq2p40O z0>rHv9=`6TJto*Ik^01K5~0?>E4hGCv#LaTD3T5KGfUK8hm3#p>WbfE6Y1iygcq7p zaZQu!X+ON}h2!Ue z*p@V{1U70&-!0WE2%hIJTDm#2FKvPogIKPkv;F%Q^cgR*^J-?ZVzKNFN=xRX@|_M| zuhxr@Z!22xt7p6P&s!ge9Red>wdpuXy8B@HVgR4Sh zTS6U}Rig6Z0Q0zlmiPFFjJ zfgwffhl^MBo`-&4I_t3w5}YO8?H0K`h_}K zb>vy(kP(M;J^#;9Wzh~=Oy1P!6yhUG^EJ7*2Q0+}eE3hv1fg2nBF(3o`Y-A9bEzJq zY=WUQ1A&lvgcsG^+siUDZ_t0|p^n>1;C^t4#-I(4TBKtv(dU!W4+ldZi2cjvvKLzk zAA8He#mF=+DVRy)oL~F7jz-Z=G5NTQ!SeLvXEFr{h$(`t{FHzm4_z9|7fJP^bA9p& z^2S?;NZK)P?t!cxnK!5jaRmCk%vD?0lb|%lVtS{nx6f&8=4Kn@-0B9%G?_mi=YHyH zHSngFj!_Wu`jxjNJL#hVWM-bz%j|}2=1hYTftQ77H80StrXMJ4W z3y-%-t<8-puq9-iOLsPksDOCW>Q`Nlk~)U1Y`C;noq=M+bQL_VGZL724zj)WC~0k= zcxyk-J^QUPmzknYF^U!0I6Y52uT;b=ZA~$M@MeJ;qtH>}7Av-^bOXXuOBm99Yu~SN zJ(iPVhUqz706PFj84G#}fL>5^_3S`WEcangz3DVZ0!icGZNdD#ukWpnZ~5ZQ8s z!3-O_(LG-Dp^JhGNjl&A{pQct-QNz^Q5Qsr1GzYSl(CdZi!{@MD%c8cIOEwKZpQ!G z3m*xc6SypDR6Vm5Tple; zyPP#pcQS?FtY>0XR)-j`gPR*i^*mUxE=Wi>T;DbsAnbizTpTBM0&UC!`;4t4^q#}a zbS$;Y&R@<>lN(IbW7~ILx!&fW_oOZ?BuDa2Z_YwJ&Yb#Id$UL=wnsoP2?cw+))g=T zVY@^>f09B;RW;C+PhTag%Ie44%S)i>UOI{@+26k<8udXe#)@BG%;arw!+mr^&d9tl z>Vi4|qQ#TnPT7z!0)nX%TGl}H>ggEAwz3_4gidO>V*Do@w(NhzEArtwhqwhB!QWgKqJec z+PTA#u5>PSSUDPr;96n5_;8ZDLBS}XJc*DwCc3Ttr?c^nfuHLo+?%Y_=NDrYm0eK& zaT&*-2HQ=+;3%OdA}kHu&r1rA{FhVwHuyV z_I3G|bDRRj6=BeAvXrnV_@}d|dDu+BRhPKC4#4dkRBnn3My$i?g@eFAFw#;%s(X-B zvT|KjQF|6&?})xCL0|P}l?|Y|+4eYI6;O21llIfHCVd-VczGkvY{v50G524`mi$qC zJt5PU9xGRdO&VM=H+|bT?VZVyCz)q_hpoY=9_2grF3S5b= z=kT#QAT3wlw#h};-IFF4Wi(kuM2efP8;3BLwQgL*C~%lRyt5;@efJ%%qM2*JtMgep z_FraR$To5hh&b{KX!j|x+D^cP*G9$yQD1HCiRpi^$s0~Sd zny5JITS$Hy`Vqr3qIWwWq3*uLOr~)!jNGe-IM=f*ZW;?lZ-`?dAE&BX>$@&xTzM;> z=mKF{3OnTAZ8(N8-7*`nszaF_u#kezkBB}uH89=|5Ojzq3KM&vY73D}uajVNhTQ0m zM_R%UMP}jTj!e*F^l^*hW<4d?9& z0bAy#`H-`Zbeez3e)u9)J~DZDP2NJiTqwOg7HlsvWaFwE1o%_WJ2NWhb@{3qS8R`o(1 z6nYcAaqJvA0o^%G-`21+5>OCwHtv;aIybmMUo@D63y?8heuEi~p<#{NRxV8Q6IWH2 zq1|M9Jksu5qQa1%obdWqKK>ew<70{?!a!9rBM>2Az2lEmgSA$tcn%dR69_w8lFpA$ z76Vow@SX4thmlQGv)1hMQv*&|L`tCAG~SLq>O z7PwSQRBp(efU7gp9NGUGp+${fWjVddIkgNRj>l z+E$G*&?;**l*1bG;$G*M6uvFugky4ph-J2Upk8QsEUYxoo2)z{la+65B=}uV%)#R_ zy(fk1xg6HsT)0wE-1~|fBG;B zi*10T}Z>aO~uw%!}LWon(GUT>IdrnK#+uCV* zn^p-FYP8cMUDaTH4J2}%kF2%p>8f1)!3oq%KJ*X3+8*e4W9%^f#w)RKc%ND44{q|c z4QTU5_Ap_7!T~fBYuAAesW@ZmJ?-VsSXp+bO7iE|2;%&MeY%A2Alg#7=+uJVMzVAH zfV7iAR&pxnD1&{ohd=&D=C|At)arwv2is@d&*4(cIcnSpTN3f}AOhu4|JqQzwo-H2 zp0#vZa_du82d$?}crut(H#T_jF)!OZs}Maw0_9vfxfRiO$FHaGLos5r00jMH4iOhy zXZQ*PDw>>aJA5uO-Bgjgr(DU}hyWLK(KC&78|_o#o2arOkSZ!qX=Y!F?AP*iM~dMr zdmQ10{cI-6f>L8=HoykJZ(0LhDm;EzQ~N?rIpXjs4!+#0E%P2Q z6uiwESn6Czzc`g^6D|L#OQ|V8oqR#^7}6%SrBw&W@QnOC0`VE{hIH$N*IC}_)!t6( zo%&_9BobZ$xaK%T~`4D3liH_ zLiJxhKc**WDO5XiJ6zr))S%BlA~**Tktxi&W-EAHN32S#Hh zR&9plfMe|`o_5jdXZ4dL^}qDoZ$+fX2GE=9Xs*`Oh01r=LQ03;g27v>2ALXMYT<6H zYyAaUoK>lc5Jrn{y0~?U*$#Isw1w=YgRg^pE)2KQE~mt_q&M~{VOcAnRet{IPSazK zxgSyUS^0JD+0N!SES>ylw^cfCELL|$Ql(3g3{*VSv>WJP!hMR_C* zPmz>jAIRbg-p%0z`4;k8TvY|VXY0I8PNa@*zyY$kjbso7-He&3RA<)C6rfsdsix`RNN$BD-?D?a(G z>X$iRlT;VgG@QMfdNM!mvAgZ07Sym149B6%Mbi})*e`>bhzPf->$d^yb8k|w-AzNt zqj&{jup*lckIMKdl%FZDQDG8i4P13g8lZW#q;GT@bW%{7;*t|2f0a=7%5{tKKIc z6{`OGvjjTmlFO>rck^^rX20hugeUidodm5$KA~|6rxpv=9qbdtp!W#D!SKuq`uaznOK1gTbki}ca<-!hnoX7mEL^7?4_G>t#5Kl0e+%8#T2tdw z_l#rjJgfgsfN~K6pNlMTvVwyMNm_6+J*vuOp>%aoNpj}}#ejd;=4#vli=Kjn{3wS$ zt#v9WH+4*3G(A+Nr1i@Sdq5;Sh4Y4au%UKgmM4igSR&I7gTd$NCdcx%P5?&lRxuyH zlDos7hC75R^ zf)x0do9pu^1iIy~>Aw*Edl%M5CLriSc;Xe2lp&tRL5k15Zg|(0xo&6~)$WJS{qt>s zU*8qKA3k6K&R*K!6yG;CKK57YnYl0fpSq6u?visfhAj%Wlx|&`L|oq=uM2HzdVh_= zzi{EEqDFJM=3ZUY0ZT%{FZ-}g{zVD%-Pf;={C~$OeYpnpwhvSm27n2s(}}9aG3GzY zxqmH7PA)qu{w^ydaqot$DPr>6x6;`=#?5b7wEp=9K3}dOnl$LuRDV55XX3|>(blcE z@yRD^Sa2?av3E@k3qAbV53O-U91!QRnkca9Dv{0oIxpMy;$jGUxTu!k4dkqoUwrMW zRrHKXi1$lZg8hGu?B~|=$MmH{v=VzL#kbF zG=ePd3sL|D3@D5__=Zo%Jo8^^@_oZ3*N}+bXJ&H4n2@84YxN>1>!5Ed4;Q`s0@7a1 z>_KnV8Pfrv?p311OX{MdZCn)$3iF&8Ohh~Z4fu|0%d|{>`1G|OcAn#$WmtJ03;-bt z>kan-8Ei8XG+|7@w;`05Svn`~O|AFonR!c_E!RhmaS7t4ydjJJ9iEX@L#jJ>wJSu) ze6JUNQhsqs$DK41QAHun9+ylhZi{4Fsf;nV^IF9T*sXxCYZ`<}#lakm^YWu`DVBUD zAz&XJ31-P4J$ex*K*BMCo2EGv>Z1w1KhMcD-#pm=J_}21zLV+7}U`t ztp`4IF>+fY+-$t4SLrImK*$*7&JZ-^VIUp=QPxyhJBP7knWWmUFTm{qR~k(+aAbij3T*%-TNNPn}1$){vnQItD^m}mkji9tPX}TWOJ)& z@&t4+2Gv98UJX>-Gr{qB^&hYtuyo}M9ePv>ve=im;l;3SI^dNt%m)tD<8_XK z&oNT_$``8UJ>|>lL2;VbUeSv>s8>kkCGHa)S59|-nQcwD08G(pQ3nM|6Z8SG6f%%aF)DVfX#fQ+XJ4+Oxi?$Y%kNyTKrB3MdFTHG+*4(p*DZt0von$6;+;UX4e~hk?WD10pe+_*Nte7 za)Nrq+7Mmw4@jQnA`>bhr$|Kd2Pc1L%{k#%yxU5}m=S#42-5|E0Vq|lqRyrg<|9uz zyPV|1Wf+UW<|{o>+wdCw!89dncD|wf6vLZVMeE40LV#oC_we*R1Ddt@rs`!)h#1G-DoR`X9`9b&cWF&Kda;ew3)PItNd z_~Q7=XVstWYd_kW&ThXL9%d&A#mr;pg%%yMJqR()-PV>0TZ<(7d)#ypUOmJGEn&k# zB2o@vV)n8!e<-CCuHx{mYN7t+@l|iq?3!P)Hn!UyMG8MHjB^xrk}*0j{fKpjSfUj> zUybz8qL=c_dLJi#B-|e^_!VQgWhYA_DcNDA2$6nZ@QO_@i_W>zX%T(x0Xw?X(_L4e zWS=KF4QHI@9!RbOo69QDVXQOJm0Ur*uCu}CDCP~Oa!x)n)B%i;=@1f!1Co>EO1~Lw z?G9UB65aVm*)%@g_*MF_zGj%wJ?NO>wgV2)$8&&LqW!PD7aVS@#} zJ&hBqf$+UcI5xYNn`N&9x5gd@uWg@ziSQbC7epmeL>hZ(nX=&8cta*#fVO5lH^kogfaO)ouC{Ma!@Y7zh-QufLgJRV zSR&@TgddcAR0-Bt#qLR{U=KM|Q9G$$>UU4Eqh@c?>qrwiRL3x0zSB{OD9%1GO|q|z z6mtBmTJX5%&f{3kD;+va%R6OYZX-3JyG)L^M#iKDSLWV@=ID(F!*%!$zB}i2E+zs# z5Q(#seuetVuC?=I0HMNW00q;g*m;frq>h@WF%0X#8aaC4$hVY=x9n{5_hKh$=H(BkL& zBW$(z>fR3YD(hPXxs)jJnpg|cW3+rWb*VVgNOSeW3t{kQ_NZukv(7KczHYN?<(p${ zKxT?6?+6ztDc!gXkn(j}F5s*Db4_^GE17#l9U~*-CPzQEd0ECa0@)*0Yc#EJTCK(} zlqT8Q2H3BcfgIYCruWa!XCaPeCI?u&t;C75|c=w{y*pcJN56D_`gC6)>Cmv z%OQ|Nr>0y0eVJi3v5UQsw*r58!18#!dRk~4NtazSStN8bdo;?kQfl~0!~3?#x}nwH zb}RAT{H<1+dpn0Nl&>|5hqTkVU{h^uTHoDtC4@*(kNI`y2ptn5Sob{*Q7kljb>xKaLb; zZ-)=PzW?dCg>+6%22F>SD5}3M(yX5D>(aByd*iNk7SClMrsKyGjGHAUJAnD z>Y|_)9Z3ufAeB@ekTlMt6l-71G>!P&j}=x13NY2gzoPqm5N5SJFFh@BgSQ2(rtVpg zQzy!SX1avxV|G_BKoeSz-Ah26vzg^4Iy)#XA-MeWPu@J)eEuAOkRp&-VR-qf_ zCy!>P&dhE5L7O8e<>Kp>0O{6{T-K3kk%1vy*x8 zGs~K(S)YN;rGI{6`mb>OepdWf#!piE_b^}lEcymHxt@>TSHQ*kr3l3|6QbR0i4{mo zQ?Aup4Vo>UYF_WtyX%4S*igTF^cH*y;rhmiIT$qup11x%Jn96dJFK1y`Cy z+SM&D6Q!6p?B%LPZ##{I{Ch6yens6Z6PJpv_MjN1^{%gYDM6IuI4QVnLT)y}?{c?C z5DiWR>Z$JN~Lm}-nK1!nWJ0^kdD;kzMi{9yK zGY_kJ;-PYiSaZMir;IFGyWa83y&pGZ7^dxrSIy)Q$5K4g>z0L#)y&PN?=y2(8(pEk zUwj19QZ$Itj1Vzru@I-FH?fhOwy8>2#Vwusrv(HGHWrZqY$By1lKpo-f5%JbY-;lx zNOVFJz)xJ%<`tlr0yqg|N-+SAeGsn;4YxOJ_-=KU&sgt@ptVq2It^sWyIv44-}Lwq zWhF7I7C6JJYzoyoYTRm*N~bYOt6;Y12oF+mAt&*PTTWsMrxQ5QV&+S%y9A+%qrgEe z04LQmWZwx`yUPF3eY(c6_Qu=wxv#oTd6P1oROhi2sp1r@RSc{G=KC;l=c2)}0&u6H zFWX0mkpGo&r?~nzSs5T-O!InvM081W8ZVVAq|FR*z0M^sUJP|@7g2ZPnwcW49V zRN_xx7ycH0A;j7EI&?dAAX80qAyubfc&Rmb@-@90q#GmN%743lf>Q8NxNki0b*C2O zm}4yY>T}k-M;Vk7CzOu>9yVH?Ro=Os>V9*hJXIGEX{2>@ZdNdiZ}O2%td0yZfF#o# zgXkT2x8-arV`+XHz(ENsrdBBHaIn@!&0EXS=yHz@j3%2 z8LBj^Q-;Un?`T-Q!N+1-;mQ{L)`mr(Bm`{X+5!a7`}(CHB>^Px!~kOt;VAe5K(mv$@;L zGxK6@-kXlRnYEw>PO%px% zW0n`C*pyQs35(AeA8SUI980 z%eK}2<`WFA{3JGAj4Y&IavRt(49{iw$^cARoy`RTZJmeEd3AZ6`L;OG1~ssB+i+Y^ zmsWafYZ|ax${D@eTq3e3S!X9F68e2wzZMCa5APCBjWJMQk_S-2jC?~E0U+FAcUz{I z3w zNGTWU#CZl?a(dA!m<~L&pIFgax~3Q!;9k`cUh+^B|MVh0;3p>KA>8gPs9X)eI+FxO zq^T>YeVn_QTPt&AJl9#?REAByqK6@eQ$qOrQV1mjZTKiM7lr6R)9c|b>=k!JYDX>l zH%C5t$aq(alu>R@(w&`Z+Y;0rokY4k@{FdV{jG6mL=Mei1P%H8ST2ldJa-)Hpk=E+ z+rJ}@paW(I5gF(t2DQO4JoqdXQ*aKi^=NR*NvCnDq;VR^Anfj+4e2h~p?sb07zdcP zb9AL@*r9tDV7qdZEdoHTfAg??Q&P}Yyr0oh5QgA)T*+X$4+L>Hjr9GoikF`mSO zd=+ewFi7o^S&ZZtl9_CdPUq=maQ5{_WuOWc^De0BZULQs-EOp4s#?=#G?J<4%1})p zpJzFzGqQ{1q`SCnkwNMP8z8L6=m^wg5K}N?0;%)|^d(OwPNa74%&XzSTB!1%noS@d zTV=7kU{+fX<`s!V*k$;L%T+jdF$`wX4l#F}&hq+w$TvpT7Q6wCv;auZkGzIZPpygXj`5{!SG;G6E;bBaEV2Ht+TJ}vfSp|2zxp-E^J6*PqPnxkxlW@6r(;Iyb;e_ZIJ6w0_8lpR@oeUvp$s`IUxjpvD(R5~bP4YX* zK5Slm+`}a5fm-trkOHqKMbZ`fP0^PXd#vOjX9LS4NWyxS>{+p5X#(OD0>id`L_z8+ zFjJE>8PABtc9f?y*J|aaG*4=QP0H&=qeG&|UN?!7%%=}d#+iTs2u+|E`gkO})ocn+ z<&bA$5WT6wC%3P~aXb)Y%nLgX^9Yv2=KJW$dperUL{)i#)}wgdd$|?6P$_Euzn1#vRqBUY4_-;r z#&XZ70RLqnD(CK~7*ZI_BVwII+IRlxPo^-kwo}B>x4Yxs_DuW(btTJinN}Z{env-< zUE2W*F(&M;gLhx9YM@71I-kF8d3}RyeFF~ftmS~STdDdQf1>`O`=LQ={)NuW@vbst zUA_5Z>_(4%FGFq#7DNgPuo%9pXJEA@>eFEupOffenj#Zmt`SrFS%0w>cZwdrl}y^#$t*hD4C z{wc3*GZ*Sd3xz<8f~Mku<`kDAqzlZLnN|D5(zrKQ@GlUt*;1<6k14B}@m#n~{vi1yYd8#ANwH9o3^}j~!Uz_ufG#x3F(BEw8bwq7g)7LcR5lZOyL72aCl@7K0ez`W#a-BvkDJO4ufVE#^AM;b5P^z3Ky0Uu^CKy(p zQj-&~!?Dnl6p5~nIFlyuhy9ZOuN}W%S`zw3VmW^IpRfMUm~G)a`t2Vm{vVP@-&zKiUs}pL^4mYlzllqE|89gsefW1H{7V~w7PWN4 z&beCu%7foJp$uxF89qcSoxQDcxq;=1+dtpi|I_u8qi_ECg8r)5KRF+7zXvm~_S!xrRI^Pgd_1S=HID_)A; z^EH_5UiA!L!lgM<qX?cT&T{L5U@!>d(s;(#G z1IkA#Ns-9Z%cC;vufRpGdgX2on1>whPe`3D+{0I)r9|&RcLVx>iO_crGb5 zK_L<#8U!VXX~8(Q^A;V30a3WEuFjA_>0(3S#~t@R-X8=?D4w?QMj@9^_2w6(H&VbD zz7iBq#X3Sw7_|8gGIP9pTRJD1lkYXIpr0-YrqYnLmH;a%2}j0Qmp5mi@iJ)T?}}8#Q0>GGVem9jP3jTcjXT z8SBe8AJVhC^$hJhHG063t@dtdVY^!V>0Bt9JN>HD_N}?pZ;IRv*Yh4ME2o~KJ0{gs zSWz4VfP*unurViqpGD@7D#!wU^&&uj)5Qt|J()4}mRpF`;%{!%UN|wcz<< zAI4rtB6Tb^4Vv6X?!F_TZK=F_5(9MR)a`6;iEXVMj$2wn7#%g6NOpc82?TifEh=x6 zsv3IMlB5y&H4x;YTCQY%0_`l(k>lGqEm9CMK~q|wNjN?JkzI z=~;Cv?+Mr;J7b+BuvB>ny3$sjR-{Xr1tQ|d#8 zL0s))LqZqAnsR>FL)>CC#oyv(weeiqwsVoCpUr%$gxd}lMIS7yqtGVZ}D*&Xal9s|xoCWp&S zy4b?%C7B&rg1l6DDHx(FNY3)OM4Wip$yn-Zmuf!|qfL_9_Z!A5VBH?8;bQYVU)pz? z{&)t9ge?Rq4|OiU4Du9KW_h@*uL0J*ab>wsGxexwJp@QIwa!N-qJGoBQBnV9bBNGK z%j`fYm9TNp4+lVUXIqoa%$Gd)XqyknV{&FI>7j4ue5*j{!^{si`$9pPZgF)#7C1k} z2y_yFqVI1ui3{Y}(3FH6SvmV?F3|F!mL#(m>{>D{o}}{3^EJ7tNKR$_S1@fPreZ}L z%gW{OZS3uWdi~7(8MD%>w3hVMk1vtBs3YKvv3p?2^})arW{{YKc(*dUY3M&zF(qH^JZNbeCdaYQ ze?HI=r#E7@-RMID8BF^KVjCmU(5UST+9@-Y@W^rbRV)_jZ=j|EbWFr7`tmi{G9Q4* zQ&Y;e);eJ!3FXnJNwJ0oe6;}4$T(!nVBr2&n{I2otK^D2H6H z<=G8s)eppxpJXwBjzs{g#GS^fvxT|nSw`~nBnjFJ61PFNKU!OSItzthGw>s#FswE) zLs&lgUK-c=vFlJ)k5Z(ArqMo}wF^M%3ke6VUkNjYZDKRJSN)Ryt!aXq0ics0!;*~EA z?Kjd!a&tSj2j)IMSr#eZ;^*yj`3BV*x`wiveV1+=9&CB|?Jl}ZX^wvn$nZzb>n^I_HesoBB z_}NZq4w0v;H6M;*WpHkxgL|h+hE}c`CrL^qgU{*C9oL<6(ow+JOu(Qzo~QyC46Hlnq2Ar)VgStN^wY1-=w2OBomiUI{u+G3wl^w?i!sKVVtmvD<|Qf0~N9w|@V@%ilX|AQiQ<|4nc0Pv3-yJ|p(a z*3y%<<7VuH}z45r}>QSFeOA{F|y5Q9fr*P6e}Jz zeyi>d)S+k1zbo5E2rH*-F8Gi#2`lNj`O6jQLUQ~QWv(9Mxf`Q2*>opZOe|tidr`~Q zmzzc5^4D8A+%{!Sx3C_89^_PgZR3f^`64839nmzk!oBK`g&JeJ{pBF#+7ezCIdJjh ziouf6=oZIcSXe~Q({9TNhb_h)ZisE5dK5s1302OrR2#_Ux;e9KP6~-a(9SlqN=X;b z&plk)B<6O$~sD1=>X5?@*Y3c3CD^w2;FV#%}u2VjIgK95SQg% zStqfdX~tjqrG8)XfW`_AdjG4z zkmz=UL`DW8<){CIDb`X=wLP7iVT4=sc)9mB`t)|3*qq!nY(1zR+Q3^>_U>}cfV$Y7faSs*W zX3^nEZ1sn2@~l|YDc}G8+SBRxL9ufxDmVUR0M;W;H&WJfzeQC>`efJF3C1tP%)Op@ zUj7?KgxF>dBx7ObufG#fe*UZy+LONB;j#WY}Zb=6M&s5iL1jTIG_j$`OPH zsSSns!NY};pQ$$lfCU(UdCHl&Kfu=RlJb8AQ~=tbuvOu*SI_8!f5qUKy*!8b8^ntk z%l{6dE+f`qU;$p3D*?AYN}6)m>p>P&-bmuGIzGfZoP^>dwnYdamEoWV;+WIAYmX>P z9ZFAH^8W^-^y-(y|6T9JGKTJ56XrprxpIvKe#>3zj%+_P;i`?R#2F?17CJsI7pK33f z;eeXn^Xu;cUAV@6cTRb8g3t>?rjtD^Z6{H~s|ZSYgV!CNS=qJ~){&31o=>~o>NtG+ z#*-gSUxPlJI$VWFqlIJQ;_+aIsKuez*q%SCmZj;-V{jLpUdWT*=g+;6MJ9sWV)z z+^ZGyg`#s?m!{O3;`GR3lV0dEy5QN;3x!R~QL60*Lz0X{Cl?l{SF`U>WB6)r4t~26 zUUGvp-nN#CUCz1GGNC(6-h-nTs2+JY0}fc+D|Qu zOmg-JO_A;=umAd)8^I|Z8G8QL)nz#J=e1P&*uzCIv%?UhIu-rA14R8T;kD28GI0BZ<@!}!s<*LPDX zh^pCbxi1-t4HBO4$~Yvw6*?Lf%`oY(0_mANnaPp2xOfE;Q)d_3s$~t+gGk8IWBijz zn^?|)fCZb%ockeHI)*P@>g1UrTE-Y%l;2j=Q{de-YKdfvNI^D;UuI5+mBX0V_!iN%f>{L?uFu{lb^OqDQdN)5SR&Nlu(?H@~%J=~7eHZ-4>xudm?SS7IK{fMZ{*?N@(*#g^k z1~=bf+TVWSP!=Q9WAXS30%mk-z38^_22Dfb*@S`U+&vKG5vD-P7wZub=?~MmJrla@ z_wjLaf_5)?)2G{DS2JO%Dbw=HJi2IcEHrs?RpnfiM#-R|mfLX4Q5)O9Ss)ywdu`R+ z358$;^jEJ;Ztr6^D%o6jl-|v+7udTOuqN>C-YbuL~ECWt*YGS=JV22U*5%`J)wc<|=%-Q$=dK|1JB6>vv6)HWh{v$n8`qT_Uq2{gS zP~I(79<`?(bg|L=AVR+M#b#3M;|{R>ba%P zUR9RistMQt1<&MEN(GM%>P8Q>R({zk;|K3p;&BP*J0x7{q+O|_!z?72R{WuGWdcXlfrfxv)g&3hXv6rD;sMTyObRx_kLAx2GtQ4T>91`2K8S=rrN-Rl*tpSwkw*vsVNNZs5$NxZ^;+q$em;kTm+ja zb-BZzjVr6%HZqs{LJS*~%I#j%$rTz6bb=1gTaRZro;%Wse0MUOBq7D)hMXQ?{s4As zGcBzefCEX>V6D}3I4G{y?cF9vvg5t(-g}>Xn%y0AYGD5UYccz|t4ph?>BTF0E3Lk~ zTcfHg#@5F>rW^wOu(a`2c^Mtxoe4sCZm&*Cgn$ zekYXBcTMe8Z2_PFiLvF81Rai^QQ$eHXAl?4Uh|n;kY#vVuBYxIPm{a518OCEt&o7E z^P=aHR?v)rki0LOk6BHkL1p#VB`*p}sN(TS=bcD#vGUCGasHYxLIpp)!ZQtC+l*9Q zX|{2gZdWaBBdE78lUlE4)n&h_9k~j#`uRod6P-;u_;9RM{{bvWa2fKF^$smkeQ>(Q zyZPIlc$#vsGQ5FzODF}&3iDirftzc=<%|G?>BX#9+=pN0Idd~u7Ty4}1_gDO!{IfC zy=oSH5-5piOqF!Tx_^n7$sp&cJE_9IDraCZ%#+s^tTS``%ml3x)L^oLrY?vb3u09- zOzTRkVPELr-fs5kvgj4_CYBRF|4#8wyc#v-D#?@;BEG{(qt%_j`M(eCXQd3}vK+nf zJHCG)`R5&bGJoFYT=(o1^ZE|+o64*it?}`Xor~Mhw^7DXSLGXUU{^_x;#v0V`tGw| zQ;$1BJ{Um;^UHk)cVNNO%6=Xlmbo6%;dtSg?-CT>;l>|Q=0F)=C8?^jqeG>s#AIif zcA|rCjgY}L0Iv{O2YHiZ0W;eeZR{`%pY44)MX)40lAIP>Hc7Mggcz;|Pnvk9N*x3O zZ=|ta@RvGeDr`uF8W9Vni`sfqLhGi6+({)q|njOL(|ZtI_iKFK?4K`RjDZ?Kp??@fCZ$3l!Ok7 z^e!Mx)HjTCrrmSy`@8p@`P_5w@AAiDueP(-de&a+*-!aCCPXY3LnbF9AGQe0-&zK1tV6ImPzyX!h2jlntC;Bbg7mogy^Ite- z&2xI$yDVoJ+gRe#GqypgYxz%>_iFkRZeysJ{OKQMm)PnY`2pB%a` z5+Q$}O($zzLSGYhU!UQbLIn`nNDn8)lKu6PXM`cTyu1PzV_P(}yi6XDG41g)&dtQR z{BC4l!=tW4u15;8vN`KZ4_HdVtQ_y;&wHkf;Q>k#(*sCjs5r7ho6ZR7sLd>%;kv_ ztlU{qR9#DsD->bhYh&Y@o!(FGS1rtbBjnB@o{wL+x-4a7H+U@;e14NgW2aY_~yk3ge-`IbSH7F}ev@aE}uu7?T7B70Q9%3P+ zgkAg4yn~NDpejR*F7*XSe4u~fXs@qr=}YUOerF+;z-UK`nU~Js2!YDe#3+20*Esng z>UJjNM?}xFrnd#YIiaM3c7D2u96Jji1+SX18*gX{9@wWTVyb3K)_6>G+PX>eoZQ4v z{$tAb6tD-DuNO?OuM|F}rfE`M+G6G)b~)557|g5$3X_N{pr zj;#M5TCM83-Kup<+TadJMqh|@-tUPuyWR3 zkDafYxgu^l!4+b-Dkb+bn--D$;;vx*^;tv0^_|O=$!WJ1uHAV&@08`UAJ0bN%D*8n zkrNZ=0fZ;;7nMFI2aq4I#d0H!HA%|n?e`0N9Yi_6M0wkN(fTG^ULHR1#Sv)gVcREp zNl7;#(8dXaVWo47#+21!m09a_mo>8(N6P}aQaR~pc(0718>=9&1*4j%K#pd;+pG_i zn0P`zZP5hdJM3>fN`wxPD>t|w00}iSSTb2>q{~V zWjzVlXWL&8-Bt*WFcU-f)ZIrd^cA-yf8=*wG&)X$eZXF~(19V$G?dz!_M#0ao{;MK z9@w(Q>qz;M@WZwPUcn6l4H&0o7sm`Tm|6vtUGTG`m%cj})7wcWU_eM;VF19Wb(TfO z_Mc5_avo%R(UjV9avL?6lSMx)+)I-7P8V%S2Q7KnM6R-~VwV#B)^(niXk}TSrQ}ae zn~j;H$2~B}=%pn9sP(1T~GBhII zMtUd)KiWRx=x%XW=euJA9b-up42jUIJz=(7- zso$s`>1$X}HZ(A5ns9Rq!V-QFxqVzi%7k6BxM9}nHM5AG2 zhDKQGoT`#+VeE6=7C|4UzVtu}bHKUmZ-t*FK{9=0A$jVS3z{_WDW-}Sm)JJtpgkR= z%=4{NUVJuR4nA}ptVh>vBofoTM=u2jEb_W^2L&6{&40CZ#EkpwuMTp-Ah-E?`tzou4^PAg(NRE;pfRs-~MScb|cj7<&&7No$?H zkIeN-m&Mw7$aV%;e>_w$UDP6KIoqtK>KO0brs3|QDmTS;i=Nqxq0SVA%U;6d`k<3UHX-%ym(PPZxaT3CiE0{dLqeWb7haZ~ll-D55d*WQ73(@^}Jlo2Y zfaB%m@1{=sxStbnKXVOFpo9uEvLL0e;znu?NauO=y2=ATD0g<;4cw{O0?q9+ffx); ziUKX*8`5UHfi6g&5sGZAo@B^O8tt6Do88$Mv<@ob zrzol`{b|56I4A9(uzO)ZxE1Fpbjl;~P5zJOfycd73=bVEZL-rp#MRA+k0|#z`y+lX zY<Ims9)~YD)$T z^Lm2Dlhe|yt$f?p^K7g)H0d70xMuwLYQI-2`hsoJ?8&^+_8zbNM=jFgaaU1>7_*a^ zp2WdnhpKef)Kd8&6$=>8@lCi>v>o6|JwUUqEfB@*w+d~lO7B8nwWM{#oe&iiP=)n+ z#s)BWyjuW+-tA!xHhk(}Hs))RX465z1SKm+7*tQi1*YpzNqQTsDJifQ!oxSWyn~8S zN>jrPOc_IGY?KL}uZY9G1%o-{q^%SJKc4vxYh9CZ*3pgwv-lyy7l;z z-&$#dhS1M+U(d+Td&`&+5DJ?=ac`Eb)HglSr= zd8stE6+vF4R6QNkX!hl>PYNB56wbe4an1C$IgJvBp(^KfE>IULqWiap>L1l#!`U?h zS$yc~V1>u+!Ork1WtKuOJBWkoLkS;rx<=4zwHy&+7_Y8L1g1c*TA`nNqqS$G!Y;Dp z7kmG2er8F`sm-@)NRxV1%Iuav$}uBsCej0*CXUR_7>C4sN;Z~b(mxEBOkQ+x zu`1X`ZM(|@JugdM!kUw3McMvvR+3q|R3K39qm^exU)7=eA%cbYxc7B^h3)h0%NhsQ zFrLh|8NI?nm2}*mZE4DjwKRRzr#X+49=>$e6Z>!(kt6Yn#=Gc62Y6Avq+iHy;!B8b zAhPE`FJrcRP6BFU<%6rq>{Y#D?>;QWcvbp0qFw;Mq>$pe9n zbMCXOg5%C4Yy5bOa&=Qkeg+dAgw>dgIx%A(l(RE9P=r=~ySCP&G;es~@@!yFBaJN#+0cgEBQehCrRNO_aTH0@{l!LUp}Ng1228lB!)r=yAE~syJpNAY;dorqayJQSOA;1C zRkSYc>qS|?WEkllKuWuNh0)asbIq<^J!Y;!>HrEGO@=yS=#3;loBqLgnS%)_O-kN= zvl!`QeY;}J>{OYLniVf1^UBQhd*b<$^$WB2tj`1}tTY7XhBnt5ZyHbaJFR6zaeOWRKAYU!gURwD~AZKdTMPi}u#-_a1Tk<$e6RSf~&`qhkKe$)zy*%s-oK4^D`-97Y<5k%+Fd4EQ$-8X85SwA)8`nfls`M+ZK17A44 zm$qi*Ea|R3S19FeE9KwpfaF}y&{L$>=&-8_I8VjzMw(mhPNPhYtp=_dD z{iy@-4~|CSlsW+kdkutT+({7vb;h}Y5-?-CK+BE2F>z^N+h$_mE0-rpaD4;f1lSk| zT%yH>)wtUY-n@0s^fnS^-q~7~ufs+0Bzr^AT1`&QzRn7Ux-`1#4~8>jZ?&9>W-*z)Z!E(C6xPjU$_Xu7N>OxpzZrZU z1+YQq)3@#)pS~XHnCd81kenlxuV*OCnh$W&wW$%)ml!6bd{8<XdmBeji8d`P_kWUG`pJjU+3jhHKjBBV^61nY`QpK$8;H z!DVg&?XY6=_?~!}Z9-8zK1Gd`>4(n8js_3)t|sQot$HP&77=L^dYK=Y?7}pliso5i z%wW7K1rC?nsx&3;Gib2G1^();SgD~3KZ6g=q;|rSJi{YP2KbWAW$Vp1G0J37_hiX@ zb<2tn^Uv~pORX`a3V*s#m}q_u`3~Y)eLw>!u`b>N6DSAk!|S1D83bfO8Zk&}>~p(h zBk@(ZA%oK@3QjE$h=fCxbNF(RSaGy!R65(`suB>qUjXuCc{AwR1DUoem#(+3-9?g2 zm9Fa!fFZ`_m@;LJ4qcc5){Nw_wJzSSZ$J>sc{Te6+o`$`wsg#05I*BqX5kpPxXKl- z64PWkz>rs)g3%bw#LUQ zH7|je;?Ku$c9vM5+hR4P3`Ms1C%|T~e(+XyXGM%cdK4TK^@`ae%B7HNj?>aPy-^s&^RM`$=cr2T!9YO}H7GUO*B`q3?m@>G5SkFad4$z^!r}oj* zTQc!ZKJ#c#hpNriAcXNI7}29qh8O0^ci0U_OV5J}&g|5dh@M(Tzd2_o3dOwjdVoZ75aM0Tu~K3HdO>JY34c3}IaZzkx-{$~KPh;}0```Y080?Krz5jGV%wkA@`b}l@ioC89j+G-Y{sMDIY1olO2t!h zaBlL)^(9FWNe7`Lchm+7Ud)XsvxNtv;VoMY7b^+R-GUSozGuH;=iMiK$3tMTMJ;|* z5U*@{*vspv{pEKzZ9^%?B36{GoYCrPAY#oyvkJBWm)GQNChUj@rn?zGnJX0YVW5Yt zRWuflb*>}j_sn?!9SDPb?zWY+uHGH5EB2e$3Kl-PW0`Uo@7GzKf|he`oFm#Olvd0u ztf?IcDpegeXH}mrNQQ$t^Vo16-m%yKwLy4GsVB-q8tCCYr81k?Zo?HRCjBpoPbijy#`9e|+;k{pGBrpL~y+ zy#;p@|9#VMc$8fzHls*87>_#^-gllKB6B!R|32gIW*uYYU z`!i~gWs_x-lL!0TMIaukpCj9~^DzJiI!-xlzIoEZ0iigvK=4acZ4s_;uACp9l+IXI zz}5Ij^)vM&x*P5>dz$W5<`kS56q?V0Ka1B_5gq2!trQmEp#`-V!93WqnX_%MSd8gNmWN<5R9+I$yA=% zfe)JJW?(w`Wybf- zE&^1^OiijZedJ^MWJTIcMX>N8>~WcLuD3T{My}v9X3X_#T)G)^PH@q(k#*hWl;=AY zl)~@pQSNS~XUuC*0m@2D1yxp?oud!tY)$X*Bq+b2oP{iZ*a@ov22V>{GNFd2^#Ps^ zMv^aPX5{mN>-Ct(ey3m>Or@VoIn-o6n1TIpqe&>SErCyO#?@GJHkyV&?X-oNVqSjX zh+;#)o4hDZWnvZ_y>Kl~m3>rtvNe!FAaaq@B&9{-f;G7;!mzw%x4`1}vr6Z8S6#H8Zx1=&zWvNfnptg9%e%u2z zg?Q!RHkp&439M~1@!!hu-z1$SCV?fmsfCOJ}WxEh9S#TOLu}>tpbZ*10o_(C3 zb`wVJvCvmUBxzcvH2@7TB$#q8brqi}Q4=24$T~Z|)h>FE^%i?Mzm5PrGi$SvgOBd4 z)>eXo1?+%bnWzoZ+>-w z-=5^nx85Ihwa1c@c(vaz_Pe-}27QCN_(KpbS%D^}GW6gJ2Q@!B=-Y|4VyjcQ2R69^ z54?5;&9rAPw2_L#rK87atKhu~P*T31-}_uqWKo1d;#-e9Z(w~@S>;Jg&S)~a z=zc_pw-btDQ%*Xb5oGf$JL!+$8#=fmeaPw z&N{x(9#j#D&5+;ps_u2oUQ;AFAr+*M&O%;ZV1R2b8C5wWh%vJ*Mk^1R@p^5iPdq47 z(GJu$&0+FD;VMc5)5|^fX2fM728OSh80}QKP@CG_V3^MBEKy2IgT`8R>{l6>X}=eA zY(8742rCT66XTG*#?nB$Y!}OccwLVQiE-!@I&D@3YOa3NFSI;^Rnmj}7pa>kG@ZjhVn?vZD$EkB#e?XC%2_hi>a_SV z_i92ga3HuZT^d;q!BDD-QykOM=c_1wX`p@jXGr*%3QUKrh2!m_#6-F+rK*#Q?mg#8 zqo;10&)W-`CL5Z@z37ufD+D(Qi}HB4#uVvZX7W~aY^t@`NW?cb5dqef@TP-i3QbBk zNO}FZWM7PVuN{35`Sk2J3Dso)$U&#L1#ftZASI%6eJD0 zd8en$3dVzQgiqrsU}K_6>(%La}?PWph{Vd{cCvd`}_F}VEBiMI$DSuD(~oi^wX))U35 zCW4A5RoHl41~Za5hT?_b==Ox4tRj|BR29i`QioT8Nnb3`+}lheUhh&*f>irxx+VcwF22Grg*{Of0<%!GSLKB7%cEZW@9f98R5B!|iyDp-JoD7PzF4DWPB5KKjOgEF>N?p<(5EuIos zB=i_Pp0-~Q3u8g>Ow-G24wo_y@xIrIb~Gc2hrGjeSu5w3+0E4T5)7t+%zQ&x07-8k}hp1rZxyR zs0{F=zch@5mK~ZXfx!y|inEj~rN*&ljw2Lg{>G*{ERZ{S!06LSHk{zJv~xbeKn%qW zfhWW&J?m6S3|a?d9)@4NWShjUoI4oue^FBc*>$LIV@g_8gHss7J< z{87yOX9goYx4Ws!G$&rbNMnjUux$u$n}M0-OC|e~I$>Qs^=8J3*YMFZV(5k|EJi%I zQk9o)8eiqDXN|%+g(g%#z|2y7Gu2_Xq-vPA?%~6i!73W|TN(O*^sKX??Wc~Z9vgZe zGI>KN#`|XV>iHkO#XvgvYh1Y+3#MX66{9%*)arfrkrwp;)i%EbfO4Bnj`&jT+NF}e z{qtGFN}@Dpt)yUFybSA@@ks>0?iTD^$vi6f_=$t33lk(5cJ2aIJ{}8f z#ad-L`ThB8FI$eM;)XwbmV9?p$X?>a^<`r8>g9Gusy_%05`sa=&@kyye9W1*en!PD z{tye8XBHo8p4O_?%6}yOfWCaEQ!Fgqa*z@!#qur=@sc2@Dm0zFG1Bt*KKz^r++#pb@-8ldFkKOe$+XUM>C=|mx&{BHhtzE#pY0PS3 zURTh}yLc@f9Z}I1j8JcZjoq#MrZ6IjZ7tQ0WJgCXZFu9miU`kEkE|8GowS+PS=Wf; zt4Hcm*g{OUxh`=B5)|gth9`s;-BAmu@}_{$H1%fE`!j0q@=(h+BqXI$`?q~Al%LcW ze7qSl`O#Z|M=a*p56V8-64u+V6Y(j3?53A8VePtDOdC6lnPZh) ziasD`0=Bv;V8VY{>A}92$#)Oci$^AUr51UtjE2L|z>2j}(&KufJ8+6%k_b_-*1wEf zwQHQH-Way<`Xt%#kGZzWveBf_G|cK`AYb@9hKD#eG61 z3Fnh_A3MU~q~s-Vk6w+>3{2xy$-IOuFYwXJoH)>mC|tuHOTr#+xn)FLof!4E_e~~u z?Vqz|K`qQHkLl$!U|BGI=`sgUC(L%P-vYTs=Lyg}i0?OgB60%n$kMCO)N23&fTK3k zIuwmV@1wT2SSPaNc(m_j4!az_d$s-XbCfi}EZ$3T63a&@wz5#m|KJ(XZZ}iIGHon8 z)VZUo9cxi3V8|zxtSrN9$%K^>NmgU#v$6i4GaF+|Ch_H2l-oTCSGv2pJB$Sy4z+n? zOxnS0>AqOG3I~-5B-Gjhp%|Bei_}PF8%iFvOHs-9aqb$;_ySdN$L+g*YbaD-@A(N>^9is}Cp`-s;!d=^Z%j2- zJs~Z$G(%e|^(xC2Jn%x)+Q>OF*pknnFRu4Wcl#76ZEC@aSH*kT43Ui%Dzg0KDxF0s zHLs)?WO75G8`Yh;7&CN#V|qr_V%D7_4L1aJ`%ndij-sAcr!lSiqUP+-kOTDd+d_85 ztG%`9>cKaEE&Ek3Kht24qL-#>`ZxzZ=q*<;eegHTM^fVEbjiDcH!NGyNX*v87!e%#pgIj>u(ew9~- zc%S&eZpvyMV~7+XNRn~0WOzT>F{3D;-k3NH|7dmuVKDP9|CMC?cTREW>JzV9djcG~ z)`2E^_Qs}{C-o+E6bnt7{CzW%r)!)|_i0IJLRXm3CXFRTizv1oy~VuzRD{n~b8^Em zx~@Sp2YbTOP969jXdUFlY>**4W_Pj!J6IeON$s)^&2a&qESy#1)XToBc5l;8#?xRW zZ@SLVn*OnWW3=Pxre4h31$_y)FXqRq3Fna@=1`kd|wOo6P$+lpWB zm$%9^C}NU4hnh#6!^+OOQ(oRcTS1UJHtaZ1e0?5jeye#@`|W4` zxEC*Fe48ijXx{PQoi@FOYs=3Ml@_rbbAqj7rKzG+hFBSz9o}@3n&U0X{{Fn~RxLxi z*W5(IeIS9^fvZRXsxtr@ckiHwFF>k^>9~~aR&bB=5!*Hd|El?w<{VZK%iX2g*2l)p zxMldgBaJhzR7)W6QV&>9n%{BBKWr(qxr+5Uj>VOGU_Z0&2kbfeN37o$dLS4Vc(XftuNm>bEzIg0)X#%s8D`FtV|*TTAv zvHRoP2I8rLLZh2$_n~pT2-$&H`eMNF({-g+0cWF`$PXFBN^pWd3S>(pg@`|$D_enN z-QBCx|D%Z`SMnPU@}I>u{P8}2DHpM)F+da19)~WRj3Poomm#ySiWRe>Y8Bh8*K#-F z#*h?dW!7Hj+1~t_VLww0osiIl3-mO#~&J8+CDRuOjI@n z=yMnUI8jsgi3{<@%;k4|I_gS@MrW)E_WgMwm{t$bFqZfb9iHRR={G#1%_Jw1q)mruuv)D-c=7=b~`plp25nU&jXW=z1`s5}lcM)9JX= zLn=R`-JjW5tZ$`k6)0I)(j)&MSk_MZmk2s>43 z>(=?&?6b*h#n<)9X7s>}iFroRv?>Zfrb6X+os^fa${U+P%$fyCRb#dU;>!}lFmh2T zeS|4__j6I%2w4!s$*h?SF;2%Vgro$pXaPYHd@p`KV$QeF*ch-eukMG#w5;;is*Cy( zpW^lA7l!O&uO_V}ZM`!wE4Jc|ZkAF|=w0@%=>NhoS;vlp{JZB++plCQb2%7lhX#uJ zqb2V{KB8^T7P8$t*y`E3q#kOoDV{}h^6zYM8DWdZ5nfv@U|R53Qloxa2;7we{`tfN z-VpmD8|n0rvnW8)QTiX&#NHFX>%w#No0Q)@ggnRpHrRcK{&yaJexv@Uu7~7O2KPr^ zo?u!deg2dl=GKCIA+BAocs%b;#9!&DpJ6k+>*}_5Y-H)$s8KRk6vy9A9LXVgiJ~(1 z5=gY|{=94&SNy#J_E0EP(unK2mFZ)l*=H=78)XlB+CsBs(&dnXpY<=dooZpFe z_=X$b(X&PV=Uy;2LwOUMYo4+w2sN!_{3l^jKK{a_& z@bIE+9(t^&IKIFrHKV76#}Q*&?}{r1+YvL_c9v^(1$AS0)J(ZITD}#Dek*FBK&c<9 zFx7j>iQp_=7GMVt{Gq(F^yT{l1~bT*d=!^uzW~0mlWj-N7)md4GDof*`>^?AyHN#| zgaSyAZQi@c`A zW-AH%vDJhrA3p86C-Xbwh_1;;$Hv9K+p1wY0nAfEiQL+-?!PQKx>?b>$?igdQ zAfcX-d_5im>>#k@cqOzflfQj@*wN~jfzsWO=JlF>UC(BC6I5++198yGMJh1008TmS z@aCrn+rwA?-LE+gn`0-ecZtG$Ik)qT=N0E)CSYIbMeHxP{g>$Db?xQb!~?{l4uFh8 zgNTj9(DaAuPbah_BzDJk5P4VnmlY-(bBMf4_T9m7(9bB)exb;ozDLcwL;dbA?sJrW zM)s*j2md|X1ndHRc~!$+#P#--pNrl=5BhK_*1w&+$-8IpW$XK!L0S9$WgC&b{OL=e zMw$E-d|3Hb;%VcFD<9ogCHVIIjNQtggb2FyQFi-EBK6OU{drk-ab0+gu;+$P;7{l$ zbsT$lx%Go=zv3<4WSxzamAw>>?=Jk2ga4;_L6qsb1J4(Zw1RCHki^o#!Zeb;{PCHw zP^v5pl=f;keE!wop^vIv0+SwM`Cel1%9_WG9k(Tr-wSc(R9(ETq3RVk1C~Jp91we4 zU>ZGPnPw!+hx1s+?)IK$<|Yr$eYGTv>xNI+W^&tek9^%#(!v)Gi_v$L-+UG(ZDfCp z_rch^FK)foih9EOL3Qx@0ajs-?ieWB6=oDQ{ay|1peEa?^=Is&ac7Uk!{TT3J5onE@+)geb>_eO{K_W52qf58=LsdqY3TjI37rg|+`OyvF? zWW}!&z{!7OW^D1>;~WCNmi^zZY<{NgrIhB!Y>y-pnZNXl=pP)M9F`@`5A#SoUE(&P zOp^1b3q{rAqPK7nZGO4@2T&_BKzL8f7?UP|%EK;=};4swb^!Dl7afK`y&NKbQH1qq)yc zOHj!7MV)V2eDI`aI->IW`78`_Gks`wUa!Dr$z(RVFP+m}yFb+QGb(*1wI?~};4B9l z8;^tI8>~(g>x#A}FZ8wg@(3RJgKqbem*HJE+#VZ+!qq~})6vw&(c#@r58d>V#0lex z-`1V1kY0a9Y7Ol-4@46v>i%BmcuUkG|&rKcxH0Yhavgv;rroP~u58TEUBFe~-Yv^7E#{9RT~&zzUk` zt4G*SOQ(L`bFLl#f#ddTsz6#8e=c3$0QK>k!-zy@)>v*8_u9jS09uNul&Lhq`EMrb7x<4 zUg4$b$rRlww6yF=z!S3%xvgh-JPUFC5ztHi4KbxFoJR)2pouhJ~W z{ZN1Dfe6dQ#Y=7Xea0ibU7h=Td);EMwBKya#T7ADCMih|e=oG&mI?8Byh8Z_y0ZY#jilBiLoKoY|q~dKOej= zz57;P1qZMf6J96Zr_5@o)+T9xjLoVK-Py?yltPJpd}y@P6jKl;_rQ_Y zDB3iFRr5K_7^4n5LKsD*uH=lzM`S1MFL_S*XwVfsh`k|fbL)U|+4x-xw(#@EX{|QD zWP+lhp|G@UD3e>%jAxBe<{ja-&dn%~H9U=wU`ApUWw(keKHuZDi$`KV+eE=Gr_XP} zAZQ|S7zPUie-#^Y~4>B=6jYl`=jZ1#Nrp|vlA7Q=*o-vH(zNiec`|qh2%my z%bp{{quic59+&xwKmB_?bs+ImN*!CUNT|WZ8T_6s-xhR|mFt%U^$!hf!#=)-yanA9 zY80z4f@meo4j=03kc-$95nEWPONnlo`NHvGJ8j-#evmgt9RGZxupYmWJ2Qf-(%v}l ztA4Pav`nBq@we?+8ooJ#H%jfR8iqI@h8@F6OBLMirR5Kf7hOufviJq~6}ae`5{$qNW4Idhd8MLVz;a zB;2Wu+Uele^#iY4mNTN5!)da)sl~vsY=!T;ZY7jk>^Og4b+XA?>a)V#;dnRHQ^I;^ z-$}#IMxCbblSJlkiMthUHgg83KHBY~{ExawH)p%H-CoNcCm;L5apK})!wNj+*8N8t zj%f#03j0^DhDmNogSB5>2`Syd=48J+quDok=YN0Yzr~Ebkw1UBppN*a00%r3gP^Kf z_{D*t`r(A~Qsp2f>?~G?jhcWv-~hZgXo^#K_*Yu$D17}MHQ@QWlUnMX;y7hH>po~Q za8Czl!RWUEmZFKPfJKJk(-_Gr-NL79pDl#q;+QfWSb;SgMP;u=Qv`9I~!bVIMH!`i#Etkd682&G1{2qS=rO8>fcZE5Z(w{^670wul4&-B->uF*TDKfKqDUOpzhFH7>h*0*(^C7)d>;=7q$x5>$~ znf=YB|5S6QE1fkp`AKV6pr(qfHYcxEQr{)ewX9zbo~U%_wyb3TQ$J`lJBCJvB~0s+ zIYU^qTBvu2V!G9$uN1^5d+fQ5*u1fFC7H{f)E-?sv2|i{zP$5|wrk&Kxi%lRO=we% zb=9lnDf+74lkXzjJ`eevNaQ7WGYfM}_l5C;zi{;2JNv->vSfVCCf48N{;J*}DEbQr zR$0K5J+3`H?JEvhw>TfwE^t3M1hJ2u{=_S2fizb1tqxN^PiRM0dgPria3a`w-;ns(vaaS61E#H_=V-A@;eA)jb_uvky#u^=I(7-TTYLt7aJT>WyTpEMDYHv& z5jL^+MEpt3Z&t;IvhTlV){8ysn+J}cs`%w8+^@*~`g0e4E$7`c`peTn5AOE<`gH&O zn@ii+d4&W~E;|2yJ3IGp{M-Lb!^3~Ho$az2R-7XajYVt-;@XGe*Etk^$9bsyAX`wF zMhwt_@JwLmTuS3$Fo&ii6?#fXDtPXuAQMy$y-34lnrYRegYi{u)q1IKw&jh=t@-tO zdu*iN3q%e#*AQ=Qo3^&CTd#I#8I_EdZJwJ|s`Klre&VmZ-c2)ezLopB=Eus<`q+V6 zrV-sW_by%UE1Bk%p_zk(6+fv5hF5$#BFJ5bdaqUp9pmMOUS3!V9{s}686(XAshy=1 z5sC?6V9%8fMLKC5&451o_79xX-$yfOev7)9y~9-LmM19p{PxrB?=61&UqtR*cm2?J z=QxypTl()`(|;DDMSP`cs^woA5dC~bRbOCN-u|!B3>nd*97j14N*n8fY5lyO@$_c^ zPHQ$i=uiGT;-qkY7kyXL8T#u}KhgR<5+llz@nNnN(HAs#kBI%Y9EX1Rb#yiH2|4t~ zCFVL}T@24SM{i5CK@g(_rr)wVCR zp?rypI+f0R@os{Ga$@U_$7jU{FSp3t*j0mfyk@=o!r{iPhPgg`yuh3Pp5$Fzw7W
Q3v$pc8VL6GnOSKHiL&@iVc02v}Xm@lD2b1m3{m=e> zM^^~#H~Zi}{L1-0_Q~4&z;Etz9R6oF{I}(@|J8EwBeyZIrpE~+O#5xDNeCALsUj0K zp}WI863X}8N4;Dl4SN-+@=}=M=mAh;3(g4zN;W?2BKdYgi5~Ikuo%P^?)bh3q zYsTeNtJ>l1*qr7i@Y6bv@J_xM-n>q>*R66IizV%AumoursHr3!#ru6=a+^w5FczP% z;Eo;IDIsQs=K~IZOHk70G=kESDe&C9z(VhL_ln7hM+tr(XuLeF3yZZ;wqa9(8!GXDS>nRr4{@ZUhJh}HMY2?_SaR!(KY&;jO?#V z>I$0~_+{a7{Lii6B~$3tPkS{u{_E8DLnW=2EkFI$u#pvWAejofp^y58>AFG zL!n^&NMnh5O^0$Btp{wE!0EQVLM;FnC|mGz18S)(ZawZLe%ORqMSVqIP(yK+K~CdM zKAfuupUh;7W;{cz)^T}?72+DNyY$=kx{XG+_P(>sIt{3ySLV3N?R1^#S#8TK^D_mP z#8!!P@tKMS%t~=@d@|L}@C*D71`+Pu91r%+BKw}bw)#jlBzZ=@O}XJl1iZs~^YJQ} z^M{_xUiXy0I`obZzd=gLa2)%$2f*)3ya>N4XwrYHbUgL@c3*Sx-3Mh?_^Y4mI|8bixEfmY}OaEk~7{cVY-|sF8ayUq27L z=KHBum$#mUxpk)!YM^?k4@H)pVr8`jGk!x=ucxzGw{@(d%;5U!r1pIDKoyNdZ0F9{b_%B^>PBJ9gn24t9MUyV;|E#U_!z zl?586965dHCJZKG$m^1*c4MW5nV7jmq*PAo+C}?Q?dO~Z=tG`2+zBM{$AuGDL{CSz zg?7Hew~#XEdj0(AZPn1G7xG4^P@r;?m{-EFOptdRtfRe0Z^FiNuomSxC3{JUc03FQ zQu1x%v&{C8bF!Q9s{{c;f6uj$t!tjN`_N6H|Mqd=-mm>HM0 zqR`OvgeR0>$M*b;&l2-EsS*4NkULOi-ZBYZl^f*yt^lqkhvx$#SGn)fbQeE96_QWh}tIT8bEuw+>o#9eX z^X9B#x@Dp3q?j`_NSqT=^MOAmd?MW=SlBTstgNz`n9@g-*n8n8lchfn>svKjAlo4C zYO=DjA+E@I!Wmm-zEPcsUb5IbBYE!#KAh{=CQYm044-PoE5iVaYvrKTRBLn?eXykT zTOF9`PpdelNzfyIsah`fmYPkw5gUoaZIT!-ZhaF*4p~eF+l}IIF=}d0jUF!*6~fsd z4wVwsmUvbUDB2IqR;&A)%9rNWFHf(9qYxL>fdVXN2+9ewwF(@RYl}+nMFS4pCMI$c z9}^2E{ttWK0oTN~?T!7YSO5V9L8VHUF3n1>0tqb?DFI0cD4_^~s7H|iffF!5s7g;l zf|Mi%1e8u_QW83d(!2B~e(~tJ&-3ni@80{~z2AHH`|kXH^P8DHv)5jG?X~yJ?7i0d zo4T}Yi9U`XaY{2obz*%=q&ulmJ}c`41-dCY6%wF`=4kds*dU=juIV$qpSS$77H7xC z_Ws!n{NFVsqbPg&WzTAW1#Zbs@(L%LjeCBfvWiM?{Ph--%jTlK1}lH&S^+R`7@u;P zW5&!~k@E+Xe)DKResfLOr=EI0+juJKTVB$yjtXJl90AV$D;ras{}Kt)Ip<$m@~@8b zuR-Bo7)?ie$~)D9YG1 zK7NKCVs!s(-pgJMeYAQ5~N1~`g$xT2}8tR-b5H*LSL#3J+D^A(HKC(vT+njSO!I%rwl^wT6MycJ z0kDLtqek6V*#~1A1zQtc@c?#1cu*NJ1Ur5^|5fD$Lis8az56EF{8bWLRf^339Glgw zq1lF0d#eRE7ob|K0s)^TpQ=UZ=_v`yk3e)jbcmaaYv8uLfZb^+NL>!4XOj@zEu2E& z{>x@D*9{}V$tBgXR(PmqBbeHHjX`ZpU8N*^TVo~JG5u_qo>8}dYWRM_!gO4zC7!Wm zVW!?;rm-;HRcx7QZ@zjJeSPxp|G@s~>$kh_zyz`~usV{*n)0=oKn`?=lteQT>uuWDLiF5d3*k4WEXyIUeNE50j>@~3ka(AOkx=5_n?eg6+8x06VW zzSngR{z+_2PXS)}`r3%Mw{^B4%A64`6er%2;3L(GN`_kQ3*B~9dQ1kV`Tg4Y8@0BE zvV*qoRB4fFTDM1x{3L(~jrs_P?h4FHgWk7wm|O3Hcwn#=Khmr|(ij#i;4-DiMIoB`tflTm!xvhX4H07xx0Dy#AF7bk=yZ3SfqLO z`f{6`5@U*|rZhl}muIK*fHId~ZF$kmiUO}LoJ4$XflHT)(;nb$!QSh>eC$eXb4H@5M4hE(7$(sgguXLT3(0)M(&2QN}vDal<$IT~-!~7xB@`POfwg z9B=Kr+~*y5z1Wg9%I~^#>^s3BDu_3^(?owGN(*Qih}*fzpTG#R#qLaS$J3Bd6G0V88@UlcM0NEeC%_6m4+PX{fowk z%Ek`b5~ls$J%yE02~m@3|EA26QZVe=eZg|@xhdoW7G^}MU$_0gn|CH>eqjNBy1aq& z%69(aW%qwZZx19RqDlv7`;639NkE&jwZhxBX3-cwSI`$0i{{XDC5kPBhGUEs#r2u0 zk6G{`=#Cq4C?_b|)U@SEG~6-;Hv)ugn$;XO74Lv>nmV=SkpbmU(ETqg?=o`nZJSKE zjn3$t9E1yob0$Sd3yIWGWCAV<#%-a^n+uL`x^hYay=%1bHuc>xs=6GiisYR-}ijFwVkVVNxYWmgP{V9xIkvd6faRPMenaedwYkH>(C)5@w!39m(4$B|p z8UzgAuFF_xI4m}T^${}_jMJIs>7N=vZjrl_=1QCSPVBU(i%k=Yz#h5sH60XDRxc^z~Z!=Hb63*#&btMm*!> zxHw$6QaCp@;}Z`;^)g4u(~0|{rhCMpT&$`{hF!1=g2y@l<(Kc6JmI6W7mGr(TR6v1 z-qNad9m^;gD4^y0(>X{aL$$S$=Iw4GPC&&wG!4GHC-) z^^2x=)I7X3Ak2(S`Z0#C+bCYAWlXh9VVfJ;=w8j>Pp?|6$*@wFty@czg7PYazitQ) zd&;wGVrZkOHBHpOGAmKftgB!>a>jX$tSrc;@e|@zTPqDG)M)gD5BQp&-2Z9# z8HCl5ynj32-ONl1`+ALj@WUg&CL_w;-AWyhs-!pf5KUr|Kb5`n zTCF(vg{AHm{KFTP>BdPZSP;ASRN8?rEV=WSv-E#$rUd4S>IK{vmKy(N!`uBHKeV6t ze^S(+bFjx(k2S6FWDK~EhTY!DtIheU2p&JIbUw5Gty$S$UUf}3j;@9XO>FDG;re>e z{(7Ca>mQPQ?X7V6d^OY6WEb6W?6t9B=+ZBNTrNcJPZR*V?_W@V=2(Sgw41U3@heL|HQt=0eWuzYj#h%csP5 zbUBHdB_z|!XB*#ujwmcKqti!|w+TS$9bUVlojOyBf5eUCHr6X1Vhgs$*%o#C_8AP$ z)}x3Ww?FTz6LfZvWYdf?N^D4^Z*5zZ33F@xU#cRgz8V*aw!dDtAugg)lKyih51vEJGTBOYEq-Bp*g92 ziQ{+0Ha!(z1l)~%-PN(N>IEPJl7u?P&p`beBXAYHsw7(h)eAmviEIVbxRq6M~~GLeqQ$KWi@wb#K& zB$Ug*4Vu(CR>Cv$Y%^)vE{|c;gn>N1J}>ts;6IIiE-9ey-Br^ z&^9g!ap8Oy<|`rLv2(?-IMw~Y`iX?PS!d22-k8-AQN_mFl3KTWb7;>x>8CL9k~2*R z64&Hpw_9^WNArWiL@gld7a^$#i#Q#@CAa1csk~cG_X#wrX^5aXw~0zKhK_4_EL(Y?#AO~#x%RG=NVmX-Q1y$^RX0b^gjQbs!eUfaI_j&J zbFts6Oz0&OAv~=99MqIeS!6n4GTUlX)iQ)5_jQEt0tHsy;+(?F^Okn`$iw{WPtSY` zoy*XdEqSWZ?u52yA21aHm^{SiVpFqdQ;1A75a_ukK5I96I%A<*MQ8Y>V)RkjUiQ5K z%v0~^Do2~lhy14N9$)Vs(q;Qj_Q5|okybg)_w)mR?%|~IPB%sn=n(<#cl<7kH^$1G z(j`IuI#Vvefc3*0Ahe|*gxg{iI9MvJr9edCa+~?9e2rikkPNg9c@RIYa_M^obYvz! zErJ<(ncXf61!x|{Yx$n^Aa51sSVvaBj~YIfBJ3Aw=BlMtm;l zoj6I3y+sz(GCI6)>ZVnHzMTPc3A8yxmES4QE@M%p{oTZjRLV)DQv<2ggV>bn+fBZB z^n1A{qn}i&?Rd{YmnjE4`B<%t_ZT+B=t!iP_3s>-ExeD*HcSjK(7UcVbLOXvpV^^6 z61%Dd$<$N^t&$HPfBhR5?F*ZAQ}d!7HardP;OYzddLw~qab>66bn!lh_~0Z-*qtLP zZHVsv5{0e`@gaz!j1TBOW7{O2*@|CP=uPZGkP35XQ}F}Z82nrv+xXOo>Ul4|LN@RlF-!jLmropM2YpGXFZg3?%(s9DY>7+%~YnJpWWx(6Vl-*YtFUl ztbk|ri*oY5{;*BarkDT)CUJrFM zb9t2L$n*{_-O4z)c4jNN_58NdSIJ7;SyC z1I(xPysIk8-hukBX>-h7UOUt5xie82T(>hW|GhH1CLDV@LVhD0Jw^%falFK|tcGKL z)AQvEGm{m+(XCqoRg_#vyYq#G+Vq<)2A=fu8TyUz#|^^Jz)6!=Us(FbzW1M-8mww+ zwlW5{w@HRI3;N21y6DJ}wh?df8DL4i9QGlt*mA3seQj3ds}#O=vY*z2oIIs3bNgqk zuZBCu(ui+yqArB`*`Tmx&9YU|u`npOy^*#Oy}vTC21tw8HZO3MNBw%%u6S&j~&_J`FXo>{n(#B zBJ(1;bt%o?XdAwtiG_1@2fWQXG%l{xsX2D5h3i=pEQNgx=$|87`@Q_Pvg|qs_=oU$ zu}1EE77eE|E}GxeTu3iuYTeR-NrE`NV5Tiu_@%&IQ)`Rq*?_3)R)d5L=AJncEtPL@ zw54nls!#H`=i_$BBV1nZ=JY6 zD?i`)4jUlex$jS@Bz1>PED|0Nw3Ek0|1FFCHD45ZE6btL)Y_)?(7?I{q{WX4_n zYY4JBo)j?0Z6wCP@+T@k&Q$8-2ll>RQ;~8GBqN!7l-1W#=pFj^0ntXhm1exw&a_g? z)M20YG_+uc_QJDJhSegGK2B};Q)S*xI`%Nsm=v@46IGZL$@v2F<5bR$i@p;hZ8O3G zWQL1FN{i)pnPtQsV@eF`)4~_1qHyn^E@4ehre)Zth5tsB5r3tbL~v8Y5|uXno20%H zr%Pk1XlxA`-TGQyRMdU{!`ZhNAA8?g<-k$;>r20|s3jABS6{QqWc3%P6Ez-B{Yrd1 zy5oFYP;=Y1a6#W3gnnOH1@^njKIu5QQa|$MLW*7a7~v~**=bb614q@;;AWfcIq%Kd z6&Me5m%2JJ+|wzYP)BS^*w2Bd^wYocU8~{e@+~X&zQ0QFcf#R6w^bG8+Q~-SC9VJI z*vxzDauPf`-WG%c^MhERf68=7fDdI!fzk`sllrh%apU*cR8m&U%ovv z8Eg0>p<#u$B{dFzDh00ky=t5XGM>8WmN*RKvJXBM3?NBVhbXUG?i_p!O1kmd*y}fn z(K<(4VmB%(D;5j|`g@;zxSX^t@r7k?rBrjSy{PhUJnArM>1_`UJFJVadBU!JI@7$v z`Gw_?BiC4`3`Y_}8Gq~Rp1~AQhF`omv5EI5&98S_$Hqh8j%P-@5G}toK0mXv+)MBZ z>dWZGX(mf*TS-5CVUd1q?EjskEREA@sxtqUk#0m2&&F8QuS8ppI9U@)2J4fnn2%;b zC_sQg;7<&7^*g;>-#zbt`R>MX^v|qU{_zf1E{@CjM8cy)Ma`gmZD#b>isU-E+r5+< zU2n~5GvY)u3iH$6?dv9=esynnp?`hM)eoPL=eNR)0YcuYGOWXY2+DUM1MB-lj8c!RZLB7oWX8259bpEiR)EUksHV0nC4~Om zu<%k?X_E-j>g?|GKmG>%cJ6ZH3N0~POK=3D<|KJ%`ZRt5);^{Z%<(?T$Jbmq$bAek zQRnau7qPgz-vZI?>+`JTzAud2+qjv73EtN>j8)>}G?dTC3~D%36UP7!6kB$gGc1hc zCsUDDZK&)bh`1*_uFvu%IhH*;r!po*Y}_UjBjS{LtK#L=S#r?wru>@%p`+I5 zDMD{k9Ke9MGT^1cv?^R)QWZ00o9!lhtEibfmd5irk3F$w!bd{~1Pcu@Xj)u1)lW2? z1}WeP1zPAS+z~SJ8+)EOA^RWLfH^-J_NpGmu(f*JE>feT@}dUA!jMMOzKXR~2dmi; z;?Af-Ri3c6Xx$-HgPS1A)T7*IY$gG?n4#TswVSd`Q^mwKiKhgPjOONu>J4UfBgBE)HBt8KlYjY)FGsX`_NoxruYOAW8uA6}a zO+6^Ro*lf%qM8`h?2ej4)r`yrOqUg_@_uC}FZ>{~?QR%J)XJfiGpAvyf6A>2KjG@JAE{kR|cx?}#FqGC4B3)0$z9*}A zD_SxPV8)eoO>xvud2m$~B?&w9;60eKK}L(Exum&nfnnFB`6o8ZnoAj8Bo(Cjc(^vT zy?QjIeH`A#(1Oexgmm^P^X5GKrN+oe^-8DrWotQ=si-AIQ>@G9y*H|{Z^bjPF&9z% zwWKpCG}Me;UsuR;hICCqnWx&B^XU0UV5C)d9~VguC*aZQx9+eTP#f}40zmi z^*zgj-|F+@(PL_1D$qT5i6McVI>(r1mm(}A_c1>ODaj1p(V7dS_#^NMahIqVMWoe6 zZ|-NJw@m+%M48yjmx>7~Jnt+d4=o3Rp}ivpc0`DZbSk(oyGkJGU`&um;LJpS6x5oJ z+~99*`7R!?zQjrI>@WeD%hlXit*1BUp*ibUcCr8vI}I+m$>q>=NpTxdtvx6!hK>j} zY)n8+Y0K`O_9NQsLlB-sU*{C%s36ZzkUj}EPFnITiO&5lJG0*eNhaciqQo4b*)eXt zAY8eob`MwI$>e?-7H%w3a{S*r+W%e6{DkVjzOz34*a-obVX9gA5$BK2@>v6daK+0C zwE39CFsfcp$Jf(Z}4>l;1Q z&wQ2E2)e-uzzJ{Nn3CpEE(;P{NLIUjJ$W$V-n?{H<>Y9Mcn^s6h=+oSrKGtg(jzRc z56uTI>5-v;8*FS=ayY+H`c8LGZu|L#R||=5@5oZYMwxz=IGmv8Ks*8qi?=J?rajfr z+uhkIClqs@K17#jBbYP;jF){#zh_Fw|5>-;ZO6`5eT8tGe$b9$fic;zWee9FQ3PvW zCAU{wt6tSUQf3#CB{1|ma>%+d2z zn|q`OSW7-V{9@w7&PPFK4=k+wFz8;4Qy9Hv(Puj2bN>NRXBMnu0U-&b>Ri1-vNEvjD1WZ7!ak$MkH??VnoHg`NqQ0U-^016LP33 znUJS#mZ^?umkb!~9MfZq2X3tv&MxQ3!f&&x6kQTkjL63{MXL=ZD$q&wNOBFpa<+NQ z*!$h^hN^Gd&fK2Z9J@1f?_u~0Wb@$*7IOYglTt}Sw+af^7J&8B8Dvk37Z1M*QRk4} z3CIdLzz&Vw$WKVQwUPTfPJ|7_bChLYx)jw}XxJ-;Cbllp`d;sBx1N%n9wx ziT2-D-u-r7;d=sGeuhKk12TPwZ3Ux%wUUGLsbGGXU1z1h1(&BXnRLCMjSLaY2 zW}389xz>lR6(IRGz9^@rC_s^KD(5V;-}q;}-|5rt^|RI)m`Nc+y1Yi}GNcqDoS5p{ z_i7PMoq7SuYO_ap2B!;*A=;%(eq`WSIo{pt!>?^*B+&wSE$;wQv(`XDjDNc z;_V^!dSXa2}wgZM<1Z;w*-rab3c8#b^4nr54aYR8;_02ooso|6NB@I>~-z+df7$ zm)DdvMllN60yp&}57M=iUrBR5WdK?Y$KDhY%s&9)e4W#yH*Kp+TtOSJrTSifKs9`m z>2Dgku*suOJ87+$4~rO+Ix6~2!Bl?Lwgc{358EYMz%B|+!CKll1(z5w6i5JAtquU; z0_*o(98_wllB34p0?})VIzMLIw{k&BEg?e4%+2Z2r!qe1Mm#3q-Ow$@89nPz3xcg) zu#1NW5$mDbm!B2o&{8iTbIk{C`#X`Sg-6O9@l-a!M)4Uu52O|u=Rtu;8WCV+)@K^j z$h6`%Mh!nJ?uN(l+!C;A^IU}r0T&nyZvkbQ#JRpAr-Kh`f2Y2URep@x&PJjl0Xd7l{oHfW9d6KWbnGyRt zrRlx;^KiQqbTUa#BF@kJEuUt)pgi7l1X2VctO`(JMKGGf!?)XVQ5P}#BstlbmNh*Q zDiXdpz`G`wUA>f>K3xAh?k0*8A4R36isfQ>rPO~>DWJ}2HKI)|l~-VeeQvUu*$>^U zrGyOYYeFIel~$KTRX16C+oOIQAi2#2RI`P=?KmgG)3Ae5dlN^KERTsYJ`W9Fm@1qm z1^AN7W+``aBlHr3)_!c{1O7j^4Y%tVuQ?i+6)0I!amz(?Ax+JlXGXMog&tv$KfpGh5ArU0# zS;{6=&8N(-B(bDk*RR!EUT{%$nb^YTUI%gH+>n_A&1w&=t- z9ffIrU1Bb-^&n@9QIgV*{>|$;`kRMx$8ubZqX6MNneZb0?NtD?ASUptCb_v(kIhGA z?nln}n!1l1`xIJRKM@m}X4}KxkX4#oJJ>EhH@?7;TBDGl$kVAOY^&gYy6F08qi{Xn z0Dih{6L6tOitO4`KAK56^*jD{Pvyq&VA8cpWT*O;dOQHGBE1PY$-mQj4VHf;{P+d( z|C>BAGn}2gHSD9kVVV$WW?YysmVa!}40}{l9iCS}(bT(S)!|x?>3yMHe%d{EdY^Qq zYZ)U`b@T+lz$Rja>VQ;m9A)aBNwkTHArmlk_mwe0+ty>108si^Glr8JhB8%}Q)IC= zD!w>tc+}qH^zj1wLX}8Jx(dui3p8M=UXQ!JPKQ*e4R-0_tkA1P$#I3^IGd?dFevd2 zNUR{gw&2jFmHX7xc-;ARh7S|-6Gs6N>AlbgXL-X<2L_7NVMAGGkOD0uKp>b7EL{~C zf{_z%@2AURt)e^>mKO@%g2-i!;Hdod&Jo_ob2Rfsa_lD z9v9veBpD$(JIu;c_aP#K+w6ZSvS#}Br1qdRxFuk|*FlC1z4%xaJ9biTI2~9vU*?yS zT)Ug*VaY?Z$;`F3PB~dH+F2w51mRrBAP3Lct?RORS6`>oxvl$&fp{LguYq_v&W>V~ zB=xe3MQ*fOTqpx3o@ew#%Ev|0sidz+TtI@dK{vd@!h1@ViyRttNO$DPf^Lx{UIc83 z6R?v+xEPj_cU)f`=2db=&)~ze3a=9o>UNVKEhw-#WTOjcW7fO42G6rfjgiIJPqLlj z;0YULmMH-8KFPvb|A{BzefS9<=xLLd>L5dJ64MPho|18A>|9vqhr>LbC2HFJc`kR9 zlt?ncNQgiX>lGGqqMLcAmOLA7y+2CR+fMq)l_Nz3z46yjQ|_~gbG8w~!e~kCJrIOA zJ>Yd`zlwrSgZ^e>;T^~;`Ki2S{$+MLj)U0N1hnnl$NQuu_`2-Cjz zu6SHUc9)c)C$+FszwSx{v<1i6(5OJQiQ@e--#n^Fx%v3Fiy&BrvD4jWRu z@jPUZ&1}#6nV$E3js%tmu5De5kbrX|3;I2$yHs47eMvJ(r<0WWD_J~U+j)9$?L+{A zZbL$VH7%$%SQnFhrtA$R42|mIP!DZx9diSHJVkvQ$ik=L_QH9HGBiY%6+_RCWJJRt%Co<_N0jt}Jf!s*IK zZ1#?9W9$+eO?XPSHwwoprb`{~`TWcVGlY|MZ6>-Wx;wLxCmE$7596hWRJ%JL^*`&; zLRwyzEWEbv>=PcIOS{|K2`~^$?1UwjKhrd;_&|D%6!RUF@bCA8+OIZyFv|PvyDnUn zbagt3wm6$N>MgivXUc|z*fGJy*r&>U?Qo6{_GSdu3GycaaS_G+A-o7E)n@nv&#Ws& z$-R7Ko?+GgsK==0QjF8HS=38lQ!N$8NY@?FZL^q0%Xl$r`NhHjPQJG31ydV^89*A2 zQW$9IHuop6K031ep)M$knh{$+?v zI5;VOZq&dUmkKtpV(bEI0GS&Im-^$B#((WRy_Ck?8i;n1Y!M;OprxOX@B&Hoi;fvuvDE7aXv_~#3C4yZ~Tq1uBbniY+ zK+^GNaLk!?KmU|!m!~^?DdTq(X>p8ypLRNn^t|W${Zr?@usn4O=+_a0-BOat4!6K* zH`}H{#c?tj?nI#)sXv25!Sn!FetP~lv%M=`XJm29XyanF+{AXOgL{Q27rVLA$$h8q zMk!y{QA_R4!4y!fa>K*$uz1BWi&NGjd|~E{D-2aDAw8aGEvDg8c^^+LQ7W03oie^? z%nOWcd}?|LTSz`=Uv~*7#%4>`ge{V1jqpBpIbz7+Vue>r7kX7jHIdDs?o*PC@v`I* z1Lps33{Kq-7gKs1hbVwTt(l+kT+KQJI*8)5muSo1$zA%m~|-tB4|1SW!O4k0yuOWI;1XTp#MXlnPG_9Y6KhBre;fW+y# z)E7*KUAPm(AF8jvF>(H;jy4@_cU1O7eeKYF0-=mNN5vD`6MLqd@!4Nk3M^eLj5O5f zCDX(e&k?&M*z1xU15xZ-*UVgf?9`YmVTGd;-^PpKG|g))Ts2Xhlg%=>v$!?L`>^Zg z%8KG7LTK4^+NHC;8j7-W(JTc9jlYWGd^mhJSPd^!on~!$;QW&Umj+&o4Yn==+)N*o zNB#1N)X=E%M6TK~i&~T}QHyElQr=v;8fdsWsd)Y6t6W^}%gy3Y#c;8lCw4+G2RU(Z z&n3IV@|@$X>jLwwY9*|0*Uw`wh~dxq26=@hs;m^@xlRG=A`&M+&K?YlUOX`0Z|mQ`iut4s?V; zLY=AcwzL=&n;sN6DGa(SN2EhQO~jrM6?F&wiBFQ3^euP-PvgRYcx!!b2`PZ+2THgFOH3nX*H@R$|{m3M$)Kpw$DDgJO=O0;f= zTuYCAHQb80zzo1b*l2N2c&7JQSa1@~jLOmgoOI1V97?q+6oRiC`ac(1l# z@77)2p{;8;o9XwX4qX8-kz-E`_|#EXQ~J@SFY_h}^#l`$+I;{7hRrjA;_ac35QUPX z9D_+B!l{Jv9$D6j6=22V#4<9uxh}1#gyiz969(4wVrZ{b$U6fdiey+U+`KioRO>Ci z*HG*kM7A=HOB$LgESfhl7^aO-?M3AmdoE(vt#zK#}j2%UW*4We>$~zC)8C9#!6n&t@$iDci!WQ;7$Mb472 z2J%AA)~vVkEQRByh;~VhUg|Nt z^(j1TLAWxpo~lY0wg*G-MH#@{q*t5{Q=9AkIA*^W&FV!-0cAD7@?}Shx?e8T~OO>)0k*#pORPKhJBgX zyUp2?RTmN??XAw`RiB(Rdo-ONu7R;%-g)NN=ipg027+8Lv?<+F<6a;sp0!Lbo@+fF zX_xzY@R=PgqTt37cQwAC$tL4?^^_1UA9p`MIB>wWV%exZEQyVcO=+JorI8s~pXsLX zyrjSsG7jhsUkjpXX8z5utiO<9Kkc$`&Z^~d4oeG4Vb8idN;qiaocR2}~%Lc4T!D7zP%s|V~k7xr^>0^>IKAYoHnb}Un z0W79?Vvg?6**VYer`zkGi+{6VW5WksP_2m*x;O6B)^?ZG?JC1aGY zH#%ykYv3ZlBO7D~*TIg2s$%3bh2l@b?1od;nW>l#eOx3Ql}iU`8uoY(HHG6#n_Ojp zsZpc1ZTW*0s@y+`R7Zv$LY%J0zCaX}hzS+yhUGb%H5(8c`nbqfFs0fUPMk$!lDwx-(#C-70}vp|oS0 za`~CotY}jwZ(zvmM+Z5T6P?^be->6>R!!kKi`HMzhCjH?T5?%5zSpav_S|pZJ}+Yu z4iL29D2_lh|8D&WQ;$LwOFFD$EFkr0a!U1w^OOf`>U=6{h-$Z=b42f443~PZ(R#qx z_`{u3?b5=-+f`Jtm$ROl4t$X+3_E1a@IuQ@-7QZuVNDXCPP!NU2{J^e**OT z3|d?Z{NumMIr1Cx{~{pD_AeRyU1T69Hf14#KiwGwp%fDG${&5#uy6Wmnne3Bm}+$DHBmZi=hMZbmfU7vSnejXO8(fnr2q1LGlpGwHzroj8ffQR z5T=(~Hz4SZrzZC~EcR-~{8^CbP&e=m7J&zUFo6B%N&D^2#5T?BsncWi-DfTL&^1VknBF# zcFmRXw;sn3iOoI+BCg_cEsf|w@bZqPBfujM2u}>d`g$&JhsnQx3v~r(%Ly@!XiS%6 zsc2J9HtAA7anEt_pIPyVBD&p$)ho2bo-XUUz8$L|z3u-B_&2OpEdCf^>;&1<&?1HH)G|YDQ4GRNGDf5HuiFz5^HeWRNjC zO32jF;>nW#n7AZ9UT!Hb*P}#SW(Lld&|x(*k#K$z6{91S$jG8wzI!rTHGM{*Kq=1O zfZbkO2n|WRbz{%+7W}%gc|tREQA4_)Wi6zP-N7Q5a{BXUHIvhb`WX zuIY+IE!y*Uq*>GAxGSXR_bry$n>%}C3h zjW``~5zt&A$;(S=$ubjq7-KjQhl%QqV3pu4%x=c5dVThLA#&LLm^oxd^5G|WYa+s> zl#|yDOq-Y63=t?Ej1wSWvvRZGARy~6)Mgz;AqWY`-4efaB1OZ@yh@r)IH$T{st;s?cWfJezTMO^Y|uuf5%Cr?|x+u{}kGUlM^0LFWs!e{}mu+5H2;{L#bRnK2}g&3J|Yxl@~v55M6T z7!+Z`oOa<{m3vs&hq0$PplVOmdP=!nVR2{D0U4Pn0%k%JMH?j;&CWbTpu@Fu%P`(8 zp&IB%n6`}kg{j~uTKS3aw73zbKZv7<6IV$&Bn-ziZpM|e9iR09&Ghlrp$ji@zR0p$ z>{K+hT=ZO7=c_6(+V}4KqT@>nXEjPH1sCl=--=znSSGS%R9BF#dNLEXPke^nF~Diw zK0C$)C(IY^3u#HoxL~yZ9gyKf5)1pl+KZA(&3)Zt?sPxrDq`|Fbcgmy-HGSE-id++ z)+6P8;v-r;ljci__|$1Cu*E?YyXsZU#Km~BCHdxdgpIXtU^PV^uC`{?^szwp6reSP z)Y_2+3!Gbj#WGd@N8M~-b>3)PR@|DyP0Nw%Ag z2R_6)DSpYl^)yb=*44W%NhWPlG2e@!)^zFY(M_2`q-ZQ#495Mv3lrStt`8OuSO$5$ znHG4b!Pa&$BhBEBCUd$2%Z?-AH)JW^+eqWvTCk9czMNLCq>%(|wlHT-o=E8!2qtKf z*H3h;GfC6eb(of(VRh5Q$3Wp&M&OJ|M{z0lmQD7Hhqsmu4u$`Od0%MF zgVZ#+&as2vJR?{8Y1{I28YsNW=mw=kv4x3OtEeP8VKNt294#H3@r9*miliElo+#91 zHo**ifh0Q zug@Y$Gx|^C+(*^J`^a&kO=TGK4pn_bvB3Vn8MlrVeJ&Cw{yfo6+)|#n{;{~D(LhZk8~w?!rqsTsFn#mH*0Da(t-~1( zIq#m<8aft!VTl?LxxYAryWd`%9i&klflxte(h6&=1Na^$jux9q#K}5q3?yi~Z0yP|@|AzzLH~;Df5tCS{&Z%% z66pgy_j@jP!+5rnmd}04WMZHQcWzns8Q2W4wsrc>#2KYlo;k@74{klMIOujHEGc61 z^GH2%YuZaE*^Sa~5E%4PRc$_JDyfxAEvuXQOZO-Kk);8ZXYmi_f%o^?)cB1EyDA5c zIcUFEw;BrW+K%=nRRnmxu2&=)Aqrj(I=)wz+y&QM4v3z1Ke)m|es~4DSdO=I58S<)5q54wN>Kp8nTO!dnJq#{20}VDZ!0{hO zE=lI*k^u$F^OU@+BJH$5i=GA}?RzU!xN_>H^ z^?8VGzR#WDZGtaasYj_Vxq!>&;v9REL=0ZyIUc4N*lCrxiZ|Pmj)F)JFrh+mN}GB> zEs*_Gq8MbvT8S!($kSdDG-YD3hCYvBVTW=K#j8GKH-eOUvoG*Y%e+}(;sK<*-_hc? zCTulg0)AN$29yYh0GHU{oa~b%m3O5_vuygW#i5DXl8a;^?>R#nA_O7*w7a}lO8Uvf zG|E&4Y_RoH5U|;T*m>y-%WCVRoJCIFBze;dg;DbNv~Tw` z{#;WP@z|K-PW~W}6pjsUXvsv`)btFir8NbP>M5%>HkF2{;OM=f1LoH78I*MD&o$uH z0=Q`PBjGg;ZE){syS z)T(y>VvQzlMNlx`G@a=d^tWyQD}uEBbu916s0-#8$6Q!1^(@cNyfQ-P-oxq%X8-hD&S<%jN2Sd)jJ(k4p&!IfVUl9y6DnD zivE*6_n0xjp4JK=+iCUPba;Pmr{*9or@Opw#FIB8Q00+kE`rAuU?L|Q*0~w~veu^Q zy6O1n<+4jkrOK(?N>iyyjB&ehGDI|dA->%cb2V(aH^OB(wwZJ7uI-Lfjs!YL0=2t= zZ6_Ez(Qly_KH{){lxkJcd-w3f?nh$-9JL^sOx0PFUF`i^G&IoiR=1H&olFEM@Wi8Z zk_XngvR0+BuqPICC68ML=}qm{1#egA9lvmLT{Cwd*oC zySxc=r5%fsU^Wi-MtRvruHoEU5a^h0?)w;b?xt#LdBlOLg+?_VVym3BMNCYK|H!!K z)rgOZksi-{7AQv_ay6Rl0Y-db!T4jFsdJ$0`Kw5?!jxq%S#|r=Kww6CMrc&r1P*C9 zA53xeX6jBygHTB(@^ncy4ksX84P1K04n>zfds)E7D{vTmd#QJUOxkPVT%2S_VTP(fI&Kk}6jdmwjA0$mv&E+`g0<+EW@16s~b4yQ|x)v8#w4iA0_aPchKrqnZXp=(lk$TQ*RoDO5-p;CiAr#8d~ z8W#XETf1J2@>l|`MoP@h3GtW~fabcZ8mqt7veVDJ#MaT3FRP0`4aP2`%KdCE5B2A;l|y$?ykA0Mi*^GORuqDUY0Gvyn8CfztHQh8V}$Y zI0?d}UA>3wF%l1}Y8qCeSaeDFiz%h+m67dz`tRm4qzIT`wJ_?` z>=yMwfwQ*o&KH)EtM<6K{^WWsq6M+0K)u<2&_G6Ym$g!vsbr}+Mh^mHpTfo`FUf<} z%%)qO8(2gcP~I+!nI{qkAGTOZ)2Sk5=B~?QH_jerBLuFiaVPwYvcW}^{iOcajovJ~ zVE-GCM*j+Ccf$hHL4k0wDM9{HZt+cj&IeL(GmS~i{R7BE?XuH#5~q_)ZO8>&WuQr8 zu&}F||D%fC$k|J%yf!i`1cP44t_~zxgElTwyQQQ>==b*lV$}lMTXGxB7wuY9n$6Pf zrd>2VN+<#~epF?(7PmOE?@B;%i_H865&I%B5g6sCDw@4oCdst6(|w6Mt+qd%kR1*P z4o>5>ujnT6#A}TMfhB=bE$+BaiM=#k?9^zNoTyz?pk2d6`4*SOM-sZtgcFpoK6cLq zKtrHRB{2apeNq$GO*Q%#!zyNb@Nje^$;4*Vb5sa5zSHbAaj4pD$nJIhal`zWOnAl*4; zrdu65=V1?4FXAnO%Vga7nEI=t9l$Xu4>diF5Sa6Nl=`3xCmaufi_KY-&N2Jql}F;~ z1KjTF6Z7S1xFhDV^CEz8Q$LEgtG(|Y--jQ&vk&W;A3bK;(Eh^I1%+@U8m-y7K5Vpty-sS{k&*OVck zJfA=Dvo$Qz@VR0@NU?2s^u-qB7Q#zERVacw$R23ool*W_cI<{ArmKuGQl$8+EVWLK zf~@td34_#l3=4$rN2xTqLSuYh>H8`nH9ON{@7>K8P-_%XU$R zbC!0vsZ7bj{O^iQ?&%Mke??gq`=C8K7-yn4=SrE&>PoprBX)mbImmMN$V&YCfIQ!| zzIXRfd~4jxW?Z1&#@nJ8;#LEA{CbU3c4u$$gd7OU7>rh#(u>_vpky9ni-1zSb28VM z10wr$%M1JHtw~DCG0q5R&g0~X10`J{W@E-+E@=7Gs6F^>7!f33n`%LsCv;(0!VBuj zh5d1wCgtR1k)=)ybHP|ZIbpH+{wVD=ZI5;QoQQ)-vRm`#bcc>{fib-Qb11Mc*6kv@ zg#(m}IqPu(g6m<^yDPcG{I~vKAV-y4?wRP``)_~O$OUuE?!qlA%5p%x=Wc%}WnE%H zU`)bq-q=S&TRf}F362#C)LB!?SADp@z;43WSNOd$!{H&fid~uRFGhFu^yCP+D#_e zQ4dliq4x%Y4-oB`zY}FZaR2}$ZheIW*2zl7nA6V|5QswuHTE9n=@OyaVq5mP^wt2% zN5;*Xoz-VcmculNex0`UdlQ$Ptkq>&?|e*-30TZmPRLzh-+B^lOZQwBu(z5G%A!;wnB86UGihb&}YW0>6{Gp*?ueRG@KMDdC7+B~58cP?y zSH9u>Qg4nU3`7@X$d3117xpeFw^;nWso zepw)(n|o1LX<1zAJ=rq#hK}BsYL}MY-m{x~-RX+8-LFOMP5)@$zvQyXwCK&YlxG3j zDtE4b$gg)aSIPanYQ0gR(k88?lg|33FdaReFP&Pxde7RFZO7C?eoSCGfc-ELWCvnl z)3?=Qd~Ma%6D#-fGX|>WXl;+U)BS4K$xk*X*`54;71%JHs-DExGWW{^BgYGq_s!vF z*fj4+^FsAn3!VujD({ywAJAF8Q{jc@eLrRuSx)Ph2R?9XzVomWD!<$?YQUfh1J9Mh zVJ~h5|7Wl&uq|?U|8MriT7Ewk#+)S+Hz@r*DgL>tz_x7V$(&6}#mbi(8PH=@XfzoN nkF4Oi>R4=Y*5aLaUf=$7!-atry3tJ^b`G;TIZN{F|K9`vlZ#yd literal 0 HcmV?d00001 diff --git a/docs/source/api_reference/index.rst b/docs/source/api_reference/index.rst index c18fe744e4..681a203a35 100644 --- a/docs/source/api_reference/index.rst +++ b/docs/source/api_reference/index.rst @@ -85,6 +85,7 @@ documentation. CI runs this same checker after style checks and before tests. :maxdepth: 1 public_api + skill_fitting embodichain/embodichain.gen_sim.task_engine benchmarks locomotion_tasks diff --git a/docs/source/api_reference/skill_fitting.rst b/docs/source/api_reference/skill_fitting.rst new file mode 100644 index 0000000000..eafb0aabe5 --- /dev/null +++ b/docs/source/api_reference/skill_fitting.rst @@ -0,0 +1,50 @@ +Video-selected Atomic Motion Reconstruction +=========================================== + +The fit-trajectory command maps an annotator's inclusive video interval to +explicit joint timestamps, estimates a motion goal and invokes the existing +Atomic Skill engine. PickUpMotionGoal, SlideMotionGoal, TwistMotionGoal and +HandOverMotionGoal accept explicit world TCP anchors without an object identity +or mesh. These goals declare no held-object effect. Collision evidence and +physical success remain unverified. + +The fit preserves the original joint clock. A bounded monotone projection fits +the phase clock to the independently generated native path. Phase endpoints and +Pickup's internal pre-grasp anchor remain mandatory. Native reconstruction +errors are checked separately from the final boundary correction. Three +original samples at each outer boundary retain the original discrete +position/velocity/acceleration stencils. This does not establish continuous-time +smoothness of noisy measurements or physical task success. + +The source joint order, calibration, open/grasp commands, derivative limits and +video/joint time offset must be explicit. HDF5 image timing derived from FPS is +identified as derived, not measured. Missing hand events can be supplied as a +complete segment-relative phase partition; video pixels are not automatically +interpreted as contact evidence. + +Recording and persistence +------------------------- + +.. automodule:: embodichain.data_pipeline.skill_fitting + :members: + +Native skill fitting +-------------------- + +.. automodule:: embodichain.lab.sim.atomic_skills.fitting + :members: + +Numerical primitives +-------------------- + +.. automodule:: embodichain.compute.trajectory.phase_alignment + :members: + +.. automodule:: embodichain.compute.trajectory.splicing + :members: + +Command line +------------ + +.. automodule:: embodichain.toolkits.skill_fitting.cli + :members: diff --git a/embodichain/cli/main.py b/embodichain/cli/main.py index cafae7473e..5be436b4aa 100644 --- a/embodichain/cli/main.py +++ b/embodichain/cli/main.py @@ -37,6 +37,11 @@ class Command: COMMANDS = ( + Command( + name="fit-trajectory", + target="embodichain.toolkits.skill_fitting.cli:main", + help="Fit a video-selected joint segment through native atomic skills.", + ), Command( name="expansion", target="embodichain.cli.expansion:main", diff --git a/embodichain/compute/trajectory/phase_alignment.py b/embodichain/compute/trajectory/phase_alignment.py new file mode 100644 index 0000000000..ad2cc9b7d7 --- /dev/null +++ b/embodichain/compute/trajectory/phase_alignment.py @@ -0,0 +1,92 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Bounded monotone clock fitting to an independently generated joint path.""" + +from __future__ import annotations + +import numpy as np + +__all__ = ["align_joint_phase"] + + +def align_joint_phase( + native: np.ndarray, observations: np.ndarray +) -> tuple[np.ndarray, np.ndarray]: + """Project observations onto native edges with monotone progress. + + The output always lies on the native joint polyline. The observations + determine only the phase clock. Coarse-to-fine searches operate in bounded + chunks instead of allocating a full source-by-native distance matrix. + Call separately between every required skill anchor. + + Args: + native: Independently generated native path, shaped (N,D). + observations: Source phase on its unchanged clock, shaped (T,D). + + Returns: + Native-path samples on the observed clock and normalized native index + progress. Endpoint and direction mismatches remain visible as errors. + """ + path, source = (np.asarray(v, dtype=np.float64) for v in (native, observations)) + if ( + path.ndim != 2 + or source.ndim != 2 + or not len(path) + or not len(source) + or path.shape[1] != source.shape[1] + or not np.isfinite(path).all() + or not np.isfinite(source).all() + or max(len(path), len(source)) > 200_000 + ): + raise ValueError("Phase alignment requires bounded finite joint paths.") + if len(path) == 1 or np.max(np.abs(path - path[0])) < 1e-12: + return np.repeat(path[:1], len(source), axis=0), np.linspace(0, 1, len(source)) + coarse = np.unique(np.linspace(0, len(path) - 1, min(128, len(path))).astype(int)) + radius = int(np.ceil((len(path) - 1) / max(1, len(coarse) - 1))) + 2 + offsets = np.arange(-radius, radius + 1) + indices = np.empty(len(source)) + for start in range(0, len(source), 64): + values = source[start : start + 64] + distances = np.sum((values[:, None] - path[coarse][None]) ** 2, axis=-1) + centers = coarse[np.argmin(distances, axis=1)] + edges = np.clip(centers[:, None] + offsets, 0, len(path) - 2) + left, delta = path[edges], path[edges + 1] - path[edges] + squared = np.sum(delta**2, axis=-1) + fraction = np.clip( + np.sum((values[:, None] - left) * delta, axis=-1) + / np.maximum(squared, 1e-30), + 0, + 1, + ) + error = np.sum( + (values[:, None] - left - fraction[..., None] * delta) ** 2, axis=-1 + ) + best = np.argmin(error, axis=1) + indices[start : start + len(values)] = ( + edges[np.arange(len(values)), best] + fraction[np.arange(len(values)), best] + ) + indices = np.maximum.accumulate(indices) + indices[0], indices[-1] = 0.0, len(path) - 1.0 + if len(source) == 1: + indices[0] = len(path) - 1.0 + result = np.column_stack( + [ + np.interp(indices, np.arange(len(path)), path[:, joint]) + for joint in range(path.shape[1]) + ] + ) + return result, indices / (len(path) - 1) diff --git a/embodichain/compute/trajectory/splicing.py b/embodichain/compute/trajectory/splicing.py new file mode 100644 index 0000000000..8f89c57a48 --- /dev/null +++ b/embodichain/compute/trajectory/splicing.py @@ -0,0 +1,187 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Fixed-clock joint replacement with unchanged boundary derivative stencils.""" + +from __future__ import annotations + +import numpy as np +from numpy.typing import NDArray + +__all__ = ["splice_joint_segment", "joint_path_diagnostics"] + + +def _path(qpos: np.ndarray, timestamps: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + q, t = np.asarray(qpos), np.asarray(timestamps, dtype=np.float64) + if ( + q.ndim != 2 + or q.dtype.kind != "f" + or len(q) < 3 + or t.shape != (len(q),) + or not np.isfinite(q).all() + or not np.isfinite(t).all() + or np.any(np.diff(t) <= 0) + ): + raise ValueError( + "Expected finite floating (T,D) joints and increasing (T,) times." + ) + return q, t + + +def splice_joint_segment( + original: np.ndarray, + replacement: np.ndarray, + timestamps: np.ndarray, + *, + start: int, + stop: int, + joint_indices: tuple[int, ...], + blend_samples: int = 12, + anchors: tuple[int, ...] = (), +) -> NDArray[np.floating]: + """Replace a half-open segment on its original clock. + + A quintic correction envelope preserves three original samples at both + ends. Thus the three-point position/velocity/acceleration stencils crossing + the outer joins are identical to the original. This is a discrete C2 + boundary contract, not a claim that noisy observations are a smooth curve. + Anchor rows use the native replacement exactly. Callers must independently + validate TCP/contact constraints and limits after blending. + + Args: + original: Full source joints, shaped (T,D). + replacement: Native reconstructed segment, shaped (stop-start,D). + timestamps: Source sample times; never retimed. + start: First included source sample. + stop: First excluded source sample. + joint_indices: Explicit editable columns. + blend_samples: Width of each quintic boundary connection. + anchors: Absolute source indices that must retain the native solution. + + Returns: + An owned array with source dtype and unchanged values outside the mask. + """ + q, _ = _path(original, timestamps) + count = stop - start + candidate = np.asarray(replacement) + if ( + type(start) is not int + or type(stop) is not int + or not 0 <= start < stop <= len(q) + or count < 12 + or candidate.shape != (count, q.shape[1]) + or not np.isfinite(candidate).all() + or type(blend_samples) is not int + or blend_samples < 4 + or not joint_indices + or len(set(joint_indices)) != len(joint_indices) + or any(type(i) is not int or not 0 <= i < q.shape[1] for i in joint_indices) + ): + raise ValueError("Invalid replacement, editable columns, or connection width.") + width = min(blend_samples, (count - 6) // 2) + interior_anchors = [a - start for a in anchors if start + 3 <= a < stop - 3] + if interior_anchors: + width = min(width, min(interior_anchors) - 3, count - 4 - max(interior_anchors)) + if width < 3: + raise ValueError("Boundary connection overlaps a protected skill anchor.") + u = np.linspace(0.0, 1.0, width + 1) + ramp = u**3 * (10.0 + u * (-15.0 + 6.0 * u)) + weight = np.ones(count) + weight[:3] = weight[-3:] = 0.0 + weight[3 : 3 + width + 1] = ramp + weight[-3 - width - 1 : -3] = ramp[::-1] + ids = list(joint_indices) + for anchor in anchors: + if type(anchor) is not int or not start <= anchor < stop: + raise ValueError("Anchor is outside the selected segment.") + index = anchor - start + if index < 3 or index >= count - 3: + if not np.allclose( + candidate[index, ids], q[anchor, ids], atol=1e-6, rtol=0 + ): + raise ValueError( + "Native anchor conflicts with the immutable boundary stencil." + ) + else: + # A hard anchor cannot introduce an isolated blend-weight jump. + if weight[index] < 1.0 - 1e-12: + raise ValueError( + "Boundary connection overlaps a protected skill anchor." + ) + result = q.copy() + source = q[start:stop, ids] + result[start:stop, ids] = source + weight[:, None] * ( + candidate[:, ids].astype(np.float64) - source + ) + # Preserve these bytes, including floating signed zero. + result[start : start + 3, ids] = q[start : start + 3, ids] + result[stop - 3 : stop, ids] = q[stop - 3 : stop, ids] + return result + + +def joint_path_diagnostics( + qpos: np.ndarray, + timestamps: np.ndarray, + *, + lower: np.ndarray, + upper: np.ndarray, + velocity_limits: np.ndarray, + acceleration_limits: np.ndarray, +) -> dict[str, object]: + """Measure finite-grid limits using physical timestamps. + + Args: + qpos: Full joint reference, shaped (T,D). + timestamps: Strictly increasing physical sample times. + lower: Per-joint position lower bounds. + upper: Per-joint position upper bounds. + velocity_limits: Positive per-joint speed caps. + acceleration_limits: Positive per-joint acceleration caps. + + Returns: + Limit acceptance and worst per-joint derivative magnitudes. Collision + and physical effect verification are intentionally absent. + """ + q, t = _path(qpos, timestamps) + arrays = [ + np.asarray(v, dtype=np.float64) + for v in (lower, upper, velocity_limits, acceleration_limits) + ] + if any(v.shape != (q.shape[1],) or not np.isfinite(v).all() for v in arrays): + raise ValueError("Limits must be finite per-joint vectors.") + lo, hi, speed, accel = arrays + if np.any(lo >= hi) or np.any(speed <= 0) or np.any(accel <= 0): + raise ValueError("Position intervals and derivative caps must be positive.") + # Edge velocities cannot hide a single-step jump through central averaging. + edge_v = np.diff(q.astype(np.float64), axis=0) / np.diff(t)[:, None] + edge_a = np.diff(edge_v, axis=0) / ( + (np.diff(t)[1:] + np.diff(t)[:-1])[:, None] * 0.5 + ) + peak_v = np.max(np.abs(edge_v), axis=0) + peak_a = np.max(np.abs(edge_a), axis=0) + return { + "accepted": bool( + np.all(q >= lo - 1e-6) + and np.all(q <= hi + 1e-6) + and np.all(peak_v <= speed * (1 + 1e-5)) + and np.all(peak_a <= accel * (1 + 1e-5)) + ), + "position_violations": int(np.count_nonzero((q < lo - 1e-6) | (q > hi + 1e-6))), + "peak_velocity": peak_v.tolist(), + "peak_acceleration": peak_a.tolist(), + "velocity_ratio": float(np.max(peak_v / speed)), + "acceleration_ratio": float(np.max(peak_a / accel)), + } diff --git a/embodichain/data_pipeline/skill_fitting.py b/embodichain/data_pipeline/skill_fitting.py new file mode 100644 index 0000000000..949e1c6db4 --- /dev/null +++ b/embodichain/data_pipeline/skill_fitting.py @@ -0,0 +1,340 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Joint recordings, explicit video alignment, and self-contained fit bundles.""" + +from __future__ import annotations + +from dataclasses import dataclass +import hashlib +import json +import math +import os +from pathlib import Path +import tempfile +from typing import Any +import zipfile + +import numpy as np + +__all__ = [ + "JointRecording", + "load_joint_recording", + "map_video_interval", + "save_skill_fit", + "load_skill_fit", + "file_digest", +] + +_MAX_BYTES = 256 * 1024 * 1024 + + +def file_digest(path: str | Path) -> str: + """Hash a source without retaining its contents. + + Args: + path: Existing source file. + + Returns: + Lowercase SHA-256 digest. + """ + digest = hashlib.sha256() + with Path(path).open("rb") as stream: + for block in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(block) + return digest.hexdigest() + + +@dataclass(frozen=True) +class JointRecording: + """Owned joints with explicit order, clock and observation semantics. + + Args: + timestamps: Strictly increasing physical times in seconds. + qpos: Numeric (T,D) samples in robot joint units. + joint_names: Ordered, unique joint identifiers. + semantics: measured_state, actual_submission or design_reference. + """ + + timestamps: np.ndarray + qpos: np.ndarray + joint_names: tuple[str, ...] + semantics: str + + def __post_init__(self) -> None: + q = np.asarray(self.qpos) + t = np.asarray(self.timestamps, dtype=np.float64) + names = tuple(self.joint_names) + if ( + q.ndim != 2 + or q.shape[1] == 0 + or q.dtype.kind not in "fi" + or len(q) < 12 + or q.nbytes + t.nbytes > _MAX_BYTES + or t.shape != (len(q),) + or not np.isfinite(q).all() + or not np.isfinite(t).all() + or np.any(np.diff(t) <= 0) + or len(names) != q.shape[1] + or len(set(names)) != len(names) + or any(not isinstance(n, str) or not n or len(n) > 256 for n in names) + ): + raise ValueError("Invalid joint recording, joint order, or physical clock.") + if self.semantics not in { + "measured_state", + "actual_submission", + "design_reference", + }: + raise ValueError( + "Declare measured_state, actual_submission or design_reference." + ) + q, t = ( + np.array( + q, dtype=q.dtype if q.dtype.kind == "f" else np.float64, copy=True + ), + t.copy(), + ) + q.setflags(write=False) + t.setflags(write=False) + object.__setattr__(self, "qpos", q) + object.__setattr__(self, "timestamps", t) + object.__setattr__(self, "joint_names", names) + + +def _read_npz(path: Path) -> dict[str, np.ndarray]: + """Preflight decoded NPY allocation sizes before materializing arrays.""" + total = 0 + with zipfile.ZipFile(path) as archive: + names = archive.namelist() + if len(names) > 64 or len(set(names)) != len(names): + raise ValueError("Too many or duplicate NPZ members.") + for member in archive.infolist(): + if not member.filename.endswith(".npy") or member.file_size > _MAX_BYTES: + raise ValueError("Invalid or oversized NPZ member.") + with archive.open(member) as stream: + version = np.lib.format.read_magic(stream) + if version == (1, 0): + shape, _, dtype = np.lib.format.read_array_header_1_0(stream) + elif version == (2, 0): + shape, _, dtype = np.lib.format.read_array_header_2_0(stream) + else: + raise ValueError("Unsupported NPY header version.") + size = math.prod(shape) * dtype.itemsize + total += size + if dtype.hasobject or total > _MAX_BYTES or size > member.file_size: + raise ValueError("NPZ decoded arrays exceed the allocation budget.") + with np.load(path, allow_pickle=False) as data: + return {name: data[name] for name in data.files} + + +def load_joint_recording( + path: str | Path, + *, + joint_names: tuple[str, ...], + semantics: str, + qpos_key: str = "qpos", + timestamps_key: str = "timestamps", + sample_dt: float | None = None, + env_index: int = 0, +) -> JointRecording: + """Load NPZ, HDF5, or a trusted local Atomic replay PT file. + + PT loading uses PyTorch's weights-only reader. NPZ is the portable input. + Missing clocks require an explicit sample_dt; recording FPS is never guessed. + + Args: + path: Source recording. + joint_names: Expected robot order, also used when the file has no names. + semantics: Explicit meaning of qpos. + qpos_key: NPZ/HDF5 key, or PT positions key. + timestamps_key: Physical time key. + sample_dt: Explicit period for recordings without timestamps or dt. + env_index: Selected row of a batched PT replay. + + Returns: + A validated recording reordered by explicit joint names. + """ + source = Path(path) + if type(env_index) is not int or env_index < 0: + raise ValueError("env_index must be a nonnegative integer.") + if source.suffix == ".npz": + data = _read_npz(source) + elif source.suffix in {".h5", ".hdf5"}: + import h5py + + with h5py.File(source, "r") as handle: + keys = [qpos_key] + ([timestamps_key] if timestamps_key in handle else []) + if any(handle[k].dtype.hasobject for k in keys): + raise ValueError( + "Joint and time datasets must contain fixed-size numeric values." + ) + if ( + sum(handle[k].size * handle[k].dtype.itemsize for k in keys) + > _MAX_BYTES + ): + raise ValueError("HDF5 numeric input exceeds the allocation budget.") + data = {key: handle[key][...] for key in keys} + elif source.suffix == ".pt": + import torch + + if source.stat().st_size > _MAX_BYTES: + raise ValueError("PT replay exceeds the input budget.") + raw = torch.load(source, weights_only=True, map_location="cpu") + if qpos_key == "qpos": + qpos_key = "positions" + data = {} + for name in (qpos_key, timestamps_key, "dt", "planned_dt"): + if name in raw: + value = raw[name] + if not isinstance(value, torch.Tensor): + raise ValueError("PT numeric fields must be tensors.") + value = value.numpy() + if value.ndim == (3 if name == qpos_key else 2): + value = value[env_index] + data[name] = value + if timestamps_key not in data: + interval_key = "planned_dt" if qpos_key == "planned_positions" else "dt" + if interval_key in data: + data[timestamps_key] = np.cumsum(data[interval_key], dtype=np.float64) + else: + raise ValueError("Joint recordings support .npz, .h5/.hdf5 and local .pt.") + q = np.asarray(data[qpos_key]) + if timestamps_key in data: + times = np.asarray(data[timestamps_key], dtype=np.float64) + elif sample_dt is not None and math.isfinite(sample_dt) and sample_dt > 0: + times = np.arange(len(q), dtype=np.float64) * sample_dt + else: + raise ValueError("Missing joint timestamps; provide an explicit sample_dt.") + if "joint_names" in data: + source_names = tuple(str(v) for v in data["joint_names"]) + if len(set(source_names)) != len(source_names) or set(source_names) != set( + joint_names + ): + raise ValueError("Source and robot joint identifiers differ.") + q = q[:, [source_names.index(name) for name in joint_names]] + return JointRecording(times, q, joint_names, semantics) + + +def map_video_interval( + frame_times: np.ndarray, + frame_durations: np.ndarray, + joint_times: np.ndarray, + *, + start_frame: int, + end_frame: int, + time_offset: float, +) -> tuple[int, int]: + """Map an inclusive video interval to a half-open joint interval. + + Args: + frame_times: Video presentation times relative to its first frame. + frame_durations: Positive display duration of every indexed frame. + joint_times: Strictly increasing physical joint times. + start_frame: Included annotation start. + end_frame: Included annotation end. + time_offset: Joint time corresponding to video time zero. + + Returns: + First included and first excluded joint indices. + """ + ft, fd, jt = ( + np.asarray(v, dtype=np.float64) + for v in (frame_times, frame_durations, joint_times) + ) + if ( + ft.ndim != 1 + or fd.shape != ft.shape + or jt.ndim != 1 + or not len(jt) + or not np.isfinite(ft).all() + or not np.isfinite(fd).all() + or not np.isfinite(jt).all() + or not math.isfinite(time_offset) + or np.any(np.diff(ft) <= 0) + or np.any(fd <= 0) + or np.any(np.diff(jt) <= 0) + or type(start_frame) is not int + or type(end_frame) is not int + or not 0 <= start_frame <= end_frame < len(ft) + ): + raise ValueError("Invalid indexed video clock or inclusive frame interval.") + left = ft[start_frame] + time_offset + right = ft[end_frame] + fd[end_frame] + time_offset + tolerance = 1e-7 + if left < jt[0] - tolerance or left > jt[-1] + tolerance or right <= jt[0]: + raise ValueError("Video interval is outside the joint recording clock.") + if ( + right + > jt[-1] + max(float(fd[end_frame]), float(np.median(np.diff(jt)))) + tolerance + ): + raise ValueError("Video interval exceeds the joint recording.") + return ( + int(np.searchsorted(jt, left - tolerance, side="left")), + int(np.searchsorted(jt, right - tolerance, side="left")), + ) + + +def save_skill_fit( + path: str | Path, arrays: dict[str, np.ndarray], metadata: dict[str, Any] +) -> None: + """Atomically write a standalone numeric result, without pickle. + + Args: + path: Destination NPZ. + arrays: Original, native and patched samples plus visualization geometry. + metadata: Versioned fit parameters, profile, clocks and diagnostics. + """ + destination = Path(path) + if metadata.get("schema_version") != 1: + raise ValueError("Fit metadata must declare schema_version=1.") + values = {k: np.asarray(v) for k, v in arrays.items()} + if "metadata_json" in values or any(v.dtype.hasobject for v in values.values()): + raise ValueError("Result arrays cannot use objects or reserved names.") + if sum(v.nbytes for v in values.values()) > _MAX_BYTES: + raise ValueError("Result exceeds the numeric budget.") + values["metadata_json"] = np.array( + json.dumps(metadata, allow_nan=False, sort_keys=True) + ) + destination.parent.mkdir(parents=True, exist_ok=True) + fd, temporary = tempfile.mkstemp(dir=destination.parent, suffix=".npz") + try: + with os.fdopen(fd, "wb") as stream: + np.savez_compressed(stream, **values) + stream.flush() + os.fsync(stream.fileno()) + os.replace(temporary, destination) + finally: + if os.path.exists(temporary): + os.unlink(temporary) + + +def load_skill_fit(path: str | Path) -> tuple[dict[str, np.ndarray], dict[str, Any]]: + """Read a self-contained result without the source recording or simulator. + + Args: + path: NPZ written by save_skill_fit. + + Returns: + Owned numeric arrays and metadata. + """ + data = _read_npz(Path(path)) + metadata = json.loads(str(data.pop("metadata_json"))) + if metadata.get("schema_version") != 1: + raise ValueError("Unsupported skill-fit schema.") + required = {"timestamps", "original_qpos", "native_qpos", "patched_qpos"} + if not required <= data.keys(): + raise ValueError("Fit bundle is missing trajectory arrays.") + return data, metadata diff --git a/embodichain/lab/sim/atomic_skills/__init__.py b/embodichain/lab/sim/atomic_skills/__init__.py index 6c7a7dbd99..e798ecc9e0 100644 --- a/embodichain/lab/sim/atomic_skills/__init__.py +++ b/embodichain/lab/sim/atomic_skills/__init__.py @@ -201,8 +201,10 @@ CoordinatedPlacementOptions, EndEffectorPoseGoal, GraspGoal, + PickUpMotionGoal, HandOver, HandOverGoal, + HandOverMotionGoal, HandOverOptions, HeldObjectPoseGoal, JointPositionGoal, @@ -235,10 +237,12 @@ PushObjectToolCalibration, Slide, SlideGoal, + SlideMotionGoal, SlideJointTarget, SlideOptions, Twist, TwistGoal, + TwistMotionGoal, TwistOptions, ) from .runner import ( @@ -388,8 +392,10 @@ "GRASP_COMMAND", "GRASP_CAPABILITY", "GraspGoal", + "PickUpMotionGoal", "HandOver", "HandOverGoal", + "HandOverMotionGoal", "HandOverOptions", "HeldObjectPoseGoal", "HeldObjectState", @@ -454,10 +460,12 @@ "SlideAffordance", "Slide", "SlideGoal", + "SlideMotionGoal", "SlideJointTarget", "SlideOptions", "Twist", "TwistGoal", + "TwistMotionGoal", "TwistOptions", "RecoveryPolicy", "PlanningRecovery", diff --git a/embodichain/lab/sim/atomic_skills/fitting.py b/embodichain/lab/sim/atomic_skills/fitting.py new file mode 100644 index 0000000000..53c51100f8 --- /dev/null +++ b/embodichain/lab/sim/atomic_skills/fitting.py @@ -0,0 +1,803 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Identify explicit motion goals and reconstruct them through native skills.""" + +from __future__ import annotations + +from dataclasses import asdict, replace +import time +from typing import Any, TYPE_CHECKING + +import numpy as np +from scipy.spatial.transform import Rotation +import torch + +from embodichain.compute.trajectory.phase_alignment import align_joint_phase +from embodichain.compute.trajectory.splicing import ( + joint_path_diagnostics, + splice_joint_segment, +) +from embodichain.data_pipeline.skill_fitting import JointRecording +from embodichain.utils import configclass +from .control import OPEN_COMMAND, GRASP_COMMAND +from .policies import MotionPolicy, RecoveryPolicy +from .primitives import ( + PickUpMotionGoal, + PickUpOptions, + SlideMotionGoal, + SlideOptions, + TwistMotionGoal, + TwistOptions, + HandOverMotionGoal, + HandOverOptions, +) +from .state import RobotObservation + +if TYPE_CHECKING: + from .engine import AtomicSkillEngine + +__all__ = ["SkillFitCfg", "fit_skill_segment"] + + +@configclass +class SkillFitCfg: + """Finite fitting budgets and acceptance tolerances in physical units.""" + + control_dt: float = 0.01 + blend_samples: int = 12 + max_samples: int = 20_000 + max_candidates: int = 3 + max_seconds: float = 120.0 + max_joint_rmse: float = 0.15 + max_tcp_rmse_m: float = 0.015 + max_rotation_rmse_rad: float = 0.08 + max_contact_error_m: float = 0.005 + max_contact_rotation_rad: float = 0.03 + min_twist_angle_rad: float = 0.05 + + def __post_init__(self) -> None: + for key, value in asdict(self).items(): + if isinstance(value, bool) or not np.isfinite(value) or value <= 0: + raise ValueError(f"SkillFitCfg.{key} must be finite and positive.") + for key in ("blend_samples", "max_samples", "max_candidates"): + if type(getattr(self, key)) is not int: + raise ValueError(f"{key} must be an integer.") + if ( + self.max_candidates > 9 + or self.max_samples > 200_000 + or self.blend_samples < 4 + ): + raise ValueError("Fitting budgets are outside supported bounds.") + + +def _transitions( + q: np.ndarray, opened: np.ndarray, closed: np.ndarray +) -> tuple[list, list]: + delta = closed - opened + progress = ((q - opened) @ delta) / (delta @ delta) + changes = np.diff(progress) + result = [] + for sign in (1, -1): + ids = np.flatnonzero(sign * changes > max(1e-5, np.ptp(progress) * 1e-3)) + 1 + groups = np.split(ids, np.flatnonzero(np.diff(ids) > 2) + 1) if len(ids) else [] + # A meaningful open/close event covers at least 10% of the profile travel. + result.append( + [ + (int(g[0] - 1), int(g[-1] + 1)) + for g in groups + if len(g) and abs(progress[g[-1]] - progress[g[0] - 1]) >= 0.1 + ] + ) + return result[0], result[1] + + +def _rot_error(left: np.ndarray, right: np.ndarray) -> np.ndarray: + return Rotation.from_matrix( + left[..., :3, :3] @ right[..., :3, :3].swapaxes(-1, -2) + ).magnitude() + + +def _approach(poses: np.ndarray, close_start: int) -> tuple[np.ndarray, float, int]: + grasp = poses[close_start] + candidates = np.flatnonzero(_rot_error(poses[:close_start], grasp) < 0.00003) + if len(candidates) < 2: + raise ValueError("No stable TCP orientation before grasp; provide phase hints.") + # The farthest point after attaining the grasp orientation is the pre-grasp. + offsets = grasp[:3, 3] - poses[candidates, :3, 3] + norms = np.linalg.norm(offsets, axis=1) + index = int(candidates[np.flatnonzero(norms >= np.max(norms) - 1e-8)[-1]]) + distance = float(np.max(norms)) + if distance < 1e-4: + raise ValueError("Approach displacement is not observable.") + return (grasp[:3, 3] - poses[index, :3, 3]) / distance, distance, index + + +def _twist_line( + poses: np.ndarray, minimum_angle: float +) -> tuple[np.ndarray, np.ndarray, float, float]: + rotation = Rotation.from_matrix(poses[:, :3, :3] @ poses[0, :3, :3].T) + vectors = rotation.as_rotvec() + _, singular, vt = np.linalg.svd(vectors, full_matrices=False) + axis = vt[0] + angles = np.unwrap(vectors @ axis) + angle = float(angles[-1]) + if abs(angle) < minimum_angle: + raise ValueError("Twist angle is too small to identify a rotation line.") + if angle < 0: + axis, angles, angle = -axis, -angles, -angle + rotations = Rotation.from_rotvec(angles[:, None] * axis).as_matrix() + matrix = np.eye(3)[None] - rotations + right = poses[:, :3, 3] - np.einsum("nij,j->ni", rotations, poses[0, :3, 3]) + # Resolve the axis-point gauge by choosing its closest point to the initial TCP. + origin = np.linalg.lstsq( + np.concatenate((matrix.reshape(-1, 3), axis[None]), axis=0), + np.concatenate((right.reshape(-1), [axis @ poses[0, :3, 3]])), + rcond=None, + )[0] + predicted = origin + np.einsum("nij,j->ni", rotations, poses[0, :3, 3] - origin) + error = float(np.sqrt(np.mean(np.sum((predicted - poses[:, :3, 3]) ** 2, axis=1)))) + return axis, origin, angle, error + + +def _phase_map( + q_native: np.ndarray, + native_lengths: dict[str, int], + phases: list[dict[str, Any]], + observations: np.ndarray, + approach_anchor: tuple[int, int] | None = None, +) -> tuple[np.ndarray, np.ndarray]: + if list(native_lengths) != [p["name"] for p in phases]: + raise ValueError( + f"Source phases must match native phases {list(native_lengths)}." + ) + output = np.empty_like(observations, dtype=np.float64) + timing = np.empty(len(observations)) + cursor = 0 + for phase in phases: + count = native_lengths[phase["name"]] + part = q_native[cursor : cursor + count] + cursor += count + start, stop = phase["start"], phase["stop"] + if stop <= start: + raise ValueError("Every source phase must contain a sample.") + if phase["name"] == "approach" and approach_anchor is not None: + source_anchor, native_anchor = approach_anchor + if not start < source_anchor < stop or not 0 < native_anchor < count - 1: + raise ValueError("Pre-grasp anchor must be inside its approach phase.") + first, clock_a = align_joint_phase( + part[: native_anchor + 1], observations[start : source_anchor + 1] + ) + second, clock_b = align_joint_phase( + part[native_anchor:], observations[source_anchor:stop] + ) + output[start : source_anchor + 1], output[source_anchor:stop] = ( + first, + second, + ) + timing[start : source_anchor + 1] = ( + clock_a * native_anchor / max(1, count - 1) + ) + timing[source_anchor:stop] = ( + native_anchor + clock_b * (count - 1 - native_anchor) + ) / max(1, count - 1) + else: + output[start:stop], timing[start:stop] = align_joint_phase( + part, observations[start:stop] + ) + if cursor != len(q_native): + raise ValueError("Native phase lengths do not cover the generated plan.") + return output, timing + + +def fit_skill_segment( + recording: JointRecording, + engine: AtomicSkillEngine, + *, + skill_id: str, + endpoints: dict[str, dict[str, str]], + start: int, + stop: int, + acceleration_limits: np.ndarray, + cfg: SkillFitCfg | None = None, + phase_hints: list[dict[str, Any]] | None = None, +) -> tuple[dict[str, np.ndarray], dict[str, Any]]: + """Identify and regenerate one segment without object pose or geometry. + + Only the native AtomicSkillEngine creates candidate joint motion. Source + joints supply observations, initial seeds, phase clocks and outer boundary + stencils. The returned reference is not a measured-state prediction. + + Args: + recording: Explicitly timed, named source joints. + engine: Engine bound to a one-row robot kinematics port. + skill_id: pick_up, slide, twist or hand_over. + endpoints: Existing explicit participant-to-control-part bindings. + start: First included recording sample. + stop: First excluded recording sample. + acceleration_limits: Explicit per-joint acceleration caps. + cfg: Numerical budgets and independent native/patched acceptance gates. + phase_hints: Optional complete contiguous phase partition. Indices are + relative to this segment, with half-open bounds. + + Returns: + Full-length original/native/patched arrays and fit metadata. Rejected + fits retain their native candidate and leave patched_qpos unchanged. + """ + cfg = SkillFitCfg() if cfg is None else cfg + cfg.validate() + cfg.__post_init__() + began = time.monotonic() + if skill_id not in {"pick_up", "slide", "twist", "hand_over"}: + raise ValueError("Unsupported reconstruction skill.") + if tuple(engine.robot.joint_names) != recording.joint_names: + raise ValueError("Recording must use the engine's exact named joint order.") + if ( + not 0 <= start < stop <= len(recording.qpos) + or not 12 <= stop - start <= cfg.max_samples + ): + raise ValueError("Invalid or oversized selected segment.") + q = recording.qpos[start:stop] + times = recording.timestamps[start:stop] + robot = engine.robot + device = engine.device + hints = {} if phase_hints is None else {p["name"]: p for p in phase_hints} + if phase_hints is not None and ( + not phase_hints + or len(hints) != len(phase_hints) + or phase_hints[0]["start"] != 0 + or phase_hints[-1]["stop"] != len(q) + or any( + type(p["start"]) is not int + or type(p["stop"]) is not int + or not 0 <= p["start"] < p["stop"] <= len(q) + for p in phase_hints + ) + or any(a["stop"] != b["start"] for a, b in zip(phase_hints, phase_hints[1:])) + ): + raise ValueError("Phase hints must be a complete contiguous segment partition.") + participant_data = {} + for role, ports in endpoints.items(): + arm, hand = ports["motion"], ports["grasp"] + arm_ids, hand_ids = robot.get_joint_ids(arm), robot.get_joint_ids(hand) + profile = engine.control_profiles[hand] + opened = profile.commands[OPEN_COMMAND].positions + closed = profile.commands[GRASP_COMMAND].positions + opened = torch.as_tensor(opened).cpu().numpy().reshape(-1) + closed = torch.as_tensor(closed).cpu().numpy().reshape(-1) + with torch.no_grad(): + poses = ( + robot.compute_fk( + torch.as_tensor(q[:, arm_ids], device=device, dtype=torch.float32), + name=arm, + to_matrix=True, + ) + .cpu() + .numpy() + .astype(np.float64) + ) + closes, opens = _transitions(q[:, hand_ids], opened, closed) + close_name = "pickup_close" if role == "source" else "close" + open_name = ( + "release" + if role == "source" + else ("receive_release" if role == "destination" else "open") + ) + if close_name in hints: + closes = [(hints[close_name]["start"], hints[close_name]["stop"])] + if open_name in hints: + opens = [(hints[open_name]["start"], hints[open_name]["stop"])] + participant_data[role] = (poses, closes, opens, arm_ids, hand_ids) + + params: dict[str, Any] = {"pose_origin": "fk_derived"} + contact_ranges: list[tuple[str, int, int]] = [] + + def tensor(value: np.ndarray) -> torch.Tensor: + return torch.tensor(value, device=device, dtype=torch.float32) + + def partition(boundaries: list[tuple[str, int]]) -> list[dict[str, Any]]: + return [ + {"name": name, "start": left, "stop": right} + for (name, left), (_, right) in zip(boundaries, boundaries[1:]) + ] + + if skill_id != "hand_over": + if set(endpoints) != {"primary"}: + raise ValueError("Single-arm skills require the primary participant.") + poses, closes, opens, _, _ = participant_data["primary"] + if len(closes) != 1: + raise ValueError( + "Expected one observable closure; select a complete skill or provide matching command data." + ) + close_start, lift_start = closes[0] + if ( + np.max( + np.linalg.norm( + poses[close_start:lift_start, :3, 3] - poses[close_start, :3, 3], + axis=1, + ) + ) + > cfg.max_contact_error_m + or np.max(_rot_error(poses[close_start:lift_start], poses[close_start])) + > cfg.max_contact_rotation_rad + ): + raise ValueError( + "The native skill requires a stationary arm during closure." + ) + direction, pre_distance, pre_index = _approach(poses, close_start) + if "reach" in hints: + pre_index = hints["reach"]["start"] + offset = poses[close_start, :3, 3] - poses[pre_index, :3, 3] + pre_distance = float(np.linalg.norm(offset)) + if pre_distance <= 1e-4: + raise ValueError("Hinted pre-grasp displacement is not observable.") + direction = offset / pre_distance + grasp = poses[close_start] + close_count = lift_start - close_start + if skill_id == "pick_up": + lift = poses[-1, :3, 3] - grasp[:3, 3] + if lift[2] <= 0 or np.linalg.norm(lift[:2]) > cfg.max_contact_error_m: + raise ValueError("Pickup requires an observable world-Z lift.") + if float(_rot_error(poses[-1], grasp)) > cfg.max_contact_rotation_rad: + raise ValueError("Pickup lift changes TCP orientation.") + goal = PickUpMotionGoal(tensor(grasp)) + options = PickUpOptions( + lift_height=float(lift[2]), + pre_grasp_distance=pre_distance, + approach_direction=tensor(direction), + hand_interp_steps=close_count, + ) + phases = partition( + [ + ("approach", 0), + ("close", close_start), + ("lift", lift_start), + ("end", len(q)), + ] + ) + params.update( + lift_height=float(lift[2]), + pre_grasp_distance=pre_distance, + approach_direction=direction.tolist(), + grasp_xpos=grasp.tolist(), + ) + contact_ranges = [("primary", close_start, len(q))] + else: + releases = [event for event in opens if event[0] >= lift_start] + if len(releases) != 1: + raise ValueError("Slide/Twist requires one observable release.") + open_start, retract_start = releases[0] + if skill_id == "slide": + move = poses[open_start, :3, 3] - grasp[:3, 3] + distance = float(np.linalg.norm(move)) + if distance < 1e-4: + raise ValueError("Slide displacement is not observable.") + rail = move / distance + retract_distance = np.linalg.norm( + poses[-1, :3, 3] - poses[open_start, :3, 3] + ) + mode = "push" if retract_distance > cfg.max_contact_error_m else "pull" + axis = rail if mode == "push" else -rail + projected = (poses[lift_start:open_start, :3, 3] - grasp[:3, 3]) @ rail + residual = ( + poses[lift_start:open_start, :3, 3] + - grasp[:3, 3] + - projected[:, None] * rail + ) + if np.max(np.linalg.norm(residual, axis=1)) > cfg.max_contact_error_m: + raise ValueError("Slide contact motion is not a straight rail.") + goal = SlideMotionGoal(tensor(grasp), tensor(axis)) + options = SlideOptions( + direction=mode, + translation_distance=distance, + approach_distance=pre_distance, + hand_interp_steps=close_count, + ) + boundaries = [ + ("approach", 0), + ("reach", pre_index), + ("close", close_start), + (mode, lift_start), + ("open", open_start), + ] + if mode == "push": + boundaries.append(("return", retract_start)) + phases = partition(boundaries + [("end", len(q))]) + params.update( + translation_axis=axis.tolist(), + translation_distance=distance, + direction=mode, + approach_distance=pre_distance, + grasp_xpos=grasp.tolist(), + ) + else: + axis, origin, angle, error = _twist_line( + poses[lift_start : open_start + 1], cfg.min_twist_angle_rad + ) + if error > cfg.max_contact_error_m: + raise ValueError("Twist does not fit a rigid rotation line.") + goal = TwistMotionGoal(tensor(grasp), tensor(axis), tuple(origin)) + options = TwistOptions( + twist_angle=angle, + pre_grasp_distance=pre_distance, + hand_interp_steps=close_count, + twist_waypoint_count=2, + ) + phases = partition( + [ + ("approach", 0), + ("reach", pre_index), + ("close", close_start), + ("twist", lift_start), + ("open", open_start), + ("retract", retract_start), + ("end", len(q)), + ] + ) + params.update( + twist_axis=axis.tolist(), + axis_origin=origin.tolist(), + twist_angle=angle, + pre_grasp_distance=pre_distance, + grasp_xpos=grasp.tolist(), + axis_fit_rmse_m=error, + axis_point_convention="closest_to_initial_tcp", + ) + contact_ranges = [("primary", close_start, open_start)] + else: + if set(endpoints) != {"source", "destination"}: + raise ValueError( + "Handover requires explicit source/destination participants." + ) + source, source_closes, source_opens, source_ids, _ = participant_data["source"] + receive, receive_closes, receive_opens, _, _ = participant_data["destination"] + if len(receive_closes) != 1 or len(source_opens) != 1 or len(source_closes) > 1: + raise ValueError( + "Select one complete handover with observable hand events." + ) + rc0, rc1 = receive_closes[0] + so0, so1 = source_opens[0] + if rc1 >= so0: + raise ValueError("Handover needs a receiving hold before source release.") + for observed in (source, receive): + hold = observed[rc1 - 1 : so0 + 1] + if ( + np.max(np.linalg.norm(hold[:, :3, 3] - hold[0, :3, 3], axis=1)) + > cfg.max_contact_error_m + or np.max(_rot_error(hold, hold[0])) > cfg.max_contact_rotation_rad + ): + raise ValueError( + "Handover exchange requires stationary overlap anchors." + ) + moving = ( + np.flatnonzero( + np.max(np.abs(np.diff(q[:, source_ids], axis=0)), axis=1) > 1e-5 + ) + + 1 + ) + transfer_end = ( + hints["approach"]["start"] + if "approach" in hints + else int(max((i for i in moving if i < rc0), default=0)) + 1 + ) + retreat_end = ( + hints["place"]["start"] + if "place" in hints + else int(max((i for i in moving if i >= so1), default=so1)) + 1 + ) + if not transfer_end < rc0 or retreat_end >= len(q): + raise ValueError( + "Handover phase split needs explicit phase hints or stationary boundaries." + ) + pickup = source[source_closes[0][0]] if source_closes else None + placement = receive_opens[0] if len(receive_opens) == 1 else None + goal = HandOverMotionGoal( + tensor(source[rc1 - 1]), + tensor(receive[rc1 - 1]), + tensor(source[retreat_end - 1]), + None if pickup is None else tensor(pickup), + None if placement is None else tensor(receive[placement[0]]), + None if placement is None else tensor(receive[-1]), + ) + options = HandOverOptions( + arm_selection="bound", + release_at_target=placement is not None, + hand_interp_steps=rc1 - rc0, + hold_steps=so0 - rc1, + retreat_steps=max(3, retreat_end - so1), + ) + boundaries = [] + if source_closes: + sc0, sc1 = source_closes[0] + boundaries.extend([("pickup_approach", 0), ("pickup_close", sc0)]) + transfer_start = sc1 + else: + transfer_start = 0 + boundaries.extend( + [ + ("transfer", transfer_start), + ("approach", transfer_end), + ("close", rc0), + ("hold", rc1), + ("release", so0), + ("retreat", so1), + ] + ) + if placement: + boundaries.extend( + [ + ("place", retreat_end), + ("receive_release", placement[0]), + ("receive_retreat", placement[1]), + ] + ) + phases = partition(boundaries + [("end", len(q))]) + contact_ranges = [ + ("source", source_closes[0][0] if source_closes else 0, so0), + ("destination", rc0, placement[0] if placement else len(q)), + ] + params.update( + source_exchange=source[rc1 - 1].tolist(), + destination_exchange=receive[rc1 - 1].tolist(), + relative_exchange=( + np.linalg.inv(source[rc1 - 1]) @ receive[rc1 - 1] + ).tolist(), + source_retreat=source[retreat_end - 1].tolist(), + ) + + if phase_hints is not None: + if [p["name"] for p in phase_hints] != [p["name"] for p in phases]: + raise ValueError( + "Phase hints must name the complete native phase sequence." + ) + phases = [dict(p) for p in phase_hints] + if ( + phases[0]["start"] != 0 + or phases[-1]["stop"] != len(q) + or any( + type(p["start"]) is not int + or type(p["stop"]) is not int + or p["stop"] <= p["start"] + for p in phases + ) + or any(a["stop"] != b["start"] for a, b in zip(phases, phases[1:])) + ): + raise ValueError("Phases must be a complete nonempty contiguous partition.") + context = engine.initial_context(control_dt=cfg.control_dt) + context = replace( + context, + robot=RobotObservation( + timestamp=0, + qpos=tensor(q[0])[None], + qvel=torch.zeros_like(tensor(q[0])[None]), + ), + ) + if context.batch_size != 1: + raise ValueError("Skill reconstruction currently requires B=1.") + # Native timing remains inspectable; phasewise mapping has its own fixed + # source clock and all final derivative checks occur after that mapping. + policy = MotionPolicy( + strategy="motion_gen" if skill_id == "pick_up" else "ik_interp", + sample_count=max(len(q), 80), + acceleration_limit=0.5, + ) + candidates = [] + for candidate in range(cfg.max_candidates if skill_id == "pick_up" else 1): + if time.monotonic() - began > cfg.max_seconds: + break + distance = ( + pre_distance + + (0.0, -0.005, 0.005, -0.01, 0.01, -0.02, 0.02, -0.03, 0.03)[candidate] + if skill_id == "pick_up" + else None + ) + trial = ( + replace(options, pre_grasp_distance=max(0.0001, distance)) + if distance is not None + else options + ) + invocation = engine.make_invocation( + skill_id, + goal, + control_parts=endpoints, + motion_policy=policy, + skill_options=trial, + recovery_policy=RecoveryPolicy(max_replans=0, max_skill_retries=0), + ) + plan = engine.plan(invocation, context) + if not bool(plan.plan_success.all()): + continue + native = plan.joint_trajectory.positions[0].detach().cpu().numpy() + lengths = {p.name: p.stop - p.start for p in plan.segments} + anchor = None + if skill_id == "pick_up": + with torch.no_grad(): + approach_fk = ( + robot.compute_fk( + tensor( + native[ + : lengths["approach"], participant_data["primary"][3] + ] + ), + name=endpoints["primary"]["motion"], + to_matrix=True, + ) + .cpu() + .numpy() + ) + target_pre = grasp[:3, 3] - direction * trial.pre_grasp_distance + native_pre = int( + np.argmin(np.linalg.norm(approach_fk[:, :3, 3] - target_pre, axis=1)) + ) + anchor = ( + (pre_index, native_pre) if pre_index > 0 and native_pre > 0 else None + ) + mapped, timing = _phase_map(native, lengths, phases, q, anchor) + owned = sorted( + {j for data in participant_data.values() for ids in data[3:] for j in ids} + ) + score = float(np.sqrt(np.mean((mapped[:, owned] - q[:, owned]) ** 2))) + candidates.append((score, mapped, native, lengths, trial, timing)) + if not candidates: + raise ValueError("Native skill generation failed within the fitting budget.") + score, mapped, raw_native, lengths, selected_options, timing = min( + candidates, key=lambda item: item[0] + ) + if skill_id == "pick_up": + params["pre_grasp_distance"] = selected_options.pre_grasp_distance + # Every non-participant column stays observed, including passive/mimic state. + editable = tuple(owned) + mapped[:, [i for i in range(q.shape[1]) if i not in editable]] = q[ + :, [i for i in range(q.shape[1]) if i not in editable] + ] + candidate_full = recording.qpos.copy() + candidate_full[start:stop] = mapped + patched = splice_joint_segment( + recording.qpos, + mapped, + recording.timestamps, + start=start, + stop=stop, + joint_indices=editable, + blend_samples=cfg.blend_samples, + anchors=tuple(start + p["start"] for p in phases[1:]), + ) + errors = {} + failures = [] + for role, (observed, _, _, arm_ids, _) in participant_data.items(): + + def fk(values: np.ndarray) -> np.ndarray: + with torch.no_grad(): + return ( + robot.compute_fk( + tensor(values[:, arm_ids]), + name=endpoints[role]["motion"], + to_matrix=True, + ) + .cpu() + .numpy() + ) + + native_tcp, patched_tcp = fk(mapped), fk(patched[start:stop]) + position = np.linalg.norm(native_tcp[:, :3, 3] - observed[:, :3, 3], axis=1) + rotation = _rot_error(native_tcp, observed) + errors[role] = { + "native_tcp_rmse_m": float(np.sqrt(np.mean(position**2))), + "native_rotation_rmse_rad": float(np.sqrt(np.mean(rotation**2))), + } + if ( + errors[role]["native_tcp_rmse_m"] > cfg.max_tcp_rmse_m + or errors[role]["native_rotation_rmse_rad"] > cfg.max_rotation_rmse_rad + ): + failures.append(f"{role}: native TCP reconstruction exceeds tolerance") + for contact_role, a, b in contact_ranges: + if contact_role == role: + if ( + position[a:b].max() > cfg.max_contact_error_m + or rotation[a:b].max() > cfg.max_contact_rotation_rad + ): + failures.append( + f"{role}: native contact path exceeds observation tolerance" + ) + correction = np.linalg.norm( + patched_tcp[a:b, :3, 3] - native_tcp[a:b, :3, 3], axis=1 + ) + correction_rotation = _rot_error(patched_tcp[a:b], native_tcp[a:b]) + if ( + correction.max() > cfg.max_contact_error_m + or correction_rotation.max() > cfg.max_contact_rotation_rad + ): + failures.append( + f"{role}: boundary correction changes protected contact motion" + ) + if score > cfg.max_joint_rmse: + failures.append("native joint RMSE exceeds tolerance") + limits = robot.get_qpos_limits()[0].cpu().numpy() + velocity = robot.get_qvel_limits()[0].cpu().numpy() + acceleration = np.asarray(acceleration_limits, dtype=np.float64) + diagnostics = joint_path_diagnostics( + patched, + recording.timestamps, + lower=limits[:, 0], + upper=limits[:, 1], + velocity_limits=velocity, + acceleration_limits=acceleration, + ) + if not diagnostics["accepted"]: + failures.append("patched path exceeds joint limits") + accepted = not failures + + def serializable(value: Any) -> Any: + if isinstance(value, torch.Tensor): + return value.detach().cpu().tolist() + if isinstance(value, dict): + return {k: serializable(v) for k, v in value.items()} + if isinstance(value, (tuple, list)): + return [serializable(v) for v in value] + return value + + report = { + "schema_version": 1, + "skill_id": skill_id, + "accepted": accepted, + "failures": failures, + "parameters": params, + "phases": phases, + "native_phase_lengths": lengths, + "native_joint_rmse": score, + "native_options": serializable(asdict(selected_options)), + "native_goal": serializable(asdict(goal)), + "initial_qpos": q[0].tolist(), + "algorithm_revision": 1, + "per_joint_native_rmse": np.sqrt(np.mean((mapped - q) ** 2, axis=0)).tolist(), + "time_mapping": "phasewise_monotone_native_path_projection", + "native_motion_policy": { + "strategy": policy.strategy, + "sample_count": policy.sample_count, + "acceleration_limit": policy.acceleration_limit, + "control_dt": cfg.control_dt, + }, + "patched_joint_rmse": float( + np.sqrt(np.mean((patched[start:stop, owned] - q[:, owned]) ** 2)) + ), + "tcp_errors": errors, + "limits": diagnostics, + "start": start, + "stop": stop, + "editable_joints": owned, + "qpos_semantics": recording.semantics, + "output_semantics": "design_reference", + "physical_success": "unknown", + "collision_evidence": "unverified", + "fit_cfg": asdict(cfg), + "candidate_count": len(candidates), + "elapsed_seconds": time.monotonic() - began, + "joint_names": list(recording.joint_names), + } + return { + "timestamps": recording.timestamps.copy(), + "original_qpos": recording.qpos.copy(), + "native_qpos": candidate_full, + "patched_qpos": patched if accepted else recording.qpos.copy(), + "proposed_qpos": patched, + "native_phase_qpos": raw_native, + "phase_progress": timing, + "modified_mask": ( + (patched != recording.qpos) + if accepted + else np.zeros_like(patched, dtype=bool) + ), + }, report diff --git a/embodichain/lab/sim/atomic_skills/primitives/__init__.py b/embodichain/lab/sim/atomic_skills/primitives/__init__.py index fef056f58a..4440dea4eb 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/__init__.py +++ b/embodichain/lab/sim/atomic_skills/primitives/__init__.py @@ -31,7 +31,7 @@ CoordinatedPlacementGoal, CoordinatedPlacementOptions, ) -from .hand_over import HandOver, HandOverGoal, HandOverOptions +from .hand_over import HandOver, HandOverGoal, HandOverMotionGoal, HandOverOptions from .move_end_effector import ( EndEffectorPoseGoal, MoveEndEffector, @@ -44,7 +44,7 @@ ) from .move_joints import JointPositionGoal, MoveJoints, MoveJointsOptions from .open_door import OpenDoor, OpenDoorGoal, OpenDoorOptions -from .pick_up import GraspGoal, PickUp, PickUpOptions +from .pick_up import GraspGoal, PickUpMotionGoal, PickUp, PickUpOptions from .place import AssembleGoal, Place, PlaceGoal, PlaceOptions from .press import ( Press, @@ -63,10 +63,11 @@ from .slide import ( Slide, SlideGoal, + SlideMotionGoal, SlideJointTarget, SlideOptions, ) -from .twist import Twist, TwistGoal, TwistOptions +from .twist import Twist, TwistGoal, TwistMotionGoal, TwistOptions BUILTIN_SKILL_TYPES: tuple[type[AtomicSkill], ...] = ( ExecuteDemoSkill, @@ -104,8 +105,10 @@ "CoordinatedPlacementOptions", "EndEffectorPoseGoal", "GraspGoal", + "PickUpMotionGoal", "HandOver", "HandOverGoal", + "HandOverMotionGoal", "HandOverOptions", "HeldObjectPoseGoal", "JointPositionGoal", @@ -138,9 +141,11 @@ "PushObjectToolCalibration", "Slide", "SlideGoal", + "SlideMotionGoal", "SlideJointTarget", "SlideOptions", "Twist", "TwistGoal", + "TwistMotionGoal", "TwistOptions", ] diff --git a/embodichain/lab/sim/atomic_skills/primitives/hand_over.py b/embodichain/lab/sim/atomic_skills/primitives/hand_over.py index c41f9075cf..fa3b670b0b 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_skills/primitives/hand_over.py @@ -87,6 +87,43 @@ ) +@dataclass(frozen=True, slots=True, eq=False) +class HandOverMotionGoal: + """Explicit dual-TCP anchors for effect-free handover reconstruction. + + Args: + source_exchange: Source TCP at the exchange. + destination_exchange: Receiving TCP at the exchange. + source_retreat: Released source TCP retreat target. + source_grasp: Optional initial pickup TCP; omission starts closed. + destination_place: Optional receiving TCP placement target. + destination_retreat: Receiving TCP after release; required with place. + """ + + goal_kind: ClassVar[str] = "handover_motion" + source_exchange: PoseGoalValue + destination_exchange: PoseGoalValue + source_retreat: PoseGoalValue + source_grasp: PoseGoalValue | None = None + destination_place: PoseGoalValue | None = None + destination_retreat: PoseGoalValue | None = None + + def __post_init__(self) -> None: + for name in ( + "source_exchange", + "destination_exchange", + "source_retreat", + "source_grasp", + "destination_place", + "destination_retreat", + ): + value = getattr(self, name) + if value is not None: + validate_pose_goal(value, name, allow_waypoints=False) + if (self.destination_place is None) != (self.destination_retreat is None): + raise ValueError("Placement requires both receiving TCP anchors.") + + @dataclass(frozen=True, slots=True, eq=False) class HandOverGoal(ObjectSkillGoal): """Object to pick and hand over, plus its final object pose.""" @@ -198,7 +235,7 @@ class _DirectionalPlan: policy_batching: dict[str, object] | None = None -class HandOver(AtomicSkill[HandOverGoal, HandOverOptions]): +class HandOver(AtomicSkill[HandOverGoal | HandOverMotionGoal, HandOverOptions]): """Pick an object with the nearer arm and transfer it to the other arm. For each environment, the action chooses the arm whose root link is closer @@ -230,7 +267,7 @@ class HandOver(AtomicSkill[HandOverGoal, HandOverOptions]): """ skill_id: ClassVar[str] = "hand_over" - GoalType: ClassVar[type] = HandOverGoal + GoalType: ClassVar[tuple[type, ...]] = (HandOverGoal, HandOverMotionGoal) OptionsType: ClassVar[type] = HandOverOptions open_loop: ClassVar[bool] = True _SURFACE_POINT_COUNT: ClassVar[int] = 1000 @@ -273,6 +310,8 @@ def _scene_dependencies( ) -> tuple[str, ...]: """Track both the initial object and any late-bound final target.""" dependencies = set(super()._scene_dependencies(request)) + if isinstance(request.goal, HandOverMotionGoal): + return tuple(sorted(dependencies)) entity_id = request.goal.semantics.entity_id if entity_id is not None: dependencies.add(entity_id) @@ -350,6 +389,8 @@ def _plan( ) -> SkillPlan: """Plan pickup and transfer, with optional placement and release.""" goal = self.require_goal(request) + if isinstance(goal, HandOverMotionGoal): + return self._plan_motion(request, context) options = request.skill_options resources = self._resolve_resources(request) # Each segment plans one explicitly named arm. The task layer merges @@ -673,6 +714,145 @@ def _plan( ), ) + def _plan_motion( + self, + request: ResolvedSkillRequest[HandOverMotionGoal, HandOverOptions], + context: PlanningContext, + ) -> SkillPlan: + """Compose native arm/hand phases without an object attachment claim.""" + goal, options = request.goal, request.skill_options + if options.arm_selection != "bound": + raise ValueError("Handover motion goals require bound participant slots.") + if options.release_at_target != (goal.destination_place is not None): + raise ValueError( + "release_at_target must match the explicit placement anchors." + ) + resources = self._resolve_resources(request) + source, destination = resources.first, resources.second + count = max(3, request.motion_policy.sample_count // 8) + full = context.last_qpos.clone() + segments: list[torch.Tensor] = [] + lengths: dict[str, int] = {} + success = torch.ones(context.batch_size, dtype=torch.bool, device=self.device) + + def pose(value: PoseGoalValue) -> torch.Tensor: + return resolve_batched_pose( + resolve_pose_goal(value, context, name="motion_anchor"), + num_envs=context.batch_size, + device=self.device, + name="motion_anchor", + ) + + def append( + name: str, + participant: _Participant | None = None, + target: torch.Tensor | None = None, + command: torch.Tensor | None = None, + n: int = count, + ) -> None: + nonlocal full, success + if participant is not None and target is not None: + valid, path = plan_named_arm_trajectory( + self.motion_generator, + participant.arm.control_part, + full[:, list(participant.arm.joint_ids)], + target, + n, + request.motion_policy, + context.require_control_dt(), + ) + success &= valid + result = full[:, None].expand(-1, path.shape[1], -1).clone() + result[:, :, list(participant.arm.joint_ids)] = path + else: + result = full[:, None].expand(-1, n, -1).clone() + if participant is not None and command is not None: + ids = list(participant.hand.joint_ids) + result[:, :, ids] = interpolate_hand_qpos( + full[:, ids], command, n_waypoints=result.shape[1] + ) + full = result[:, -1].clone() + lengths[name] = result.shape[1] + segments.append(result) + + if goal.source_grasp is not None: + grasp = pose(goal.source_grasp) + pre = translate_pose_world( + grasp, -grasp[:, :3, 2] * options.pre_grasp_distance + ) + append( + "pickup_approach", + source, + torch.stack((pre, grasp), dim=1), + source.hand_open_qpos, + ) + append( + "pickup_close", + source, + command=source.hand_grasp_qpos, + n=options.hand_interp_steps, + ) + else: + # Retain the profile's effort-producing command on a closed source. + full[:, list(source.hand.joint_ids)] = source.hand_grasp_qpos + append("transfer", source, pose(goal.source_exchange)[:, None]) + receive = pose(goal.destination_exchange) + pre = translate_pose_world( + receive, -receive[:, :3, 2] * options.pre_grasp_distance + ) + append( + "approach", + destination, + torch.stack((pre, receive), dim=1), + destination.hand_open_qpos, + ) + append( + "close", + destination, + command=destination.hand_grasp_qpos, + n=options.hand_interp_steps, + ) + append("hold", n=max(1, options.hold_steps)) + append( + "release", + source, + command=source.hand_open_qpos, + n=options.hand_interp_steps, + ) + append( + "retreat", + source, + pose(goal.source_retreat)[:, None], + n=options.retreat_steps, + ) + if goal.destination_place is not None: + append("place", destination, pose(goal.destination_place)[:, None]) + append( + "receive_release", + destination, + command=destination.hand_open_qpos, + n=options.hand_interp_steps, + ) + append( + "receive_retreat", destination, pose(goal.destination_retreat)[:, None] + ) + return self.build_plan( + request, + context, + success=success, + trajectory=TimedTrajectory.from_uniform_step( + torch.cat(segments, dim=1), + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), + segment_lengths=lengths, + expected_effects=StateDelta(), + diagnostics=PlannerDiagnostics( + backend=self.planning_services.planner_name, + metadata={"grounding": "explicit_tcp", "physical_success": "unknown"}, + ), + ) + def _plan_existing_hold( self, request: ResolvedSkillRequest[HandOverGoal, HandOverOptions], @@ -2282,4 +2462,4 @@ def _assemble_segment( ) -__all__ = ["HandOver", "HandOverGoal", "HandOverOptions"] +__all__ = ["HandOver", "HandOverGoal", "HandOverMotionGoal", "HandOverOptions"] diff --git a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py index 9c3a5f7822..ed1bdbe833 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py @@ -89,6 +89,24 @@ ) +@dataclass(frozen=True, slots=True, eq=False) +class PickUpMotionGoal: + """Explicit world TCP anchor for effect-free pickup motion reconstruction. + + This goal shares PickUp's native phase generator. It carries no object + identity and creates no held-object state or physical-success evidence. + + Args: + grasp_xpos: Observed or FK-derived world grasp TCP transform. + """ + + goal_kind: ClassVar[str] = "pickup_motion" + grasp_xpos: PoseGoalValue + + def __post_init__(self) -> None: + validate_pose_goal(self.grasp_xpos, "grasp_xpos", allow_waypoints=False) + + @dataclass(frozen=True, slots=True, eq=False) class GraspGoal(ObjectSkillGoal): """Pickup target with an affordance-selected or supplied grasp pose.""" @@ -307,11 +325,11 @@ def __post_init__(self) -> None: ) -class PickUp(AtomicSkill[GraspGoal, PickUpOptions]): +class PickUp(AtomicSkill[GraspGoal | PickUpMotionGoal, PickUpOptions]): """Approach a grasp pose, close the gripper, lift.""" skill_id: ClassVar[str] = "pick_up" - GoalType: ClassVar[type] = GraspGoal + GoalType: ClassVar[tuple[type, ...]] = (GraspGoal, PickUpMotionGoal) OptionsType: ClassVar[type] = PickUpOptions binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( @@ -338,6 +356,8 @@ def _scene_dependencies( ) -> tuple[str, ...]: """Monitor the acquired object and late-bound downstream targets.""" dependencies = set(super()._scene_dependencies(request)) + if isinstance(request.goal, PickUpMotionGoal): + return tuple(sorted(dependencies)) entity_id = request.goal.semantics.entity_id # An explicit object pose is a scene-independent planning input. Do # not retain the semantic entity as a dynamic dependency in that @@ -474,6 +494,8 @@ def _plan( ) -> SkillPlan: """Plan approach, close, and lift segments without committing attachment.""" target = self.require_goal(request) + if isinstance(target, PickUpMotionGoal): + return self._plan_motion(request, context) options = replace( request.skill_options, downstream_object_target_poses=tuple( @@ -639,6 +661,73 @@ def _plan( provenance=None, ) + def _plan_motion( + self, + request: ResolvedSkillRequest[PickUpMotionGoal, PickUpOptions], + context: PlanningContext, + ) -> SkillPlan: + """Use the native pickup phases with explicit TCP grounding only.""" + options = request.skill_options + if ( + options.fixed_object_to_eef is not None + or options.rotate_upright is not None + or options.downstream_object_target_poses + or options.approach_along_grasp_axis + ): + raise ValueError("Pickup motion goals cannot use object-relative options.") + arm = request.binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + hand = request.binding.endpoint("primary", "grasp") + hand_target = hand.require_target(JointPositionTarget) + pose = resolve_pose_target( + resolve_pose_goal(request.goal.grasp_xpos, context, name="grasp_xpos"), + num_envs=context.batch_size, + device=self.device, + ) + direction = options.approach_direction.to(pose) + if direction.ndim == 2 and direction.shape[0] != context.batch_size: + raise ValueError("approach_direction must match the planning batch.") + direction = direction / torch.linalg.vector_norm( + direction, dim=-1, keepdim=True + ) + commands = { + command: hand.joint_positions( + command, + num_envs=context.batch_size, + device=self.device, + dtype=context.robot.qpos.dtype, + ) + for command in (OPEN_COMMAND, GRASP_COMMAND) + } + success, full, lengths = self._get_full_pickup_trajectory( + pose, + context.robot.qpos[:, list(arm.joint_ids)], + context.last_qpos, + request.motion_policy, + options, + direction, + arm, + hand_target, + commands[OPEN_COMMAND], + commands[GRASP_COMMAND], + context.require_control_dt(), + ) + return self.build_plan( + request, + context, + success=success, + trajectory=TimedTrajectory.from_uniform_step( + full, env_ids=context.env_ids, step_dt=context.require_control_dt() + ), + expected_effects=StateDelta(), + segment_lengths=lengths, + diagnostics=PlannerDiagnostics( + backend=self.planning_services.planner_name, + metadata={"grounding": "explicit_tcp", "physical_success": "unknown"}, + ), + ) + def _plan_from_nominal( self, request: ResolvedSkillRequest[GraspGoal, PickUpOptions], @@ -647,6 +736,8 @@ def _plan_from_nominal( ) -> SkillPlan: """Assemble a Pick from an explicitly supplied external motion.""" target = self.require_goal(request) + if isinstance(target, PickUpMotionGoal): + raise ValueError("Pickup motion goals require native generation.") motion = self._validate_nominal_motion( request, context, nominal_source, segments=("approach", "close", "lift") ) @@ -1118,4 +1209,4 @@ def _upright_adjusted_grasp_poses( return adjusted_grasp_xpos -__all__ = ["GraspGoal", "PickUp", "PickUpOptions"] +__all__ = ["PickUpMotionGoal", "GraspGoal", "PickUp", "PickUpOptions"] diff --git a/embodichain/lab/sim/atomic_skills/primitives/slide.py b/embodichain/lab/sim/atomic_skills/primitives/slide.py index 0955e338b8..93ce1f1e90 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/slide.py +++ b/embodichain/lab/sim/atomic_skills/primitives/slide.py @@ -114,6 +114,27 @@ def __post_init__(self) -> None: ) +@dataclass(frozen=True, slots=True, eq=False) +class SlideMotionGoal: + """World TCP and rail direction for effect-free native Slide motion. + + Args: + grasp_xpos: Explicit world grasp TCP transform. + translation_axis: World rail direction; Options owns push/pull sign. + """ + + goal_kind: ClassVar[str] = "slide_motion" + grasp_xpos: PoseGoalValue + translation_axis: torch.Tensor + + def __post_init__(self) -> None: + validate_pose_goal(self.grasp_xpos, "grasp_xpos", allow_waypoints=False) + axis = self.translation_axis + if axis.shape != (3,) or not torch.isfinite(axis).all() or axis.norm() < 1e-8: + raise ValueError("translation_axis must be a finite nonzero world vector.") + object.__setattr__(self, "translation_axis", axis.clone()) + + @dataclass(frozen=True, slots=True, eq=False) class SlideGoal(ObjectSkillGoal): """Translating articulation link described by a slide affordance.""" @@ -191,11 +212,11 @@ def __post_init__(self) -> None: raise ValueError("Release retreat requires hand_interp_steps >= 2.") -class Slide(AtomicSkill[SlideGoal, SlideOptions]): +class Slide(AtomicSkill[SlideGoal | SlideMotionGoal, SlideOptions]): """Open-loop approach, grasp, and axis-constrained sliding motion.""" skill_id: ClassVar[str] = "slide" - GoalType: ClassVar[type] = SlideGoal + GoalType: ClassVar[tuple[type, ...]] = (SlideGoal, SlideMotionGoal) OptionsType: ClassVar[type] = SlideOptions open_loop: ClassVar[bool] = True binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( @@ -231,16 +252,21 @@ def _plan( "must preserve every Cartesian sample." ) target = self.require_goal(request) - affordance = self._require_slide_affordance(target.semantics) + affordance = ( + None + if isinstance(target, SlideMotionGoal) + else self._require_slide_affordance(target.semantics) + ) + joint_target = target.joint_target if isinstance(target, SlideGoal) else None options = request.skill_options interpolation_dt = context.require_control_dt() # Resolve late-bound scene targets before any already-satisfied fast # path so invalid or low-confidence references fail at the goal # boundary instead of being reported as a successful hold. - resolved_link_pose = resolve_pose_goal( - target.target_pose, - context, - name="target_pose", + resolved_link_pose = ( + resolve_pose_goal(target.target_pose, context, name="target_pose") + if isinstance(target, SlideGoal) + else None ) direction = options.direction displacement = None @@ -249,18 +275,18 @@ def _plan( ) reached = torch.zeros_like(joint_valid) diagnostics: PlannerDiagnostics | None = None - if target.joint_target is not None: + if joint_target is not None: displacement, joint_valid, reached = self._joint_displacement( - target.joint_target, affordance, context + joint_target, affordance, context ) active = joint_valid & ~reached diagnostics = PlannerDiagnostics( backend=self.planning_services.planner_name, metadata={ "joint_target": { - "articulation_id": target.joint_target.articulation_id, - "joint_name": target.joint_target.joint_name, - "position": target.joint_target.position, + "articulation_id": joint_target.articulation_id, + "joint_name": joint_target.joint_name, + "position": joint_target.position, "already_satisfied": reached.detach().cpu().tolist(), } }, @@ -307,33 +333,54 @@ def _plan( dtype=context.robot.qpos.dtype, ) - link_pose = resolve_pose_target( - resolved_link_pose, - num_envs=self.num_envs, - device=self.device, - ) - translation_axis = affordance.translation_axis.to( - device=self.device, dtype=torch.float32 - ) - translation_axis = translation_axis / torch.linalg.vector_norm(translation_axis) - translation_axis_world = torch.matmul(link_pose[:, :3, :3], translation_axis) - grasp_generator = self.planning_services.grasp_pose_generator( - grasp_target.target_id - ) - candidates = affordance.get_grasp_candidates( - grasp_generator, - link_pose, - translation_axis_world, - ) - grasp_sample = affordance.sample_candidates( - candidates, - sampling=context.affordance_sampling, - env_ids=context.env_ids, - key=(request.invocation_id or self.skill_id) + ":grasp", - reference_poses=link_pose, - ) - grasp_success = grasp_sample.success - grasp_xpos = grasp_sample.poses.to(device=self.device, dtype=torch.float32) + if isinstance(target, SlideMotionGoal): + grasp_xpos = resolve_pose_target( + resolve_pose_goal(target.grasp_xpos, context, name="grasp_xpos"), + num_envs=self.num_envs, + device=self.device, + ) + axis = target.translation_axis.to(grasp_xpos) + translation_axis_world = (axis / axis.norm()).expand(self.num_envs, 3) + grasp_success = torch.ones( + self.num_envs, dtype=torch.bool, device=self.device + ) + grasp_metadata = { + "grounding": "explicit_tcp", + "physical_success": "unknown", + } + else: + link_pose = resolve_pose_target( + resolved_link_pose, + num_envs=self.num_envs, + device=self.device, + ) + translation_axis = affordance.translation_axis.to( + device=self.device, dtype=torch.float32 + ) + translation_axis = translation_axis / torch.linalg.vector_norm( + translation_axis + ) + translation_axis_world = torch.matmul( + link_pose[:, :3, :3], translation_axis + ) + grasp_generator = self.planning_services.grasp_pose_generator( + grasp_target.target_id + ) + candidates = affordance.get_grasp_candidates( + grasp_generator, + link_pose, + translation_axis_world, + ) + grasp_sample = affordance.sample_candidates( + candidates, + sampling=context.affordance_sampling, + env_ids=context.env_ids, + key=(request.invocation_id or self.skill_id) + ":grasp", + reference_poses=link_pose, + ) + grasp_success = grasp_sample.success + grasp_xpos = grasp_sample.poses.to(device=self.device, dtype=torch.float32) + grasp_metadata = grasp_sample.metadata grasp_success = normalize_success_mask( grasp_success, num_envs=self.num_envs, @@ -341,7 +388,7 @@ def _plan( name="Slide grasp-pose success", ) if not grasp_success.any(): - if target.joint_target is not None and reached.any(): + if joint_target is not None and reached.any(): return self.build_plan( request, context, @@ -549,7 +596,7 @@ def _plan( full[:, offset:, arm_joint_ids] = return_arm full[:, offset:, hand_joint_ids] = hand_open_qpos.unsqueeze(1) - if target.joint_target is not None: + if joint_target is not None: full[reached] = context.robot.qpos[reached, None] success = (success & joint_valid) | reached @@ -567,7 +614,8 @@ def _plan( backend=self.planning_services.planner_name, metadata={ **(diagnostics.metadata if diagnostics is not None else {}), - "affordance_sample": {"grasp": grasp_sample.metadata}, + "affordance_sample": {"grasp": grasp_metadata}, + **(grasp_metadata if isinstance(target, SlideMotionGoal) else {}), }, ), segment_lengths=segment_lengths, @@ -693,6 +741,7 @@ def _plan_pose_segment( __all__ = [ + "SlideMotionGoal", "Slide", "SlideGoal", "SlideJointTarget", diff --git a/embodichain/lab/sim/atomic_skills/primitives/twist.py b/embodichain/lab/sim/atomic_skills/primitives/twist.py index a4e605e1ab..9009e2ae6d 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/twist.py +++ b/embodichain/lab/sim/atomic_skills/primitives/twist.py @@ -75,6 +75,33 @@ ) +@dataclass(frozen=True, slots=True, eq=False) +class TwistMotionGoal: + """World TCP and rotation line for effect-free native Twist motion. + + Args: + grasp_xpos: Explicit world grasp TCP transform. + twist_axis: World rotation direction. + axis_origin: One world point on the rotation line. + """ + + goal_kind: ClassVar[str] = "twist_motion" + grasp_xpos: PoseGoalValue + twist_axis: torch.Tensor + axis_origin: tuple[float, float, float] + + def __post_init__(self) -> None: + validate_pose_goal(self.grasp_xpos, "grasp_xpos", allow_waypoints=False) + axis = self.twist_axis + if axis.shape != (3,) or not torch.isfinite(axis).all() or axis.norm() < 1e-8: + raise ValueError("twist_axis must be a finite nonzero world vector.") + if len(self.axis_origin) != 3 or not all( + math.isfinite(v) for v in self.axis_origin + ): + raise ValueError("axis_origin must be one finite world point.") + object.__setattr__(self, "twist_axis", axis.clone()) + + @dataclass(frozen=True, slots=True, eq=False) class TwistGoal(ObjectSkillGoal): """Target object described by a twist affordance.""" @@ -116,11 +143,11 @@ def __post_init__(self) -> None: raise ValueError("twist_angle must be finite.") -class Twist(AtomicSkill[TwistGoal, TwistOptions]): +class Twist(AtomicSkill[TwistGoal | TwistMotionGoal, TwistOptions]): """Open-loop approach, grasp, twist, release, and retract motion.""" skill_id: ClassVar[str] = "twist" - GoalType: ClassVar[type] = TwistGoal + GoalType: ClassVar[tuple[type, ...]] = (TwistGoal, TwistMotionGoal) OptionsType: ClassVar[type] = TwistOptions open_loop: ClassVar[bool] = True binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( @@ -158,7 +185,11 @@ def _plan( "twist path must preserve every Cartesian sample." ) target = self.require_goal(request) - affordance = self._require_twist_affordance(target.semantics) + affordance = ( + None + if isinstance(target, TwistMotionGoal) + else self._require_twist_affordance(target.semantics) + ) options = request.skill_options interpolation_dt = context.require_control_dt() binding = request.binding @@ -184,24 +215,46 @@ def _plan( dtype=context.robot.qpos.dtype, ) - link_pose = resolve_pose_target( - resolve_pose_goal(target.target_pose, context, name="target_pose"), - num_envs=self.num_envs, - device=self.device, - ) - grasp_sample = affordance.sample_grasp_pose( - link_pose, - sampling=context.affordance_sampling, - env_ids=context.env_ids, - key=(request.invocation_id or self.skill_id) + ":grasp", - ) - grasp_xpos = grasp_sample.poses.to(device=self.device, dtype=torch.float32) - grasp_xpos = find_symmetric_nearest_xpos( - grasp_xpos, - reference_xpos=self.robot.compute_fk( - qpos=start_arm_qpos, name=control_part, to_matrix=True - ), - ) + if isinstance(target, TwistMotionGoal): + grasp_xpos = resolve_pose_target( + resolve_pose_goal(target.grasp_xpos, context, name="grasp_xpos"), + num_envs=self.num_envs, + device=self.device, + ) + # Identity is the world coordinate frame, not an invented object pose. + link_pose = torch.eye(4, device=self.device).expand(self.num_envs, 4, 4) + twist_axis = target.twist_axis + axis_origin = target.axis_origin + grasp_success = torch.ones( + self.num_envs, dtype=torch.bool, device=self.device + ) + grasp_metadata = { + "grounding": "explicit_tcp", + "physical_success": "unknown", + } + else: + link_pose = resolve_pose_target( + resolve_pose_goal(target.target_pose, context, name="target_pose"), + num_envs=self.num_envs, + device=self.device, + ) + grasp_sample = affordance.sample_grasp_pose( + link_pose, + sampling=context.affordance_sampling, + env_ids=context.env_ids, + key=(request.invocation_id or self.skill_id) + ":grasp", + ) + grasp_xpos = grasp_sample.poses.to(device=self.device, dtype=torch.float32) + grasp_xpos = find_symmetric_nearest_xpos( + grasp_xpos, + reference_xpos=self.robot.compute_fk( + qpos=start_arm_qpos, name=control_part, to_matrix=True + ), + ) + twist_axis = affordance.twist_axis + axis_origin = affordance.require_axis_origin() + grasp_success = grasp_sample.success + grasp_metadata = grasp_sample.metadata pre_grasp_xpos = translate_pose_world( grasp_xpos, -grasp_xpos[:, :3, 2] * options.pre_grasp_distance, @@ -214,8 +267,8 @@ def _plan( twist_xpos = self._twisted_grasp_poses( link_pose, grasp_xpos, - affordance.twist_axis, - affordance.require_axis_origin(), + twist_axis, + axis_origin, options.twist_angle, n_twist - 1, ) @@ -234,7 +287,7 @@ def _plan( grasp_xpos[:, :3, 2], n_waypoints=n_reach - 1, ) - reach_active = grasp_sample.success & approach_success + reach_active = grasp_success & approach_success reach_hold_pose = self.robot.compute_fk( qpos=approach_arm[:, -1], name=control_part, @@ -270,8 +323,8 @@ def sample_twist_path(progress: torch.Tensor) -> torch.Tensor: sampled = self._twisted_grasp_poses_at_fractions( link_pose, grasp_xpos, - affordance.twist_axis, - affordance.require_axis_origin(), + twist_axis, + axis_origin, options.twist_angle, progress, ) @@ -300,7 +353,7 @@ def sample_twist_path(progress: torch.Tensor) -> torch.Tensor: interpolation_dt=interpolation_dt, ) success = ( - grasp_sample.success + grasp_success & approach_success & reach_success & twist_success @@ -380,7 +433,10 @@ def sample_twist_path(progress: torch.Tensor) -> torch.Tensor: expected_effects=StateDelta(), diagnostics=PlannerDiagnostics( backend=self.planning_services.planner_name, - metadata={"affordance_sample": grasp_sample.metadata}, + metadata={ + "affordance_sample": grasp_metadata, + **(grasp_metadata if isinstance(target, TwistMotionGoal) else {}), + }, ), segment_lengths={ "approach": lengths[0], @@ -503,4 +559,4 @@ def _twisted_grasp_poses_at_fractions( return eef_poses -__all__ = ["Twist", "TwistGoal", "TwistOptions"] +__all__ = ["TwistMotionGoal", "Twist", "TwistGoal", "TwistOptions"] diff --git a/embodichain/lab/sim/motion/motion_generator.py b/embodichain/lab/sim/motion/motion_generator.py index 473e4c564d..47c66e5875 100644 --- a/embodichain/lab/sim/motion/motion_generator.py +++ b/embodichain/lab/sim/motion/motion_generator.py @@ -325,11 +325,29 @@ class MotionGenerator: } def __init__( - self, cfg: MotionGenCfg, *, env_ids: tuple[int, ...] | None = None + self, + cfg: MotionGenCfg, + *, + env_ids: tuple[int, ...] | None = None, + planner: BasePlanner | None = None, ) -> None: + """Create a generator with an optional already-bound planner. + Args: + cfg: Backend configuration used when no planner is supplied. + env_ids: Optional physical row selection. + planner: Borrowed planner, including a planner bound to an offline + kinematics port. It must use the configured backend type. + """ # Create planner based on planner_type - self.planner: BasePlanner = self._create_planner(cfg.planner_cfg) + if ( + planner is not None + and planner.cfg.planner_type != cfg.planner_cfg.planner_type + ): + raise ValueError("Explicit planner must use the configured backend type.") + self.planner: BasePlanner = ( + self._create_planner(cfg.planner_cfg) if planner is None else planner + ) self.robot = self.planner.robot self.device = self.robot.device diff --git a/embodichain/lab/sim/motion/planners/base_planner.py b/embodichain/lab/sim/motion/planners/base_planner.py index 99ff6a1d79..0ee24f961a 100644 --- a/embodichain/lab/sim/motion/planners/base_planner.py +++ b/embodichain/lab/sim/motion/planners/base_planner.py @@ -22,7 +22,7 @@ from abc import ABC, abstractmethod from collections.abc import Mapping from dataclasses import MISSING, dataclass -from typing import Literal +from typing import Literal, TYPE_CHECKING from embodichain.utils import logger from embodichain.utils import configclass @@ -30,6 +30,9 @@ from .utils import MoveType, PlanState, PlanResult from ._joint_limits import _resolve_joint_constraints +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + __all__ = [ "BasePlannerCfg", "CollisionWorldInfo", @@ -208,14 +211,25 @@ class BasePlanner(ABC): cfg: Configuration object for the planner. """ - def __init__(self, cfg: BasePlannerCfg): + def __init__(self, cfg: BasePlannerCfg, *, robot: Robot | None = None): + """Bind a simulator robot or an explicit offline kinematics adapter. + + Args: + cfg: Planner configuration. + robot: Borrowed robot-compatible kinematics port. When supplied, + construction does not look up a SimulationManager. + """ self.cfg: BasePlannerCfg = cfg if cfg.robot_uid is MISSING: logger.log_error("robot_uid is required in planner config", ValueError) - self.robot = SimulationManager.get_instance(cfg.sim_instance_id).get_robot( - cfg.robot_uid + self.robot = ( + robot + if robot is not None + else SimulationManager.get_instance(cfg.sim_instance_id).get_robot( + cfg.robot_uid + ) ) if self.robot is None: logger.log_error(f"Robot {cfg.robot_uid} not found", ValueError) diff --git a/embodichain/toolkits/skill_fitting/__init__.py b/embodichain/toolkits/skill_fitting/__init__.py new file mode 100644 index 0000000000..6de86881d7 --- /dev/null +++ b/embodichain/toolkits/skill_fitting/__init__.py @@ -0,0 +1,21 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Video-selected, object-free atomic motion reconstruction command.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/embodichain/toolkits/skill_fitting/__main__.py b/embodichain/toolkits/skill_fitting/__main__.py new file mode 100644 index 0000000000..ccb993c975 --- /dev/null +++ b/embodichain/toolkits/skill_fitting/__main__.py @@ -0,0 +1,22 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from .cli import main + +if __name__ == "__main__": + main() diff --git a/embodichain/toolkits/skill_fitting/_pipeline.py b/embodichain/toolkits/skill_fitting/_pipeline.py new file mode 100644 index 0000000000..d40b922ef7 --- /dev/null +++ b/embodichain/toolkits/skill_fitting/_pipeline.py @@ -0,0 +1,222 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Shared offline fitting orchestration for CLI and annotation hosts.""" + +from __future__ import annotations + +from collections.abc import Callable +import math +from pathlib import Path +from typing import Any + +import numpy as np + +from embodichain.utils import configclass + +__all__ = [] + + +@configclass +class FitInputs: + """Explicit recording/profile inputs; importing this type needs no simulator.""" + + joints: str = "" + robot_profile: str = "" + qpos_semantics: str = "" + time_offset: float | None = None + qpos_key: str = "qpos" + timestamps_key: str = "timestamps" + joint_dt: float | None = None + env_index: int = 0 + device: str = "cpu" + phase_hints: list[dict[str, Any]] | None = None + fit_config: dict[str, Any] = {} + + def check(self) -> None: + """Validate the closed request before reading files or creating solvers.""" + for key in ("joints", "robot_profile", "qpos_key", "timestamps_key", "device"): + value = getattr(self, key) + if not isinstance(value, str) or not value.strip() or len(value) > 4096: + raise ValueError(f"{key} 需要非空字符串。") + if self.qpos_semantics not in { + "measured_state", + "actual_submission", + "design_reference", + }: + raise ValueError("请声明关节数据的语义。") + for key in ("time_offset", "joint_dt"): + value = getattr(self, key) + if key == "joint_dt" and value is None: + continue + if type(value) not in (int, float) or not math.isfinite(value): + raise ValueError(f"{key} 需要有限数值。") + if key == "joint_dt" and value <= 0: + raise ValueError("joint_dt 必须为正数。") + if type(self.env_index) is not int or self.env_index < 0: + raise ValueError("env_index 需要非负整数。") + if not isinstance(self.fit_config, dict): + raise ValueError("fit_config 需要 JSON 对象。") + if self.phase_hints is not None: + if not isinstance(self.phase_hints, list) or not self.phase_hints: + raise ValueError("phase_hints 需要非空 JSON 数组,或 null。") + for phase in self.phase_hints: + if ( + not isinstance(phase, dict) + or phase.keys() != {"name", "start", "stop"} + or not isinstance(phase["name"], str) + or type(phase["start"]) is not int + or type(phase["stop"]) is not int + ): + raise ValueError("阶段提示需要 name、start、stop 字段。") + + +def fit_recording( + inputs: FitInputs, + *, + skill_id: str, + frame_times: np.ndarray, + frame_durations: np.ndarray, + start_frame: int, + end_frame: int, + provenance: dict[str, Any], + progress: Callable[[str], None] = lambda _: None, +) -> tuple[dict[str, np.ndarray], dict[str, Any]]: + """Reuse the native fitter with a host-owned physical video clock. + + The provided clock may contain only the two selected endpoints. The actual + inclusive frame indices are preserved separately in the result provenance. + Native solvers, FK geometry and all numeric allocations belong to the caller's + single fitting thread. Frame decoding remains with the annotation worker. + """ + inputs.check() + from embodichain.data_pipeline.skill_fitting import ( + file_digest, + load_joint_recording, + map_video_interval, + ) + from embodichain.lab.sim.atomic_skills import ( + AtomicSkillEngine, + ControlPartCommandProfile, + ) + from embodichain.lab.sim.atomic_skills.fitting import SkillFitCfg, fit_skill_segment + from embodichain.lab.sim.motion.motion_generator import ( + MotionGenerator, + MotionGenCfg, + ) + from embodichain.lab.sim.motion.planners import ( + TrapezoidalPlanner, + TrapezoidalPlannerCfg, + ) + from ._robot import RobotPort, RobotProfile + import torch + + if set(inputs.fit_config) - set(SkillFitCfg.__dataclass_fields__): + raise ValueError("Unknown fit configuration fields.") + cfg = SkillFitCfg(**inputs.fit_config) + cfg.validate() + paths = [ + Path(inputs.joints).expanduser().resolve(), + Path(inputs.robot_profile).expanduser().resolve(), + ] + progress("正在读取关节记录与机器人标定…") + digests = {str(path): file_digest(path) for path in paths} + profile = RobotProfile.load(paths[1]) + models = { + part: file_digest(solver["urdf_path"]) + for part, solver in profile.solvers.items() + } + recording = load_joint_recording( + paths[0], + joint_names=profile.joint_names, + semantics=inputs.qpos_semantics, + qpos_key=inputs.qpos_key, + timestamps_key=inputs.timestamps_key, + sample_dt=inputs.joint_dt, + env_index=inputs.env_index, + ) + start, stop = map_video_interval( + frame_times, + frame_durations, + recording.timestamps, + start_frame=0, + end_frame=len(frame_times) - 1, + time_offset=inputs.time_offset, + ) + if not 12 <= stop - start <= cfg.max_samples: + raise ValueError( + f"选区映射为 {stop - start} 个关节采样,需要 12–{cfg.max_samples} 个。" + ) + progress("正在辨识参数、调用原生技能并校验连续回填…") + robot = RobotPort(profile, recording.qpos[start], inputs.device) + planner_cfg = TrapezoidalPlannerCfg(robot_uid="offline_skill_fit") + planner = TrapezoidalPlanner(planner_cfg, robot=robot) + motion = MotionGenerator(MotionGenCfg(planner_cfg=planner_cfg), planner=planner) + commands = { + part: ControlPartCommandProfile.joint_positions( + **{ + key: torch.tensor(value, device=robot.device, dtype=torch.float32) + for key, value in command.items() + } + ) + for part, command in profile.hand_commands.items() + } + engine = AtomicSkillEngine(motion, control_profiles=commands) + arrays, report = fit_skill_segment( + recording, + engine, + skill_id=skill_id, + endpoints=profile.participants, + start=start, + stop=stop, + acceleration_limits=np.asarray(profile.acceleration_limits), + cfg=cfg, + phase_hints=inputs.phase_hints, + ) + progress("正在生成关节曲线和 FK 对比…") + grid = np.unique( + np.r_[ + np.linspace( + 0, len(recording.qpos) - 1, min(len(recording.qpos), 2000) + ).astype(int), + start, + stop - 1, + ] + ) + arrays["geometry_indices"] = grid + for name in ("original", "native", "patched", "proposed"): + arrays[f"{name}_links"], arrays[f"{name}_tcp"] = robot.geometry( + arrays[f"{name}_qpos"][grid] + ) + report.update( + **provenance, + robot_profile=profile.metadata(), + robot_profile_sha256=digests[str(paths[1])], + robot_model_sha256=models, + source_joint_sha256=digests[str(paths[0])], + video_interval={ + "start_frame": start_frame, + "end_frame": end_frame, + "interval": "closed", + }, + time_offset=inputs.time_offset, + ) + if any(file_digest(path) != digest for path, digest in digests.items()) or any( + file_digest(profile.solvers[part]["urdf_path"]) != digest + for part, digest in models.items() + ): + raise ValueError("关节记录、机器人配置或 URDF 在拟合期间发生变化。") + return arrays, report diff --git a/embodichain/toolkits/skill_fitting/_robot.py b/embodichain/toolkits/skill_fitting/_robot.py new file mode 100644 index 0000000000..ea05308ecd --- /dev/null +++ b/embodichain/toolkits/skill_fitting/_robot.py @@ -0,0 +1,264 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Simulator-free robot port using the existing robot-owned FK/IK solvers.""" + +from __future__ import annotations + +import json +from pathlib import Path +from typing import Any + +import numpy as np +import torch + +from embodichain.lab.sim.motion.solvers import SolverCfg +from embodichain.lab.sim.utility.solver_utils import create_pk_serial_chain +from embodichain.utils import configclass + +__all__ = [] + + +@configclass +class RobotProfile: + """Explicit robot calibration and command layout, independent of objects.""" + + joint_names: tuple[str, ...] = () + control_parts: dict[str, list[str]] = {} + participants: dict[str, dict[str, str]] = {} + solvers: dict[str, dict[str, Any]] = {} + base_poses: dict[str, list[list[float]]] = {} + hand_commands: dict[str, dict[str, list[float]]] = {} + lower: list[float] = [] + upper: list[float] = [] + velocity_limits: list[float] = [] + acceleration_limits: list[float] = [] + calibration_revision: str = "" + + @classmethod + def load(cls, path: str | Path) -> RobotProfile: + """Load a closed profile; resolve solver URDF paths against the file.""" + source = Path(path).resolve() + raw = json.loads(source.read_text()) + allowed = set(cls.__dataclass_fields__) + if set(raw) - allowed: + raise ValueError( + f"Unknown robot profile fields: {sorted(set(raw) - allowed)}" + ) + raw["joint_names"] = tuple(raw.get("joint_names", ())) + result = cls(**raw) + result.validate() + result.check() + for solver in result.solvers.values(): + model = Path(solver["urdf_path"]).expanduser() + solver["urdf_path"] = str( + (source.parent / model).resolve() if not model.is_absolute() else model + ) + return result + + def check(self) -> None: + """Validate named layout, finite limits and explicit endpoint commands.""" + names = self.joint_names + if not names or len(set(names)) != len(names) or not self.calibration_revision: + raise ValueError( + "Profile needs unique joint names and a calibration revision." + ) + for key in ("lower", "upper", "velocity_limits", "acceleration_limits"): + value = np.asarray(getattr(self, key), dtype=float) + if value.shape != (len(names),) or not np.isfinite(value).all(): + raise ValueError(f"{key} must be one finite value per joint.") + if ( + np.any(np.asarray(self.lower) >= self.upper) + or np.any(np.asarray(self.velocity_limits) <= 0) + or np.any(np.asarray(self.acceleration_limits) <= 0) + ): + raise ValueError("Profile has invalid position or derivative limits.") + for part, joints in self.control_parts.items(): + if ( + not joints + or len(set(joints)) != len(joints) + or not set(joints) <= set(names) + ): + raise ValueError(f"Invalid explicit joint list for {part}.") + for role, endpoints in self.participants.items(): + if set(endpoints) != {"motion", "grasp"}: + raise ValueError(f"{role} must bind motion and grasp.") + arm, hand = endpoints["motion"], endpoints["grasp"] + if arm not in self.solvers or hand not in self.hand_commands: + raise ValueError(f"{role} lacks a solver or command profile.") + if arm not in self.control_parts or hand not in self.control_parts: + raise ValueError("Endpoint refers to an undeclared control part.") + for part, commands in self.hand_commands.items(): + if set(commands) != {"open", "grasp"} or part not in self.control_parts: + raise ValueError("Hand profiles require exactly open/grasp commands.") + for values in commands.values(): + ids = [names.index(n) for n in self.control_parts[part]] + if ( + len(values) != len(ids) + or not np.isfinite(values).all() + or np.any(np.asarray(values) < np.asarray(self.lower)[ids]) + or np.any(np.asarray(values) > np.asarray(self.upper)[ids]) + ): + raise ValueError("Hand command is invalid or outside joint limits.") + if np.linalg.norm(np.subtract(commands["open"], commands["grasp"])) < 1e-8: + raise ValueError("Open and grasp commands must be distinguishable.") + + def metadata(self) -> dict[str, Any]: + """Return a JSON-compatible copy for fit provenance.""" + return {key: getattr(self, key) for key in self.__dataclass_fields__} + + +class RobotPort: + """Borrowed kinematics-only Robot interface; never acquires a simulator.""" + + def __init__( + self, profile: RobotProfile, initial_qpos: np.ndarray, device: str + ) -> None: + import warp as wp + + wp.init() + profile.check() + self.profile = profile + self.device = torch.device(device) + self.joint_names = list(profile.joint_names) + self.dof = len(self.joint_names) + self.num_instances = 1 + self.control_parts = profile.control_parts + self._qpos = torch.tensor( + initial_qpos, device=self.device, dtype=torch.float32 + )[None] + self.mimic_ids: list[int] = [] + self.mimic_parents: list[int] = [] + self.mimic_multipliers: list[float] = [] + self.solvers = {} + self.bases = {} + for part, declaration in profile.solvers.items(): + cfg = SolverCfg.from_dict(declaration) + joints = profile.control_parts[part] + cfg.joint_names = list(joints) + chain = create_pk_serial_chain( + urdf_path=cfg.urdf_path, + end_link_name=cfg.end_link_name, + root_link_name=cfg.root_link_name, + device=self.device, + ) + if chain.get_joint_parameter_names() != list(joints): + raise ValueError(f"URDF chain joint order differs for {part}.") + ids = self.get_joint_ids(part) + cfg.user_qpos_limits = [ + [profile.lower[i] for i in ids], + [profile.upper[i] for i in ids], + ] + self.solvers[part] = cfg.init_solver( + device=self.device, pk_serial_chain=chain + ) + base = torch.tensor( + profile.base_poses.get(part, np.eye(4).tolist()), + device=self.device, + dtype=torch.float32, + ) + from embodichain.lab.sim.atomic_skills.goals import validate_pose_goal + + validate_pose_goal(base, f"{part} base", allow_waypoints=False) + if base.shape != (4, 4): + raise ValueError("Each arm base must be one calibrated transform.") + self.bases[part] = base + + def get_joint_ids(self, name: str | None = None) -> list[int]: + return ( + list(range(self.dof)) + if name is None + else [self.joint_names.index(n) for n in self.control_parts[name]] + ) + + def get_qpos(self, name: str | None = None) -> torch.Tensor: + return self._qpos[:, self.get_joint_ids(name)].clone() + + def get_qvel(self) -> torch.Tensor: + return torch.zeros_like(self._qpos) + + def get_qvel_limits( + self, name: str | None = None, joint_ids: list[int] | None = None + ) -> torch.Tensor: + ids = self.get_joint_ids(name) if joint_ids is None else joint_ids + return torch.tensor(self.profile.velocity_limits, device=self.device)[ids][None] + + def get_qpos_limits( + self, name: str | None = None, joint_ids: list[int] | None = None + ) -> torch.Tensor: + ids = self.get_joint_ids(name) if joint_ids is None else joint_ids + return torch.tensor( + np.column_stack((self.profile.lower, self.profile.upper)), + device=self.device, + dtype=torch.float32, + )[None, ids] + + def get_solver(self, name: str) -> Any: + return self.solvers[name] + + def compute_fk( + self, qpos: torch.Tensor, name: str, to_matrix: bool = True, **kwargs: Any + ) -> torch.Tensor: + return self.bases[name] @ self.solvers[name].get_fk(qpos) + + def compute_batch_fk( + self, qpos: torch.Tensor, name: str, to_matrix: bool = True, **kwargs: Any + ) -> torch.Tensor: + return self.compute_fk(qpos.reshape(-1, qpos.shape[-1]), name).reshape( + *qpos.shape[:-1], 4, 4 + ) + + def compute_ik( + self, pose: torch.Tensor, name: str, joint_seed: torch.Tensor, **kwargs: Any + ) -> tuple[torch.Tensor, torch.Tensor]: + target = torch.linalg.inv(self.bases[name]) @ pose + return self.solvers[name].get_ik(target_xpos=target, qpos_seed=joint_seed) + + def compute_batch_ik( + self, pose: torch.Tensor, name: str, joint_seed: torch.Tensor, **kwargs: Any + ) -> tuple[torch.Tensor, torch.Tensor]: + shape = pose.shape[:-2] + seed = joint_seed + if seed.ndim == 2: + seed = seed[:, None].expand(*shape, seed.shape[-1]) + valid, q = self.compute_ik( + pose.reshape(-1, 4, 4), name, seed.reshape(-1, seed.shape[-1]) + ) + return valid.reshape(shape), q.reshape(*shape, q.shape[-1]) + + def geometry(self, qpos: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + """Batch FK into compact arm-link skeletons and TCPs for standalone viewing.""" + skeletons, tcp = [], [] + for part, solver in self.solvers.items(): + joints = torch.as_tensor( + qpos[:, self.get_joint_ids(part)], + device=self.device, + dtype=torch.float32, + ) + base = self.bases[part] + with torch.no_grad(): + transforms = solver.pk_serial_chain.forward_kinematics_tensor(joints) + nodes = (base @ transforms)[:, :, :3, 3].permute(1, 0, 2) + tool = self.compute_fk(joints, part)[:, :3, 3] + skeletons.append(torch.cat((nodes, tool[:, None]), dim=1).cpu().numpy()) + tcp.append(tool.cpu().numpy()) + # Separate arms are separate polylines; no invented link between bases. + max_nodes = max(v.shape[1] for v in skeletons) + padded = [ + np.pad(v, ((0, 0), (0, max_nodes - v.shape[1]), (0, 0)), mode="edge") + for v in skeletons + ] + return np.stack(padded, axis=1), np.stack(tcp, axis=1) diff --git a/embodichain/toolkits/skill_fitting/cli.py b/embodichain/toolkits/skill_fitting/cli.py new file mode 100644 index 0000000000..8095c34c63 --- /dev/null +++ b/embodichain/toolkits/skill_fitting/cli.py @@ -0,0 +1,217 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Fit a video-selected joint segment with the native atomic skill engine.""" + +from __future__ import annotations + +import argparse +import json +from pathlib import Path +import threading +from typing import Sequence + +import numpy as np + +__all__ = ["main"] + + +def main(argv: Sequence[str] | None = None) -> None: + """Run offline reconstruction and persist accepted or rejected candidates. + + Args: + argv: Explicit command arguments, or the process command line. + """ + parser = argparse.ArgumentParser(description=__doc__) + inputs = parser.add_mutually_exclusive_group(required=True) + inputs.add_argument("--video", type=Path) + inputs.add_argument( + "--hdf5", type=Path, help="HDF5 image sequence used by the annotator." + ) + parser.add_argument("--image-key") + parser.add_argument("--fps", type=float) + parser.add_argument("--video-stream", type=int) + parser.add_argument("--joints", type=Path, required=True) + parser.add_argument("--robot-profile", type=Path, required=True) + parser.add_argument("--annotations", type=Path) + parser.add_argument("--segment-id") + parser.add_argument("--start-frame", type=int) + parser.add_argument("--end-frame", type=int) + parser.add_argument( + "--skill", + required=True, + choices=("pickup", "pick_up", "slide", "twist", "handover", "hand_over"), + ) + parser.add_argument( + "--time-offset", + type=float, + required=True, + help="Joint timestamp corresponding to video time zero, in seconds.", + ) + parser.add_argument("--qpos-key", default="qpos") + parser.add_argument("--timestamps-key", default="timestamps") + parser.add_argument("--joint-dt", type=float) + parser.add_argument("--env-index", type=int, default=0) + parser.add_argument( + "--qpos-semantics", + required=True, + choices=("measured_state", "actual_submission", "design_reference"), + ) + parser.add_argument( + "--phase-hints", + type=Path, + help="JSON complete half-open phase partition, relative to segment.", + ) + parser.add_argument( + "--fit-config", type=Path, help="Closed JSON SkillFitCfg overrides." + ) + parser.add_argument("--device", default="cpu") + parser.add_argument("--output", type=Path, required=True) + args = parser.parse_args(argv) + from embodichain.data_pipeline.skill_fitting import file_digest, save_skill_fit + from embodichain.toolkits.trajectory_annotator._storage import AnnotationStore + from ._pipeline import FitInputs, fit_recording + + try: + if args.hdf5: + from embodichain.toolkits.trajectory_annotator.hdf5 import Hdf5FrameSource + + source = Hdf5FrameSource(args.hdf5, image_key=args.image_key, fps=args.fps) + else: + from embodichain.toolkits.trajectory_annotator.video import VideoFrameSource + + source = VideoFrameSource(args.video, stream_index=args.video_stream) + annotation_digest = ( + None if args.annotations is None else file_digest(args.annotations) + ) + # Never allow result export to replace any input. + input_paths = [ + args.video, + args.hdf5, + args.joints, + args.robot_profile, + args.annotations, + args.phase_hints, + args.fit_config, + ] + if args.output.resolve() in {p.resolve() for p in input_paths if p is not None}: + raise ValueError("Output must differ from every input file.") + try: + info = source.prepare(threading.Event(), lambda **_: None) + revision, selected_id = None, None + if args.annotations: + store = AnnotationStore(args.annotations.resolve(), 8 * 1024**2) + store.initialize(info) + document = store.snapshot()["document"] + matches = [ + s for s in document["segments"] if s["id"] == args.segment_id + ] + if len(matches) != 1: + raise ValueError( + "--segment-id must select exactly one existing annotation." + ) + selected = matches[0] + first, last = selected["start_frame"], selected["end_frame"] + revision, selected_id = document["revision"], selected["id"] + else: + first = 0 if args.start_frame is None else args.start_frame + last = ( + info.frame_count - 1 if args.end_frame is None else args.end_frame + ) + if info.kind == "video": + base = info.details["time_base"] + clock = np.asarray(source.pts, dtype=np.float64) + clock = (clock - clock[0]) * base["numerator"] / base["denominator"] + durations = ( + np.r_[np.diff(clock), np.diff(clock)[-1]] + if len(clock) > 1 + else np.array([1 / info.fps]) + ) + else: + clock = np.arange(info.frame_count, dtype=np.float64) / info.fps + durations = np.full(info.frame_count, 1 / info.fps) + source.check_unchanged() + finally: + source.close() + inputs = FitInputs( + joints=str(args.joints), + robot_profile=str(args.robot_profile), + qpos_semantics=args.qpos_semantics, + time_offset=args.time_offset, + qpos_key=args.qpos_key, + timestamps_key=args.timestamps_key, + joint_dt=args.joint_dt, + env_index=args.env_index, + device=args.device, + fit_config=( + {} + if args.fit_config is None + else json.loads(args.fit_config.read_text()) + ), + phase_hints=( + None + if args.phase_hints is None + else json.loads(args.phase_hints.read_text()) + ), + ) + if not 0 <= first <= last < info.frame_count: + raise ValueError("Invalid inclusive video interval.") + skill = {"pickup": "pick_up", "handover": "hand_over"}.get( + args.skill, args.skill + ) + arrays, report = fit_recording( + inputs, + skill_id=skill, + frame_times=clock[first : last + 1], + frame_durations=durations[first : last + 1], + start_frame=first, + end_frame=last, + provenance=dict( + source_identity=info.identity, + source_timing=info.timing, + annotation_revision=revision, + annotation_id=selected_id, + annotation_sha256=annotation_digest, + ), + ) + source.check_unchanged() + if ( + args.annotations is not None + and file_digest(args.annotations) != annotation_digest + ): + raise ValueError("Annotation revision changed during fitting.") + save_skill_fit(args.output, arrays, report) + print( + json.dumps( + { + "output": str(args.output.resolve()), + "accepted": report["accepted"], + "parameters": report["parameters"], + "native_joint_rmse": report["native_joint_rmse"], + "tcp_errors": report["tcp_errors"], + "failures": report["failures"], + }, + indent=2, + ) + ) + if not report["accepted"]: + raise SystemExit(2) + except (ValueError, OSError, KeyError) as exc: + parser.exit(2, f"Skill fitting failed: {exc}\n") + + +if __name__ == "__main__": + main() diff --git a/embodichain/toolkits/trajectory_annotator/README.md b/embodichain/toolkits/trajectory_annotator/README.md index ff3e26cf1a..c616c37425 100644 --- a/embodichain/toolkits/trajectory_annotator/README.md +++ b/embodichain/toolkits/trajectory_annotator/README.md @@ -1,6 +1,6 @@ # 遥操轨迹图像序列标注 -在浏览器逐帧查看遥操视频、HDF5 相机图像或内存图像序列,为原子技能标注起止帧。支持时间轴定位、动作列表、区间编辑和删除、撤销重做、JSON 自动保存与继续编辑。运行时使用 CPU,不启动仿真。 +在浏览器逐帧查看遥操视频、HDF5 相机图像或内存图像序列,为原子技能标注起止帧。支持时间轴定位、技能列表、区间编辑和删除、撤销重做、JSON 自动保存与继续编辑。标注解码使用 CPU;可选关节拟合复用离线 FK/IK,不启动仿真。 ![原子技能区间标注界面](../../../docs/architecture/images/trajectory-annotation.png) @@ -59,6 +59,54 @@ embodichain annotate-trajectory --hdf5 /data/episode.hdf5 \ embodichain run-example toolkits/trajectory_annotation ``` +## 原子技能参数辨识与连续回填 + +同一网页的时间轴下方提供拟合面板,复用 `fit-trajectory` 的原生技能重建与连续拼接核心,支持 +**Pickup / Slide / Twist / HandOver**。不需要物体 pose、mesh 或仿真;需要与输入机械臂对应的 +URDF、TCP、关节顺序、夹爪命令和关节限制配置。配置与关节格式详见 +[拟合示例](../../../examples/toolkits/skill_fitting/README.md)。 + +![网页中的参数辨识、关节误差与 FK 对比](../../../docs/architecture/images/trajectory-annotation-fitting.png) + +```bash +embodichain annotate-trajectory --video /data/episode.mp4 \ + --joints /data/joints.npz --robot-profile /data/robot_profile.json \ + --qpos-semantics measured_state --time-offset 0 \ + --annotations /data/labels/episode.segments.json +``` + +上述拟合参数均可省略,之后在网页填写或修改本机文件路径。`run-example toolkits/trajectory_annotation` +支持同样的拟合参数与 `--video` / `--hdf5`。HDF5 内的关节也可直接使用同一文件: + +```bash +embodichain annotate-trajectory --hdf5 /data/episode.hdf5 \ + --joints /data/episode.hdf5 --qpos-key observations/qpos \ + --timestamps-key observations/timestamps \ + --robot-profile /data/robot_profile.json \ + --qpos-semantics measured_state --time-offset 0 +``` + +1. 提交并选中右侧的一个原子技能区间。标注名称可自由填写,拟合技能需在下拉框中明确指定。 +2. 填写关节记录、机器人配置、数据语义,以及视频零时刻对应的关节时间(秒)。不会从视频 FPS 推断关节采样周期。 +3. 点击“拟合并回填选中区间”;页面先保存标注,再在后台执行拟合。拟合期间可继续逐帧查看视频。 +4. 查看参数与校验报告,对比原始、原生技能(按源时钟对齐)、连续回填的关节曲线和误差。 + 曲线点击按真实帧时间定位视频,支持 VFR;FK 骨架与 TCP 路径随视频更新,可拖动旋转、滚轮缩放并放大显示几何误差。 +5. “下载完整回填 NPZ”导出全部原始/重建/回填/校正候选关节、时间戳与元数据。原始视频、HDF5、关节记录均不被覆盖。 + +“数据字段、阶段提示与拟合容差”可设置 `qpos` / `timestamps` 字段、显式 `joint_dt`、PT 环境索引、 +完整阶段分区 JSON,以及 `SkillFitCfg` 覆盖项。也可用 `--phase-hints` / `--fit-config` 从 JSON 文件预填。 +没有可辨识的夹爪开闭或完整阶段提示时会报告失败;RGB 不用于自动判断接触。 + +每次拟合以输入的原始关节记录为基准,仅替换**选中的一个区间**,保留原始时间戳与区间外采样, +多次拟合不会自动累积修改。候选未通过独立校验时仍可查看和下载,`patched_qpos` 保持原始值, +`proposed_qpos` 保留被拒绝的校正候选。标注修订发生变化后旧结果会标记过期并阻止下载,需重新拟合。 +输出语义为设计参考,报告中的物理成功与碰撞状态仍为未知/未验证。 + +后台最多运行一个拟合任务,不排队;帧解码仍由原先的单一线程持有,只读取选区端点的时间, +不重新建立视频索引。服务最多保留一个结果,退出时清理临时 NPZ。网页最多预览约 1000 个关节采样、 +128 个关节和 600 个 FK 采样,预览 JSON 上限 16 MiB;下载保留全部采样。 +原生规划的时间预算为协作式检查,不能中断单次原生求解;关闭服务会等待正在执行的拟合结束。 + ## 图像帧输入接口 核心只依赖 [`FrameSource`](frames.py) 协议,不依赖视频或 HDF5。已有 RGB 图像可直接传入: diff --git a/embodichain/toolkits/trajectory_annotator/_cli.py b/embodichain/toolkits/trajectory_annotator/_cli.py index e5a8623b4e..22b25f827f 100644 --- a/embodichain/toolkits/trajectory_annotator/_cli.py +++ b/embodichain/toolkits/trajectory_annotator/_cli.py @@ -22,10 +22,64 @@ from collections.abc import Sequence from pathlib import Path import math +import json +from typing import Any __all__ = ["cli"] +def _add_fitting_arguments(parser: argparse.ArgumentParser) -> None: + """Share optional web fitting defaults with the runnable annotation example.""" + group = parser.add_argument_group("轨迹拟合与连续回填(可在网页中填写或修改)") + group.add_argument("--joints", type=Path, help="关节记录:NPZ / HDF5 / Atomic PT。") + group.add_argument( + "--robot-profile", type=Path, help="FK/IK、TCP、关节顺序与限制的 JSON 配置。" + ) + group.add_argument( + "--qpos-semantics", + choices=("measured_state", "actual_submission", "design_reference"), + ) + group.add_argument( + "--time-offset", type=float, help="视频零时刻对应的关节时间,秒;必须显式指定。" + ) + group.add_argument("--qpos-key", default="qpos") + group.add_argument("--timestamps-key", default="timestamps") + group.add_argument( + "--joint-dt", type=float, help="无关节时间戳时显式指定的采样周期,秒。" + ) + group.add_argument("--env-index", type=int, default=0) + group.add_argument( + "--phase-hints", type=Path, help="完整阶段分区 JSON(半开、选区相对关节索引)。" + ) + group.add_argument("--fit-config", type=Path, help="SkillFitCfg 覆盖项 JSON。") + group.add_argument("--device", default="cpu", help="离线 FK/IK 计算设备。") + + +def _fitting_options(args: argparse.Namespace) -> dict[str, Any]: + result: dict[str, Any] = {} + for name in ( + "joints", + "robot_profile", + "qpos_semantics", + "time_offset", + "qpos_key", + "timestamps_key", + "joint_dt", + "env_index", + "device", + ): + value = getattr(args, name) + if value is not None: + result[name] = ( + str(value.expanduser().resolve()) if isinstance(value, Path) else value + ) + for name in ("phase_hints", "fit_config"): + path = getattr(args, name) + if path is not None: + result[name] = json.loads(path.read_text()) + return result + + def cli(argv: Sequence[str] | None = None) -> None: """Launch the local annotation website for a video or HDF5 image sequence. @@ -68,6 +122,7 @@ def cli(argv: Sequence[str] | None = None) -> None: parser.add_argument( "--no-browser", action="store_true", help="只打印网址,不自动打开浏览器。" ) + _add_fitting_arguments(parser) args = parser.parse_args(argv) source_path = (args.video or args.hdf5).resolve() if not source_path.is_file(): @@ -117,7 +172,11 @@ def factory() -> VideoFrameSource: print(f"输入:{source_path}", flush=True) launch_annotation( - factory, annotations, port=args.port, open_browser=not args.no_browser + factory, + annotations, + port=args.port, + open_browser=not args.no_browser, + fitting_options=_fitting_options(args), ) except ImportError as error: parser.exit( diff --git a/embodichain/toolkits/trajectory_annotator/_fitting.py b/embodichain/toolkits/trajectory_annotator/_fitting.py new file mode 100644 index 0000000000..c7f57dca09 --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/_fitting.py @@ -0,0 +1,258 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""One bounded offline fit and a decimated comparison for the annotation host.""" + +from __future__ import annotations + +from collections.abc import Callable +from concurrent.futures import Future +from copy import deepcopy +from dataclasses import asdict +import hashlib +import json +from pathlib import Path +import tempfile +import threading +from typing import Any, BinaryIO +from uuid import uuid4 + +import numpy as np + +from embodichain.toolkits.skill_fitting._pipeline import FitInputs +from ._frames import FramePreview +from ._models import ConflictError + +__all__ = [] + + +def _comparison( + arrays: dict[str, np.ndarray], report: dict[str, Any] +) -> dict[str, Any]: + """Bound browser data independently of the full-resolution downloadable NPZ.""" + count, dof = arrays["original_qpos"].shape + # Keep phase/selection boundaries even when uniform drawing points miss them. + anchors = [report["start"], report["stop"] - 1] + anchors.extend(report["start"] + p["start"] for p in report["phases"]) + grid = np.unique( + np.r_[np.linspace(0, count - 1, min(count, 1000)).astype(int), anchors] + ) + # At most 128 columns enter the browser; every column remains in the NPZ. + columns = min(dof, 128) + geometry = arrays["geometry_indices"] + view = np.linspace(0, len(geometry) - 1, min(len(geometry), 600)).astype(int) + result = dict( + report=report, + indices=grid.tolist(), + timestamps=arrays["timestamps"][grid].tolist(), + joint_names=report["joint_names"][:columns], + total_joints=dof, + geometry_times=arrays["timestamps"][geometry[view]].tolist(), + ) + for name in ("original", "native", "patched", "proposed"): + result[f"{name}_qpos"] = arrays[f"{name}_qpos"][grid, :columns].tolist() + result[f"{name}_links"] = arrays[f"{name}_links"][view].tolist() + result[f"{name}_tcp"] = arrays[f"{name}_tcp"][view].tolist() + # Fail explicitly rather than letting a large browser payload stall scrubbing. + if len(json.dumps(result, allow_nan=False).encode()) > 16 * 1024**2: + raise ValueError("拟合预览超过 16 MiB;请减少关节数或使用离线拟合命令。") + return result + + +class FitSession: + """Retain one job/result, reject concurrent submissions and own its lifetime.""" + + def __init__(self, defaults: dict[str, Any] | None = None) -> None: + raw = deepcopy(defaults or {}) + if set(raw) - set(FitInputs.__dataclass_fields__): + raise ValueError("Unknown fitting input defaults.") + self.defaults = asdict(FitInputs(**raw)) + json.dumps(self.defaults, allow_nan=False) + self._lock = threading.RLock() + self._state: dict[str, Any] = dict( + phase="idle", job_id=None, message="选择已标注区间后开始拟合。" + ) + self._selection: dict[str, Any] | None = None + self._revision: int | None = None + self._preview: dict[str, Any] | None = None + self._bundle: Path | None = None + self._directory: tempfile.TemporaryDirectory[str] | None = None + self._thread: threading.Thread | None = None + self._closed = False + + def snapshot(self, document: dict[str, Any]) -> dict[str, Any]: + """Return status with selection/revision invalidation computed live.""" + with self._lock: + stale = self._revision is not None and ( + document["revision"] != self._revision + or self._selection not in document["segments"] + ) + return { + **deepcopy(self._state), + "stale": stale, + "selection": deepcopy(self._selection), + "annotation_revision": self._revision, + } + + def start( + self, + payload: dict[str, Any], + document: dict[str, Any], + interval: Callable[[int, int], Future[tuple[FramePreview, FramePreview]]], + check_current: Callable[[], None], + ) -> dict[str, Any]: + """Reserve a job before enqueueing any decoder/solver work.""" + if payload.keys() != {"segment_id", "expected_revision", "skill_id", "inputs"}: + raise ValueError( + "拟合请求需要 segment_id、expected_revision、skill_id、inputs。" + ) + if ( + type(payload["expected_revision"]) is not int + or payload["expected_revision"] != document["revision"] + ): + raise ConflictError("标注修订已变化,请保存并刷新后重试。") + matches = [s for s in document["segments"] if s["id"] == payload["segment_id"]] + if len(matches) != 1: + raise ConflictError("请先提交并保存选中的原子技能区间。") + if payload["skill_id"] not in ("pick_up", "slide", "twist", "hand_over"): + raise ValueError("仅支持 pick_up、slide、twist、hand_over。") + raw = payload["inputs"] + if not isinstance(raw, dict) or set(raw) - set(self.defaults): + raise ValueError("拟合输入字段不符合格式。") + inputs = FitInputs(**{**self.defaults, **raw}) + inputs.check() + json.dumps(asdict(inputs), allow_nan=False) + with self._lock: + if self._closed or self._state["phase"] == "running": + raise ConflictError("已有拟合正在运行,请等待当前任务完成。") + first, last = matches[0]["start_frame"], matches[0]["end_frame"] + clock = interval(first, last) + if self._directory is None: + self._directory = tempfile.TemporaryDirectory( + prefix="embodichain-annotation-fit-" + ) + if self._bundle is not None: + self._bundle.unlink(missing_ok=True) + self._preview, self._bundle = None, None + self._selection, self._revision = deepcopy(matches[0]), document["revision"] + job_id = uuid4().hex + self._state = dict( + phase="running", + job_id=job_id, + message="正在获取选区的精确视频时钟…", + error=None, + inputs=asdict(inputs), + skill_id=payload["skill_id"], + ) + self._thread = threading.Thread( + target=self._run, + args=( + inputs, + payload["skill_id"], + deepcopy(document), + clock, + check_current, + ), + name="trajectory-fit", + daemon=True, + ) + self._thread.start() + return self.snapshot(document) + + def _progress(self, text: str) -> None: + with self._lock: + self._state["message"] = text + + def _run( + self, + inputs: FitInputs, + skill: str, + document: dict[str, Any], + clock: Future[tuple[FramePreview, FramePreview]], + check_current: Callable[[], None], + ) -> None: + try: + from embodichain.data_pipeline.skill_fitting import save_skill_fit + from embodichain.toolkits.skill_fitting._pipeline import fit_recording + + left, right = clock.result() + check_current() + endpoints = [left] if left.index == right.index else [left, right] + arrays, report = fit_recording( + inputs, + skill_id=skill, + frame_times=np.array([v.time_seconds for v in endpoints]), + frame_durations=np.array([v.delay_seconds for v in endpoints]), + start_frame=left.index, + end_frame=right.index, + provenance=dict( + source_identity=document["source"]["identity"], + source_timing=document["source"]["timing"], + annotation_revision=document["revision"], + annotation_id=self._selection["id"], + annotation_sha256=hashlib.sha256( + json.dumps(document, sort_keys=True).encode() + ).hexdigest(), + annotation_digest_kind="canonical_document_json", + annotation_segment=self._selection, + ), + progress=self._progress, + ) + check_current() + preview = _comparison(arrays, report) + bundle = Path(self._directory.name) / f"{self._state['job_id']}.npz" + save_skill_fit(bundle, arrays, report) + with self._lock: + self._preview, self._bundle = preview, bundle + self._state.update( + phase="done", + accepted=report["accepted"], + message=( + "已生成连续回填轨迹。" + if report["accepted"] + else "候选未通过校验;回填轨迹保留原始值。" + ), + ) + except Exception as error: + with self._lock: + self._state.update(phase="error", message="拟合失败", error=str(error)) + + def preview(self, job_id: str, document: dict[str, Any]) -> dict[str, Any]: + """Return the immutable drawing data only for a current completed job.""" + with self._lock: + self._require_result(job_id, document) + return self._preview + + def _require_result(self, job_id: str, document: dict[str, Any]) -> None: + if self._state["job_id"] != job_id or self._state["phase"] != "done": + raise ValueError("拟合结果不存在或尚未完成。") + if self.snapshot(document)["stale"]: + raise ConflictError("结果对应的标注修订已变化,请重新拟合。") + + def open_bundle(self, job_id: str, document: dict[str, Any]) -> BinaryIO: + """Open under the job lock so replacing a result cannot break a download.""" + with self._lock: + self._require_result(job_id, document) + return self._bundle.open("rb") + + def close(self) -> None: + """Join cooperative native fitting before removing the sole result bundle.""" + with self._lock: + self._closed = True + if self._thread is not None: + self._thread.join() + if self._directory is not None: + self._directory.cleanup() diff --git a/embodichain/toolkits/trajectory_annotator/_frames.py b/embodichain/toolkits/trajectory_annotator/_frames.py index b731368704..fec8209501 100644 --- a/embodichain/toolkits/trajectory_annotator/_frames.py +++ b/embodichain/toolkits/trajectory_annotator/_frames.py @@ -168,6 +168,9 @@ def __init__( phase="starting", completed=0, total=None, error=None ) self._pending: tuple[int, Future[FramePreview]] | None = None + self._interval: ( + tuple[int, int, Future[tuple[FramePreview, FramePreview]]] | None + ) = None self._thread: threading.Thread | None = None def status(self) -> dict[str, Any]: @@ -200,6 +203,31 @@ def request(self, index: int) -> Future[FramePreview]: self._condition.notify() return future + def interval( + self, first: int, last: int + ) -> Future[tuple[FramePreview, FramePreview]]: + """Capture exact endpoint timing on the decoder owner, without a new index. + + One pending interval is permitted independently of the latest seek. A + video scrub cannot supersede a fitting job's clock snapshot. + + Args: + first: Included start frame index. + last: Included end frame index. + + Returns: + A future containing both owned endpoint previews and their timing. + """ + future: Future[tuple[FramePreview, FramePreview]] = Future() + with self._condition: + if self._status["phase"] != "ready" or self._cancelled.is_set(): + raise RuntimeError("帧源尚未就绪或已关闭。") + if self._interval is not None: + raise RuntimeError("已有待处理的选区时间请求。") + self._interval = (first, last, future) + self._condition.notify() + return future + def _run(self) -> None: try: self.reader = FrameReader( @@ -216,9 +244,21 @@ def _run(self) -> None: while not self._cancelled.is_set(): with self._condition: self._condition.wait_for( - lambda: self._pending is not None or self._cancelled.is_set() + lambda: self._pending is not None + or self._interval is not None + or self._cancelled.is_set() ) + interval, self._interval = self._interval, None pending, self._pending = self._pending, None + if interval is not None: + first, last, result = interval + if result.set_running_or_notify_cancel(): + try: + result.set_result( + (self.reader.preview(first), self.reader.preview(last)) + ) + except Exception as error: + result.set_exception(error) if pending is None: continue index, future = pending @@ -243,6 +283,9 @@ def _run(self) -> None: if self._pending is not None and not self._pending[1].done(): self._pending[1].set_exception(RuntimeError("帧源已关闭。")) self._pending = None + if self._interval is not None and not self._interval[2].done(): + self._interval[2].set_exception(RuntimeError("帧源已关闭。")) + self._interval = None def _check_running(self) -> None: if self._cancelled.is_set(): diff --git a/embodichain/toolkits/trajectory_annotator/_server.py b/embodichain/toolkits/trajectory_annotator/_server.py index 987b4f4085..8854c1247f 100644 --- a/embodichain/toolkits/trajectory_annotator/_server.py +++ b/embodichain/toolkits/trajectory_annotator/_server.py @@ -22,20 +22,23 @@ from contextlib import asynccontextmanager from collections.abc import AsyncIterator, Callable import json +import math from pathlib import Path import secrets from typing import Any from fastapi import FastAPI, HTTPException, Request -from fastapi.responses import FileResponse, Response +from fastapi.responses import FileResponse, JSONResponse, Response, StreamingResponse from fastapi.staticfiles import StaticFiles from starlette.concurrency import run_in_threadpool from starlette.middleware.trustedhost import TrustedHostMiddleware +from starlette.background import BackgroundTask from ._cfg import AnnotationCfg from ._models import ConflictError from ._storage import AnnotationStore from ._frames import SupersededError, FrameWorker +from ._fitting import FitSession from .frames import FrameSource __all__ = ["create_app"] @@ -45,6 +48,8 @@ def create_app( source_factory: Callable[[], FrameSource], annotations: Path, cfg: AnnotationCfg | None = None, + *, + fitting_options: dict[str, Any] | None = None, ) -> FastAPI: """Create one local app with a lifespan-owned frame source and annotation file. @@ -52,6 +57,7 @@ def create_app( source_factory: Create a fresh frame source on its owning worker thread. annotations: Annotation JSON destination. cfg: Optional tool resource configuration. + fitting_options: Optional initial joint/profile settings for the web form. Returns: An ASGI app ready for a single Uvicorn process. @@ -60,18 +66,21 @@ def create_app( cfg.validate() store = AnnotationStore(annotations, cfg.document_bytes) worker = FrameWorker(source_factory, cfg, store.initialize) + fitting = FitSession(fitting_options) token = secrets.token_urlsafe(32) @asynccontextmanager async def lifespan(app: FastAPI) -> AsyncIterator[None]: - nonlocal worker, store + nonlocal worker, store, fitting store = AnnotationStore(annotations, cfg.document_bytes) worker = FrameWorker(source_factory, cfg, store.initialize) + fitting = FitSession(fitting_options) worker.start() try: yield finally: await run_in_threadpool(worker.close) + await run_in_threadpool(fitting.close) app = FastAPI( title="Trajectory Annotator", lifespan=lifespan, docs_url=None, redoc_url=None @@ -91,6 +100,25 @@ def authorize(request: Request) -> None: ): raise HTTPException(403, "请通过本工具页面提交修改。") + async def read_json(request: Request, limit: int) -> dict[str, Any]: + if ( + request.headers.get("content-type", "").split(";", 1)[0] + != "application/json" + ): + raise HTTPException(415, "请求必须使用 application/json。") + content = bytearray() + async for chunk in request.stream(): + if len(content) + len(chunk) > limit: + raise HTTPException(413, "请求内容超过大小上限。") + content.extend(chunk) + try: + payload = json.loads(content) + if not isinstance(payload, dict): + raise ValueError("请求需要 JSON 对象。") + return payload + except (ValueError, UnicodeError) as error: + raise HTTPException(422, str(error)) from error + @app.get("/api/status") async def status() -> dict[str, Any]: return {**worker.status(), "session_token": token} @@ -131,22 +159,37 @@ async def frame(index: int, request_id: int = 0) -> Response: }, ) + @app.get("/api/frame-index") + async def frame_at_time(time_seconds: float) -> dict[str, int]: + """Locate a chart click through the same source interface, including VFR.""" + ready() + if not math.isfinite(time_seconds) or time_seconds < 0: + raise HTTPException(422, "视频时间需要有限非负数。") + # A binary search does not assume FPS or require retaining a second + # timestamp index. Seeks stay on the bounded, single-owner decoder. + low, high = 0, store.snapshot()["document"]["source"]["frame_count"] + try: + while low < high: + middle = (low + high) // 2 + preview = await asyncio.wrap_future(worker.request(middle)) + if preview.time_seconds <= time_seconds + 1e-9: + low = middle + 1 + else: + high = middle + return {"index": max(0, low - 1)} + except SupersededError as error: + raise HTTPException( + 409, "时间定位被新寻帧替代,请重新点击曲线。" + ) from error + except (RuntimeError, OSError, ValueError) as error: + raise HTTPException(422, str(error)) from error + @app.put("/api/annotations") async def save(request: Request) -> dict[str, Any]: authorize(request) ready() - if ( - request.headers.get("content-type", "").split(";", 1)[0] - != "application/json" - ): - raise HTTPException(415, "请求必须使用 application/json。") - content = bytearray() - async for chunk in request.stream(): - if len(content) + len(chunk) > cfg.document_bytes: - raise HTTPException(413, "标注内容超过大小上限。") - content.extend(chunk) + payload = await read_json(request, cfg.document_bytes) try: - payload = json.loads(content) if not isinstance(payload, dict) or payload.keys() != { "segments", "expected_revision", @@ -167,6 +210,70 @@ def persist() -> dict[str, Any]: except OSError as error: raise HTTPException(507, f"保存失败:{error}") from error + @app.get("/api/fitting") + async def fit_status() -> dict[str, Any]: + ready() + state = fitting.snapshot(store.snapshot()["document"]) + try: + await run_in_threadpool(store.check_unchanged) + except (ConflictError, OSError, ValueError) as error: + state.update(stale=True, error=str(error)) + return {**state, "defaults": fitting.defaults} + + @app.post("/api/fitting/jobs", status_code=202) + async def fit_start(request: Request) -> dict[str, Any]: + authorize(request) + ready() + payload = await read_json(request, min(cfg.document_bytes, 256 * 1024)) + try: + await run_in_threadpool(store.check_unchanged) + document = store.snapshot()["document"] + + def check_current() -> None: + if worker.reader is None: + raise RuntimeError("帧源尚未准备。") + worker.reader.check_unchanged() + store.check_unchanged() + if store.snapshot()["document"] != document: + raise ConflictError("标注在拟合期间发生变化,请重新拟合。") + + return fitting.start(payload, document, worker.interval, check_current) + except ConflictError as error: + raise HTTPException(409, str(error)) from error + except (ValueError, TypeError, OSError, RuntimeError) as error: + raise HTTPException(422, str(error)) from error + + @app.get("/api/fitting/jobs/{job_id}/preview") + async def fit_preview(job_id: str) -> Response: + ready() + try: + await run_in_threadpool(store.check_unchanged) + result = fitting.preview(job_id, store.snapshot()["document"]) + return await run_in_threadpool(JSONResponse, result) + except ConflictError as error: + raise HTTPException(409, str(error)) from error + except (ValueError, OSError) as error: + raise HTTPException(404, str(error)) from error + + @app.get("/api/fitting/jobs/{job_id}/bundle") + async def fit_bundle(job_id: str) -> StreamingResponse: + ready() + try: + await run_in_threadpool(store.check_unchanged) + stream = fitting.open_bundle(job_id, store.snapshot()["document"]) + except ConflictError as error: + raise HTTPException(409, str(error)) from error + except (ValueError, OSError) as error: + raise HTTPException(404, str(error)) from error + return StreamingResponse( + iter(lambda: stream.read(1024 * 1024), b""), + media_type="application/octet-stream", + headers={ + "Content-Disposition": f'attachment; filename="trajectory.{job_id}.fit.npz"' + }, + background=BackgroundTask(stream.close), + ) + @app.post("/api/cancel") async def cancel(request: Request) -> dict[str, Any]: authorize(request) diff --git a/embodichain/toolkits/trajectory_annotator/_storage.py b/embodichain/toolkits/trajectory_annotator/_storage.py index 51434bb0a1..79d71e32ad 100644 --- a/embodichain/toolkits/trajectory_annotator/_storage.py +++ b/embodichain/toolkits/trajectory_annotator/_storage.py @@ -170,6 +170,18 @@ def snapshot(self) -> dict[str, Any]: "persisted": self._digest is not None, } + def check_unchanged(self) -> None: + """Reject fits based on an annotation file edited outside this session.""" + with self._lock: + content = self._read() + digest = ( + hashlib.sha256(content).hexdigest() if content is not None else None + ) + if digest != self._digest: + raise ConflictError( + "磁盘上的标注文件已被其他程序修改,请重新打开项目。" + ) + def save( self, skill_intervals: object, expected_revision: object ) -> dict[str, Any]: diff --git a/embodichain/toolkits/trajectory_annotator/app.py b/embodichain/toolkits/trajectory_annotator/app.py index 7d2c0bb4b6..4d69ee8858 100644 --- a/embodichain/toolkits/trajectory_annotator/app.py +++ b/embodichain/toolkits/trajectory_annotator/app.py @@ -22,7 +22,7 @@ from pathlib import Path import socket import threading -from typing import TYPE_CHECKING +from typing import Any, TYPE_CHECKING import webbrowser if TYPE_CHECKING: @@ -33,7 +33,10 @@ def create_annotation_app( - source_factory: Callable[[], FrameSource], annotations: str | Path + source_factory: Callable[[], FrameSource], + annotations: str | Path, + *, + fitting_options: dict[str, Any] | None = None, ) -> FastAPI: """Create a source-independent FastAPI application for a single process. @@ -41,13 +44,17 @@ def create_annotation_app( source_factory: Create a fresh FrameSource on the app's worker thread. The app closes it on shutdown or failed preparation. annotations: Destination JSON path; persisted only after an edit/save. + fitting_options: Initial joint/profile settings, with the same field + names as the web fitting form. Omit to configure them in the browser. Returns: ASGI application with lifespan-managed frame input and preview cache. """ from ._server import create_app - return create_app(source_factory, Path(annotations)) + return create_app( + source_factory, Path(annotations), fitting_options=fitting_options + ) def launch_annotation( @@ -56,6 +63,7 @@ def launch_annotation( *, port: int = 0, open_browser: bool = True, + fitting_options: dict[str, Any] | None = None, ) -> None: """Run the local website until Ctrl+C, accepting any frame source factory. @@ -64,6 +72,7 @@ def launch_annotation( annotations: JSON output file, distinct from any source file. port: Loopback port; zero asks the OS to select an available port. open_browser: Open the website after the server starts. + fitting_options: Initial joint/profile settings for offline reconstruction. Raises: ValueError: Port is outside 0–65535. @@ -76,7 +85,9 @@ def launch_annotation( annotations = Path(annotations).resolve() server = uvicorn.Server( uvicorn.Config( - create_annotation_app(source_factory, annotations), + create_annotation_app( + source_factory, annotations, fitting_options=fitting_options + ), host="127.0.0.1", port=port, access_log=False, diff --git a/embodichain/toolkits/trajectory_annotator/web/app.js b/embodichain/toolkits/trajectory_annotator/web/app.js index aa3ee41a52..fbd6a348c5 100644 --- a/embodichain/toolkits/trajectory_annotator/web/app.js +++ b/embodichain/toolkits/trajectory_annotator/web/app.js @@ -1,7 +1,9 @@ // Copyright (c) 2021-2026 DexForce Technology Co., Ltd. Licensed under Apache-2.0. // Completed atomic skill intervals are immutable snapshots; draft and displayed-frame state // stay separate so neither an in-flight seek nor autosave can move an endpoint. +import { createFittingPanel } from "/static/fitting.js"; const $ = (id) => document.getElementById(id); +let fittingPanel = null; const state = { project: null, skillIntervals: [], selected: null, draft: null, lastLabel: "", displayed: null, target: 0, delay: 1 / 30, playing: false, loading: false, @@ -109,6 +111,7 @@ async function pumpFrames() { state.loading = false; $("frame-current").textContent = index; $("frame-time").textContent = timecode(Number(response.headers.get("X-Frame-Time"))); + fittingPanel?.displayTime(Number(response.headers.get("X-Frame-Time"))); $("scrubber").value = index; // Do not change a number the user is still typing. if (document.activeElement !== $("goto-frame")) $("goto-frame").value = index; @@ -292,6 +295,7 @@ function renderSelection() { $("delete").disabled = !skillInterval || !!state.draft; $("undo").disabled = !state.undo.length || !!state.draft; $("redo").disabled = !state.redo.length || !!state.draft; + fittingPanel?.selectionChanged(); } function renderSkillIntervals() { $("skill-interval-list").replaceChildren(); @@ -392,6 +396,7 @@ async function save() { } finally { state.saving = false; renderSave(); + fittingPanel?.selectionChanged(); } } function exportJson() { @@ -587,6 +592,23 @@ async function initialize() { tick.append(caption); return tick; })); + fittingPanel = createFittingPanel({ + context: () => ({ selection: selected(), revision: state.project.document.revision, dirty: dirty(), draft: state.draft, token: state.token }), + saveAnnotations: async () => { + if (state.draft) throw new Error("请先提交或取消当前标注草稿。"); + while (state.saving) await new Promise((resolve) => setTimeout(resolve, 50)); + await save(); + if (dirty() || state.saveError || state.conflict) throw new Error("标注尚未成功保存,请处理保存错误后重新拟合。"); + }, + seekTime: async (seconds) => { + pause(); + const serial = state.serial; + const response = await checked(await fetch(`/api/frame-index?time_seconds=${encodeURIComponent(seconds)}`)); + const result = await response.json(); + if (serial === state.serial) requestFrame(result.index); + }, + }); + fittingPanel.initialize(); bindControls(); zoom(1); renderSkillIntervals(); renderDraft(); requestFrame(0); message("已就绪。定位起点按 I,终点按 O,填写名称后按 Enter 提交。 "); $("video-stage").focus({ preventScroll: true }); diff --git a/embodichain/toolkits/trajectory_annotator/web/fitting.js b/embodichain/toolkits/trajectory_annotator/web/fitting.js new file mode 100644 index 0000000000..56b9c3e161 --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/web/fitting.js @@ -0,0 +1,315 @@ +// Copyright (c) 2021-2026 DexForce Technology Co., Ltd. Licensed under Apache-2.0. +// Drawing data is bounded; the downloadable NPZ retains the original sample grid. +const $ = (id) => document.getElementById(id); +const colors = { original: "#fb8696", native: "#f2b664", patched: "#66d5bc", proposed: "#b6a1ff" }; +const fields = { + joints: "fit-joints", robot_profile: "fit-profile", qpos_semantics: "fit-semantics", + time_offset: "fit-offset", qpos_key: "fit-qpos-key", timestamps_key: "fit-timestamps-key", + joint_dt: "fit-joint-dt", env_index: "fit-env-index", device: "fit-device", +}; + +async function jsonRequest(url, options) { + const response = await fetch(url, options); + const data = await response.json(); + if (!response.ok) throw new Error(typeof data.detail === "string" ? data.detail : JSON.stringify(data.detail || data)); + return data; +} + +function nearest(values, value) { + let low = 0, high = values.length; + while (low < high) { + const mid = (low + high) >>> 1; + if (values[mid] < value) low = mid + 1; else high = mid; + } + if (!low) return 0; + if (low === values.length) return low - 1; + return value - values[low - 1] < values[low] - value ? low - 1 : low; +} + +function surface(canvas) { + const { width, height } = canvas.getBoundingClientRect(); + if (!width || !height) return null; + const ratio = window.devicePixelRatio || 1; + const w = Math.round(width * ratio), h = Math.round(height * ratio); + if (canvas.width !== w || canvas.height !== h) { canvas.width = w; canvas.height = h; } + const ctx = canvas.getContext("2d"); + ctx.setTransform(ratio, 0, 0, ratio, 0, 0); + ctx.clearRect(0, 0, width, height); + return { ctx, width, height }; +} + +export function createFittingPanel({ context, saveAnnotations, seekTime }) { + const state = { job: null, data: null, timer: null, loading: false, starting: false, time: 0, hover: null, seeking: false }; + const camera = { yaw: -0.8, pitch: 0.5, zoom: 1, center: [0, 0, 0], radius: 1 }; + const series = () => state.data?.report.accepted ? ["original", "native", "patched"] : ["original", "native", "patched", "proposed"]; + const stale = () => !!state.job && (state.job.stale || context().dirty || state.job.revision !== context().revision); + + function notify(text, kind = "") { + $("fit-message").textContent = text; + $("fit-message").className = `fit-message ${kind}`; + } + + function selectionChanged() { + const current = context(); + $("fit-selection").textContent = current.selection ? `${current.selection.label} · [${current.selection.start_frame}, ${current.selection.end_frame}] 帧` : "请在右侧选择一段已标注区间"; + $("fit-run").disabled = !current.selection || !!current.draft || state.starting || state.job?.phase === "running"; + $("fit-run").textContent = state.starting || state.job?.phase === "running" ? "正在拟合…" : "拟合并回填选中区间"; + if (!state.job) return; + const outdated = stale(); + const labels = { running: "拟合中", error: "拟合失败", done: state.job.accepted ? "通过校验" : "未通过校验" }; + $("fit-state").textContent = outdated ? "标注已变化" : labels[state.job.phase] || "未开始"; + $("fit-state").className = `status-pill ${outdated ? "dirty" : state.job.phase === "done" && state.job.accepted ? "saved" : state.job.phase === "error" ? "error" : ""}`; + const link = $("fit-download"); + link.setAttribute("aria-disabled", String(outdated || state.job.phase !== "done")); + if (outdated) { + link.removeAttribute("href"); + notify("标注已变化,当前结果属于先前修订。请重新拟合后下载。", "warning"); + } else if (state.job.phase === "done") link.href = `/api/fitting/jobs/${state.job.job_id}/bundle`; + } + + function readInputs() { + const result = Object.fromEntries(Object.entries(fields).map(([key, id]) => [key, $(id).value.trim()])); + for (const key of ["time_offset", "joint_dt", "env_index"]) result[key] = result[key] === "" ? null : Number(result[key]); + result.phase_hints = $("fit-phase-hints").value.trim() ? JSON.parse($("fit-phase-hints").value) : null; + result.fit_config = $("fit-config").value.trim() ? JSON.parse($("fit-config").value) : {}; + if (!result.joints || !result.robot_profile) throw new Error("请填写关节记录路径和机器人配置路径。"); + if (!result.qpos_semantics || result.time_offset === null) throw new Error("请显式选择数据语义,并填写视频零时刻对应的关节时间。"); + return result; + } + + async function start() { + if (state.starting || state.job?.phase === "running") return; + state.starting = true; + selectionChanged(); + try { + const inputs = readInputs(); + const selection = structuredClone(context().selection); + await saveAnnotations(); + const current = context(); + if (!selection || JSON.stringify(selection) !== JSON.stringify(current.selection)) throw new Error("选区已变化,请重新开始拟合。"); + const job = await jsonRequest("/api/fitting/jobs", { + method: "POST", headers: { "Content-Type": "application/json", "X-Annotation-Token": current.token }, + body: JSON.stringify({ segment_id: selection.id, expected_revision: current.revision, skill_id: $("fit-skill").value, inputs }), + }); + state.job = { ...job, revision: current.revision }; + state.data = null; + $("fit-result").hidden = true; + notify(job.message); + clearTimeout(state.timer); + void poll(); + } catch (error) { notify(error.message, "error"); } + finally { state.starting = false; selectionChanged(); } + } + + async function poll(initial = false) { + clearTimeout(state.timer); + try { + const job = await jsonRequest("/api/fitting"); + if (initial) { + const defaults = job.inputs || job.defaults; + for (const [key, id] of Object.entries(fields)) $(id).value = defaults[key] ?? ""; + $("fit-phase-hints").value = defaults.phase_hints ? JSON.stringify(defaults.phase_hints, null, 2) : ""; + $("fit-config").value = Object.keys(defaults.fit_config || {}).length ? JSON.stringify(defaults.fit_config, null, 2) : ""; + if (job.skill_id) $("fit-skill").value = job.skill_id; + } + if (!job.job_id) return; + state.job = { ...job, revision: job.annotation_revision }; + notify(job.error || job.message, job.error ? "error" : job.phase === "done" && !job.accepted ? "warning" : ""); + selectionChanged(); + if (job.phase === "running") state.timer = setTimeout(() => void poll(), 600); + else if (job.phase === "done" && !stale() && !state.data && !state.loading) { + state.loading = true; + try { + const data = await jsonRequest(`/api/fitting/jobs/${job.job_id}/preview`); + if (state.job.job_id === job.job_id) { + state.data = data; + showResult(); + } + } finally { state.loading = false; } + } + } catch (error) { + notify(`无法读取拟合任务:${error.message}`, "error"); + // Retain the running job guard; polling failure is not solver completion. + if (state.job?.phase === "running") state.timer = setTimeout(() => void poll(), 1500); + } + } + + function showResult() { + const { report } = state.data; + const selection = state.job.selection; + $("fit-result-title").textContent = `${selection.label} · ${report.skill_id} · [${selection.start_frame}, ${selection.end_frame}]`; + $("fit-result").hidden = false; + $("fit-inputs").open = false; + $("fit-proposed-legend").hidden = report.accepted; + $("fit-joint").replaceChildren(...state.data.joint_names.map((name, index) => { + const option = document.createElement("option"); option.value = index; option.textContent = name; return option; + })); + const tcp = Math.max(...Object.values(report.tcp_errors).map((value) => value.native_tcp_rmse_m)); + const metrics = [ + [report.accepted ? "通过" : "未通过", "独立校验"], + [report.native_joint_rmse.toExponential(2), "原生关节 RMSE · 原始关节单位"], + [`${(tcp * 1000).toFixed(3)} mm`, "最大参与臂 TCP RMSE"], + [`${report.stop - report.start} 个采样`, "选区关节采样 · 原时钟"], + ]; + $("fit-metrics").replaceChildren(...metrics.map(([value, label]) => { + const node = document.createElement("div"); node.className = "fit-metric"; + const strong = document.createElement("strong"); strong.textContent = value; + node.append(strong, document.createTextNode(label)); return node; + })); + $("fit-parameters").replaceChildren(); + for (const [name, value] of Object.entries(report.parameters)) { + if (typeof value !== "number") continue; + const label = document.createElement("dt"), number = document.createElement("dd"); + label.textContent = name; + number.textContent = `${Number(value.toPrecision(6))}${/distance|height/.test(name) ? " m" : /angle/.test(name) ? " rad" : ""}`; + $("fit-parameters").append(label, number); + } + $("fit-report").textContent = JSON.stringify(report, null, 2); + if (report.failures.length) notify(`${state.job.message}\n${report.failures.join("\n")}`, "warning"); + if (state.data.total_joints > state.data.joint_names.length) notify("仅预览前 128 个关节;完整 NPZ 包含所有关节。"); + const lower = [Infinity, Infinity, Infinity], upper = [-Infinity, -Infinity, -Infinity]; + for (const frame of state.data.original_links) for (const arm of frame) for (const point of arm) for (let axis = 0; axis < 3; axis++) { + lower[axis] = Math.min(lower[axis], point[axis]); upper[axis] = Math.max(upper[axis], point[axis]); + } + camera.center = lower.map((value, axis) => (value + upper[axis]) / 2); + camera.radius = Math.max(0.05, Math.hypot(...upper.map((value, axis) => value - lower[axis])) / 2); + camera.yaw = -0.8; camera.pitch = 0.5; camera.zoom = 1; + selectionChanged(); draw(); + } + + function chart(canvas, errors = false) { + const s = surface(canvas); + if (!s) return; + const { ctx, width, height } = s, data = state.data, joint = Number($("fit-joint").value); + const left = 62, top = 21, right = width - 14, bottom = height - 27; + const times = data.timestamps, t0 = times[0], span = Math.max(1e-9, times.at(-1) - t0); + const x = (time) => left + (time - t0) / span * (right - left); + const names = errors ? series().filter((name) => name !== "original") : series(); + const values = {}; + let low = errors ? 0 : Infinity, high = errors ? 0 : -Infinity; + for (const name of names) { + values[name] = data[`${name}_qpos`].map((q, i) => q[joint] - (errors ? data.original_qpos[i][joint] : 0)); + for (const value of values[name]) { low = Math.min(low, value); high = Math.max(high, value); } + } + let padding = Math.max((high - low) * 0.12, errors ? 1e-7 : 1e-4); + low -= padding; high += padding; + const y = (value) => bottom - (value - low) / (high - low) * (bottom - top); + ctx.font = "10px ui-monospace,monospace"; ctx.fillStyle = "#8496b1"; + ctx.fillText(errors ? "相对原始的误差" : "关节位置 · 原始关节单位", left, 13); + for (let i = 0; i <= (errors ? 2 : 4); i++) { + const value = low + (high - low) * i / (errors ? 2 : 4), py = y(value); + ctx.strokeStyle = "#253248"; ctx.beginPath(); ctx.moveTo(left, py); ctx.lineTo(right, py); ctx.stroke(); + ctx.textAlign = "right"; + ctx.fillText(Math.abs(value) < 0.001 && value !== 0 ? value.toExponential(1) : value.toFixed(3), left - 6, py + 3); + } + for (let i = 0; i <= 4; i++) { + const time = t0 + span * i / 4; + ctx.textAlign = "center"; ctx.fillText(`${(time - data.report.time_offset).toFixed(2)}s`, x(time), height - 9); + } + ctx.save(); ctx.beginPath(); ctx.rect(left, top, right - left, bottom - top); ctx.clip(); + const a = data.indices.indexOf(data.report.start), b = data.indices.indexOf(data.report.stop - 1); + ctx.fillStyle = "#66d5bc0c"; ctx.fillRect(x(times[a]), top, Math.max(1, x(times[b]) - x(times[a])), bottom - top); + for (const name of names) { + ctx.strokeStyle = colors[name]; ctx.lineWidth = name === "original" ? 2.8 : 1.5; + ctx.setLineDash(name === "native" ? [5, 3] : name === "proposed" ? [2, 3] : []); + ctx.beginPath(); values[name].forEach((v, i) => i ? ctx.lineTo(x(times[i]), y(v)) : ctx.moveTo(x(times[i]), y(v))); ctx.stroke(); + } + ctx.setLineDash([]); + const time = state.hover ?? state.time + data.report.time_offset; + if (time >= t0 && time <= times.at(-1)) { + ctx.strokeStyle = "#d6e2f0aa"; ctx.lineWidth = 1; ctx.beginPath(); ctx.moveTo(x(time), top); ctx.lineTo(x(time), bottom); ctx.stroke(); + } + ctx.restore(); ctx.textAlign = "left"; + } + + function geometry() { + const s = surface($("fit-geometry")); + if (!s) return; + const { ctx, width, height } = s, data = state.data; + const scale = Math.min(width, height) * 0.38 / camera.radius * camera.zoom; + const cy = Math.cos(camera.yaw), sy = Math.sin(camera.yaw), cp = Math.cos(camera.pitch), sp = Math.sin(camera.pitch); + function project(point) { + const [x, y, z] = point.map((value, axis) => value - camera.center[axis]); + return [width / 2 + (cy * x - sy * y) * scale, height / 2 - (sp * (sy * x + cy * y) + cp * z) * scale]; + } + function line(points, color, thickness, dash = []) { + ctx.strokeStyle = color; ctx.lineWidth = thickness; ctx.setLineDash(dash); ctx.beginPath(); + points.forEach((point, i) => { const [x, y] = project(point); if (i) ctx.lineTo(x, y); else ctx.moveTo(x, y); }); + ctx.stroke(); ctx.setLineDash([]); + } + const radius = camera.radius; + for (let i = -4; i <= 4; i++) { + const v = i * radius / 4; + line([[camera.center[0] - radius, camera.center[1] + v, 0], [camera.center[0] + radius, camera.center[1] + v, 0]], "#27354a", 0.6); + line([[camera.center[0] + v, camera.center[1] - radius, 0], [camera.center[0] + v, camera.center[1] + radius, 0]], "#27354a", 0.6); + } + const multiplier = Number($("fit-exaggeration").value); + const index = nearest(data.geometry_times, state.time + data.report.time_offset); + function exaggerate(point, original) { return point.map((value, axis) => original[axis] + (value - original[axis]) * multiplier); } + for (const name of series()) { + const arms = data[`${name}_tcp`][0].length; + for (let arm = 0; arm < arms; arm++) { + const points = data[`${name}_tcp`].map((value, i) => exaggerate(value[arm], data.original_tcp[i][arm])); + line(points, `${colors[name]}99`, name === "original" ? 2.5 : 1.5, name === "native" ? [5, 3] : []); + const skeleton = data[`${name}_links`][index][arm].map((point, node) => exaggerate(point, data.original_links[index][arm][node])); + line(skeleton, colors[name], name === "original" ? 4 : 2, name === "native" ? [5, 3] : []); + const [px, py] = project(exaggerate(data[`${name}_tcp`][index][arm], data.original_tcp[index][arm])); + ctx.fillStyle = colors[name]; ctx.beginPath(); ctx.arc(px, py, 3, 0, Math.PI * 2); ctx.fill(); + } + } + ctx.font = "10px ui-monospace,monospace"; + ["X", "Y", "Z"].forEach((name, axis) => { + const origin = [camera.center[0] - radius * 0.8, camera.center[1] - radius * 0.8, 0]; + const end = [...origin]; end[axis] += radius * 0.35; + line([origin, end], ["#dc7e8a", "#7aba9d", "#80a4e1"][axis], 1.4); + const [x, y] = project(end); ctx.fillStyle = "#b4c5dc"; ctx.fillText(name, x + 3, y); + }); + ctx.fillStyle = "#8496b1"; + ctx.fillText(`TCP · 视频 ${state.time.toFixed(3)}s · ${multiplier}×`, 12, 18); + } + + function draw() { + if (!state.data) return; + chart($("fit-joint-chart")); chart($("fit-error-chart"), true); geometry(); + const data = state.data, joint = Number($("fit-joint").value), index = nearest(data.timestamps, state.hover ?? state.time + data.report.time_offset); + $("fit-cursor").textContent = `预览采样 · 视频 ${(data.timestamps[index] - data.report.time_offset).toFixed(3)}s · ${data.joint_names[joint]} · 原始 ${data.original_qpos[index][joint].toFixed(6)} · 技能 ${data.native_qpos[index][joint].toFixed(6)} · 回填 ${data.patched_qpos[index][joint].toFixed(6)}`; + } + + for (const id of ["fit-joint-chart", "fit-error-chart"]) { + const canvas = $(id); + const timeAt = (event) => { + const bounds = canvas.getBoundingClientRect(), data = state.data; + const fraction = Math.max(0, Math.min(1, (event.clientX - bounds.left - 62) / (bounds.width - 76))); + return data.timestamps[0] + fraction * (data.timestamps.at(-1) - data.timestamps[0]); + }; + canvas.addEventListener("pointermove", (event) => { if (state.data) { state.hover = timeAt(event); draw(); } }); + canvas.addEventListener("pointerleave", () => { state.hover = null; draw(); }); + canvas.addEventListener("click", async (event) => { + if (!state.data || state.seeking) return; + state.seeking = true; + try { await seekTime(Math.max(0, timeAt(event) - state.data.report.time_offset)); } + catch (error) { notify(error.message, "error"); } + finally { state.seeking = false; } + }); + } + const canvas = $("fit-geometry"); + let drag = null; + canvas.addEventListener("pointerdown", (event) => { + if (event.button !== 0) return; + event.preventDefault(); drag = { id: event.pointerId, x: event.clientX, y: event.clientY }; + canvas.setPointerCapture(event.pointerId); + }); + canvas.addEventListener("pointermove", (event) => { + if (drag?.id !== event.pointerId) return; + camera.yaw += (event.clientX - drag.x) * 0.008; + camera.pitch = Math.max(-1.5, Math.min(1.5, camera.pitch + (event.clientY - drag.y) * 0.008)); + drag.x = event.clientX; drag.y = event.clientY; draw(); + }); + const release = (event) => { if (drag?.id === event.pointerId) { drag = null; if (canvas.hasPointerCapture(event.pointerId)) canvas.releasePointerCapture(event.pointerId); } }; + canvas.addEventListener("pointerup", release); canvas.addEventListener("pointercancel", release); canvas.addEventListener("lostpointercapture", release); + canvas.addEventListener("wheel", (event) => { event.preventDefault(); camera.zoom = Math.max(0.2, Math.min(6, camera.zoom * Math.exp(-event.deltaY * 0.001))); draw(); }, { passive: false }); + $("fit-reset-view").onclick = () => { camera.yaw = -0.8; camera.pitch = 0.5; camera.zoom = 1; draw(); }; + $("fit-joint").onchange = draw; $("fit-exaggeration").onchange = draw; $("fit-run").onclick = () => void start(); + new ResizeObserver(draw).observe($("fit-result")); + return { initialize: () => void poll(true), selectionChanged, displayTime: (time) => { state.time = time; draw(); } }; +} diff --git a/embodichain/toolkits/trajectory_annotator/web/index.html b/embodichain/toolkits/trajectory_annotator/web/index.html index a7d3e9929a..f98e7e4d57 100644 --- a/embodichain/toolkits/trajectory_annotator/web/index.html +++ b/embodichain/toolkits/trajectory_annotator/web/index.html @@ -50,6 +50,42 @@
已标注当前草稿拖动时间轴定位 · ← → 精调一帧
+
+

原子技能拟合与轨迹回填

选中一段已提交的标注,使用关节数据辨识参数并重建。

未开始
+
请在右侧选择一段已标注区间
+
关节记录与机器人配置 本机文件路径 · 可从启动参数预填 +
+ + + + + + +
+
数据字段、阶段提示与拟合容差
+ + + + +
+

视频只用于选区与时间同步;参数辨识使用关节角与机器人 FK/IK 标定。每次回填一个选区,保留原始关节时钟与选区外的采样。

+
+

可读取 NPZ、HDF5 或 Atomic PT;需要包含配置中的夹爪关节。

+
+
diff --git a/embodichain/toolkits/trajectory_annotator/web/styles.css b/embodichain/toolkits/trajectory_annotator/web/styles.css index 5865d70e92..be9b970ced 100644 --- a/embodichain/toolkits/trajectory_annotator/web/styles.css +++ b/embodichain/toolkits/trajectory_annotator/web/styles.css @@ -13,6 +13,7 @@ button.danger { color:var(--danger); } button.danger:hover:not(:disabled) { background:#492736; border-color:#a65368; } :focus-visible { outline:2px solid var(--accent); outline-offset:3px; } input,select { color:var(--text); background:#0e1521; border:1px solid #34425a; border-radius:6px; padding:8px 10px; min-width:0; } +textarea { font:12px/1.5 ui-monospace,SFMono-Regular,Consolas,monospace; resize:vertical; min-width:0; color:var(--text); background:#0e1521; border:1px solid #34425a; border-radius:6px; padding:8px 10px; } input:disabled { color:var(--muted); opacity:.6; } input::placeholder { color:#65738b; } input[type=number] { font-variant-numeric:tabular-nums; } @@ -88,6 +89,43 @@ main { max-width:1680px; padding:22px 28px 12px; margin:auto; } .timeline-legend i { width:8px; height:8px; border-radius:2px; } .legend-saved { background:#4c82b6; } .legend-draft { border:1px dashed #e6bc78; } +.fit-target,.fit-result-heading { display:flex; align-items:center; justify-content:space-between; gap:12px; padding:0 17px 14px; } +.fit-target>span { color:var(--accent); font-size:12px; } +.fit-target>button { font-size:12px; } +.fit-inputs { margin:0 17px; padding:12px 0; border-top:1px solid var(--border); } +.fitting-card summary { cursor:pointer; font-size:12px; color:#c5d4e7; } +.fitting-card summary .muted { margin-left:8px; font-size:10px; } +.fit-grid { display:grid; grid-template-columns:repeat(2,minmax(0,1fr)); gap:12px; padding-top:12px; } +.fit-grid label { display:flex; flex-direction:column; gap:5px; font-size:11px; color:var(--muted); } +.fit-grid input,.fit-grid select { width:100%; font-size:12px; } +.fit-grid small { font-size:10px; } +.fit-wide { grid-column:1/-1; } +.fit-advanced { margin-top:14px; } +.fit-footnote { font-size:11px; color:var(--muted); margin-top:12px; } +.fit-message { margin:0 17px 15px; font-size:12px; color:var(--muted); white-space:pre-wrap; overflow-wrap:anywhere; } +.fit-message.error { color:var(--danger); } +.fit-message.warning { color:#efc88e; } +.fit-result { padding:15px 17px; border-top:1px solid var(--border); } +.fit-result-heading { padding:0 0 12px; font-size:12px; } +.button-link { display:inline-block; text-decoration:none; font-size:11px; color:var(--accent); border:1px solid #397969; background:#1b3a34; border-radius:6px; padding:7px 10px; white-space:nowrap; } +.button-link[aria-disabled=true] { opacity:.4; pointer-events:none; } +.fit-metrics { display:grid; grid-template-columns:repeat(4,minmax(0,1fr)); gap:8px; } +.fit-metric { border:1px solid var(--border); background:#111a27; border-radius:6px; padding:9px; font-size:10px; color:var(--muted); } +.fit-metric strong { display:block; color:var(--text); font-size:15px; font-weight:500; margin-bottom:2px; overflow-wrap:anywhere; } +.fit-parameters { display:flex; flex-wrap:wrap; align-items:baseline; gap:5px 9px; padding:10px 0 0; margin:0; font:11px/1.6 ui-monospace,SFMono-Regular,Consolas,monospace; } +.fit-parameters dt { color:var(--muted); }.fit-parameters dd { color:var(--accent); margin:0 12px 0 0; } +.fit-view-controls { display:flex; align-items:center; flex-wrap:wrap; gap:12px; margin:16px 0 8px; font-size:11px; color:var(--muted); } +.fit-view-controls label { display:flex; align-items:center; gap:7px; } +.fit-view-controls select { max-width:270px; padding:4px 7px; font-size:11px; } +.fit-view-controls button { padding:4px 8px; font-size:11px; margin-left:auto; } +.fit-legend { display:flex; flex-wrap:wrap; gap:14px; font-size:10px; margin-bottom:8px; } +.fit-legend .original { color:#fb8696; }.fit-legend .native { color:#f2b664; }.fit-legend .patched { color:#66d5bc; }.fit-legend .proposed { color:#b6a1ff; } +.fit-chart,.fit-geometry { display:block; width:100%; background:#0e1622; border:1px solid #273449; border-radius:6px; } +.fit-chart { height:210px; cursor:crosshair; }.fit-error-chart { height:120px; margin-top:6px; } +.fit-cursor { min-height:20px; font:10px/1.8 ui-monospace,SFMono-Regular,Consolas,monospace; color:var(--muted); } +.fit-geometry { height:280px; cursor:grab; touch-action:none; }.fit-geometry:active { cursor:grabbing; } +.fit-report { margin-top:12px; }.fit-report pre { max-height:320px; overflow:auto; font-size:11px; color:#aebed3; white-space:pre-wrap; overflow-wrap:anywhere; } +@media (max-width:700px) { .fit-grid { grid-template-columns:1fr; }.fit-metrics { grid-template-columns:repeat(2,minmax(0,1fr)); }.fit-target,.fit-result-heading { align-items:flex-start; flex-direction:column; }.fit-inputs summary .muted { display:block; margin-left:15px; } } .editor-grid { display:grid; grid-template-columns:minmax(0,1fr) minmax(0,1fr) minmax(48px,.8fr); gap:10px; padding:0 17px; } .editor-grid label,.note-field { display:flex; flex-direction:column; gap:6px; font-size:11px; color:var(--muted); } .editor-grid input { width:100%; } @@ -154,3 +192,4 @@ dialog>p { font-size:11px; line-height:1.8; } @media (max-width:1100px) { main { padding:16px; } .app-header { padding:15px 18px; } .workspace { grid-template-columns:minmax(0,1fr) 320px; gap:12px; } .small { display:none; } .fps { display:none; } .transport { gap:12px; } .editor-actions kbd { display:none; } } @media (max-width:850px) { .app-header { grid-template-columns:auto minmax(0,1fr) auto; } .header-actions { grid-column:3; grid-row:1; } .header-shortcuts { grid-column:1/-1; justify-content:flex-start; } .workspace { grid-template-columns:1fr; } .annotations-card { position:static; max-height:none; min-height:350px; } .annotation-table-wrap { max-height:350px; } .app-header { gap:10px; } .header-actions { margin-left:auto; } .eyebrow { font-size:8px; } .brand h1 { font-size:16px; } .brand-mark { width:35px; height:35px; } .status-pill { margin-right:0; } .header-actions button { font-size:11px; } .app-footer>span:last-child { display:none; } .video-stage { max-height:55vh; } } @media (max-width:480px) { .app-header { grid-template-columns:auto minmax(0,1fr); } .header-actions { grid-column:1/-1; grid-row:auto; width:100%; justify-content:flex-end; } .editor-grid { grid-template-columns:1fr 1fr 1fr; gap:8px; } .label-field { grid-column:1/-1; } .editor-actions .spacer { display:none; } .timeline-legend>span:last-child { display:none; } .shortcut-list { grid-template-columns:140px 1fr; } #video-meta { display:none; } } +@media (max-width:850px) { .media-column,.annotation-column { display:contents; }.video-card { grid-row:1; }.timeline-card { grid-row:2; }.annotations-card { grid-row:3; }.skill-editor-card { grid-row:4; }.fitting-card { grid-row:5; } } diff --git a/examples/toolkits/skill_fitting/README.md b/examples/toolkits/skill_fitting/README.md new file mode 100644 index 0000000000..2eec7785c0 --- /dev/null +++ b/examples/toolkits/skill_fitting/README.md @@ -0,0 +1,129 @@ +# Video and joints to atomic motion + +Run in `py312`. The production command supports explicit `pick_up`, `slide`, +`twist` and `hand_over` motion goals. It needs no object poses, object meshes, +or running simulator. Existing annotation files select the segment; automatic +video contact recognition is not part of this command. + +## Reproduce the Pickup round trip + +```bash +conda activate py312 +embodichain run-example sim/atomic_skill --skill pickup --headless \ + --save-video --output-dir /tmp/pickup-source +embodichain run-example toolkits/skill_fitting \ + --replay-dir /tmp/pickup-source --output-dir /tmp/pickup-fit +``` + +The second command reads only the replay's joint commands and MP4. Its +sample-owned robot profile supplies the UR5 URDF, mounted PGI TCP, named arm/hand +bindings, open/grasp commands and explicit reference limits. It does not read the +replay JSON's sampled grasps or object information. The reference limits in this +example are demonstration settings, not calibrated hardware safety limits. + +Outputs are `robot_profile.json` and `pickup_fit.npz`. The command exits with +status 2 when a generated candidate fails the acceptance gates. Such results +retain `native_qpos` and `proposed_qpos`, with diagnostics; `patched_qpos` remains +the unchanged original reference. + +## Fit an annotated recording + +```bash +embodichain fit-trajectory \ + --video /data/episode.mp4 --joints /data/joints.npz \ + --annotations /data/episode.segments.json --segment-id seg_example \ + --skill pick_up --robot-profile /data/robot_profile.json \ + --qpos-semantics measured_state --time-offset 0 \ + --output /data/episode.pickup.fit.npz +``` + +Video intervals are zero-based and inclusive. The video adapter uses decoded +presentation timestamps; the command maps the last selected frame's display +duration into a half-open joint interval. `--time-offset` is the joint timestamp +of video time zero. The joint sampling clock is never inferred from video FPS. +Without an annotation file, `--start-frame` and `--end-frame` select an interval; +their defaults cover the full video. + +For existing HDF5 camera annotations, replace `--video` with `--hdf5` and +optionally select `--image-key` / `--fps`. This reuses the annotator's source +identity and explicitly identifies FPS-derived timing. + +Joint inputs: + +- NPZ: `qpos` shaped `(T,D)` and strictly increasing `timestamps` in seconds. + Optional `joint_names` allows explicit column reordering. +- HDF5: select numeric fields using `--qpos-key observations/qpos` and + `--timestamps-key observations/timestamps`. Missing timestamps require + `--joint-dt`; they are then declared uniformly sampled. +- Local Atomic PT replay: weights-only loading of the selected environment's + `positions` and cumulative `dt`, including recorded final holds. Use + `--qpos-key planned_positions` to explicitly select the planned clock. + +Declare whether the input is measured state, submitted commands or a design +reference. Reconstructed output is always a design reference. +Complete joint layouts currently include the configured hand columns. For a +hand whose recorded state does not expose an open/close event, a complete +`--phase-hints` partition can provide event indices. Arm-only recordings require +explicitly preparing the missing hand reference; the CLI does not invent an +observed aperture or closing command from RGB. + +`--robot-profile` is a closed JSON configuration. The generated example profile +shows the fields. Joint lists and participant bindings are explicit; custom +names are retained. Solver configurations use existing FK/IK solvers with an +explicit URDF, root/end links, TCP and fixed arm base poses. Paths resolve against +the profile file. Both arms of a handover need a calibrated common frame. + +`--fit-config` accepts the declared `SkillFitCfg` fields, including finite sample, +candidate and wall-clock budgets and independent joint/TCP/contact tolerances. +The wall-clock budget is cooperative between native plans, not a preemptive +native-solver timeout. + +## Phases and limitations + +The command uses hand opening/closure and FK to estimate phases. `--phase-hints` +can supply a JSON array of `{name, start, stop}` with half-open, segment-relative +joint indices. It must cover the whole selected segment contiguously. + +| Skill | Source phases | +| --- | --- | +| `pick_up` | `approach`, `close`, `lift` | +| pull `slide` | `approach`, `reach`, `close`, `pull`, `open` | +| push `slide` | `approach`, `reach`, `close`, `push`, `open`, `return` | +| `twist` | `approach`, `reach`, `close`, `twist`, `open`, `retract` | +| transfer-only `hand_over` | `transfer`, `approach`, `close`, `hold`, `release`, `retreat` | + +Handover optionally prepends `pickup_approach`, `pickup_close` and appends +`place`, `receive_release`, `receive_retreat`. Receiving closure must precede +source release. Participants are always explicitly bound. + +Pickup requires an observable stable-orientation approach and a world-Z lift. +Slide fits a translating rail; arbitrary tabletop sliding may have a different +skill contract. Twist needs sufficient rotation around one line; the stored +axis point is the closest point on that line to the initial TCP, not an object +origin. Unobservable or incompatible segments fail explicitly. Tests cover all +four skills using an independent Cartesian robot oracle; the recorded robot +round-trip is qualified on the UR5 Pickup example. + +## Standalone result + +The NPZ contains original/native/patched/proposed joints, physical timestamps, +source-derived monotone phase progress, raw native phase joints, modification +mask, and decimated FK link/TCP geometry. `metadata_json` stores parameters, +native options, phases, initial qpos, calibration/profile/annotation/source +identities, timing interpretation and independent native/patched diagnostics. +It uses numeric arrays and JSON, with pickle disabled. + +For plotting: + +```python +from embodichain.data_pipeline.skill_fitting import load_skill_fit + +arrays, report = load_skill_fit("/data/episode.pickup.fit.npz") +error = arrays["patched_qpos"] - arrays["original_qpos"] +``` + +All original joint samples are included; visualization geometry uses the saved +`geometry_indices`. Viewing does not require the source video, joint recording, +URDF, or simulator. The annotation website also runs this pipeline and displays +video-linked joint curves, errors, rotatable FK skeletons, TCP paths and geometric +error magnification. See the [web example](../trajectory_annotation/README.md). diff --git a/examples/toolkits/skill_fitting/__main__.py b/examples/toolkits/skill_fitting/__main__.py new file mode 100644 index 0000000000..79e23fcd05 --- /dev/null +++ b/examples/toolkits/skill_fitting/__main__.py @@ -0,0 +1,93 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Blind-fit the fixed-scene UR5/PGI Pickup replay using only its joints/video.""" + +from __future__ import annotations + +import argparse +import json +from pathlib import Path + +from embodichain.data import get_data_path +from examples.sim.atomic_skill.tcp_presets import UR5_PGI_GRASP_TCP + + +def main() -> None: + """Prepare the sample-owned robot calibration and invoke the production CLI.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--replay-dir", type=Path, required=True) + parser.add_argument( + "--output-dir", type=Path, default=Path("/tmp/embodichain-skill-fit") + ) + parser.add_argument("--device", default="cpu") + args = parser.parse_args() + directory = args.output_dir.resolve() + directory.mkdir(parents=True, exist_ok=True) + profile = { + "joint_names": [f"joint{i}" for i in range(1, 7)] + + ["gripper_finger1_joint_1", "gripper_finger2_joint_1"], + "control_parts": { + "arm": [f"joint{i}" for i in range(1, 7)], + "eef": ["gripper_finger1_joint_1"], + }, + "participants": {"primary": {"motion": "arm", "grasp": "eef"}}, + "solvers": { + "arm": { + "class_type": "URSolver", + "ur_type": "ur5", + "urdf_path": get_data_path("UniversalRobots/UR5/UR5.urdf"), + "root_link_name": "base_link", + "end_link_name": "ee_link", + "tcp": [list(row) for row in UR5_PGI_GRASP_TCP], + } + }, + "base_poses": {"arm": [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]}, + "hand_commands": {"eef": {"open": [0.0], "grasp": [0.036]}}, + "lower": [-6.283185307] * 6 + [0, -0.04], + "upper": [6.283185307] * 6 + [0.04, 0.04], + "velocity_limits": [3.15] * 6 + [0.5, 0.5], + "acceleration_limits": [12.0] * 6 + [40.0, 40.0], + "calibration_revision": "atomic-example-ur5-pgi-grasp-tcp-2026-11", + } + path = directory / "robot_profile.json" + path.write_text(json.dumps(profile, indent=2) + "\n") + from embodichain.toolkits.skill_fitting.cli import main as fit + + fit( + [ + "--video", + str(args.replay_dir / "00_pickup_cube_auto_play.mp4"), + "--joints", + str(args.replay_dir / "00_pickup_cube_auto_play.pt"), + "--robot-profile", + str(path), + "--skill", + "pickup", + "--time-offset", + "0", + "--qpos-semantics", + "actual_submission", + "--device", + args.device, + "--output", + str(directory / "pickup_fit.npz"), + ] + ) + + +if __name__ == "__main__": + main() diff --git a/examples/toolkits/trajectory_annotation/README.md b/examples/toolkits/trajectory_annotation/README.md index 047124ebfb..2801163886 100644 --- a/examples/toolkits/trajectory_annotation/README.md +++ b/examples/toolkits/trajectory_annotation/README.md @@ -28,6 +28,7 @@ embodichain run-example toolkits/trajectory_annotation \ - `--output-dir`:可选,默认 `outputs/trajectory_annotation`;首次保存时自动创建目录。 - JSON 文件名自动使用 `.<相机名>.segments.json`。例如 `episode.hdf5`、`cam_high` 对应 `episode.cam_high.segments.json`。 - `--annotations`:也可直接指定完整 JSON 路径,与显式传入 `--output-dir` 二选一。 +- `--video`:使用本地视频替代 HDF5 输入;JSON 文件名为 `<视频名>.segments.json`。 可进一步指定相机和帧率: @@ -50,6 +51,36 @@ embodichain annotate-trajectory \ 右侧显示全部区间,点击帧号跳转,选中后 Delete 删除,Ctrl+Z 撤销,Ctrl+S 保存。 帧号从 0 开始,起止均包含;例如 `[100, 150]` 包含 51 帧。HDF5 的时间显示按 FPS 推算。 +## 在网页中重建并回填 Pickup + +先生成 UR5 / PGI 的示例录像和对应机器人配置(已有这些文件时可直接执行第三条命令): + +```bash +embodichain run-example sim/atomic_skill --skill pickup --headless \ + --save-video --output-dir /tmp/pickup-source +embodichain run-example toolkits/skill_fitting \ + --replay-dir /tmp/pickup-source --output-dir /tmp/pickup-fit + +embodichain run-example toolkits/trajectory_annotation \ + --video /tmp/pickup-source/00_pickup_cube_auto_play.mp4 \ + --joints /tmp/pickup-source/00_pickup_cube_auto_play.pt \ + --robot-profile /tmp/pickup-fit/robot_profile.json \ + --qpos-semantics actual_submission --time-offset 0 \ + --output-dir /tmp/pickup-labels +``` + +在网页中用 **I / O** 选中完整 Pickup,命名并提交,选择拟合技能 **Pickup**,点击 +**“拟合并回填选中区间”**。会显示通过/拒绝状态、参数和误差,以及与视频联动的关节曲线、 +可旋转的 FK 骨架和 TCP 路径;可下载包含完整关节轨迹的 NPZ。此示例辨识出的 `lift_height` 约为 **0.16 m**。 + +同一面板也支持 Slide、Twist、HandOver。任意真实记录需提供自己的机器人配置,示例 UR5 配置不能直接用于 Piper。 +`--joints`、`--robot-profile`、`--qpos-semantics`、`--time-offset` 可省略后在网页中填写; +`--qpos-key`、`--timestamps-key`、`--joint-dt`、`--env-index`、`--phase-hints`、`--fit-config` 也可预填高级设置。 +默认 HDF5 标注流程保持可独立使用。 + +每次仅回填当前选区,以原始关节记录为基准。未通过校验时回填数组保持原始值,并保留候选; +修改标注后需重新拟合再下载。完整数据格式与边界说明见[工具说明](../../../embodichain/toolkits/trajectory_annotator/README.md)。 + ## 输入已有图像数组 ```bash diff --git a/examples/toolkits/trajectory_annotation/__main__.py b/examples/toolkits/trajectory_annotation/__main__.py index e58017dbfc..bef2e43276 100644 --- a/examples/toolkits/trajectory_annotation/__main__.py +++ b/examples/toolkits/trajectory_annotation/__main__.py @@ -14,12 +14,13 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Annotate the supplied cam_high HDF5 demo through the common frame interface. +"""Annotate image frames and fit selected joint segments in the same local website. Run from the repository root in conda environment py312: embodichain run-example toolkits/trajectory_annotation Override --hdf5 and --output-dir for another recording and output directory. Use --image-key or --fps to override camera selection and timing. Ctrl+C stops the app. +Alternatively pass --video, --joints and --robot-profile for offline skill fitting. """ from __future__ import annotations @@ -28,7 +29,10 @@ from pathlib import Path from embodichain.toolkits.trajectory_annotator.app import launch_annotation -from embodichain.toolkits.trajectory_annotator.hdf5 import Hdf5FrameSource +from embodichain.toolkits.trajectory_annotator._cli import ( + _add_fitting_arguments, + _fitting_options, +) def main() -> None: @@ -38,12 +42,13 @@ def main() -> None: description=__doc__, formatter_class=argparse.ArgumentDefaultsHelpFormatter, ) - parser.add_argument( + inputs = parser.add_mutually_exclusive_group() + inputs.add_argument( + "--video", type=Path, help="本地视频,作为 HDF5 图像序列的替代输入。" + ) + inputs.add_argument( "--hdf5", type=Path, - default=Path( - "/home/oem/piper-sim-data/label_down/episode_20261022T073344_323022Z/episode.hdf5" - ), help="HDF5 recording; omit to use the supplied local demo.", ) parser.add_argument( @@ -66,23 +71,45 @@ def main() -> None: ) parser.add_argument("--port", type=int, default=0) parser.add_argument("--no-browser", action="store_true") + _add_fitting_arguments(parser) args = parser.parse_args() - if not args.hdf5.is_file(): - parser.error(f"HDF5 file does not exist: {args.hdf5}") + if args.video is None and args.hdf5 is None: + args.hdf5 = Path( + "/home/oem/piper-sim-data/label_down/episode_20261022T073344_323022Z/episode.hdf5" + ) + source_path = args.video or args.hdf5 + if not source_path.is_file(): + parser.error(f"Input file does not exist: {source_path}") + if args.video and (args.image_key is not None or args.fps is not None): + parser.error("--image-key and --fps are only valid with HDF5.") if args.image_key is not None and not args.image_key.strip("/"): parser.error("--image-key must name an image dataset.") if args.annotations is None: if args.output_dir.exists() and not args.output_dir.is_dir(): parser.error(f"--output-dir must be a directory: {args.output_dir}") camera = (args.image_key or "cam_high").strip("/").rsplit("/", 1)[-1] - annotations = args.output_dir / f"{args.hdf5.stem}.{camera}.segments.json" + name = f"{source_path.stem}.{camera}" if args.hdf5 else source_path.stem + annotations = args.output_dir / f"{name}.segments.json" else: annotations = args.annotations + if args.hdf5: + from embodichain.toolkits.trajectory_annotator.hdf5 import Hdf5FrameSource + + def factory() -> Hdf5FrameSource: + return Hdf5FrameSource(args.hdf5, image_key=args.image_key, fps=args.fps) + + else: + from embodichain.toolkits.trajectory_annotator.video import VideoFrameSource + + def factory() -> VideoFrameSource: + return VideoFrameSource(args.video) + launch_annotation( - lambda: Hdf5FrameSource(args.hdf5, image_key=args.image_key, fps=args.fps), + factory, annotations, port=args.port, open_browser=not args.no_browser, + fitting_options=_fitting_options(args), ) diff --git a/tests/compute/test_trajectory_splicing.py b/tests/compute/test_trajectory_splicing.py new file mode 100644 index 0000000000..6cfa812bc8 --- /dev/null +++ b/tests/compute/test_trajectory_splicing.py @@ -0,0 +1,100 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import numpy as np +import pytest + +from embodichain.compute.trajectory.phase_alignment import align_joint_phase +from embodichain.compute.trajectory.splicing import ( + splice_joint_segment, + joint_path_diagnostics, +) + + +def test_splice_retains_external_values_and_boundary_derivative_stencils() -> None: + times = np.linspace(0, 2, 101) ** 1.3 + source = np.column_stack((times**2, np.cos(times))).astype(np.float32) + replacement = source[10:90].copy() + replacement[:, 0] += 0.1 + output = splice_joint_segment( + source, + replacement, + times, + start=10, + stop=90, + joint_indices=(0,), + blend_samples=12, + anchors=(50,), + ) + assert output.dtype == source.dtype + np.testing.assert_array_equal(output[:13], source[:13]) + np.testing.assert_array_equal(output[87:], source[87:]) + np.testing.assert_array_equal(output[:, 1], source[:, 1]) + assert output[50, 0] == replacement[40, 0] + # Independent physical-time derivative check across both outer joins. + for values in (source, output): + velocity = np.gradient(values.astype(float), times, axis=0, edge_order=2) + acceleration = np.gradient(velocity, times, axis=0, edge_order=2) + if values is source: + expected = acceleration[[9, 10, 89, 90]] + else: + np.testing.assert_allclose( + acceleration[[9, 10, 89, 90]], expected, atol=1e-10 + ) + + +def test_splice_rejects_a_connection_that_overwrites_a_contact_anchor() -> None: + source = np.zeros((40, 1)) + with pytest.raises(ValueError, match="overlaps a protected"): + splice_joint_segment( + source, + np.ones((30, 1)), + np.arange(40.0), + start=5, + stop=35, + joint_indices=(0,), + anchors=(9,), + ) + + +def test_derivative_gate_detects_a_single_step_jump() -> None: + path = np.zeros((20, 1)) + path[10] = 1 + result = joint_path_diagnostics( + path, + np.arange(20) * 0.01, + lower=np.array([-2]), + upper=np.array([2]), + velocity_limits=np.array([10]), + acceleration_limits=np.array([1000]), + ) + assert not result["accepted"] + assert result["peak_velocity"] == pytest.approx([100]) + assert result["peak_acceleration"] == pytest.approx([20000]) + + +def test_phase_clock_changes_timing_without_changing_the_native_path() -> None: + native = np.column_stack((np.linspace(0, 1, 101), np.zeros(101))) + progress = np.linspace(0, 1, 51) ** 2 + observed = np.column_stack((progress, np.full(51, 0.1))) + fitted, clock = align_joint_phase(native, observed) + np.testing.assert_allclose(fitted[:, 0], progress, atol=1e-12) + np.testing.assert_array_equal(fitted[:, 1], 0) + assert np.all(np.diff(clock) >= 0) + # An incompatible geometric displacement remains observable. + np.testing.assert_allclose(np.linalg.norm(fitted - observed, axis=1), 0.1) diff --git a/tests/data_pipeline/test_skill_fitting.py b/tests/data_pipeline/test_skill_fitting.py new file mode 100644 index 0000000000..e73c0a4259 --- /dev/null +++ b/tests/data_pipeline/test_skill_fitting.py @@ -0,0 +1,94 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from pathlib import Path +import zipfile +import io + +import numpy as np +import pytest + +from embodichain.data_pipeline.skill_fitting import ( + JointRecording, + load_joint_recording, + map_video_interval, + save_skill_fit, + load_skill_fit, +) + + +def test_inclusive_vfr_segment_uses_pts_offset_and_last_frame_duration() -> None: + # Frames cover [10,10.04), [10.04,10.14), [10.14,10.20) in joint time. + start, stop = map_video_interval( + np.array([0, 0.04, 0.14]), + np.array([0.04, 0.10, 0.06]), + np.arange(1001) * 0.02, + start_frame=1, + end_frame=2, + time_offset=10, + ) + assert (start, stop) == (502, 510) + + +def test_joint_loader_reorders_named_columns_without_guessing_time( + tmp_path: Path, +) -> None: + path = tmp_path / "joints.npz" + q = np.column_stack((np.arange(20), np.arange(20) + 100)).astype(float) + np.savez(path, qpos=q, joint_names=np.array(["b", "a"])) + with pytest.raises(ValueError, match="Missing joint timestamps"): + load_joint_recording(path, joint_names=("a", "b"), semantics="measured_state") + source = load_joint_recording( + path, joint_names=("a", "b"), semantics="measured_state", sample_dt=0.1 + ) + np.testing.assert_array_equal(source.qpos, q[:, ::-1]) + assert not source.qpos.flags.writeable + + +def test_fit_bundle_loads_without_any_source_files(tmp_path: Path) -> None: + output = tmp_path / "fit.npz" + q = np.arange(60, dtype=float).reshape(20, 3) + arrays = { + "timestamps": np.arange(20) * 0.1, + "original_qpos": q, + "native_qpos": q + 1, + "patched_qpos": q + 2, + } + save_skill_fit(output, arrays, {"schema_version": 1, "physical_success": "unknown"}) + loaded, metadata = load_skill_fit(output) + np.testing.assert_array_equal(loaded["patched_qpos"], q + 2) + assert metadata["physical_success"] == "unknown" + + +def test_npz_header_budget_is_checked_before_materializing_arrays( + tmp_path: Path, +) -> None: + path = tmp_path / "bad.npz" + header = io.BytesIO() + np.lib.format.write_array_header_1_0( + header, {"descr": " None: + with pytest.raises(ValueError, match="clock"): + JointRecording(np.zeros(20), np.zeros((20, 1)), ("x",), "actual_submission") diff --git a/tests/sim/atomic_skills/test_actions.py b/tests/sim/atomic_skills/test_actions.py index 8c6ca4f048..68eee7b0c0 100644 --- a/tests/sim/atomic_skills/test_actions.py +++ b/tests/sim/atomic_skills/test_actions.py @@ -54,8 +54,10 @@ EntityState, ExecutionEventKind, GraspGoal, + PickUpMotionGoal, HandOver, HandOverGoal, + HandOverMotionGoal, HandOverOptions, HeldObjectPoseGoal, HeldObjectState, @@ -97,6 +99,7 @@ SlideAffordance, Slide, SlideGoal, + SlideMotionGoal, SlideJointTarget, SlideOptions, RobotObservation, @@ -107,6 +110,7 @@ TwistAffordance, Twist, TwistGoal, + TwistMotionGoal, TwistOptions, ) from embodichain.lab.sim.atomic_skills.goals import collect_scene_dependencies @@ -937,19 +941,72 @@ def _plan_segment_contract_case(case_id: str) -> SkillPlan: def test_builtin_descriptors_expose_strict_goal_types() -> None: assert MoveEndEffector.GoalType is EndEffectorPoseGoal assert MoveJoints.GoalType is JointPositionGoal - assert PickUp.GoalType is GraspGoal + assert PickUp.GoalType == (GraspGoal, PickUpMotionGoal) assert AxisAlign.GoalType is AxisAlignGoal assert MoveHeldObject.GoalType is HeldObjectPoseGoal assert Place.GoalType == (PlaceGoal, AssembleGoal) assert Pour.GoalType is PourGoal assert Press.GoalType is PressGoal assert PushObject.GoalType is PushObjectGoal - assert Slide.GoalType is SlideGoal + assert Slide.GoalType == (SlideGoal, SlideMotionGoal) assert OpenDoor.GoalType is OpenDoorGoal - assert Twist.GoalType is TwistGoal + assert Twist.GoalType == (TwistGoal, TwistMotionGoal) assert CoordinatedPickment.GoalType is CoordinatedPickGoal assert CoordinatedPlacement.GoalType is CoordinatedPlacementGoal - assert HandOver.GoalType is HandOverGoal + assert HandOver.GoalType == (HandOverGoal, HandOverMotionGoal) + + +@pytest.mark.parametrize("kind", ("pick_up", "slide", "twist", "hand_over")) +def test_explicit_motion_goals_plan_without_scene_or_physical_effects( + kind: str, +) -> None: + pose = torch.eye(4) + if kind == "hand_over": + action = _bind_action(_dual_motion_generator(), HandOver()) + context = _dual_context() + binding = _dual_binding(action, "source", "destination") + goal = HandOverMotionGoal(pose, pose, pose) + options = HandOverOptions(arm_selection="bound", release_at_target=False) + else: + action = _bind_action( + _motion_generator(), + { + "pick_up": PickUp, + "slide": Slide, + "twist": Twist, + }[kind](), + ) + context = _context() + binding = _binding(action) + goal = { + "pick_up": lambda: PickUpMotionGoal(pose), + "slide": lambda: SlideMotionGoal(pose, torch.tensor([1.0, 0.0, 0.0])), + "twist": lambda: TwistMotionGoal( + pose, torch.tensor([0.0, 0.0, 1.0]), (0.0, 0.0, 0.0) + ), + }[kind]() + options = { + "pick_up": PickUpOptions, + "slide": SlideOptions, + "twist": TwistOptions, + }[kind]() + plan = _plan_action( + action, + SkillInvocation( + skill_id=kind, + goal=goal, + binding=binding, + skill_options=options, + motion_policy=MotionPolicy(sample_count=100), + ), + context, + ) + assert plan.plan_success.all() + assert not plan.scene_dependencies + assert not plan.expected_effects.held_object_updates + assert not plan.effect_candidates.held_object_updates + assert plan.diagnostics.metadata["grounding"] == "explicit_tcp" + assert plan.joint_trajectory.robot_dof == context.robot.robot_dof @pytest.mark.parametrize( diff --git a/tests/sim/atomic_skills/test_fitting.py b/tests/sim/atomic_skills/test_fitting.py new file mode 100644 index 0000000000..aa758114a2 --- /dev/null +++ b/tests/sim/atomic_skills/test_fitting.py @@ -0,0 +1,311 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Independent Cartesian robot oracle for the inverse/native/splice contract.""" + +from __future__ import annotations + +from types import SimpleNamespace + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation +import torch + +from embodichain.data_pipeline.skill_fitting import JointRecording +from embodichain.lab.sim.atomic_skills import ( + AtomicSkillEngine, + ControlPartCommandProfile, +) +from embodichain.lab.sim.atomic_skills.fitting import fit_skill_segment, SkillFitCfg +from embodichain.lab.sim.motion.motion_generator import MotionGenerator, MotionGenCfg +from embodichain.lab.sim.motion.planners import ( + TrapezoidalPlanner, + TrapezoidalPlannerCfg, +) + + +class CartesianRobot: + """Six independent pose coordinates and one hand, with analytic FK/IK.""" + + device = torch.device("cpu") + dof = 7 + num_instances = 1 + joint_names = tuple(f"coordinate{i}" for i in range(7)) + control_parts = {"tool_motion": range(6), "fingers": [6]} + mimic_ids = () + mimic_parents = () + mimic_multipliers = () + + def get_joint_ids(self, name=None): + return list(range(self.dof)) if name is None else list(self.control_parts[name]) + + def get_qpos(self, name=None): + return torch.zeros(1, len(self.get_joint_ids(name))) + + def get_qpos_limits(self, name=None, joint_ids=None): + ids = self.get_joint_ids(name) if joint_ids is None else joint_ids + return torch.tensor([[-10.0, 10.0]] * len(ids))[None] + + def get_qvel_limits(self, name=None, joint_ids=None): + ids = self.get_joint_ids(name) if joint_ids is None else joint_ids + return torch.full((1, len(ids)), 100.0) + + def get_solver(self, name): + return SimpleNamespace(supports_continuous_batch_ik=False) + + def compute_fk(self, qpos, name, to_matrix=True, **kwargs): + result = torch.eye(4).repeat(len(qpos), 1, 1) + result[:, :3, 3] = qpos[:, :3] + result[:, :3, :3] = torch.tensor( + Rotation.from_rotvec(qpos[:, 3:6].numpy()).as_matrix(), dtype=torch.float32 + ) + return result + + def compute_batch_fk(self, qpos, name, to_matrix=True, **kwargs): + return self.compute_fk(qpos.reshape(-1, 6), name).reshape( + *qpos.shape[:-1], 4, 4 + ) + + def compute_ik(self, pose, name, joint_seed, **kwargs): + qpos = torch.cat( + ( + pose[:, :3, 3], + torch.tensor( + Rotation.from_matrix(pose[:, :3, :3].numpy()).as_rotvec(), + dtype=torch.float32, + ), + ), + dim=-1, + ) + return torch.ones(len(pose), dtype=torch.bool), qpos + + def compute_batch_ik(self, pose, name, joint_seed, **kwargs): + valid, q = self.compute_ik(pose.reshape(-1, 4, 4), name, joint_seed) + return valid.reshape(pose.shape[:2]), q.reshape(*pose.shape[:2], 6) + + +def _engine(): + cfg = TrapezoidalPlannerCfg(robot_uid="analytic_cartesian_oracle") + planner = TrapezoidalPlanner(cfg, robot=CartesianRobot()) + generator = MotionGenerator(MotionGenCfg(planner_cfg=cfg), planner=planner) + return AtomicSkillEngine( + generator, + control_profiles={ + "fingers": ControlPartCommandProfile.joint_positions( + open=torch.tensor([0.0]), grasp=torch.tensor([0.03]) + ) + }, + ) + + +def _pickup(): + # Independent source: a vertical approach, linear closure, 12 cm lift. + q = np.zeros((150, 7), dtype=np.float32) + q[:, 0] = 0.2 + q[:, 2] = np.r_[ + np.full(6, 0.2), + np.linspace(0.2, 0.05, 61), + np.full(13, 0.05), + np.linspace(0.05, 0.17, 61), + np.full(9, 0.17), + ] + q[:, 6] = np.r_[np.zeros(67), np.linspace(0, 0.03, 13), np.full(70, 0.03)] + return JointRecording( + np.arange(150) * 0.01, q, CartesianRobot.joint_names, "design_reference" + ) + + +@pytest.mark.parametrize("accept", (True, False)) +def test_pickup_inverse_native_and_splice_preserve_source_and_report_rejection( + accept, +) -> None: + recording = _pickup() + before = recording.qpos.copy() + arrays, report = fit_skill_segment( + recording, + _engine(), + skill_id="pick_up", + endpoints={"primary": {"motion": "tool_motion", "grasp": "fingers"}}, + start=6, + stop=141, + acceleration_limits=np.full(7, 1000.0 if accept else 0.001), + cfg=SkillFitCfg(max_candidates=1), + ) + assert report["parameters"]["lift_height"] == pytest.approx(0.12, abs=1e-6) + assert report["accepted"] == accept + assert report["physical_success"] == "unknown" + np.testing.assert_array_equal(recording.qpos, before) + np.testing.assert_array_equal(arrays["patched_qpos"][:9], before[:9]) + np.testing.assert_array_equal(arrays["patched_qpos"][138:], before[138:]) + assert "phase_progress" in arrays + if not accept: + np.testing.assert_array_equal(arrays["patched_qpos"], before) + assert report["failures"] + assert report["limits"]["acceleration_ratio"] > 1 + assert np.ptp(arrays["native_phase_qpos"][:, 2]) > 0.1 + + +@pytest.mark.parametrize("skill", ("slide", "twist")) +def test_axis_skills_identify_geometric_parameters_and_use_native_motion(skill) -> None: + engine = _engine() + # Known rigid source path, independent of the skill's planner code. + grasp = np.array([0.2, 0, 0.05, 2.0, 0, 0]) + rotation = Rotation.from_rotvec(grasp[3:]).as_matrix() + pre = grasp.copy() + pre[:3] -= rotation[:, 2] * 0.1 + if skill == "slide": + # Native pull approaches against the positive rail, then moves negative. + pre = grasp.copy() + pre[0] -= 0.1 + end = grasp.copy() + end[0] -= 0.15 + contact = np.linspace(grasp, end, 61) + else: + angle = np.linspace(0, 0.6, 61) + contact = np.repeat(grasp[None], 61, axis=0) + contact[:, 3:] = Rotation.from_matrix( + Rotation.from_rotvec(angle[:, None] * np.array([0, 0, 1])).as_matrix() + @ rotation + ).as_rotvec() + end = contact[-1] + arm = np.concatenate( + ( + np.repeat(pre[None], 6, axis=0), + np.linspace(pre, grasp, 61), + np.repeat(grasp[None], 13, axis=0), + contact, + np.repeat(end[None], 13, axis=0), + ) + ) + hand = np.r_[ + np.zeros(67), + np.linspace(0, 0.03, 13), + np.full(61, 0.03), + np.linspace(0.03, 0, 13), + ] + if skill == "twist": + arm = np.concatenate((arm, np.linspace(end, pre, 61))) + hand = np.r_[hand, np.zeros(61)] + arm = np.concatenate((arm, np.repeat(arm[-1:], 9, axis=0))) + hand = np.r_[hand, np.zeros(9)] + q = np.column_stack((arm, hand)).astype(np.float32) + source = JointRecording( + np.arange(len(q)) * 0.01, q, CartesianRobot.joint_names, "design_reference" + ) + arrays, report = fit_skill_segment( + source, + engine, + skill_id=skill, + endpoints={"primary": {"motion": "tool_motion", "grasp": "fingers"}}, + start=0, + stop=len(q), + acceleration_limits=np.full(7, 1000.0), + cfg=SkillFitCfg(max_candidates=1), + ) + assert report["accepted"], report["failures"] + parameter, expected = ( + ("translation_distance", 0.15) if skill == "slide" else ("twist_angle", 0.6) + ) + assert report["parameters"][parameter] == pytest.approx(expected, abs=1e-5) + assert report["native_joint_rmse"] < 0.01 + np.testing.assert_array_equal(arrays["patched_qpos"][:3], q[:3]) + + +def test_handover_identifies_both_participants_without_object_attachment() -> None: + class DualCartesianRobot(CartesianRobot): + dof = 14 + joint_names = tuple(f"coordinate{i}" for i in range(14)) + control_parts = { + "sender_motion": range(6), + "sender_hand": [6], + "receiver_motion": range(7, 13), + "receiver_hand": [13], + } + + robot = DualCartesianRobot() + cfg = TrapezoidalPlannerCfg(robot_uid="dual_cartesian_oracle") + planner = TrapezoidalPlanner(cfg, robot=robot) + engine = AtomicSkillEngine( + MotionGenerator(MotionGenCfg(planner_cfg=cfg), planner=planner), + control_profiles={ + part: ControlPartCommandProfile.joint_positions( + open=torch.tensor([0.0]), grasp=torch.tensor([0.03]) + ) + for part in ("sender_hand", "receiver_hand") + }, + ) + state = np.zeros(14) + state[:3], state[7:10], state[6] = [0.2, 0, 0.2], [0.2, -0.3, 0.2], 0.03 + parts = [] + + def stage(indices, values, count): + nonlocal state + target = state.copy() + target[indices] = values + path = np.linspace(state, target, count) + parts.append(path) + state = target + + stage(slice(0, 3), [0.2, 0, 0.3], 40) + stage(slice(7, 10), [0.2, 0, 0.2], 20) + stage(slice(7, 10), [0.2, 0, 0.3], 20) + stage(13, 0.03, 13) + stage(13, 0.03, 6) + stage(6, 0.0, 13) + stage(slice(0, 3), [0.2, -0.2, 0.3], 40) + stage(6, 0.0, 9) + q = np.concatenate(parts).astype(np.float32) + source = JointRecording( + np.arange(len(q)) * 0.01, q, robot.joint_names, "design_reference" + ) + _, report = fit_skill_segment( + source, + engine, + skill_id="hand_over", + endpoints={ + "source": {"motion": "sender_motion", "grasp": "sender_hand"}, + "destination": {"motion": "receiver_motion", "grasp": "receiver_hand"}, + }, + start=0, + stop=len(q), + acceleration_limits=np.full(14, 1000.0), + cfg=SkillFitCfg(max_candidates=1), + ) + assert report["accepted"], report["failures"] + assert report["physical_success"] == "unknown" + np.testing.assert_allclose( + report["parameters"]["relative_exchange"], np.eye(4), atol=1e-6 + ) + + +def test_moving_arm_during_closure_is_rejected_as_template_mismatch() -> None: + original = _pickup() + q = original.qpos.copy() + q[70:77, 0] += 0.03 + source = JointRecording( + original.timestamps, q, original.joint_names, original.semantics + ) + with pytest.raises(ValueError, match="stationary arm during closure"): + fit_skill_segment( + source, + _engine(), + skill_id="pick_up", + endpoints={"primary": {"motion": "tool_motion", "grasp": "fingers"}}, + start=6, + stop=141, + acceleration_limits=np.full(7, 1000.0), + ) diff --git a/tests/test_main.py b/tests/test_main.py index b352d28079..146a7afab8 100644 --- a/tests/test_main.py +++ b/tests/test_main.py @@ -42,6 +42,7 @@ "decompose-urdf", "eval-policy", "expansion", + "fit-trajectory", "list-robot", "list-task", "show-task", diff --git a/tests/toolkits/trajectory_annotator/conftest.py b/tests/toolkits/trajectory_annotator/conftest.py index 135b4c3190..5608facd8a 100644 --- a/tests/toolkits/trajectory_annotator/conftest.py +++ b/tests/toolkits/trajectory_annotator/conftest.py @@ -24,10 +24,78 @@ import socket import threading import time +from typing import Any import pytest +@pytest.fixture +def fitting_oracle(monkeypatch: pytest.MonkeyPatch) -> dict[str, Any]: + """Isolate host lifecycle from IK with known accepted/rejected numeric bundles. + + Native reconstruction itself is independently covered by the Cartesian robot + tests. This oracle protects web timing, background ownership and export. + """ + import numpy as np + from embodichain.toolkits.skill_fitting import _pipeline + + control: dict[str, Any] = dict( + accepted=True, entered=threading.Event(), release=threading.Event(), calls=[] + ) + control["release"].set() + + def fit(inputs: Any, **kwargs: Any) -> tuple[dict[str, np.ndarray], dict[str, Any]]: + control["calls"].append(kwargs) + control["entered"].set() + assert control["release"].wait( + timeout=10 + ), "Test did not release the fitting worker." + if control.get("error"): + raise ValueError("No observable closing event; provide phase hints.") + # A 3 s recording, 100 Hz, with two known reference columns. + times = np.arange(300, dtype=np.float64) / 100 + original = np.column_stack((times / 10, -times / 20)) + native = original.copy() + native[20:84, 0] += 0.02 + proposed = original.copy() + proposed[23:81, 0] += 0.01 + patched = proposed if control["accepted"] else original.copy() + report = dict( + schema_version=1, + skill_id=kwargs["skill_id"], + accepted=control["accepted"], + failures=( + [] if control["accepted"] else ["patched path exceeds joint limits"] + ), + joint_names=["arm_joint", "hand_joint"], + start=20, + stop=84, + phases=[dict(name="approach", start=0, stop=64)], + parameters=dict(lift_height=0.12), + native_joint_rmse=0.02, + tcp_errors={"primary": {"native_tcp_rmse_m": 0.003}}, + time_offset=inputs.time_offset, + **kwargs["provenance"], + ) + arrays = dict(timestamps=times, geometry_indices=np.arange(len(times))) + for name, q in ( + ("original", original), + ("native", native), + ("patched", patched), + ("proposed", proposed), + ): + arrays[f"{name}_qpos"] = q + tcp = np.column_stack((q[:, 0], np.zeros(len(q)), 0.4 + q[:, 1])) + arrays[f"{name}_tcp"] = tcp[:, None, :] + arrays[f"{name}_links"] = np.stack( + (np.zeros_like(tcp), tcp / 2, tcp), axis=1 + )[:, None, :, :] + return arrays, report + + monkeypatch.setattr(_pipeline, "fit_recording", fit) + return control + + @pytest.fixture def make_video(tmp_path: Path) -> Callable[..., Path]: """Encode identifiable frames with B frames and optional VFR/nonzero PTS.""" diff --git a/tests/toolkits/trajectory_annotator/test_annotation.py b/tests/toolkits/trajectory_annotator/test_annotation.py index 1a321830db..1c0fe60dc3 100644 --- a/tests/toolkits/trajectory_annotator/test_annotation.py +++ b/tests/toolkits/trajectory_annotator/test_annotation.py @@ -340,3 +340,117 @@ def held(self: FrameReader, index: int) -> object: release.set() worker.close() assert worker.status()["phase"] == "closed" + + +@pytest.mark.parametrize("accepted", [True, False]) +def test_web_fitting_vfr_clock_concurrency_export_and_stale_revisions( + make_video: Callable[..., Path], + tmp_path: Path, + fitting_oracle: dict[str, object], + accepted: bool, +) -> None: + """A fit never owns decoding, blocks edits, exports rejection as success or outlives its annotation.""" + import io + import numpy as np + from fastapi.testclient import TestClient + from embodichain.toolkits.trajectory_annotator._server import create_app + from embodichain.toolkits.trajectory_annotator.video import VideoFrameSource + + video = make_video(variable=True, start=90) + fitting_oracle["accepted"] = accepted + fitting_oracle["release"].clear() + app = create_app(lambda: VideoFrameSource(video), tmp_path / "annotations.json") + with TestClient(app, base_url="http://127.0.0.1") as client: + deadline = time.monotonic() + 10 + while client.get("/api/status").json()["phase"] != "ready": + assert time.monotonic() < deadline + time.sleep(0.01) + headers = { + "X-Annotation-Token": client.get("/api/project").json()["session_token"] + } + selected = skill_interval(5, 20) + saved = client.put( + "/api/annotations", + json=dict(segments=[selected], expected_revision=0), + headers=headers, + ).json() + payload = dict( + segment_id=selected["id"], + expected_revision=saved["revision"], + skill_id="pick_up", + inputs=dict( + joints="known.npz", + robot_profile="robot.json", + qpos_semantics="measured_state", + time_offset=0, + ), + ) + assert client.post("/api/fitting/jobs", json=payload).status_code == 403 + assert ( + client.post( + "/api/fitting/jobs", + json={**payload, "expected_revision": 0}, + headers=headers, + ).status_code + == 409 + ) + assert ( + client.post( + "/api/fitting/jobs", + json={**payload, "inputs": {**payload["inputs"], "time_offset": None}}, + headers=headers, + ).status_code + == 422 + ) + response = client.post("/api/fitting/jobs", json=payload, headers=headers) + assert response.status_code == 202 + job = response.json()["job_id"] + try: + assert fitting_oracle["entered"].wait(timeout=3) + assert ( + client.post( + "/api/fitting/jobs", json=payload, headers=headers + ).status_code + == 409 + ) + assert client.get("/api/frames/47").status_code == 200 + assert client.get("/api/fitting").json()["phase"] == "running" + finally: + fitting_oracle["release"].set() + while (status := client.get("/api/fitting").json())["phase"] == "running": + assert time.monotonic() < deadline + time.sleep(0.01) + assert status["phase"] == "done" and status["accepted"] == accepted + # The source has nonzero PTS and an extra tick every five frames. FPS + # multiplication would instead yield 5/30 and 20/30, so it must fail here. + call = fitting_oracle["calls"][0] + np.testing.assert_allclose(call["frame_times"], [6 / 30, 24 / 30]) + assert call["frame_durations"][-1] == pytest.approx(1 / 30) + assert client.get("/api/frame-index?time_seconds=0.2").json() == {"index": 5} + assert client.get("/api/frame-index?time_seconds=nan").status_code == 422 + preview = client.get(f"/api/fitting/jobs/{job}/preview").json() + assert preview["report"]["parameters"]["lift_height"] == 0.12 + response = client.get(f"/api/fitting/jobs/{job}/bundle") + assert response.status_code == 200 + with np.load(io.BytesIO(response.content), allow_pickle=False) as bundle: + original, patched = bundle["original_qpos"], bundle["patched_qpos"] + np.testing.assert_array_equal(bundle["timestamps"], np.arange(300) / 100) + np.testing.assert_array_equal(patched[:23], original[:23]) + np.testing.assert_array_equal(patched[81:], original[81:]) + if accepted: + assert patched[50, 0] - original[50, 0] == pytest.approx(0.01) + else: + np.testing.assert_array_equal(patched, original) + assert not np.array_equal(bundle["proposed_qpos"], original) + changed = {**selected, "end_frame": 21} + assert ( + client.put( + "/api/annotations", + json=dict(segments=[changed], expected_revision=1), + headers=headers, + ).status_code + == 200 + ) + assert client.get("/api/fitting").json()["stale"] + for endpoint in ("bundle", "preview"): + assert client.get(f"/api/fitting/jobs/{job}/{endpoint}").status_code == 409 diff --git a/tests/toolkits/trajectory_annotator/test_browser.py b/tests/toolkits/trajectory_annotator/test_browser.py index 3d29563fa1..68c499659a 100644 --- a/tests/toolkits/trajectory_annotator/test_browser.py +++ b/tests/toolkits/trajectory_annotator/test_browser.py @@ -107,6 +107,65 @@ def create_skill_interval(page: Page) -> None: expect(page.locator("#skill-interval-count")).to_have_text("1") +def test_web_fitting_comparison_download_rejection_and_invalidation( + page: Page, + annotation_server: tuple[str, Path], + fitting_oracle: dict[str, Any], + tmp_path: Path, +) -> None: + """Exercise the new production web workflow without coupling browser tests to IK.""" + import numpy as np + from playwright.sync_api import expect + + fitting_oracle["accepted"] = False + open_project(page, annotation_server[0]) + expect(page.locator("#fit-run")).to_be_disabled() + create_skill_interval(page) + page.locator("#fit-joints").fill("known.npz") + page.locator("#fit-profile").fill("robot.json") + page.locator("#fit-semantics").select_option("measured_state") + page.locator("#fit-offset").fill("0") + page.locator("#fit-run").click() + expect(page.locator("#fit-state")).to_have_text("未通过校验", timeout=15000) + expect(page.locator("#fit-result")).to_be_visible() + expect(page.locator("#fit-proposed-legend")).to_be_visible() + expect(page.locator("#fit-message")).to_contain_text( + "patched path exceeds joint limits" + ) + page.locator("#fit-joint").select_option("1") + expect(page.locator("#fit-cursor")).to_contain_text("hand_joint") + page.locator("#fit-exaggeration").select_option("20") + chart = page.locator("#fit-joint-chart").bounding_box() + # The plot spans 0..2.99 s. This point is video t=.2 s, where the VFR + # recording's displayed frame is 5 (its nonzero first PTS is normalized). + x = chart["x"] + 62 + 0.2 / 2.99 * (chart["width"] - 76) + page.mouse.click(x, chart["y"] + 65) + expect(page.locator("#frame-current")).to_have_text("5") + with page.expect_download() as download: + page.locator("#fit-download").click() + destination = tmp_path / "fit.npz" + download.value.save_as(destination) + with np.load(destination, allow_pickle=False) as data: + np.testing.assert_array_equal(data["patched_qpos"], data["original_qpos"]) + page.locator("#edit").click() + page.locator("#end-frame").fill("6") + page.locator("#commit").click() + expect(page.locator("#fit-state")).to_have_text("标注已变化") + expect(page.locator("#fit-download")).to_have_attribute("aria-disabled", "true") + # An unobservable segment must surface the error and release the job guard, + # allowing a corrected configuration/selection to be fitted again. + fitting_oracle["error"] = True + page.locator("#fit-run").click() + expect(page.locator("#fit-state")).to_have_text("拟合失败") + expect(page.locator("#fit-message")).to_contain_text("No observable closing event") + expect(page.locator("#fit-run")).to_be_enabled() + fitting_oracle["error"], fitting_oracle["accepted"] = False, True + page.locator("#fit-run").click() + expect(page.locator("#fit-state")).to_have_text("通过校验") + expect(page.locator("#fit-proposed-legend")).to_be_hidden() + expect(page.locator("#fit-parameters")).to_contain_text("lift_height") + + def test_keyboard_edit_delete_undo_and_resume( page: Page, annotation_server: tuple[str, Path] ) -> None: From 5f6169bbf084fea6e4bf4f19166c4060570e1ae6 Mon Sep 17 00:00:00 2001 From: matafela Date: Thu, 12 Nov 2026 18:05:36 +0800 Subject: [PATCH 03/45] feat(toolkits): select skills and fit annotated core intervals --- .../topics/atomic-actions/atomic-actions.md | 6 +- embodichain/toolkits/skill_fitting/cli.py | 17 +++- .../toolkits/trajectory_annotator/README.md | 24 +++-- .../toolkits/trajectory_annotator/_fitting.py | 15 ++- .../toolkits/trajectory_annotator/_models.py | 15 ++- .../toolkits/trajectory_annotator/_server.py | 9 ++ .../toolkits/trajectory_annotator/_skills.py | 52 +++++++++++ .../toolkits/trajectory_annotator/_storage.py | 7 +- .../toolkits/trajectory_annotator/web/app.js | 60 ++++++++++-- .../trajectory_annotator/web/fitting.js | 17 +++- .../trajectory_annotator/web/index.html | 13 ++- .../trajectory_annotator/web/styles.css | 10 +- examples/toolkits/skill_fitting/README.md | 11 +++ .../toolkits/trajectory_annotation/README.md | 6 +- tests/sim/atomic_skills/test_fitting.py | 10 ++ .../trajectory_annotator/test_annotation.py | 92 ++++++++++++++++++- .../trajectory_annotator/test_browser.py | 57 +++++++++++- 17 files changed, 386 insertions(+), 35 deletions(-) create mode 100644 embodichain/toolkits/trajectory_annotator/_skills.py diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index b9f19b2484..27a241daf1 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -158,7 +158,11 @@ Video-selected reconstruction enters through `embodichain fit-trajectory` fitting panel (`toolkits/trajectory_annotator/_fitting.py`). Both hosts reuse `toolkits/skill_fitting/_pipeline.py`; the web host retains one background job and result, keeps decoding on its existing owner, and invalidates results after -annotation revisions change. Its bounded joint/FK preview follows video time; +annotation revisions change. Annotation choices cover all built-in skills without +importing simulation; only supported reconstruction types enter fitting. A saved +optional core interval separates loose annotation boundaries from reconstruction, +preserving surrounding motion; DemoSkill labels do not create executable assets. +Its bounded joint/FK preview follows video time; the downloadable NPZ retains full samples. See the [web example](../../../examples/toolkits/trajectory_annotation/README.md). `atomic_skills/fitting.py` identifies explicit diff --git a/embodichain/toolkits/skill_fitting/cli.py b/embodichain/toolkits/skill_fitting/cli.py index 8095c34c63..b88d4fd35c 100644 --- a/embodichain/toolkits/skill_fitting/cli.py +++ b/embodichain/toolkits/skill_fitting/cli.py @@ -83,6 +83,7 @@ def main(argv: Sequence[str] | None = None) -> None: args = parser.parse_args(argv) from embodichain.data_pipeline.skill_fitting import file_digest, save_skill_fit from embodichain.toolkits.trajectory_annotator._storage import AnnotationStore + from embodichain.toolkits.trajectory_annotator._skills import _canonical_skill_id from ._pipeline import FitInputs, fit_recording try: @@ -111,7 +112,7 @@ def main(argv: Sequence[str] | None = None) -> None: raise ValueError("Output must differ from every input file.") try: info = source.prepare(threading.Event(), lambda **_: None) - revision, selected_id = None, None + revision, selected_id, selected = None, None, None if args.annotations: store = AnnotationStore(args.annotations.resolve(), 8 * 1024**2) store.initialize(info) @@ -124,7 +125,8 @@ def main(argv: Sequence[str] | None = None) -> None: "--segment-id must select exactly one existing annotation." ) selected = matches[0] - first, last = selected["start_frame"], selected["end_frame"] + core = selected.get("core_interval", selected) + first, last = core["start_frame"], core["end_frame"] revision, selected_id = document["revision"], selected["id"] else: first = 0 if args.start_frame is None else args.start_frame @@ -172,6 +174,11 @@ def main(argv: Sequence[str] | None = None) -> None: skill = {"pickup": "pick_up", "handover": "hand_over"}.get( args.skill, args.skill ) + annotated_skill = ( + None if selected is None else _canonical_skill_id(selected["label"]) + ) + if annotated_skill is not None and annotated_skill != skill: + raise ValueError("The fitted skill must match the selected annotation.") arrays, report = fit_recording( inputs, skill_id=skill, @@ -185,6 +192,12 @@ def main(argv: Sequence[str] | None = None) -> None: annotation_revision=revision, annotation_id=selected_id, annotation_sha256=annotation_digest, + annotation_segment=selected, + annotation_video_interval=( + None + if selected is None + else {key: selected[key] for key in ("start_frame", "end_frame")} + ), ), ) source.check_unchanged() diff --git a/embodichain/toolkits/trajectory_annotator/README.md b/embodichain/toolkits/trajectory_annotator/README.md index c616c37425..89479158b7 100644 --- a/embodichain/toolkits/trajectory_annotator/README.md +++ b/embodichain/toolkits/trajectory_annotator/README.md @@ -86,7 +86,7 @@ embodichain annotate-trajectory --hdf5 /data/episode.hdf5 \ --qpos-semantics measured_state --time-offset 0 ``` -1. 提交并选中右侧的一个原子技能区间。标注名称可自由填写,拟合技能需在下拉框中明确指定。 +1. 在右侧下拉菜单选择原子技能并提交区间。菜单包含项目内置技能(含 DemoSkill),拟合面板自动跟随标注类型;当前只有 Pickup / Slide / Twist / HandOver 支持参数重建。 2. 填写关节记录、机器人配置、数据语义,以及视频零时刻对应的关节时间(秒)。不会从视频 FPS 推断关节采样周期。 3. 点击“拟合并回填选中区间”;页面先保存标注,再在后台执行拟合。拟合期间可继续逐帧查看视频。 4. 查看参数与校验报告,对比原始、原生技能(按源时钟对齐)、连续回填的关节曲线和误差。 @@ -97,7 +97,13 @@ embodichain annotate-trajectory --hdf5 /data/episode.hdf5 \ 完整阶段分区 JSON,以及 `SkillFitCfg` 覆盖项。也可用 `--phase-hints` / `--fit-config` 从 JSON 文件预填。 没有可辨识的夹爪开闭或完整阶段提示时会报告失败;RGB 不用于自动判断接触。 -每次拟合以输入的原始关节记录为基准,仅替换**选中的一个区间**,保留原始时间戳与区间外采样, +标注范围允许包含技能前后的过渡动作。在编辑区间中勾选“单独设置拟合核心区间”,填写标注范围内部的核心起止帧; +标注边界保持不变,网页和 `fit-trajectory --annotations ... --segment-id ...` 均仅重建核心区间, +保留前后过渡采样。没有设置核心区间时使用整个标注范围;核心区间仍须包含完整技能阶段。 +不规则示教可标记为 **DemoSkill**(`execute_demo_skill`)并保留原始轨迹;此标记本身不创建可执行 DemoSkill 资产, +也不执行参数重建。旧文件中的自由文本标签仍可读取、编辑,不会自动改名。 + +每次拟合以输入的原始关节记录为基准,仅替换**选中的一个核心区间**,保留原始时间戳与区间外采样, 多次拟合不会自动累积修改。候选未通过独立校验时仍可查看和下载,`patched_qpos` 保持原始值, `proposed_qpos` 保留被拒绝的校正候选。标注修订发生变化后旧结果会标记过期并阻止下载,需重新拟合。 输出语义为设计参考,报告中的物理成功与碰撞状态仍为未知/未验证。 @@ -132,9 +138,9 @@ launch_annotation( ## 标注 1. 用时间轴、帧号输入框或播放按钮定位原子技能起点,左右键逐帧精调。 -2. 按 **I** 设置起点,输入原子技能名称,例如“翻转包裹”。名称输入框中按 Enter 返回视频区。 +2. 按 **I** 设置起点,从下拉菜单选择原子技能,例如 Pickup、Twist 或 DemoSkill。选择后按 Enter 返回视频区。 3. 定位原子技能最后一帧,按 **O** 设置终点。 -4. 检查原子技能名称、起止帧和帧数,按 **Enter** 提交。已完成区间显示在时间轴和右侧列表中。 +4. 检查原子技能、起止帧和帧数,按 **Enter** 提交。已完成区间显示在时间轴和右侧列表中。 5. 选中列表中的标注,按 **E** 修改、Delete 删除,或点击起止帧跳转。 帧号从 **0** 开始,起止帧**均包含**。`[120, 185]` 包含 66 帧,对应 Python 切片 `frames[120:186]`。允许空隙和单帧原子技能,同一轨道的已完成区间不能重叠。 @@ -154,7 +160,7 @@ launch_annotation( | Home / End | 首帧 / 末帧 | | G | 输入帧号跳转 | | I / O | 将当前显示帧设为起点 / 终点 | -| L | 输入原子技能名称 | +| L | 选择原子技能 | | Enter / Esc | 提交 / 取消草稿 | | E | 编辑选中的标注 | | Delete / Backspace | 删除选中的标注 | @@ -168,12 +174,12 @@ launch_annotation( 提交后的修改在停止编辑 1 秒后自动保存。也可点击“保存”或按 Ctrl/Cmd+S。新项目在第一次保存前不创建文件;未提交草稿不写入 JSON。状态栏明确显示未保存、保存中、保存成功或失败。 -重新用同一命令打开数据会加载原标注。JSON 使用 `schema_version: 2`,包含 0 基/闭区间声明、与容器无关的 `source` 信息(类型、身份、帧数、尺寸、帧率、时间来源和适配器详情)、修订号,以及 `segments` 数组。界面与内部模型使用原子技能区间命名;保留现有 JSON/API 的 `segments` 字段和文件名后缀,以继续读取已有标注。数组元素示例: +重新用同一命令打开数据会加载原标注。JSON 使用 `schema_version: 2`(保存核心区间时使用 v3),包含 0 基/闭区间声明、与容器无关的 `source` 信息(类型、身份、帧数、尺寸、帧率、时间来源和适配器详情)、修订号,以及 `segments` 数组。界面与内部模型使用原子技能区间命名;保留现有 JSON/API 的 `segments` 字段和文件名后缀,以继续读取已有标注。数组元素示例: ```json { "id": "seg_2e841dc2-0e4c-4b37-8d97-1d064da7e8a1", - "label": "翻转包裹", + "label": "pick_up", "start_frame": 120, "end_frame": 185, "note": "可选备注" @@ -182,7 +188,9 @@ launch_annotation( 写入前校验,临时文件 fsync 后原子替换;失败时保留旧文件与页面修改。多标签页的旧修订和磁盘外部修改会被拒绝覆盖,可先“导出 JSON”保留当前已完成标注,再重新打开。导出副本不代表本地保存成功。 -文件路径相对于标注 JSON 保存。文件适配器用 SHA-256 绑定源内容,并将相机路径、流索引、帧率或颜色解释等选择纳入身份检查;移动文件后可显式传入新路径,相同内容仍可恢复。内存图像按像素、顺序和 FPS 计算身份。不同内容或选择不能套用已有标注。兼容旧版 `schema_version: 1` 的 `source_video` 文档,读取时只在内存中转换,下次成功保存才写入 v2。 +可选 `core_interval: {"start_frame": 120, "end_frame": 185}` 声明标注内部的拟合核心范围,同样使用绝对视频帧号和闭区间。其前后采样仍属于原标注,回填时保留原值。文件包含该字段时保存为 v3,旧版本读取器会显式拒绝该格式。 + +文件路径相对于标注 JSON 保存。文件适配器用 SHA-256 绑定源内容,并将相机路径、流索引、帧率或颜色解释等选择纳入身份检查;移动文件后可显式传入新路径,相同内容仍可恢复。内存图像按像素、顺序和 FPS 计算身份。不同内容或选择不能套用已有标注。兼容旧版 `schema_version: 1` 的 `source_video` 文档,读取时只在内存中转换,下次成功保存才写入 v2(含核心区间时为 v3)。 本工具生成区间标注文件。将视频帧映射到机器人控制 step、裁剪视频或切分关节轨迹需要后续消费程序显式处理;现有 recorder 的半开区间不能直接复用本工具的闭区间端点。 diff --git a/embodichain/toolkits/trajectory_annotator/_fitting.py b/embodichain/toolkits/trajectory_annotator/_fitting.py index c7f57dca09..3d99eafefe 100644 --- a/embodichain/toolkits/trajectory_annotator/_fitting.py +++ b/embodichain/toolkits/trajectory_annotator/_fitting.py @@ -35,6 +35,7 @@ from embodichain.toolkits.skill_fitting._pipeline import FitInputs from ._frames import FramePreview from ._models import ConflictError +from ._skills import RECONSTRUCTION_SKILLS, _canonical_skill_id __all__ = [] @@ -127,8 +128,13 @@ def start( matches = [s for s in document["segments"] if s["id"] == payload["segment_id"]] if len(matches) != 1: raise ConflictError("请先提交并保存选中的原子技能区间。") - if payload["skill_id"] not in ("pick_up", "slide", "twist", "hand_over"): + if payload["skill_id"] not in RECONSTRUCTION_SKILLS: raise ValueError("仅支持 pick_up、slide、twist、hand_over。") + annotated_skill = _canonical_skill_id(matches[0]["label"]) + if annotated_skill is not None and annotated_skill != payload["skill_id"]: + raise ValueError( + "拟合技能必须与选中标注一致;DemoSkill 等技能目前仅支持标注。" + ) raw = payload["inputs"] if not isinstance(raw, dict) or set(raw) - set(self.defaults): raise ValueError("拟合输入字段不符合格式。") @@ -138,7 +144,8 @@ def start( with self._lock: if self._closed or self._state["phase"] == "running": raise ConflictError("已有拟合正在运行,请等待当前任务完成。") - first, last = matches[0]["start_frame"], matches[0]["end_frame"] + core = matches[0].get("core_interval", matches[0]) + first, last = core["start_frame"], core["end_frame"] clock = interval(first, last) if self._directory is None: self._directory = tempfile.TemporaryDirectory( @@ -208,6 +215,10 @@ def _run( ).hexdigest(), annotation_digest_kind="canonical_document_json", annotation_segment=self._selection, + annotation_video_interval={ + key: self._selection[key] + for key in ("start_frame", "end_frame") + }, ), progress=self._progress, ) diff --git a/embodichain/toolkits/trajectory_annotator/_models.py b/embodichain/toolkits/trajectory_annotator/_models.py index 2e07356ebd..34169151f2 100644 --- a/embodichain/toolkits/trajectory_annotator/_models.py +++ b/embodichain/toolkits/trajectory_annotator/_models.py @@ -50,7 +50,7 @@ def validate_skill_intervals(value: object, frame_count: int) -> list[dict[str, raise ValueError( f"第 {position} 段缺少 id、label、start_frame 或 end_frame。" ) - if item.keys() - required - {"note"}: + if item.keys() - required - {"note", "core_interval"}: raise ValueError(f"第 {position} 段包含未知字段。") identifier, label, note = item["id"], item["label"], item.get("note", "") if ( @@ -82,6 +82,19 @@ def validate_skill_intervals(value: object, frame_count: int) -> list[dict[str, note=note, ) ) + if "core_interval" in item: + core = item["core_interval"] + if ( + not isinstance(core, dict) + or core.keys() != {"start_frame", "end_frame"} + or type(core["start_frame"]) is not int + or type(core["end_frame"]) is not int + or not start <= core["start_frame"] <= core["end_frame"] <= end + ): + raise ValueError( + f"第 {position} 段的拟合核心区间必须完整包含在标注区间内。" + ) + result[-1]["core_interval"] = dict(core) result.sort( key=lambda skill_interval: ( skill_interval["start_frame"], diff --git a/embodichain/toolkits/trajectory_annotator/_server.py b/embodichain/toolkits/trajectory_annotator/_server.py index 8854c1247f..2f6aca275e 100644 --- a/embodichain/toolkits/trajectory_annotator/_server.py +++ b/embodichain/toolkits/trajectory_annotator/_server.py @@ -39,6 +39,7 @@ from ._storage import AnnotationStore from ._frames import SupersededError, FrameWorker from ._fitting import FitSession +from ._skills import RECONSTRUCTION_SKILLS, SKILL_CHOICES from .frames import FrameSource __all__ = ["create_app"] @@ -133,6 +134,14 @@ async def project() -> dict[str, Any]: "annotation_path": str(store.path), "session_token": token, "autosave_ms": cfg.autosave_ms, + "skills": [ + dict( + id=identifier, + name=name, + can_fit=identifier in RECONSTRUCTION_SKILLS, + ) + for identifier, name in SKILL_CHOICES + ], } @app.get("/api/frames/{index}") diff --git a/embodichain/toolkits/trajectory_annotator/_skills.py b/embodichain/toolkits/trajectory_annotator/_skills.py new file mode 100644 index 0000000000..84997ea348 --- /dev/null +++ b/embodichain/toolkits/trajectory_annotator/_skills.py @@ -0,0 +1,52 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Annotation choices without importing simulation or optional fitting dependencies.""" + +from __future__ import annotations + +__all__ = [] + +# Annotation categories mirror BUILTIN_SKILL_TYPES. Listing them must also work +# in installations containing only the image annotation dependencies. +SKILL_CHOICES: tuple[tuple[str, str], ...] = ( + ("pick_up", "Pickup"), + ("twist", "Twist"), + ("slide", "Slide"), + ("hand_over", "HandOver"), + ("execute_demo_skill", "DemoSkill"), + ("place", "Place"), + ("pour", "Pour"), + ("move_end_effector", "MoveEndEffector"), + ("move_joints", "MoveJoints"), + ("move_held_object", "MoveHeldObject"), + ("axis_align", "AxisAlign"), + ("push_object", "PushObject"), + ("press", "Press"), + ("open_door", "OpenDoor"), + ("coordinated_pickment", "CoordinatedPickment"), + ("coordinated_placement", "CoordinatedPlacement"), +) +RECONSTRUCTION_SKILLS = frozenset({"pick_up", "slide", "twist", "hand_over"}) + + +def _canonical_skill_id(label: str) -> str | None: + """Recognize built-in names while leaving historical free-text labels intact.""" + key = label.casefold().replace("_", "").replace("-", "").replace(" ", "") + for identifier, name in SKILL_CHOICES: + if key in {identifier.replace("_", ""), name.casefold()}: + return identifier + return None diff --git a/embodichain/toolkits/trajectory_annotator/_storage.py b/embodichain/toolkits/trajectory_annotator/_storage.py index 79d71e32ad..e3d0f11d3c 100644 --- a/embodichain/toolkits/trajectory_annotator/_storage.py +++ b/embodichain/toolkits/trajectory_annotator/_storage.py @@ -110,12 +110,12 @@ def initialize(self, info: FrameSourceInfo) -> None: raise ValueError("标注文件字段不符合格式,不能覆盖原文件。") if ( type(version) is not int - or version not in {1, 2} + or version not in {1, 2, 3} or type(loaded["frame_index_base"]) is not int or loaded["frame_index_base"] != 0 or loaded["interval"] != "closed" ): - raise ValueError("只支持 schema_version=1/2、0 基帧号和闭区间。") + raise ValueError("只支持 schema_version=1/2/3、0 基帧号和闭区间。") original = loaded[source_key] if version == 1: matching = ( @@ -154,6 +154,7 @@ def initialize(self, info: FrameSourceInfo) -> None: if type(revision) is not int or revision < 0: raise ValueError("标注 revision 必须为非负整数。") document["revision"] = revision + document["schema_version"] = max(2, version) document["segments"] = validate_skill_intervals( loaded["segments"], info.frame_count ) @@ -206,6 +207,8 @@ def save( candidate["segments"] = validate_skill_intervals( skill_intervals, candidate["source"]["frame_count"] ) + if any("core_interval" in segment for segment in candidate["segments"]): + candidate["schema_version"] = 3 candidate["revision"] += 1 encoded = ( json.dumps(candidate, ensure_ascii=False, indent=2) + "\n" diff --git a/embodichain/toolkits/trajectory_annotator/web/app.js b/embodichain/toolkits/trajectory_annotator/web/app.js index fbd6a348c5..d5fb3f7063 100644 --- a/embodichain/toolkits/trajectory_annotator/web/app.js +++ b/embodichain/toolkits/trajectory_annotator/web/app.js @@ -167,9 +167,11 @@ function play() { function skillIntervalError(draft) { if (!draft) return ""; - if (!draft.label.trim()) return "请输入原子技能名称。"; + if (!draft.label.trim()) return "请选择原子技能。"; if (!Number.isInteger(draft.start_frame) || !Number.isInteger(draft.end_frame)) return "请设置整数起点和终点。"; if (!(0 <= draft.start_frame && draft.start_frame <= draft.end_frame && draft.end_frame <= maximum())) return `起止帧需满足 0 ≤ 起点 ≤ 终点 ≤ ${maximum()}。`; + const core = draft.core_interval; + if (core && (!Number.isInteger(core.start_frame) || !Number.isInteger(core.end_frame) || !(draft.start_frame <= core.start_frame && core.start_frame <= core.end_frame && core.end_frame <= draft.end_frame))) return "拟合核心区间必须完整包含在标注区间内。"; const conflict = state.skillIntervals.find((skillInterval) => skillInterval.id !== draft.id && draft.start_frame <= skillInterval.end_frame && skillInterval.start_frame <= draft.end_frame); return conflict ? `与“${conflict.label}” [${conflict.start_frame}, ${conflict.end_frame}] 重叠。` : ""; } @@ -182,11 +184,18 @@ function renderDraft(syncInputs = true) { $("end-frame").disabled = !draft; $("note").disabled = !draft; $("cancel-draft").disabled = !draft; + $("use-core").disabled = !draft || !Number.isInteger(draft.end_frame); + $("use-core").checked = !!draft?.core_interval; + $("core-fields").hidden = !draft?.core_interval; + $("core-start-frame").disabled = !draft?.core_interval; + $("core-end-frame").disabled = !draft?.core_interval; if (syncInputs) { $("label").value = draft?.label ?? state.lastLabel; $("start-frame").value = draft?.start_frame ?? ""; $("end-frame").value = draft?.end_frame ?? ""; $("note").value = draft?.note ?? ""; + $("core-start-frame").value = draft?.core_interval?.start_frame ?? ""; + $("core-end-frame").value = draft?.core_interval?.end_frame ?? ""; } const error = skillIntervalError(draft); $("draft-error").textContent = error; @@ -300,7 +309,7 @@ function renderSelection() { function renderSkillIntervals() { $("skill-interval-list").replaceChildren(); $("timeline-skill-intervals").replaceChildren(); - $("label-suggestions").replaceChildren(); + renderSkillChoices(); state.skillIntervals.forEach((skillInterval, index) => { const row = document.createElement("tr"); row.className = `annotation-row${state.selected === skillInterval.id ? " selected" : ""}`; @@ -340,9 +349,6 @@ function renderSkillIntervals() { block.title = `${skillInterval.label} [${skillInterval.start_frame}, ${skillInterval.end_frame}]`; $("timeline-skill-intervals").append(block); }); - [...new Set(state.skillIntervals.map((skillInterval) => skillInterval.label))].forEach((label) => { - const option = document.createElement("option"); option.value = label; $("label-suggestions").append(option); - }); $("empty-list").hidden = state.skillIntervals.length > 0; $("skill-interval-count").textContent = state.skillIntervals.length; const count = state.skillIntervals.reduce((total, skillInterval) => total + skillInterval.end_frame - skillInterval.start_frame + 1, 0); @@ -353,6 +359,32 @@ function renderSkillIntervals() { renderSelection(); } +function renderSkillChoices() { + const picker = $("label"); + const value = state.draft?.label ?? state.lastLabel; + const placeholder = document.createElement("option"); + placeholder.value = ""; + placeholder.textContent = "请选择原子技能"; + const options = state.project.skills.map((skill) => { + const option = document.createElement("option"); + option.value = skill.id; + option.textContent = skill.name; + return option; + }); + const ids = new Set(state.project.skills.map((skill) => skill.id)); + // Keep historical names selectable when reviewing old files. New annotations + // use canonical choices; no arbitrary text entry is needed. + for (const label of new Set([...state.skillIntervals.map((item) => item.label), value])) { + if (!label || ids.has(label)) continue; + const option = document.createElement("option"); + option.value = label; + option.textContent = `${label}(已有标注)`; + options.push(option); + } + picker.replaceChildren(placeholder, ...options); + picker.value = value; +} + function renderSave() { let text = state.persisted ? "已保存" : "尚未保存"; let kind = state.persisted ? "saved" : ""; @@ -385,6 +417,7 @@ async function save() { })); const document = await response.json(); state.project.document.revision = document.revision; + state.project.document.schema_version = document.schema_version; state.savedVersion = version; state.persisted = true; message(state.draft ? "已保存完成标注;当前草稿尚未提交。" : `已保存 ${skillIntervals.length} 段标注。`); @@ -402,6 +435,7 @@ async function save() { function exportJson() { if (!state.project) return; const document = { ...state.project.document, segments: clone(state.skillIntervals) }; + if (document.segments.some((segment) => segment.core_interval)) document.schema_version = 3; const blob = new Blob([`${JSON.stringify(document, null, 2)}\n`], { type: "application/json" }); const url = URL.createObjectURL(blob); const link = window.document.createElement("a"); @@ -464,6 +498,18 @@ function bindControls() { if (event.key === "Enter" && !event.isComposing) { event.preventDefault(); $("video-stage").focus({ preventScroll: true }); } }); } + $("use-core").onchange = () => { + if (!state.draft) return; + if ($("use-core").checked) state.draft.core_interval = { start_frame: state.draft.start_frame, end_frame: state.draft.end_frame }; + else delete state.draft.core_interval; + renderDraft(); + }; + for (const [id, key] of [["core-start-frame", "start_frame"], ["core-end-frame", "end_frame"]]) { + $(id).addEventListener("input", () => { + if (state.draft?.core_interval) state.draft.core_interval[key] = $(id).value === "" ? null : Number($(id).value); + renderDraft(false); + }); + } $("zoom-in").onclick = () => zoom(state.zoom * 2); $("zoom-out").onclick = () => zoom(state.zoom / 2); $("zoom-reset").onclick = () => zoom(1); @@ -593,7 +639,7 @@ async function initialize() { return tick; })); fittingPanel = createFittingPanel({ - context: () => ({ selection: selected(), revision: state.project.document.revision, dirty: dirty(), draft: state.draft, token: state.token }), + context: () => ({ selection: selected(), skills: state.project.skills, revision: state.project.document.revision, dirty: dirty(), draft: state.draft, token: state.token }), saveAnnotations: async () => { if (state.draft) throw new Error("请先提交或取消当前标注草稿。"); while (state.saving) await new Promise((resolve) => setTimeout(resolve, 50)); @@ -610,7 +656,7 @@ async function initialize() { }); fittingPanel.initialize(); bindControls(); zoom(1); renderSkillIntervals(); renderDraft(); requestFrame(0); - message("已就绪。定位起点按 I,终点按 O,填写名称后按 Enter 提交。 "); + message("已就绪。定位起点按 I,终点按 O,选择原子技能后提交区间。 "); $("video-stage").focus({ preventScroll: true }); } catch (error) { $("init-title").textContent = "连接标注服务失败"; diff --git a/embodichain/toolkits/trajectory_annotator/web/fitting.js b/embodichain/toolkits/trajectory_annotator/web/fitting.js index 56b9c3e161..e2767420fa 100644 --- a/embodichain/toolkits/trajectory_annotator/web/fitting.js +++ b/embodichain/toolkits/trajectory_annotator/web/fitting.js @@ -43,6 +43,8 @@ export function createFittingPanel({ context, saveAnnotations, seekTime }) { const camera = { yaw: -0.8, pitch: 0.5, zoom: 1, center: [0, 0, 0], radius: 1 }; const series = () => state.data?.report.accepted ? ["original", "native", "patched"] : ["original", "native", "patched", "proposed"]; const stale = () => !!state.job && (state.job.stale || context().dirty || state.job.revision !== context().revision); + const key = (value) => value.toLowerCase().replace(/[_\s-]/g, ""); + const selectedSkill = () => context().skills.find((skill) => context().selection && [key(skill.id), key(skill.name)].includes(key(context().selection.label))); function notify(text, kind = "") { $("fit-message").textContent = text; @@ -51,8 +53,15 @@ export function createFittingPanel({ context, saveAnnotations, seekTime }) { function selectionChanged() { const current = context(); - $("fit-selection").textContent = current.selection ? `${current.selection.label} · [${current.selection.start_frame}, ${current.selection.end_frame}] 帧` : "请在右侧选择一段已标注区间"; - $("fit-run").disabled = !current.selection || !!current.draft || state.starting || state.job?.phase === "running"; + const skill = selectedSkill(); + const core = current.selection?.core_interval; + $("fit-selection").textContent = current.selection ? `${current.selection.label} · 标注 [${current.selection.start_frame}, ${current.selection.end_frame}] 帧${core ? ` · 核心 [${core.start_frame}, ${core.end_frame}]` : ""}` : "请在右侧选择一段已标注区间"; + $("fit-selection-hint").textContent = skill && !skill.can_fit ? `${skill.name} 可保存为示教标注;当前未提供该技能的参数重建。` : core ? "仅拟合核心区间,保留标注内前后过渡动作的原始采样。" : "标注包含额外过渡动作时,可在编辑区间中单独设置拟合核心范围。"; + $("fit-skill").disabled = !!skill; + if (skill?.can_fit) $("fit-skill").value = skill.id; + if (skill && !skill.can_fit) $("fit-skill").value = ""; + if (!skill && !$("fit-skill").value) $("fit-skill").value = "pick_up"; + $("fit-run").disabled = !current.selection || !!current.draft || (!!skill && !skill.can_fit) || state.starting || state.job?.phase === "running"; $("fit-run").textContent = state.starting || state.job?.phase === "running" ? "正在拟合…" : "拟合并回填选中区间"; if (!state.job) return; const outdated = stale(); @@ -82,6 +91,7 @@ export function createFittingPanel({ context, saveAnnotations, seekTime }) { state.starting = true; selectionChanged(); try { + if (selectedSkill() && !selectedSkill().can_fit) throw new Error("当前技能支持标注,尚不支持参数重建。"); const inputs = readInputs(); const selection = structuredClone(context().selection); await saveAnnotations(); @@ -137,7 +147,8 @@ export function createFittingPanel({ context, saveAnnotations, seekTime }) { function showResult() { const { report } = state.data; const selection = state.job.selection; - $("fit-result-title").textContent = `${selection.label} · ${report.skill_id} · [${selection.start_frame}, ${selection.end_frame}]`; + const core = selection.core_interval || selection; + $("fit-result-title").textContent = `${selection.label} · ${report.skill_id} · 拟合 [${core.start_frame}, ${core.end_frame}]`; $("fit-result").hidden = false; $("fit-inputs").open = false; $("fit-proposed-legend").hidden = report.accepted; diff --git a/embodichain/toolkits/trajectory_annotator/web/index.html b/embodichain/toolkits/trajectory_annotator/web/index.html index f98e7e4d57..86a504b6c7 100644 --- a/embodichain/toolkits/trajectory_annotator/web/index.html +++ b/embodichain/toolkits/trajectory_annotator/web/index.html @@ -53,6 +53,7 @@

原子技能拟合与轨迹回填

选中一段已提交的标注,使用关节数据辨识参数并重建。

未开始
请在右侧选择一段已标注区间
+

关节记录与机器人配置 本机文件路径 · 可从启动参数预填
@@ -101,11 +102,19 @@

新建原子技能区间

在起点按 I 开始
- +
区间长度— 帧
+
+ +

标注范围可包含前后过渡动作。仅核心区间参与拟合,其余采样保留;未设置时使用整个标注区间。不规则片段可选择 DemoSkill。

+ +

@@ -116,6 +125,6 @@
正在连接本地标注服务…Space 播放 · I / O 标记 · Enter 提交 · ? 帮助
-

键盘快捷键

Space
播放 / 暂停
← →
前一帧 / 后一帧;Shift 跨 10 帧
Home End
首帧 / 末帧
G
输入帧号跳转
I O
设置当前显示帧为起点 / 终点
L
输入原子技能名称
Enter Esc
提交 / 取消草稿
E
编辑选中的标注
Delete
删除选中的标注(Backspace 也可)
Ctrl/Cmd Z
撤销;加 Shift 重做
Ctrl/Cmd S
保存已完成标注

在文本框中输入时,保留文字编辑快捷键。名称输入后按 Enter 返回视频区,再按 Enter 提交区间。起止帧都包含在标注中。

+

键盘快捷键

Space
播放 / 暂停
← →
前一帧 / 后一帧;Shift 跨 10 帧
Home End
首帧 / 末帧
G
输入帧号跳转
I O
设置当前显示帧为起点 / 终点
L
选择原子技能
Enter Esc
提交 / 取消草稿
E
编辑选中的标注
Delete
删除选中的标注(Backspace 也可)
Ctrl/Cmd Z
撤销;加 Shift 重做
Ctrl/Cmd S
保存已完成标注

在文本框中输入时,保留文字编辑快捷键。选择技能后按 Enter 返回视频区,再按 Enter 提交区间。起止帧都包含在标注中。

diff --git a/embodichain/toolkits/trajectory_annotator/web/styles.css b/embodichain/toolkits/trajectory_annotator/web/styles.css index be9b970ced..e00e559e90 100644 --- a/embodichain/toolkits/trajectory_annotator/web/styles.css +++ b/embodichain/toolkits/trajectory_annotator/web/styles.css @@ -128,8 +128,16 @@ main { max-width:1680px; padding:22px 28px 12px; margin:auto; } @media (max-width:700px) { .fit-grid { grid-template-columns:1fr; }.fit-metrics { grid-template-columns:repeat(2,minmax(0,1fr)); }.fit-target,.fit-result-heading { align-items:flex-start; flex-direction:column; }.fit-inputs summary .muted { display:block; margin-left:15px; } } .editor-grid { display:grid; grid-template-columns:minmax(0,1fr) minmax(0,1fr) minmax(48px,.8fr); gap:10px; padding:0 17px; } .editor-grid label,.note-field { display:flex; flex-direction:column; gap:6px; font-size:11px; color:var(--muted); } -.editor-grid input { width:100%; } +.editor-grid input,.editor-grid select { width:100%; } .label-field { grid-column:1/-1; } +.core-editor { margin:12px 17px 0; font-size:11px; } +.core-toggle { display:flex; align-items:center; gap:7px; color:var(--muted); } +.core-toggle input { margin:0; accent-color:var(--accent); } +.core-editor p { margin:7px 0; line-height:1.6; } +.core-fields { display:grid; grid-template-columns:1fr 1fr; gap:10px; } +.core-fields label { display:flex; flex-direction:column; gap:6px; color:var(--muted); } +.core-fields input { width:100%; } +.fitting-card > .fit-footnote { margin:0 17px 12px; } .duration-field { display:flex; flex-direction:column; gap:7px; font-size:11px; color:var(--muted); } .duration-field strong { font-size:20px; color:var(--accent); font-variant-numeric:tabular-nums; font-weight:500; } .duration-field small { font-size:11px; } diff --git a/examples/toolkits/skill_fitting/README.md b/examples/toolkits/skill_fitting/README.md index 2eec7785c0..bb3596f09a 100644 --- a/examples/toolkits/skill_fitting/README.md +++ b/examples/toolkits/skill_fitting/README.md @@ -44,6 +44,17 @@ of video time zero. The joint sampling clock is never inferred from video FPS. Without an annotation file, `--start-frame` and `--end-frame` select an interval; their defaults cover the full video. +An annotation may retain a broad real-motion interval and optionally declare +`core_interval: {"start_frame": 120, "end_frame": 185}` inside it. Both endpoints +are inclusive absolute video frames. The CLI and website reconstruct only this +core and preserve all surrounding samples; the report retains both the outer +annotation and the actual fitted video interval. The core must still contain a +complete supported skill. Schema 3 stores this optional field; legacy schema 1/2 +files without it remain supported. The annotation menu includes all built-in +skills, including DemoSkill; parameter reconstruction remains limited to the four +skills above. A DemoSkill label preserves intent without generating an executable +demonstration asset or substituting a different fitting model. + For existing HDF5 camera annotations, replace `--video` with `--hdf5` and optionally select `--image-key` / `--fps`. This reuses the annotator's source identity and explicitly identifies FPS-derived timing. diff --git a/examples/toolkits/trajectory_annotation/README.md b/examples/toolkits/trajectory_annotation/README.md index 2801163886..b74193e976 100644 --- a/examples/toolkits/trajectory_annotation/README.md +++ b/examples/toolkits/trajectory_annotation/README.md @@ -47,7 +47,7 @@ embodichain annotate-trajectory \ --annotations outputs/trajectory_annotation/episode.cam_high.segments.json ``` -操作流程:←/→ 逐帧,Space 播放/暂停,I 标起点,O 标终点,填写原子技能名称后 Enter 提交。 +操作流程:←/→ 逐帧,Space 播放/暂停,I 标起点,O 标终点,从下拉菜单选择原子技能后提交。 右侧显示全部区间,点击帧号跳转,选中后 Delete 删除,Ctrl+Z 撤销,Ctrl+S 保存。 帧号从 0 开始,起止均包含;例如 `[100, 150]` 包含 51 帧。HDF5 的时间显示按 FPS 推算。 @@ -69,11 +69,13 @@ embodichain run-example toolkits/trajectory_annotation \ --output-dir /tmp/pickup-labels ``` -在网页中用 **I / O** 选中完整 Pickup,命名并提交,选择拟合技能 **Pickup**,点击 +在网页中用 **I / O** 标注完整 Pickup,从技能下拉菜单选择 **Pickup** 并提交,拟合面板自动选择同一技能,点击 **“拟合并回填选中区间”**。会显示通过/拒绝状态、参数和误差,以及与视频联动的关节曲线、 可旋转的 FK 骨架和 TCP 路径;可下载包含完整关节轨迹的 NPZ。此示例辨识出的 `lift_height` 约为 **0.16 m**。 同一面板也支持 Slide、Twist、HandOver。任意真实记录需提供自己的机器人配置,示例 UR5 配置不能直接用于 Piper。 +区间边界较宽时,可勾选“单独设置拟合核心区间”,只重建内部完整技能,保留前后过渡采样; +不符合现有参数模型的片段可选择 **DemoSkill** 保留为示教标注,当前不执行参数拟合。 `--joints`、`--robot-profile`、`--qpos-semantics`、`--time-offset` 可省略后在网页中填写; `--qpos-key`、`--timestamps-key`、`--joint-dt`、`--env-index`、`--phase-hints`、`--fit-config` 也可预填高级设置。 默认 HDF5 标注流程保持可独立使用。 diff --git a/tests/sim/atomic_skills/test_fitting.py b/tests/sim/atomic_skills/test_fitting.py index aa758114a2..eaffc58e71 100644 --- a/tests/sim/atomic_skills/test_fitting.py +++ b/tests/sim/atomic_skills/test_fitting.py @@ -38,6 +38,16 @@ ) +def test_annotation_choices_cover_builtin_skills() -> None: + """Annotation-only users can choose every built-in skill, including DemoSkill.""" + from embodichain.lab.sim.atomic_skills.primitives import BUILTIN_SKILL_TYPES + from embodichain.toolkits.trajectory_annotator._skills import SKILL_CHOICES + + assert {identifier for identifier, _ in SKILL_CHOICES} == { + skill.skill_id for skill in BUILTIN_SKILL_TYPES + } + + class CartesianRobot: """Six independent pose coordinates and one hand, with analytic FK/IK.""" diff --git a/tests/toolkits/trajectory_annotator/test_annotation.py b/tests/toolkits/trajectory_annotator/test_annotation.py index 1c0fe60dc3..a1d2f7c9fc 100644 --- a/tests/toolkits/trajectory_annotator/test_annotation.py +++ b/tests/toolkits/trajectory_annotator/test_annotation.py @@ -78,6 +78,10 @@ def test_inclusive_ranges_allow_singletons_adjacency_and_gaps() -> None: [skill_interval(1.5, 3)], [skill_interval(0, 1), skill_interval(4, 5)], [{**skill_interval(), "label": " "}], + [{**skill_interval(), "core_interval": {"start_frame": 1, "end_frame": 3}}], + [{**skill_interval(), "core_interval": {"start_frame": 3, "end_frame": 5}}], + [{**skill_interval(), "core_interval": {"start_frame": 4, "end_frame": 3}}], + [{**skill_interval(), "core_interval": {"start_frame": True, "end_frame": 3}}], ], ) def test_invalid_intervals_cannot_be_saved(skill_intervals: object) -> None: @@ -180,9 +184,12 @@ def test_atomic_save_resume_conflict_and_write_failure( path = tmp_path / "nested" / "labels.json" store = AnnotationStore(path, 1024**2) store.initialize(source) - saved = store.save([skill_interval()], 0) + selected = {**skill_interval(), "core_interval": {"start_frame": 2, "end_frame": 3}} + saved = store.save([selected], 0) original = path.read_bytes() assert saved["revision"] == 1 + assert saved["schema_version"] == 3 + assert saved["segments"][0]["core_interval"] == {"start_frame": 2, "end_frame": 3} assert json.loads(original)["segments"][0]["label"] == "翻转包裹" restored = AnnotationStore(path, 1024**2) restored.initialize(source) @@ -368,7 +375,11 @@ def test_web_fitting_vfr_clock_concurrency_export_and_stale_revisions( headers = { "X-Annotation-Token": client.get("/api/project").json()["session_token"] } - selected = skill_interval(5, 20) + selected = { + **skill_interval(0, 30), + "label": "pick_up", + "core_interval": {"start_frame": 5, "end_frame": 20}, + } saved = client.put( "/api/annotations", json=dict(segments=[selected], expected_revision=0), @@ -386,6 +397,14 @@ def test_web_fitting_vfr_clock_concurrency_export_and_stale_revisions( ), ) assert client.post("/api/fitting/jobs", json=payload).status_code == 403 + assert ( + client.post( + "/api/fitting/jobs", + json={**payload, "skill_id": "twist"}, + headers=headers, + ).status_code + == 422 + ) assert ( client.post( "/api/fitting/jobs", @@ -426,6 +445,14 @@ def test_web_fitting_vfr_clock_concurrency_export_and_stale_revisions( call = fitting_oracle["calls"][0] np.testing.assert_allclose(call["frame_times"], [6 / 30, 24 / 30]) assert call["frame_durations"][-1] == pytest.approx(1 / 30) + assert call["provenance"]["annotation_video_interval"] == { + "start_frame": 0, + "end_frame": 30, + } + assert call["provenance"]["annotation_segment"]["core_interval"] == { + "start_frame": 5, + "end_frame": 20, + } assert client.get("/api/frame-index?time_seconds=0.2").json() == {"index": 5} assert client.get("/api/frame-index?time_seconds=nan").status_code == 422 preview = client.get(f"/api/fitting/jobs/{job}/preview").json() @@ -454,3 +481,64 @@ def test_web_fitting_vfr_clock_concurrency_export_and_stale_revisions( assert client.get("/api/fitting").json()["stale"] for endpoint in ("bundle", "preview"): assert client.get(f"/api/fitting/jobs/{job}/{endpoint}").status_code == 409 + + +def test_cli_fits_saved_core_and_retains_outer_annotation( + make_video: Callable[..., Path], tmp_path: Path, fitting_oracle: dict[str, object] +) -> None: + """CLI and web honor the same core, preserving the broad real-motion label.""" + import numpy as np + from embodichain.toolkits.skill_fitting.cli import main + from embodichain.toolkits.trajectory_annotator.video import VideoFrameSource + from embodichain.data_pipeline.skill_fitting import load_skill_fit + + video = make_video(variable=True, start=90) + annotations = tmp_path / "labels.json" + source = VideoFrameSource(video) + try: + info = source.prepare(threading.Event(), lambda **_: None) + store = AnnotationStore(annotations, 1024**2) + store.initialize(info) + store.save( + [ + { + **skill_interval(0, 30), + "label": "Pickup", + "core_interval": {"start_frame": 5, "end_frame": 20}, + } + ], + 0, + ) + finally: + source.close() + output = tmp_path / "fit.npz" + main( + [ + "--video", + str(video), + "--joints", + "known.npz", + "--robot-profile", + "robot.json", + "--annotations", + str(annotations), + "--segment-id", + "flip", + "--skill", + "pick_up", + "--qpos-semantics", + "measured_state", + "--time-offset", + "0", + "--output", + str(output), + ] + ) + call = fitting_oracle["calls"][0] + np.testing.assert_allclose(call["frame_times"][[0, -1]], [6 / 30, 24 / 30]) + _, report = load_skill_fit(output) + assert report["annotation_video_interval"] == {"start_frame": 0, "end_frame": 30} + assert report["annotation_segment"]["core_interval"] == { + "start_frame": 5, + "end_frame": 20, + } diff --git a/tests/toolkits/trajectory_annotator/test_browser.py b/tests/toolkits/trajectory_annotator/test_browser.py index 68c499659a..bfd3a315b3 100644 --- a/tests/toolkits/trajectory_annotator/test_browser.py +++ b/tests/toolkits/trajectory_annotator/test_browser.py @@ -99,7 +99,7 @@ def create_skill_interval(page: Page) -> None: go(page, 2) page.keyboard.press("i") - page.locator("#label").fill("翻转包裹") + page.locator("#label").select_option("pick_up") page.locator("#label").press("Enter") go(page, 5) page.keyboard.press("o") @@ -236,7 +236,7 @@ def test_keyboard_edit_delete_undo_and_resume( skill_interval["end_frame"], ) for skill_interval in saved["segments"] - ] == [("翻转包裹", 3, 6)] + ] == [("pick_up", 3, 6)] page.reload() expect(page.locator("#skill-interval-count")).to_have_text("1") expect(page.locator("#skill-interval-list td").nth(2)).to_have_text("6") @@ -279,6 +279,11 @@ def test_timeline_frame_geometry_and_free_drag_inside_skill_intervals( page.reload() expect(page.locator("#skill-interval-count")).to_have_text("4") expect(page.locator("#frame-current")).to_have_text("0") + # Historical free-text labels remain editable without being silently renamed. + page.locator("#skill-interval-list tr").nth(2).click() + page.locator("#edit").click() + expect(page.locator("#label")).to_have_value("翻转包裹") + page.locator("#cancel-draft").click() def check_geometry() -> dict[str, float]: bounds = page.locator("#timeline").bounding_box() @@ -325,6 +330,54 @@ def x_at(frame: int) -> float: expect(page.locator("#frame-current")).to_have_text("30") +def test_skill_dropdown_core_persistence_and_demoskill( + page: Page, annotation_server: tuple[str, Path] +) -> None: + """Broad annotation boundaries survive independently of the fitted core.""" + from playwright.sync_api import expect + + url, destination = annotation_server + open_project(page, url) + expect(page.locator("select#label")).to_be_visible() + for identifier in ("pick_up", "twist", "execute_demo_skill", "pour"): + expect(page.locator(f'#label option[value="{identifier}"]')).to_have_count(1) + create_skill_interval(page) + page.locator("#edit").click() + page.locator("#label").select_option("twist") + page.locator("#use-core").check() + page.locator("#core-start-frame").fill("1") + expect(page.locator("#commit")).to_be_disabled() + page.locator("#core-start-frame").fill("3") + page.locator("#core-end-frame").fill("4") + page.locator("#commit").click() + expect(page.locator("#fit-skill")).to_have_value("twist") + expect(page.locator("#fit-skill")).to_be_disabled() + expect(page.locator("#fit-selection")).to_contain_text("核心 [3, 4]") + page.locator("#save").click() + expect(page.locator("#save-status")).to_have_text("已保存") + saved = json.loads(destination.read_text()) + assert saved["schema_version"] == 3 + assert saved["segments"][0]["start_frame"] == 2 + assert saved["segments"][0]["end_frame"] == 5 + assert saved["segments"][0]["core_interval"] == {"start_frame": 3, "end_frame": 4} + page.reload() + expect(page.locator("#skill-interval-count")).to_have_text("1") + page.locator("#skill-interval-list tr").click() + page.locator("#edit").click() + expect(page.locator("#use-core")).to_be_checked() + expect(page.locator("#core-start-frame")).to_have_value("3") + page.locator("#label").select_option("execute_demo_skill") + page.locator("#commit").click() + expect(page.locator("#fit-run")).to_be_disabled() + expect(page.locator("#fit-selection-hint")).to_contain_text("DemoSkill") + page.locator("#save").click() + expect(page.locator("#save-status")).to_have_text("已保存") + assert ( + json.loads(destination.read_text())["segments"][0]["label"] + == "execute_demo_skill" + ) + + def test_pause_marks_visible_frame_and_latest_seek_wins( page: Page, annotation_server: tuple[str, Path], monkeypatch: pytest.MonkeyPatch ) -> None: From 4de52af8c6f25ce6450cd5688a9fcba4b057f58f Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 14 Nov 2026 08:57:43 +0000 Subject: [PATCH 04/45] feat(expansion): add rolling single-world workload execution --- agent_context/MAP.yaml | 2 +- .../expansion-engine/expansion-engine.md | 24 +- .../topics/expansion-engine/workloads.md | 85 +++ .../topics/sensor-system/sensor-system.md | 8 + docs/source/api_reference/public_api.rst | 77 +++ .../geometry/_warp/batch_contact_query.py | 90 +++ .../geometry/_warp/contact_permissions.py | 98 +++ embodichain/lab/expansion/__init__.py | 14 + embodichain/lab/expansion/_gym_frames.py | 55 +- embodichain/lab/expansion/_motion_batches.py | 166 +++++ .../lab/expansion/_planning_batches.py | 342 +++++++++ embodichain/lab/expansion/_workload_cursor.py | 105 +++ embodichain/lab/expansion/collision.py | 76 +- embodichain/lab/expansion/contracts.py | 16 +- embodichain/lab/expansion/engine.py | 120 ++++ embodichain/lab/expansion/episode_export.py | 129 ++++ embodichain/lab/expansion/persistence.py | 45 ++ embodichain/lab/expansion/workloads.py | 648 ++++++++++++++++++ embodichain/lab/gym/envs/_json.py | 4 + .../lab/sim/atomic_skills/_command_batches.py | 188 +++++ .../lab/sim/atomic_skills/runtime_commands.py | 8 + embodichain/lab/sim/cfg/physics.py | 11 + .../lab/sim/motion/motion_generator.py | 7 + .../lab/sim/motion/planners/base_planner.py | 4 +- .../lab/sim/motion/solvers/base_solver.py | 6 + .../lab/sim/motion/solvers/opw_solver.py | 3 +- embodichain/lab/sim/objects/articulation.py | 2 +- embodichain/lab/sim/physics/default.py | 2 +- .../scene/feasibility/_collision_batches.py | 335 +++++++++ .../lab/sim/scene/feasibility/collision.py | 105 +++ embodichain/lab/sim/sensors/__init__.py | 3 + .../lab/sim/sensors/contact/__init__.py | 2 + .../lab/sim/sensors/contact/permissions.py | 204 ++++++ .../integrations/_measured_batch.py | 133 ++++ .../task_program/integrations/feasibility.py | 48 ++ .../integrations/simulation/policies.py | 11 +- embodichain/utils/_owned_json.py | 115 ++++ pyproject.toml | 1 + tests/lab/expansion/test_export.py | 55 ++ tests/lab/expansion/test_persistence.py | 33 +- tests/lab/expansion/test_workloads_sim.py | 364 ++++++++++ .../atomic_skills/test_runtime_commands.py | 34 + tests/sim/motion/solvers/test_base_solver.py | 32 + .../feasibility/test_collision_planning.py | 61 ++ tests/sim/sensors/test_contact_default_e2e.py | 60 +- 45 files changed, 3857 insertions(+), 74 deletions(-) create mode 100644 agent_context/topics/expansion-engine/workloads.md create mode 100644 embodichain/compute/geometry/_warp/batch_contact_query.py create mode 100644 embodichain/compute/geometry/_warp/contact_permissions.py create mode 100644 embodichain/lab/expansion/_motion_batches.py create mode 100644 embodichain/lab/expansion/_planning_batches.py create mode 100644 embodichain/lab/expansion/_workload_cursor.py create mode 100644 embodichain/lab/expansion/episode_export.py create mode 100644 embodichain/lab/expansion/workloads.py create mode 100644 embodichain/lab/sim/atomic_skills/_command_batches.py create mode 100644 embodichain/lab/sim/scene/feasibility/_collision_batches.py create mode 100644 embodichain/lab/sim/sensors/contact/permissions.py create mode 100644 embodichain/lab/task_program/integrations/_measured_batch.py create mode 100644 embodichain/utils/_owned_json.py create mode 100644 tests/lab/expansion/test_workloads_sim.py diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index e20bf4b69e..0e27617b7f 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -198,7 +198,7 @@ topics: - id: expansion-engine title: Expansion Engine aliases: [Expansion Engine, offline expansion, 扩增引擎, 批量专家采集] - keywords: [ExpansionEngine, ExpansionRunCfg, ExpansionJobCfg, ExpansionSession, ExpansionSessionCfg, ExpansionCoverage, EpisodeCoverage, GymExpansionHost, ArtifactEpisodeSink, MediaSpool, render_observation_variants] + keywords: [ExpansionEngine, ExpansionRunCfg, ExpansionJobCfg, ExpansionSession, ExpansionSessionCfg, ExpansionCoverage, EpisodeCoverage, GymExpansionHost, ArtifactEpisodeSink, MediaSpool, render_observation_variants, WorkloadProgramExecutor, WorkloadExecutionCfg, workload_count] paths: [topics/expansion-engine/expansion-engine.md] source_of_truth: - embodichain/lab/expansion/ diff --git a/agent_context/topics/expansion-engine/expansion-engine.md b/agent_context/topics/expansion-engine/expansion-engine.md index c5bd37a172..88c2794c38 100644 --- a/agent_context/topics/expansion-engine/expansion-engine.md +++ b/agent_context/topics/expansion-engine/expansion-engine.md @@ -10,6 +10,7 @@ algorithms stay in `sim.motion.expansion`; scene and workspace algorithms stay i | Closed configuration and preflight | `embodichain/lab/expansion/configured.py` | | Canonical in-memory session, lifecycle limits and coverage port | `embodichain/lab/expansion/session.py`, `session_cfg.py`, `coverage.py` | | Shared scheduling, admission and commit ports | `embodichain/lab/expansion/engine.py` | +| Rolling workload × B execution in one caller-owned World | `embodichain/lab/expansion/workloads.py` | | Job assembly, stage recovery and inspect | `embodichain/lab/expansion/workflow.py`, `stages.py` | | Native World ownership and compatibility facade | `embodichain/lab/expansion/host.py` | | Native recipes, cohort preparation, step observation and episode assembly | `embodichain/lab/expansion/_gym_recipe.py`, `_gym_prepare.py`, `_gym_execution.py`, `_gym_frames.py`, `_gym_episode.py` | @@ -17,6 +18,7 @@ algorithms stay in `sim.motion.expansion`; scene and workspace algorithms stay i | Task Program execution with native or prototype motion | `embodichain/lab/expansion/program.py`, `real2sim.py` | | Source-pinned sampling and joint scene candidates | `embodichain/lab/expansion/recipes.py`, `scene.py` | | Artifact staging, checks and dataset export | `embodichain/lab/expansion/persistence.py`, `export.py` | +| Portable one-episode dataset export | `embodichain/lab/expansion/episode_export.py` | | Native appearance and state rerender | `embodichain/lab/expansion/visual.py` | | Bounded media blocks | `embodichain/data_pipeline/recording/media.py` | @@ -82,7 +84,7 @@ journal/duplicate-submission behavior. Cohorts have fixed robot/joint topology, backend, clock, observation schema and World appearance. Native scene-atlas input retains the complete layout/installation candidate. Assets with different topology require separate deployment Worlds. -All rows reset and settle together. Only the owner steps physics, and row generators +The configured cohort path resets and settles its rows together. Only the owner steps physics, and row generators acknowledge commands only after successful steps. Early termination must drain the Task Program safe-stop handshake; completed rows stop contributing frames. Native vector rejection cancels the row across semantic lanes and masks retained @@ -91,6 +93,21 @@ post-policy observation clock. Expected Real2Sim acceptance rejection retains measured checks and rejects only that row; source corruption and runtime errors still stop collection. +`WorkloadProgramExecutor` is an optional rolling integration for custom native +sources. Its supplied environment must have exactly `workload_count * batch_size` +rows and be the sole live World in the process. Each workload owns quotas, local +candidate ordinals and admission budgets; its B-row lane is reused after that +group drains. Preparing rows settle through ordinary control steps while live +rows continue to record and validate every boundary. The source owns scene +proposals, bridge binding and all physical checks, using `ExpansionEngine.admit_row`, +`mark_row_started` and `complete_row` instead of editing private engine queues. +Only confirmed native receipts earn quota or permit reuse; duplicate identities +cannot earn credit twice. Clock spans must equal each trajectory's command count. +Compatible fixed-grid planning and pure solver requests can be cooperatively +merged on the owner thread. Scoped planning budgets retain per-row charges; +unsupported requests keep native execution. Export callbacks receive frozen +group evidence and never access the World. See [rolling execution](workloads.md). + Native numerical evidence has T submitted actions and T+1 measured states. It includes all represented dynamic scene entities, source/action semantics, physical parameters and call provenance. Position-velocity commands must use their ExpertActionSpec; @@ -102,6 +119,11 @@ has bounded transient copies; no per-candidate dense full-episode image array is Frame readers stream selected frames from stored blocks; full artifact audits still validate bounded blocks. LeRobot export streams confirmed parents and visual children through the existing recorder and checks the exact final journal identity set. ODS remains unchanged. +`export_expansion_episodes` additionally publishes one portable LeRobot dataset +per confirmed parent. Cached evidence reads still hash every file byte and +reconstruct checked, independently owned numerical tensors; source/receipt/media +inspection remains required. Immutable JSON nodes can be shared internally, while +public mutable projections own separate containers. Pure visual replay requires complete scene state and an explicit observation-independent expert contract. Physics is disabled across the entire World; render-state publication diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md new file mode 100644 index 0000000000..6dcdf270f1 --- /dev/null +++ b/agent_context/topics/expansion-engine/workloads.md @@ -0,0 +1,85 @@ +# Rolling workload execution + +The public owner is `embodichain.lab.expansion.workloads`. This optional port is +independent of task names and downstream operator configuration. Existing +`ExpansionEngine.from_job` and cohort callers retain their interfaces and defaults. + +## Capacity and ownership + +`WorkloadExecutionCfg` declares W active groups and B physical candidate rows +per group; the supplied environment must contain exactly W times B rows. The +executor creates no simulation World, process or planning thread. It runs once +on the thread that owns its World. `world_info` records actual process, World, +backend, state and solver devices; selecting a visible CUDA device or renderer +does not select the physics device. + +`ExpansionWorkloadCfg` declares a unique group, qualified target, attempt cap and +admission wall-time budget. Candidate ordinals and seeds belong to that group, +not its lane or physical row. Admission includes in-flight candidates in the +remaining quota, avoiding extra speculative trajectories credited beyond target. +Another queued group takes the lane when the previous group has drained. A +difficult group does not silently borrow another group's B-row allocation. + +## Source ports and native accounting + +`WorkloadSource.prepare` runs inside the selected-row `episode_prepare` reset +callback. It must not step physics, alter an active peer or mutate World-wide +appearance/resources. Incompatible topology or shared resources require a +different deployment, rather than pretending they are independent rows. + +After at least the declared physical settling substeps, `activate` captures the +initial measured boundary, performs the source's required preparation checks, +registers a native candidate and uses `ExpansionEngine.admit_row`. It returns a +fresh bridge bound exactly to that row and the environment's control cadence. +Rejected preparation returns None and never earns quota. + +`started` calls `mark_row_started` after the first actual command. `observe` +captures every active T+1 boundary and performs the original physical checks; +it returns only active rows needing native safe rejection. The executor retains +the bridge acknowledgement/hold handshake. Preparing and completed peers cannot +fabricate actions or observations for another trajectory. + +`complete` receives the canonical bridge result and exact first/last World +control indices. It assembles the original source evidence and calls +`ExpansionEngine.complete_row`. Only that confirmed receipt earns a qualified +count. None is a rejection, never a placeholder success. Unavailable or ambiguous +writes retain the ordinary native stage/reconciliation contract and stop the +rolling owner from issuing unsupported quota credit. + +Frozen groups can call `export_expansion_episodes` from bounded writer threads. +The callback must not retain mutable live state or operate on the World. +Partial quotas remain explicit failures even when some episodes were exported. + +## Planning and validation + +The optional cooperative planner merges compatible fixed-grid motion requests +and pure FK/IK/Jacobian calls. Budget-bearing whole-motion requests keep their +original execution so charge semantics are preserved. Context variables and +candidate random streams remain row-local; results are independently owned. +Semantic programs and some trajectory processing still run per row. Per-control +planning readiness is a barrier; this API does not claim complete vectorization +or universal throughput improvement. + +`NewtonCollisionChecker.check_batch` uses the same physical geometry, pair +exemptions, halfspace supports and precision/saturation rules as scalar checks. +It returns discrete row reports, without claiming continuous clearance. Its +query model is never stepped and is not another simulation World. +`MeasuredTaskMonitor.batch_scope` shares measured numeric inputs while preserving +each native event state machine and receipts. `ContactPermissionChecker` checks +all valid native contact slots with the original applied-impulse and loss gates; +hosts still own sampling at every required physics substep. + +Default GPU dynamics may use ordinary reporting with +`DefaultPhysicsCfg(direct_gpu_api=False)` when complete static contact identities +are needed. It retains GPU dynamics; the Direct GPU buffer initialization flag +alone is not a physics-device measurement. Physics devices and task qualification +must be reported separately. Existing CPU deployments do not switch devices. + +## Focused validation + +`tests/lab/expansion/test_workloads_sim.py` runs a real four-row task through +native session admission and artifact receipts. It covers rejected-row safe +holds, refill, queued groups, no missing control boundaries, scalar/batched task +predicates and compatible whole-motion result parity. Geometry and contact +coverage lives in the existing native feasibility/contact suites. Per-episode +export uses the real LeRobot SDK and checks portable T+1 evidence. diff --git a/agent_context/topics/sensor-system/sensor-system.md b/agent_context/topics/sensor-system/sensor-system.md index 4a8869963e..82e9440ed1 100644 --- a/agent_context/topics/sensor-system/sensor-system.md +++ b/agent_context/topics/sensor-system/sensor-system.md @@ -125,6 +125,14 @@ changing filtering, force assumptions, topology handling or actor metadata. Geometry-only contacts and multiple backend-emitted rows per actor pair are valid; sensor adaptation does not synthesize a common contact manifold. +`ContactPermissionChecker` consumes complete native samples with explicit +actor/frame permissions, returning owned per-row device summaries. It does not +choose the host's sampling frequency or relax unavailable-impulse/loss gates. +Global static actors retain their explicit row memberships. Rebind after query +topology changes. Default CUDA dynamics can select ordinary reporting through +`DefaultPhysicsCfg.direct_gpu_api=False` when static actor identity is required; +this is independent of the physics device and leaves CPU defaults unchanged. + The contact scatter kernel remains sensor-owned because its columns, IDs and quota are integration contracts. Generic image tiling belongs to `embodichain/compute/image/_warp/tiling.py`; compatibility exports under diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index f4b35c2d77..9620eac1cb 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -1662,6 +1662,16 @@ and durable episode-boundary recovery. See the offline expansion example. ExpansionCoverage EpisodeCoverage +.. autosummary:: + + ExpansionWorkloadCfg + WorkloadExecutionCfg + WorkloadAssignment + WorkloadRowResult + WorkloadSource + WorkloadProgramExecutor + export_expansion_episodes + embodichain.lab.expansion.artifacts ----------------------------------- @@ -1766,6 +1776,21 @@ attempts at episode boundaries. Native failures invalidate the active host. ExpansionRecipeSource PreparingExpansionHost +embodichain.lab.expansion.episode_export +--------------------------------------- + +.. currentmodule:: embodichain.lab.expansion.episode_export + +Export each confirmed qualified trajectory to an independently readable LeRobot +dataset, retaining its portable T+1 evidence, source hashes and recording journal. +Export uses frozen evidence and opens no simulation World. + +.. autosummary:: + + export_expansion_episodes + +.. autofunction:: export_expansion_episodes + embodichain.lab.expansion.export -------------------------------- @@ -1920,6 +1945,32 @@ Apply one native visual profile per compatible World cohort. Render complete rec render_observation_variants inspect_observation_variants +embodichain.lab.expansion.workloads +---------------------------------- + +.. currentmodule:: embodichain.lab.expansion.workloads + +Rolling Task Program execution in one caller-owned World with capacity +``workload_count * batch_size``. Sources retain task-owned scene proposals and +required physical qualification; native session and sink receipts remain the +authority for admission and committed quotas. Preparing rows settle through +ordinary World steps while live trajectories retain every control boundary. + +.. autosummary:: + + ExpansionWorkloadCfg + WorkloadExecutionCfg + WorkloadAssignment + WorkloadRowResult + WorkloadSource + WorkloadProgramExecutor + +.. autoclass:: WorkloadProgramExecutor + :members: + +.. autoclass:: WorkloadSource + :members: + embodichain.lab.expansion.workflow ---------------------------------- @@ -3654,6 +3705,32 @@ embodichain.lab.sim.sensors.camera Camera CameraCfg +embodichain.lab.sim.sensors.contact +---------------------------------- + +.. currentmodule:: embodichain.lab.sim.sensors.contact + +.. autosummary:: + + ContactPermissionChecker + +embodichain.lab.sim.sensors.contact.permissions +---------------------------------------------- + +.. currentmodule:: embodichain.lab.sim.sensors.contact.permissions + +Check complete native contact buffers against explicit actor/frame permissions. +The host chooses the original physical sampling rate; unavailable impulse, +unknown actors and dropped evidence cannot pass. Results are owned device-side +row summaries, suitable for every-substep observers without Python contact lists. + +.. autosummary:: + + ContactPermissionChecker + +.. autoclass:: ContactPermissionChecker + :members: + embodichain.lab.sim.sensors.contact.sensor ------------------------------------------ diff --git a/embodichain/compute/geometry/_warp/batch_contact_query.py b/embodichain/compute/geometry/_warp/batch_contact_query.py new file mode 100644 index 0000000000..af31b4fa4f --- /dev/null +++ b/embodichain/compute/geometry/_warp/batch_contact_query.py @@ -0,0 +1,90 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + + +"""Per-environment reduction of one independent batched Newton query.""" + +from __future__ import annotations + +import warp as wp + +__all__: list[str] = [] + + +@wp.kernel(enable_backward=False) +def contact_summary( + count: wp.array(dtype=wp.int32), + first: wp.array(dtype=wp.int32), + second: wp.array(dtype=wp.int32), + point0: wp.array(dtype=wp.vec3), + point1: wp.array(dtype=wp.vec3), + normal: wp.array(dtype=wp.vec3), + margin0: wp.array(dtype=float), + margin1: wp.array(dtype=float), + shape_body: wp.array(dtype=wp.int32), + poses: wp.array(dtype=wp.transform), + active: wp.array(dtype=wp.int32), + width: int, + batches: int, + required: float, + tolerance: wp.array(dtype=float), + capacity: int, + broad_count: wp.array(dtype=wp.int32), + broad_capacity: int, + gjk_count: wp.array(dtype=wp.int32), + gjk_capacity: int, + split_count: wp.array(dtype=wp.int32), + split_capacity: int, + manifold_count: wp.array(dtype=wp.int32), + manifold_capacity: int, + output: wp.array2d(dtype=float), +): + index = wp.tid() + if index == 0: + if ( + count[0] >= capacity + or broad_count[0] > broad_capacity + or gjk_count[0] > gjk_capacity + or split_count[0] > split_capacity + or manifold_count[0] > manifold_capacity + ): + for row in range(batches): + wp.atomic_min(output, row, 2, 0.0) + if index < count[0] and index < capacity: + a, b = first[index], second[index] + if a < 0 or b < 0 or a >= width * batches or b >= width * batches: + for row in range(batches): + wp.atomic_min(output, row, 2, 0.0) + elif a // width != b // width: + for row in range(batches): + wp.atomic_min(output, row, 2, 0.0) + else: + row = a // width + x, y = a % width, b % width + if active[x * width + y] != 0: + p0 = wp.transform_point(poses[shape_body[a]], point0[index]) + p1 = wp.transform_point(poses[shape_body[b]], point1[index]) + distance = ( + wp.dot(normal[index], p1 - p0) - margin0[index] - margin1[index] + ) + if not wp.isfinite(distance): + wp.atomic_min(output, row, 2, 0.0) + else: + wp.atomic_min(output, row, 0, distance) + if distance <= required + tolerance[row]: + wp.atomic_min( + output, row, 1, float(wp.min(x, y) * width + wp.max(x, y)) + ) diff --git a/embodichain/compute/geometry/_warp/contact_permissions.py b/embodichain/compute/geometry/_warp/contact_permissions.py new file mode 100644 index 0000000000..5ebe6bac71 --- /dev/null +++ b/embodichain/compute/geometry/_warp/contact_permissions.py @@ -0,0 +1,98 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + + +"""Inspect every measured native contact without materializing Python pairs.""" + +from __future__ import annotations + +import warp as wp + +__all__: list[str] = [] + + +@wp.func +def actor_index(actors: wp.array(dtype=wp.int32), value: int): + lower = int(0) + upper = actors.shape[0] + while lower < upper: + middle = (lower + upper) // 2 + if actors[middle] < value: + lower = middle + 1 + else: + upper = middle + return lower + + +@wp.kernel(enable_backward=False) +def classify_contacts( + pairs: wp.array3d(dtype=wp.int32), + valid: wp.array2d(dtype=wp.bool), + impulse_valid: wp.array2d(dtype=wp.bool), + actors: wp.array(dtype=wp.int32), + memberships: wp.array2d(dtype=wp.bool), + codes: wp.array(dtype=wp.int64), + moving: wp.array(dtype=wp.bool), + allowed: wp.array2d(dtype=wp.bool), + dropped: wp.array(dtype=wp.bool), + first_error: wp.array(dtype=wp.int32), + any_drop: wp.array(dtype=wp.int32), +): + row, slot = wp.tid() + if slot == 0 and dropped[row]: + wp.atomic_max(any_drop, 0, 1) + if valid[row, slot]: + reason = int(0) + if not impulse_valid[row, slot]: + reason = 1 + else: + left, right = pairs[row, slot, 0], pairs[row, slot, 1] + a, b = actor_index(actors, left), actor_index(actors, right) + if a == actors.shape[0] or b == actors.shape[0]: + reason = 2 + elif actors[a] != left or actors[b] != right: + reason = 2 + elif not memberships[a, row] or not memberships[b, row]: + reason = 2 + else: + x, y = codes[a], codes[b] + if (moving[x] or moving[y]) and not allowed[x, y]: + reason = 3 + if reason != 0: + wp.atomic_min(first_error, row, slot * 8 + reason) + + +@wp.kernel(enable_backward=False) +def contact_failures( + pairs: wp.array3d(dtype=wp.int32), + first_error: wp.array(dtype=wp.int32), + any_drop: wp.array(dtype=wp.int32), + output: wp.array2d(dtype=wp.int32), +): + row = wp.tid() + if any_drop[0] != 0: + output[row, 0] = 4 + output[row, 1] = 0 + output[row, 2] = 0 + elif first_error[row] < pairs.shape[1] * 8: + slot = first_error[row] // 8 + output[row, 0] = first_error[row] % 8 + output[row, 1] = pairs[row, slot, 0] + output[row, 2] = pairs[row, slot, 1] + else: + output[row, 0] = 0 + output[row, 1] = 0 + output[row, 2] = 0 diff --git a/embodichain/lab/expansion/__init__.py b/embodichain/lab/expansion/__init__.py index ff136b8f58..5a32997b25 100644 --- a/embodichain/lab/expansion/__init__.py +++ b/embodichain/lab/expansion/__init__.py @@ -60,6 +60,13 @@ "load_expansion_episode", "resolve_expansion_job", "build_expansion_report", + "ExpansionWorkloadCfg", + "WorkloadExecutionCfg", + "WorkloadAssignment", + "WorkloadRowResult", + "WorkloadSource", + "WorkloadProgramExecutor", + "export_expansion_episodes", ] @@ -98,6 +105,13 @@ def __getattr__(name: str) -> object: "load_expansion_episode": "persistence", "resolve_expansion_job": "configured", "build_expansion_report": "reporting", + "ExpansionWorkloadCfg": "workloads", + "WorkloadExecutionCfg": "workloads", + "WorkloadAssignment": "workloads", + "WorkloadRowResult": "workloads", + "WorkloadSource": "workloads", + "WorkloadProgramExecutor": "workloads", + "export_expansion_episodes": "episode_export", }.get(name, "engine") module = f"{__name__}.{owner}" value = getattr(import_module(module), name) diff --git a/embodichain/lab/expansion/_gym_frames.py b/embodichain/lab/expansion/_gym_frames.py index aee59d96bd..26ee4357bc 100644 --- a/embodichain/lab/expansion/_gym_frames.py +++ b/embodichain/lab/expansion/_gym_frames.py @@ -98,6 +98,19 @@ def __init__(self, context: GymHostContext, assignments: dict) -> None: spools[row] = spool self.records, self.spools = records, spools + def add(self, assignments: dict) -> None: + """Capture newly settled rows without replacing live measured buffers.""" + if set(assignments) & self.records.keys(): + raise ValueError("cannot replace a live measured row") + initial = GymFrameCollector(self.context, assignments) + self.records.update(initial.records) + self.spools.update(initial.spools) + + def discard(self, row: int) -> None: + """Release a row after evidence assembly and persistence confirmation.""" + self.records.pop(row) + self.spools.pop(row, None) + def observe(self, ids, action, observation) -> None: ctx = self.context cfg, env, view, collision, timings = ( @@ -147,30 +160,26 @@ def observe(self, ids, action, observation) -> None: timings["sampled_validation"] = ( timings.get("sampled_validation", 0.0) + time.monotonic() - stamp ) - for row in ids: - record = records[row] - value = ( - {"qpos": action.value} - if isinstance(action.value, torch.Tensor) - else action.value - ) + value = ( + {"qpos": action.value} + if isinstance(action.value, torch.Tensor) + else action.value + ) - def active_command(name): - data = value[name][row] - if data.shape[-1] == env.robot.dof: - data = data[list(env.active_joint_ids)] - elif data.shape[-1] != len(env.active_joint_ids): - raise ValueError("submitted command joint layout differs") - return data.detach().cpu() + def active_command(name): + data = value[name] + if data.shape[-1] == env.robot.dof: + data = data[:, list(env.active_joint_ids)] + elif data.shape[-1] != len(env.active_joint_ids): + raise ValueError("submitted command joint layout differs") + return data - command = active_command("qpos") - if env.expert_action_spec.joint_command_mode == "position_velocity": - command = torch.cat( - ( - command, - active_command("qvel"), - ) - ) + commands = active_command("qpos") + if env.expert_action_spec.joint_command_mode == "position_velocity": + commands = torch.cat((commands, active_command("qvel")), dim=-1) + commands = commands.detach().cpu() + for row in ids: + record = records[row] entity_states = {entity.uid: entity for entity in frame_state.batches[row]} for (uid, field), values in record["scene"].items(): values.append( @@ -179,7 +188,7 @@ def active_command(name): dtype=torch.float32, ) ) - record["actions"].append(command.clone()) + record["actions"].append(commands[row].clone()) record["qpos"].append(measured_qpos[row].clone()) record["qvel"].append(measured_qvel[row].clone()) record["qf"].append(measured_qf[row].clone()) diff --git a/embodichain/lab/expansion/_motion_batches.py b/embodichain/lab/expansion/_motion_batches.py new file mode 100644 index 0000000000..5bb52f16f6 --- /dev/null +++ b/embodichain/lab/expansion/_motion_batches.py @@ -0,0 +1,166 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + + +"""Fuse compatible fixed-grid native motion requests before scalar transforms.""" + +from __future__ import annotations + +from copy import copy +from dataclasses import fields, is_dataclass, replace +from enum import Enum +from functools import wraps + +__all__: list[str] = [] + + +class MotionBatches: + """Keep selected physical rows and native fixed-grid rejection semantics.""" + + def __init__(self, broker): + from greenlet import getcurrent + from embodichain.lab.sim.motion.motion_generator import MotionGenerator + + self.broker = broker + self.original = original = MotionGenerator.generate + + def direct(generator, targets, options, defer_recovery): + previous = broker.motion_passthrough + broker.motion_passthrough = True + try: + return original( + generator, targets, options, defer_recovery=defer_recovery + ) + finally: + broker.motion_passthrough = previous + + @wraps(original) + def generate(generator, target_states, options=None, *, defer_recovery=False): + from embodichain.lab.sim.motion._budget import _ACTIVE_BUDGET + + task = getcurrent() + if ( + task not in broker.tasks + or options is None + or options.strategy != "ik_interp" + or options.interpolation_mode != "fixed_grid" + or options.segment_sample_count_retries + or options.segment_continuous_ik_recovery + or options.cartesian_path_sampler is not None + or generator.collision_world_info is not None + or generator._env_ids is None + or _ACTIVE_BUDGET.get() is not None + ): + return direct(generator, target_states, options, defer_recovery) + tensors = {} + try: + structure = self._describe( + (target_states, options), tensors, len(generator._env_ids) + ) + except TypeError: + return direct(generator, target_states, options, defer_recovery) + tensors["rows"] = broker.torch.tensor( + generator._env_ids, dtype=broker.torch.long + ) + mode = ( + broker.torch.is_grad_enabled(), + broker.torch.is_inference_mode_enabled(), + ) + key = ( + "motion", + id(generator.robot), + type(generator.planner), + repr(generator.planner.cfg), + structure, + defer_recovery, + mode, + ) + + def invoke(merged): + targets, settings = self._restore((target_states, options), merged) + batched = copy(generator) + batched.planner = copy(generator.planner) + batched._env_ids = tuple(merged["rows"].tolist()) + batched.planner._planning_env_ids = batched._env_ids + return original( + batched, targets, settings, defer_recovery=defer_recovery + ) + + request = (key, invoke, None, None, tensors, len(generator._env_ids), mode) + broker.tasks[task] = broker._rng() + broker._restore_rng(broker.owner_rng) + return broker.owner.switch(request) + + self.generate = generate + + def _describe(self, value, tensors, count, path=""): + torch = self.broker.torch + if isinstance(value, torch.Tensor): + if value.ndim < 1 or value.shape[0] != count: + raise TypeError("Motion tensors must retain the selected batch") + tensors[path] = value + return ("tensor", tuple(value.shape[1:]), value.dtype, value.device) + if is_dataclass(value): + return ( + type(value), + tuple( + ( + f.name, + self._describe( + getattr(value, f.name), tensors, count, path + "/" + f.name + ), + ) + for f in fields(value) + ), + ) + if isinstance(value, (tuple, list)): + return ( + type(value), + tuple( + self._describe(v, tensors, count, path + "/" + str(i)) + for i, v in enumerate(value) + ), + ) + if ( + value is None + or type(value) in (bool, int, float, str) + or isinstance(value, Enum) + ): + return (type(value), value) + raise TypeError("Unsupported mutable motion option") + + def _restore(self, value, tensors, path=""): + if isinstance(value, self.broker.torch.Tensor): + return tensors[path] + if is_dataclass(value): + return replace( + value, + **{ + f.name: self._restore( + getattr(value, f.name), tensors, path + "/" + f.name + ) + for f in fields(value) + }, + ) + if isinstance(value, (tuple, list)): + return type(value)( + self._restore(v, tensors, path + "/" + str(i)) + for i, v in enumerate(value) + ) + return value + + def close(self): + return None diff --git a/embodichain/lab/expansion/_planning_batches.py b/embodichain/lab/expansion/_planning_batches.py new file mode 100644 index 0000000000..8d7e587b51 --- /dev/null +++ b/embodichain/lab/expansion/_planning_batches.py @@ -0,0 +1,342 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + + +"""Cooperatively batch native kinematics on the sole World-owning thread.""" + +from __future__ import annotations + +from collections import defaultdict +from contextlib import contextmanager +from functools import wraps +from inspect import signature +from dataclasses import fields, is_dataclass, replace +from contextvars import ContextVar, copy_context + +__all__: list[str] = [] + +_ACTIVE: ContextVar[PlanningBatches | None] = ContextVar( + "expansion_planning", default=None +) + + +def _try_motion(generator, targets, options, defer_recovery): + active = _ACTIVE.get() + if active is None or active.motion_passthrough: + return False, None + from greenlet import getcurrent + + if getcurrent() not in active.tasks: + return False, None + return True, active.motions.generate( + generator, targets, options, defer_recovery=defer_recovery + ) + + +class PlanningBatches: + """Suspend scalar planners only at pure solver calls, then fuse their rows. + + Native Robot budget decorators, plan transforms and validation run inside + each original cursor. No World call runs on a worker thread. Results are + cloned into independent rows before any scalar planner resumes. + """ + + def __init__(self, robot, seed): + import numpy as np + import random + import torch + from greenlet import getcurrent + + self.torch, self.np, self.random = torch, np, random + self.owner = getcurrent() + self.tasks = {} + self.saved = [] + self.solo = False + self.direct_depth = 0 + self.motion_passthrough = False + self.devices = ( + [ + ( + robot.device.index + if robot.device.index is not None + else torch.cuda.current_device() + ) + ] + if robot.device.type == "cuda" + else [] + ) + self.stats = { + "solver_calls": 0, + "logical_calls": 0, + "merged_calls": 0, + "max_group": 0, + "motion_calls": 0, + "logical_motion_calls": 0, + "merged_motion_calls": 0, + "max_motion_group": 0, + } + previous = self._rng() + torch.manual_seed(seed) + np.random.seed(seed % 2**32) + random.seed(seed) + self.solve_rng = self._rng() + self._restore_rng(previous) + for solver in dict.fromkeys(robot._solvers.values()): + for name in ("get_fk", "get_ik", "get_jacobian"): + self._bind(solver, name) + from ._motion_batches import MotionBatches + + self.motions = MotionBatches(self) + + def _rng(self): + return ( + self.torch.get_rng_state(), + tuple(self.torch.cuda.get_rng_state(d) for d in self.devices), + self.np.random.get_state(), + self.random.getstate(), + self.torch.is_grad_enabled(), + self.torch.is_inference_mode_enabled(), + ) + + def _restore_rng(self, state): + self.torch.set_rng_state(state[0]) + for device, value in zip(self.devices, state[1]): + self.torch.cuda.set_rng_state(value, device) + self.np.random.set_state(state[2]) + self.random.setstate(state[3]) + + def _bind(self, solver, name): + from greenlet import getcurrent + + original = getattr(solver, name, None) + if original is None: + return + spec = signature(original) + missing = object() + before = solver.__dict__.get(name, missing) + + @wraps(original) + def query(*args, **kwargs): + task = getcurrent() + if task not in self.tasks: + counted = self.solo and self.direct_depth == 0 + if counted: + self.stats["solver_calls"] += 1 + self.stats["logical_calls"] += 1 + self.stats["max_group"] = max(self.stats["max_group"], 1) + self.direct_depth += 1 + try: + return original(*args, **kwargs) + finally: + self.direct_depth -= 1 + values = spec.bind(*args, **kwargs) + values.apply_defaults() + tensors, constants, count = {}, {}, None + for key, value in values.arguments.items(): + if isinstance(value, self.torch.Tensor): + if value.ndim < 2 or ( + count is not None and value.shape[0] != count + ): + return original(*args, **kwargs) + count = int(value.shape[0]) + tensors[key] = value + elif value is None or type(value) in (bool, int, float, str): + constants[key] = value + elif type(value) is dict and not value: + constants[key] = value + else: + return original(*args, **kwargs) + if count is None: + return original(*args, **kwargs) + mode = ( + self.torch.is_grad_enabled(), + self.torch.is_inference_mode_enabled(), + ) + key = ( + id(solver), + name, + mode, + tuple( + (key, tuple(value.shape[1:]), value.dtype, value.device) + for key, value in tensors.items() + ), + tuple((key, repr(value)) for key, value in constants.items()), + ) + request = (key, original, spec, values, tensors, count, mode) + self.tasks[task] = self._rng() + self._restore_rng(self.owner_rng) + return self.owner.switch(request) + + setattr(solver, name, query) + self.saved.append((solver, name, before, missing)) + + def _invoke(self, request, tensors): + _, original, spec, values, _, _, mode = request + if request[0][0] == "motion": + with ( + self.torch.inference_mode(mode[1]), + self.torch.set_grad_enabled(mode[0]), + ): + return original(tensors) + arguments = dict(values.arguments) + arguments.update(tensors) + # BoundArguments retains variadic **kwargs and keyword-only arguments. + bound = spec.bind_partial() + bound.arguments.update(arguments) + with self.torch.inference_mode(mode[1]), self.torch.set_grad_enabled(mode[0]): + return original(*bound.args, **bound.kwargs) + + def _part(self, result, start, stop, total=None): + if isinstance(result, self.torch.Tensor): + if result.ndim and (total is None or result.shape[0] == total): + return result[start:stop].clone() + return result.clone() + if isinstance(result, tuple): + return tuple(self._part(value, start, stop, total) for value in result) + if isinstance(result, dict): + return { + key: self._part(value, start, stop, total) + for key, value in result.items() + } + if is_dataclass(result): + return replace( + result, + **{ + f.name: self._part(getattr(result, f.name), start, stop, total) + for f in fields(result) + }, + ) + if result is None or type(result) in (bool, int, float, str): + return result + raise TypeError("Batched native solver returned an unsupported result") + + def gather(self, cursors): + """Advance every native cursor once, merging compatible pending solves.""" + from greenlet import greenlet + + if len(cursors) == 1: + row, cursor = next(iter(cursors.items())) + self.solo = True + try: + action = cursor.next() + return {} if action is None else {row: action} + finally: + self.solo = False + self.owner_rng = self._rng() + ready, waiting = {}, {} + + def advance(task, row, value=None, first=False): + state = self.owner_rng if first else self.tasks[task] + self._restore_rng(state) + try: + with ( + self.torch.inference_mode(state[5]), + self.torch.set_grad_enabled(state[4]), + ): + result = task.switch() if first else task.switch(value) + finally: + self._restore_rng(self.owner_rng) + if task.dead: + if result is not None: + ready[row] = result + else: + waiting[task] = (row, result) + + try: + for row, cursor in cursors.items(): + task = greenlet(cursor.next, parent=self.owner) + task.gr_context = copy_context() + self.tasks[task] = self.owner_rng + advance(task, row, first=True) + while waiting: + groups = defaultdict(list) + for task, (row, request) in waiting.items(): + groups[request[0]].append((task, row, request)) + waiting = {} + for group in groups.values(): + first = group[0][2] + tensors = { + key: self.torch.cat( + [entry[2][4][key] for entry in group], dim=0 + ) + for key in first[4] + } + self._restore_rng(self.solve_rng) + try: + result = self._invoke(first, tensors) + finally: + self.solve_rng = self._rng() + self._restore_rng(self.owner_rng) + if first[0][0] == "motion": + self.stats["motion_calls"] += 1 + self.stats["logical_motion_calls"] += len(group) + self.stats["merged_motion_calls"] += len(group) > 1 + self.stats["max_motion_group"] = max( + self.stats["max_motion_group"], len(group) + ) + else: + self.stats["solver_calls"] += 1 + self.stats["logical_calls"] += len(group) + self.stats["merged_calls"] += len(group) > 1 + self.stats["max_group"] = max( + self.stats["max_group"], len(group) + ) + start, parts = 0, [] + total = sum(entry[2][5] for entry in group) + for task, row, request in group: + stop = start + request[5] + parts.append(self._part(result, start, stop, total)) + start = stop + for (task, row, _), part in zip(group, parts): + advance(task, row, part) + return ready + finally: + for task in tuple(self.tasks): + if not task.dead: + self._restore_rng(self.tasks[task]) + try: + state = self.tasks[task] + with ( + self.torch.inference_mode(state[5]), + self.torch.set_grad_enabled(state[4]), + ): + task.throw() + finally: + self._restore_rng(self.owner_rng) + self.tasks.clear() + self._restore_rng(self.owner_rng) + + def close(self): + self.motions.close() + for solver, name, value, missing in reversed(self.saved): + if value is missing: + delattr(solver, name) + else: + setattr(solver, name, value) + self.saved.clear() + + +@contextmanager +def batched_planning(robot, seed): + batches = PlanningBatches(robot, seed) + token = _ACTIVE.set(batches) + try: + yield batches + finally: + try: + batches.close() + finally: + _ACTIVE.reset(token) diff --git a/embodichain/lab/expansion/_workload_cursor.py b/embodichain/lab/expansion/_workload_cursor.py new file mode 100644 index 0000000000..b61fe04fea --- /dev/null +++ b/embodichain/lab/expansion/_workload_cursor.py @@ -0,0 +1,105 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Candidate-owned RNG streams around the canonical command handshake.""" + +from __future__ import annotations + +import random +from typing import Any +import numpy as np +import torch +from tensordict import TensorDict + +from embodichain.lab.gym.envs.types import ControllerAction +from embodichain.lab.gym.envs.task_program.bridge import TaskProgramDemoBridge +from .program import _Cursor + +__all__: list[str] = [] + + +class _WorkloadCursor(_Cursor): + def __init__(self, bridge: TaskProgramDemoBridge, robot: Any, seed: int) -> None: + super().__init__(bridge) + self.robot = robot + self.rejection = None + self.last_command = None + self.devices = ( + [ + ( + robot.device.index + if robot.device.index is not None + else torch.cuda.current_device() + ) + ] + if robot.device.type == "cuda" + else [] + ) + self.cpu_rng = torch.Generator().manual_seed(seed).get_state() + self.gpu_rng = ( + torch.Generator(device=robot.device).manual_seed(seed).get_state() + if self.devices + else None + ) + self.numpy_rng = np.random.RandomState(seed % (2**32)).get_state() + self.python_rng = random.Random(seed).getstate() + + def next(self) -> ControllerAction | None: + before_numpy, before_python = np.random.get_state(), random.getstate() + with torch.random.fork_rng(devices=self.devices): + torch.set_rng_state(self.cpu_rng) + if self.devices: + torch.cuda.set_rng_state(self.gpu_rng, self.devices[0]) + np.random.set_state(self.numpy_rng) + random.setstate(self.python_rng) + try: + action = super().next() + if action is not None: + self.last_command = action + if self.finished and self.rejection is not None: + self.accepted, self.reason = False, self.rejection + return action + finally: + self.cpu_rng = torch.get_rng_state() + if self.devices: + self.gpu_rng = torch.cuda.get_rng_state(self.devices[0]) + self.numpy_rng, self.python_rng = ( + np.random.get_state(), + random.getstate(), + ) + np.random.set_state(before_numpy) + random.setstate(before_python) + + def abort(self, reason: str) -> None: + if self.finished or self.rejection is not None or self.emergency is not None: + return + if not self.pending_consumed: + # An unconsumed command must go through the original safe-stop + # iterator; acknowledging it here would manufacture a World step. + super().abort(reason) + return + ids = tuple(self.bridge.env_ids.tolist()) + hold = self.robot.get_qpos(target=True)[list(ids)].clone() + previous = self.last_command.value + if isinstance(previous, torch.Tensor): + value = hold + else: + fields = {"qpos": hold} + if "qvel" in previous: + fields["qvel"] = torch.zeros_like(hold) + value = TensorDict(fields, batch_size=[len(ids)]) + self.bridge.request_row_rejection(ids, ControllerAction(value), reason=reason) + self.rejection = reason diff --git a/embodichain/lab/expansion/collision.py b/embodichain/lab/expansion/collision.py index 966161bc50..99d9af01a0 100644 --- a/embodichain/lab/expansion/collision.py +++ b/embodichain/lab/expansion/collision.py @@ -64,6 +64,7 @@ def _measured_frame(env, snapshot, shapes): [state.qpos for state in entities[env.robot.uid]], dtype=torch.float32 ) return SimpleNamespace( + num_envs=env.num_envs, robot=SimpleNamespace(joint_names=env.robot.joint_names, get_qpos=lambda: qpos), sim=SimpleNamespace( get_rigid_object_uid_list=lambda: tuple(bodies), @@ -106,7 +107,7 @@ def __init__(self, env, profile: str, output: Path): ) self.results = {} - def prepare(self, snapshot=None): + def prepare(self, snapshot=None, *, env_ids=None): from embodichain.lab.task_program.integrations.simulation.feasibility_geometry import ( collision_bodies_from_simulation, ) @@ -116,9 +117,17 @@ def prepare(self, snapshot=None): raise ValueError( "cohort collision geometry differs; create a compatible world" ) - self.results.clear() - self.initial.clear() - self.terminal.clear() + ids = tuple(self.monitors) if env_ids is None else tuple(env_ids) + if ( + not ids + or len(set(ids)) != len(ids) + or any(row not in self.monitors for row in ids) + ): + raise ValueError("prepare requires unique physical monitor rows") + for row in ids: + self.results.pop(row, None) + self.initial.pop(row, None) + self.terminal.pop(row, None) self.shapes = { uid: tuple( self.env.sim.get_rigid_object(uid).get_collision_shapes(env_id=row) @@ -131,25 +140,36 @@ def prepare(self, snapshot=None): if snapshot is None else _measured_frame(self.env, snapshot, self.shapes) ) - for row, monitor in self.monitors.items(): - monitor.reset(measured) - self.initial[row] = all( - monitor.predicate(measured, c) for c in self.initial_conditions - ) and not all(monitor.predicate(measured, c) for c in monitor.final_checks) + for row in ids: + self.monitors[row].reset(measured) + with MeasuredTaskMonitor.batch_scope( + measured, + tuple(self.monitors.values()), + extra_conditions=self.initial_conditions, + ): + for row in ids: + monitor = self.monitors[row] + self.initial[row] = all( + monitor.predicate(measured, c) for c in self.initial_conditions + ) and not all( + monitor.predicate(measured, c) for c in monitor.final_checks + ) def sample(self, ids, action, snapshot=None): env = self.env measured = ( env if snapshot is None else _measured_frame(env, snapshot, self.shapes) ) + poses = env.robot.body_data.body_link_pose + matrices = torch.eye(4, device=poses.device, dtype=poses.dtype).repeat( + env.num_envs, len(env.robot.link_names), 1, 1 + ) + matrices[..., :3, :3] = matrix_from_quat(poses[..., 3:]) + matrices[..., :3, 3] = poses[..., :3] + matrices = matrices.detach().cpu().numpy() frames = { - f"{env.robot.uid}/{name}": env.robot.get_link_pose( - link_name=name, to_matrix=True - ) - .detach() - .cpu() - .numpy() - for name in env.robot.link_names + f"{env.robot.uid}/{name}": matrices[:, index] + for index, name in enumerate(env.robot.link_names) } frames.update( { @@ -161,20 +181,26 @@ def sample(self, ids, action, snapshot=None): for uid in env.sim.get_rigid_object_uid_list() } ) + frames["default_plane"] = np.broadcast_to(np.eye(4), (env.num_envs, 4, 4)) + with MeasuredTaskMonitor.batch_scope(measured, tuple(self.monitors.values())): + for row in ids: + self.monitors[row].observe(measured, action) + self.terminal[row] = self.monitors[row].validation(measured) + groups = {} for row in ids: - self.monitors[row].observe(measured, action) - self.terminal[row] = self.monitors[row].validation(measured) metadata = action.metadata["row_actions"][str(row)] phase = self.phase(env, SimpleNamespace(metadata=metadata), None) - row_frames = {key: value[row] for key, value in frames.items()} - row_frames["default_plane"] = np.eye(4) - report, _ = self.checker.check( - row_frames, - allowed_contacts=phase.allowed_contacts, + groups.setdefault(tuple(phase.allowed_contacts), []).append(row) + for allowed, rows in groups.items(): + reports = self.checker.check_batch( + frames, + allowed_contacts=allowed, moving_objects=self.moving, + env_ids=rows, ) - if row not in self.results or report.status != "passed": - self.results[row] = report + for row, report in zip(rows, reports): + if row not in self.results or report.status != "passed": + self.results[row] = report def close(self): self.checker.close() diff --git a/embodichain/lab/expansion/contracts.py b/embodichain/lab/expansion/contracts.py index e0612291d2..81d3265b36 100644 --- a/embodichain/lab/expansion/contracts.py +++ b/embodichain/lab/expansion/contracts.py @@ -115,19 +115,9 @@ def _poses(values: Mapping[str, torch.Tensor], name: str) -> Mapping[str, torch. def _json(value: object) -> object: - if value is None or type(value) in (str, bool, int): - return value - if type(value) is float and math.isfinite(value): - return value - if isinstance(value, (list, tuple)): - return tuple(_json(item) for item in value) - if isinstance(value, Mapping): - result = {} - for key, item in value.items(): - _text(key, "metadata key") - result[key] = _json(item) - return MappingProxyType(result) - raise ValueError("metadata must contain only finite JSON values") + from embodichain.utils._owned_json import freeze_metadata + + return freeze_metadata(value) def _phases( diff --git a/embodichain/lab/expansion/engine.py b/embodichain/lab/expansion/engine.py index 7e2aa3543d..ef38064d6e 100644 --- a/embodichain/lab/expansion/engine.py +++ b/embodichain/lab/expansion/engine.py @@ -225,6 +225,8 @@ def __init__( self._write_failures: set[str] = set() self._resolved_receipts: dict[tuple[str, int], CommitReceipt] = {} self._failed = False + self._row_assignments: dict[int, ExpansionAssignment] = {} + self._started_rows: set[int] = set() self.timings: dict[str, float] = {} def _measure(self, name, operation, *args, **kwargs): @@ -327,6 +329,8 @@ def run(self) -> Mapping[str, object]: return self._drain() def _drain(self, *, one_batch: bool = False) -> Mapping[str, object]: + if self._row_assignments: + raise RuntimeError("finish active rolling rows before draining a cohort") if self._failed: raise RuntimeError( "host failed; construct a new host before further execution" @@ -476,6 +480,122 @@ def on_started(identity: CandidateIdentity) -> None: break return self._snapshot() + def admit_row(self, item: PreparedEpisodeWorkItem) -> ExpansionAssignment | None: + """Admit one independently prepared physical row through the native session. + + Args: + item: Host-owned candidate proposed in this session, bound to its + actual World row. Cohort collection remains unchanged. + + Returns: + Assigned episode/commit identity, or None for rejected preparation + or session backpressure. No rollout starts until + :meth:`mark_row_started` is called after a consumed command. + """ + if self._configured_cfg is not None or self._failed: + raise RuntimeError("row admission requires a healthy custom engine") + if not isinstance(item, PreparedEpisodeWorkItem): + raise TypeError("rolling admission requires PreparedEpisodeWorkItem") + if ( + not 0 <= item.env_id < self.cfg.batch_size + or item.env_id in self._row_assignments + ): + raise ValueError("physical row is outside capacity or already assigned") + self.flush() + self.enqueue((item,)) + identity = item.candidate.identity + try: + checked = self._measure("admission", self.host.validate_plan, item) + if not checked.accepted: + self.session.release(identity, reason="planning_validation_failed") + self._queue.remove(item) + return None + self.session.add_prepared_episode(item.candidate, checked) + self._admitted.add(identity.candidate_id) + ready = self.session.take_ready( + identity.scene_case_id, + identity.initial_state_id, + episode_byte_budget=self.cfg.episode_byte_budget, + candidate_id=identity.candidate_id, + ) + if ready is None: + self.session.release(identity, reason="row_admission_backpressure") + self._queue.remove(item) + return None + episode_id, commit_id = self.session.episode_ids(identity) + assignment = ExpansionAssignment(item, item.env_id, episode_id, commit_id) + self._row_assignments[item.env_id] = assignment + return assignment + except BaseException: + self._failed = True + self.session.release(identity, reason="row_admission_failed") + if item in self._queue: + self._queue.remove(item) + raise + + def mark_row_started(self, assignment: ExpansionAssignment) -> None: + """Count a rolling rollout after its first actual World command. + + Args: + assignment: Exact identity returned by :meth:`admit_row`. + """ + row = assignment.env_id + if self._row_assignments.get(row) != assignment or row in self._started_rows: + raise ValueError("unexpected or repeated rolling rollout start") + self.session.mark_rollout_started(assignment.item.spec.identity) + self._started_rows.add(row) + + def complete_row( + self, assignment: ExpansionAssignment, episode: ExpertEpisode | None + ) -> CommitReceipt | None: + """Qualify and persist a row using ordinary native receipt accounting. + + Args: + assignment: Exact active row identity returned by :meth:`admit_row`. + episode: Measured evidence for the consumed commands, or rejection. + + Returns: + Confirmed native receipt or None for rejected evidence. Unconfirmed + writes raise and retain pending evidence for the normal reconciliation + API; a physical slot cannot earn quota before persistence confirms. + """ + row = assignment.env_id + if self._row_assignments.get(row) != assignment: + raise ValueError("completion does not identify an active rolling row") + identity = assignment.item.spec.identity + if episode is not None and ( + not isinstance(episode, ExpertEpisode) + or episode.identity != identity + or row not in self._started_rows + or (episode.episode_id, episode.commit_id) + != (assignment.episode_id, assignment.commit_id) + ): + self._failed = True + self.session.release(identity, reason="invalid_measured_evidence") + raise ValueError("host returned mismatched or unstarted evidence") + receipt = None + if episode is None: + self.session.release(identity, reason="host_rejected") + else: + accepted = self.session.accept_episode(episode) + if accepted: + self._pending[episode.commit_id] = episode + callback = getattr(self.host, "on_acceptance", None) + if callback is not None: + callback(assignment, episode, accepted) + if accepted: + self.flush() + key = episode.commit_id, self._submission_ids.get(episode.commit_id, 0) + receipt = self._resolved_receipts.get(key) + if receipt is None or not receipt.confirmed: + raise RuntimeError( + "unconfirmed persistence cannot release a rolling row" + ) + self._queue.remove(assignment.item) + del self._row_assignments[row] + self._started_rows.discard(row) + return receipt + def run_source(self, source: ExpansionRecipeSource) -> Mapping[str, object]: """Drive bounded source batches through the same admission/execution/writer core. diff --git a/embodichain/lab/expansion/episode_export.py b/embodichain/lab/expansion/episode_export.py new file mode 100644 index 0000000000..8c578a251a --- /dev/null +++ b/embodichain/lab/expansion/episode_export.py @@ -0,0 +1,129 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + + +"""One independently readable LeRobot dataset per accepted expansion episode.""" + +from __future__ import annotations + +import hashlib +import json +from pathlib import Path +import shutil +from tempfile import TemporaryDirectory +from uuid import NAMESPACE_URL, uuid5 + +from embodichain.data_pipeline.recording.media import ( + _digest as file_digest, + _atomic_json as atomic_json, +) + +__all__ = ["export_expansion_episodes"] + + +def _verify_dataset(dataset: Path, expected: dict) -> None: + from embodichain.data_pipeline.recording.journal import inspect_recording + from embodichain.lab.expansion import ArtifactEpisodeSink + + saved = json.loads((dataset / "expansion-episode.json").read_text()) + if saved != expected: + raise ValueError(f"LeRobot dataset belongs to different evidence: {dataset}") + info = json.loads((dataset / "meta/info.json").read_text()) + if (info["total_episodes"], info["total_frames"]) != (1, expected["steps"]): + raise ValueError(f"Expected exactly one complete LeRobot episode: {dataset}") + report = inspect_recording(dataset) + identity = str(uuid5(NAMESPACE_URL, expected["episode_id"])) + if not report["ok"] or [row["episode_uuid"] for row in report["commits"]] != [ + identity + ]: + raise ValueError(f"Incomplete LeRobot recording: {dataset}: {report}") + receipts = ArtifactEpisodeSink(dataset / "embodichain_artifacts").inspect() + if [(row.episode_id, row.commit_id) for row in receipts] != [ + (expected["episode_id"], expected["commit_id"]) + ]: + raise ValueError(f"LeRobot evidence bundle differs: {dataset}") + + +def export_expansion_episodes(directory: str | Path) -> dict: + """Save each accepted episode under its source-owned output directory. + + Each returned + ``dataset_path`` is relative to ``directory/lerobot`` and contains exactly + one LeRobot episode. The full T+1 evidence and media remain in the portable + ``embodichain_artifacts/`` bundle alongside the T action rows. + + Args: + directory: Source-owned ArtifactEpisodeSink directory with confirmed + native receipts. It is read and fully verified before exporting. + + Returns: + Manifest with the episode count, export directory and content-addressed + relative dataset paths. Each dataset contains exactly one accepted + episode and its complete portable source/media evidence. Existing exports + are verified rather than silently overwritten. No World is constructed. + """ + from embodichain.lab.expansion import ArtifactEpisodeSink, load_expansion_episode + from embodichain.lab.expansion.export import export_expansion_lerobot + + directory = Path(directory).resolve() + if not directory.is_dir(): + raise FileNotFoundError(f"Expansion directory does not exist: {directory}") + receipts = ArtifactEpisodeSink(directory).inspect() + output = directory / "lerobot" + output.mkdir(parents=True, exist_ok=True) + datasets = [] + for receipt in receipts: + episode = load_expansion_episode(receipt.storage_id) + # Content-addressed names cannot introduce path components from user IDs. + key = hashlib.sha256(receipt.commit_id.encode()).hexdigest() + destination = output / key + identity = { + "schema_version": 1, + "episode_id": episode.episode_id, + "commit_id": episode.commit_id, + "source_sha256": file_digest(Path(receipt.storage_id)), + "steps": len(episode.actions), + } + if destination.exists(): + _verify_dataset(destination, identity) + else: + # The native exporter accepts a complete job. Give it one frozen + # accepted episode, then publish only the self-contained dataset. + # No environment, physics replay or render window is constructed. + with TemporaryDirectory(prefix=".lerobot-", dir=output) as temporary: + isolated = Path(temporary) / "job" + source = Path(receipt.storage_id).parent + if (source / "artifacts.json").is_file(): + # Portable committed evidence is already frozen and fully + # verified above. Preserve its bytes and receipts instead + # of loading/freezing/compressing an identical new episode. + shutil.copytree(source, isolated / "episodes" / source.name) + else: + ArtifactEpisodeSink(isolated).submit(episode, submission_id=0) + result = export_expansion_lerobot(None, isolated) + dataset = Path(result["dataset_path"]) + atomic_json(dataset / "expansion-episode.json", identity) + _verify_dataset(dataset, identity) + dataset.rename(destination) + datasets.append({**identity, "dataset_path": key}) + manifest = { + "schema_version": 1, + "format": "lerobot", + "episodes": len(datasets), + "datasets": datasets, + } + atomic_json(output / "manifest.json", manifest) + return {**manifest, "directory": str(output)} diff --git a/embodichain/lab/expansion/persistence.py b/embodichain/lab/expansion/persistence.py index 5683a8033a..4d688b8841 100644 --- a/embodichain/lab/expansion/persistence.py +++ b/embodichain/lab/expansion/persistence.py @@ -18,12 +18,14 @@ from __future__ import annotations from dataclasses import asdict, replace +from collections import OrderedDict import hashlib import json import os from pathlib import Path import tempfile import shutil +from threading import RLock import numpy as np from embodichain.lab.gym.envs._json import json_safe_copy from .contracts import ExpertEpisode, CommitReceipt @@ -39,6 +41,10 @@ __all__ = ["ArtifactEpisodeSink", "load_expansion_episode"] +_EPISODE_CACHE = OrderedDict() +_EPISODE_CACHE_LOCK = RLock() +_EPISODE_CACHE_BYTES = 0 + def _source_references(metadata): references = [ @@ -65,6 +71,45 @@ def load_expansion_episode(path: str | Path) -> ExpertEpisode: Legacy staged episodes remain readable in place. New artifacts carry only relative references and can be moved without changing their frozen bytes. """ + global _EPISODE_CACHE_BYTES + path = Path(path).resolve() + # Re-read all evidence bytes on every call. Path/mtime keys alone could + # conceal tampering; this cache only avoids repeated parsing/JSON ownership. + with path.open("rb") as stream: + digest = hashlib.file_digest(stream, "sha256").digest() + with _EPISODE_CACHE_LOCK: + cached = _EPISODE_CACHE.get(path) + if cached is not None and cached[0] == digest: + _EPISODE_CACHE.move_to_end(path) + # Construction retains every numeric check and owns fresh tensors. + return replace(cached[1]) + episode = _load_expansion_episode(path) + with path.open("rb") as stream: + if hashlib.file_digest(stream, "sha256").digest() != digest: + raise ValueError("Evidence changed while loading") + prototype = replace(episode) + size = sum( + value.numel() * value.element_size() + for value in ( + prototype.actions, + prototype.timestamps, + *prototype.observations.values(), + ) + ) + with _EPISODE_CACHE_LOCK: + previous = _EPISODE_CACHE.pop(path, None) + if previous is not None: + _EPISODE_CACHE_BYTES -= previous[2] + if size <= 64 * 1024 * 1024: + _EPISODE_CACHE[path] = digest, prototype, size + _EPISODE_CACHE_BYTES += size + while len(_EPISODE_CACHE) > 16 or _EPISODE_CACHE_BYTES > 128 * 1024 * 1024: + _, (_, _, released) = _EPISODE_CACHE.popitem(last=False) + _EPISODE_CACHE_BYTES -= released + return episode + + +def _load_expansion_episode(path: Path) -> ExpertEpisode: from embodichain.lab.gym.envs.managers.real2sim_collection import ( load_staged_real2sim_episode, ) diff --git a/embodichain/lab/expansion/workloads.py b/embodichain/lab/expansion/workloads.py new file mode 100644 index 0000000000..3f575924ba --- /dev/null +++ b/embodichain/lab/expansion/workloads.py @@ -0,0 +1,648 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""One World, rolling physical rows and receipt-qualified workload quotas.""" + +from __future__ import annotations + +from collections.abc import Callable, Mapping, Sequence +from contextlib import nullcontext +from dataclasses import dataclass +import math +import os +from threading import get_ident +import time +from typing import Any, Protocol, TYPE_CHECKING + +from embodichain.utils import configclass +from .contracts import CommitReceipt + +if TYPE_CHECKING: + from embodichain.lab.gym.envs.task_program.bridge import TaskProgramDemoBridge + from embodichain.lab.gym.envs.types import ControllerAction + from .program import ProgramBatchResult + +__all__ = [ + "ExpansionWorkloadCfg", + "WorkloadExecutionCfg", + "WorkloadAssignment", + "WorkloadRowResult", + "WorkloadSource", + "WorkloadProgramExecutor", +] + + +@configclass +class ExpansionWorkloadCfg: + """Bound one logical source independently of its physical slot. + + Args: + workload_id: Unique source/recipe group identifier. + target_episodes: Confirmed qualified receipts required for this group. + max_attempts: Physical candidate budget, including rejected preparations. + max_wall_time_s: Admission budget starting with the first candidate. + Already active trajectories retain their ordinary safe-stop path. + """ + + workload_id: str = "default" + target_episodes: int = 1 + max_attempts: int = 8 + max_wall_time_s: float = 600.0 + + def __post_init__(self) -> None: + if ( + type(self.workload_id) is not str + or not self.workload_id + or self.workload_id.strip() != self.workload_id + ): + raise ValueError("workload_id must be a nonempty stripped string") + for name in ("target_episodes", "max_attempts"): + if type(getattr(self, name)) is not int or getattr(self, name) < 1: + raise ValueError(f"{name} must be a positive integer") + if self.max_attempts < self.target_episodes: + raise ValueError("max_attempts must cover target_episodes") + if ( + type(self.max_wall_time_s) not in (int, float) + or not math.isfinite(self.max_wall_time_s) + or self.max_wall_time_s <= 0 + ): + raise ValueError("max_wall_time_s must be finite and positive") + + +@configclass +class WorkloadExecutionCfg: + """Declare physical capacity without allocating processes or Worlds. + + Args: + workload_count: Maximum simultaneous independent source groups, W. + batch_size: Candidate rows reserved for each active group, B. + max_steps: Maximum ordinary controller commands per candidate. + settling_steps: Minimum physical substeps before a row becomes active. + seed: Base seed; each candidate owns a stream keyed by local ordinal. + batch_planning: Cooperatively merge compatible native motion/solver calls. + """ + + workload_count: int = 1 + batch_size: int = 1 + max_steps: int = 2000 + settling_steps: int = 5 + seed: int = 0 + batch_planning: bool = True + + def __post_init__(self) -> None: + for name in ("workload_count", "batch_size", "max_steps"): + if type(getattr(self, name)) is not int or getattr(self, name) < 1: + raise ValueError(f"{name} must be a positive integer") + for name in ("settling_steps", "seed"): + if type(getattr(self, name)) is not int or getattr(self, name) < 0: + raise ValueError(f"{name} must be a nonnegative integer") + if type(self.batch_planning) is not bool: + raise TypeError("batch_planning must be a bool") + + @property + def num_envs(self) -> int: + """Return the required single-World capacity, W multiplied by B.""" + return self.workload_count * self.batch_size + + +@dataclass(frozen=True) +class WorkloadAssignment: + """Logical candidate identity bound to a physical row for one attempt. + + Args: + workload_id: Source group, independent of lane/row allocation. + ordinal: Zero-based candidate index local to this workload. + env_id: Physical environment row in the sole World. + seed: Candidate-owned random stream seed. + """ + + workload_id: str + ordinal: int + env_id: int + seed: int + + +@dataclass(frozen=True) +class WorkloadRowResult: + """Canonical bridge outcome and exact World control-boundary span. + + Args: + assignment: Candidate whose bridge and measured buffer were consumed. + program: Original Task Program result for this one physical row. + world_control_start: First actually consumed command boundary, or None. + world_control_end: Last consumed command boundary, or None. + """ + + assignment: WorkloadAssignment + program: ProgramBatchResult + world_control_start: int | None + world_control_end: int | None + + +class WorkloadSource(Protocol): + """Task-owned scene/plan/evidence ports; the executor alone steps the World. + + Sources retain native ExpansionSession admission, required physical checks, + evidence assembly and sink receipts. ``complete`` may return a receipt only + after all required checks pass and native persistence confirms the write. + Callbacks must not advance physics or create another World. A completed + group's frozen evidence can be exported independently of the live World. + """ + + def prepare(self, assignments: tuple[WorkloadAssignment, ...]) -> None: + """Apply selected-row changes inside the trusted reset callback.""" + ... + + def activate( + self, assignment: WorkloadAssignment, observation: object + ) -> TaskProgramDemoBridge | None: + """Validate measured preparation, capture T+1 initial data and bind a row.""" + ... + + def started(self, assignment: WorkloadAssignment, world_control_step: int) -> None: + """Mark rollout started in the native session after its first actual step.""" + ... + + def observe( + self, + assignments: tuple[WorkloadAssignment, ...], + action: ControllerAction, + observation: object, + ) -> Sequence[int]: + """Record every active boundary; return rows requiring safe rejection.""" + ... + + def complete(self, result: WorkloadRowResult) -> CommitReceipt | None: + """Assemble native evidence and return its confirmed receipt or rejection.""" + ... + + +class WorkloadProgramExecutor: + """Refill independent rows while preserving native bridge acknowledgement. + + Args: + environment: One initialized environment with exactly W times B rows. + workloads: Ordered source groups with independent quotas and budgets. + source: Host-owned scene, Task Program, validation and persistence ports. + cfg: Capacity and control/settling budgets; creates no World itself. + checkpoint: Optional cooperative cancellation/window callback, called at + control boundaries on the owner thread. + on_workload_done: Optional frozen-group notification. It receives an + owned count mapping and must not advance or mutate the live World. + """ + + def __init__( + self, + environment: Any, + workloads: Sequence[ExpansionWorkloadCfg], + source: WorkloadSource, + cfg: WorkloadExecutionCfg, + *, + checkpoint: Callable[[], None] | None = None, + on_workload_done: Callable[[str, Mapping[str, object]], None] | None = None, + ) -> None: + import torch + + if not isinstance(cfg, WorkloadExecutionCfg): + raise TypeError("cfg must be WorkloadExecutionCfg") + cfg.__post_init__() + self.env = getattr(environment, "unwrapped", environment) + if self.env.num_envs != cfg.num_envs: + raise ValueError( + "the single World must contain workload_count * batch_size environments" + ) + events = getattr(self.env, "event_manager", None) + if events is not None and events.active_functors.get("interval"): + raise ValueError("rolling expansion requires fixed episode parameters") + if not workloads or any( + not isinstance(w, ExpansionWorkloadCfg) for w in workloads + ): + raise ValueError("workloads must contain ExpansionWorkloadCfg values") + for workload in workloads: + workload.__post_init__() + self.workloads = { + w.workload_id: ExpansionWorkloadCfg(**w.to_dict()) for w in workloads + } + if len(self.workloads) != len(workloads) or cfg.workload_count > len(workloads): + raise ValueError("workload IDs must be unique and cover every active lane") + self.cfg, self.source = WorkloadExecutionCfg(**cfg.to_dict()), source + self.checkpoint, self.on_workload_done = checkpoint, on_workload_done + self.owner = get_ident() + from embodichain.lab.sim import SimulationManager + + if len(SimulationManager._instances) != 1: + raise ValueError( + "rolling workload collection requires one live World in its process" + ) + state_device = self.env.robot.get_qpos().device + if state_device.type != self.env.sim.device.type: + raise RuntimeError("measured robot state and World devices disagree") + self._world_info = { + "process_id": os.getpid(), + "world_instance_id": self.env.sim.instance_id, + "world_object_id": id(self.env.sim.get_world()), + "world_count": len(SimulationManager._instances), + "physics_backend": self.env.sim.physics_backend, + "physics_device": str(self.env.sim.device), + "state_device": str(state_device), + "gpu_physics": bool(self.env.sim.is_use_gpu_physics), + "solvers": { + name: {"class": type(solver).__name__, "device": str(solver.device)} + for name, solver in self.env.robot._solvers.items() + }, + } + self._used = False + self.lanes: list[str | None] = [None] * cfg.workload_count + self.counts = { + name: {"attempts": 0, "rollouts": 0, "committed": 0} + for name in self.workloads + } + self.started_at: dict[str, float] = {} + self.warming: dict[int, dict[str, Any]] = {} + self.live: dict[int, dict[str, Any]] = {} + self.completed: set[str] = set() + self.receipts: set[tuple[str, str, str]] = set() + self.control_steps = self.active_control_steps = self.refills = 0 + self.hold = self.env.robot.get_qpos(target=True).clone() + self.observation = None + self.planning_stats: dict[str, int] = {} + self.torch = torch + + @property + def world_info(self) -> Mapping[str, object]: + """Return owned actual process, World, state and solver device identities.""" + from copy import deepcopy + + return deepcopy(self._world_info) + + def _eligible(self, name: str) -> bool: + workload, counts = self.workloads[name], self.counts[name] + return ( + counts["committed"] < workload.target_episodes + and counts["attempts"] < workload.max_attempts + and ( + name not in self.started_at + or time.monotonic() - self.started_at[name] < workload.max_wall_time_s + ) + ) + + def _inflight(self, name: str) -> int: + return sum( + state["assignment"].workload_id == name + for state in (*self.live.values(), *self.warming.values()) + ) + + def _group_done(self, name: str) -> None: + if name not in self.completed: + self.completed.add(name) + if self.on_workload_done is not None: + self.on_workload_done(name, dict(self.counts[name])) + + def _fill(self) -> None: + added = [] + occupied = self.live.keys() | self.warming.keys() + for lane, name in enumerate(self.lanes): + if ( + name is not None + and not self._inflight(name) + and not self._eligible(name) + ): + self._group_done(name) + name = None + if name is None: + name = next( + ( + key + for key in self.workloads + if key not in self.lanes and self._eligible(key) + ), + None, + ) + self.lanes[lane] = name + if name is None: + continue + for row in range( + lane * self.cfg.batch_size, (lane + 1) * self.cfg.batch_size + ): + if row in occupied or not self._eligible(name): + continue + counts, spec = self.counts[name], self.workloads[name] + if counts["committed"] + self._inflight(name) >= spec.target_episodes: + break + assignment = WorkloadAssignment( + name, counts["attempts"], row, self.cfg.seed + counts["attempts"] + ) + counts["attempts"] += 1 + self.started_at.setdefault(name, time.monotonic()) + self.warming[row] = {"assignment": assignment, "settled": 0} + added.append(assignment) + if not added: + return + self.refills += bool(self.live) + assignments = tuple(added) + result = self.env.reset( + seed=self.cfg.seed, + options={ + "save_data": False, + "reset_ids": [a.env_id for a in added], + "episode_prepare": lambda: self.source.prepare(assignments), + }, + ) + self.observation = result[0] + rows = [a.env_id for a in added] + self.hold[rows] = self.env.robot.get_qpos(target=True)[rows] + + def _activate(self) -> None: + from ._workload_cursor import _WorkloadCursor + from .program import ProgramBatchResult + + for row in tuple(self.warming): + state = self.warming[row] + if state["settled"] < self.cfg.settling_steps: + continue + assignment = state["assignment"] + self.env._elapsed_steps[row] = 0 + self.env._reset_physical_objective((row,)) + bridge = self.source.activate(assignment, self.observation) + del self.warming[row] + if bridge is None: + outcome = ProgramBatchResult( + (row,), (0,), (False,), ("preparation_rejected",), ((),) + ) + if ( + self.source.complete( + WorkloadRowResult(assignment, outcome, None, None) + ) + is not None + ): + raise ValueError( + "unstarted or rejected preparation cannot earn a receipt" + ) + continue + if ( + tuple(bridge.env_ids.tolist()) != (row,) + or bridge.clock.step_index != 0 + or not math.isclose( + bridge.clock.step_dt, self.env.step_dt, rel_tol=0, abs_tol=1e-12 + ) + ): + raise ValueError( + "bridge must freshly bind exactly the assigned physical row and clock" + ) + self.live[row] = { + **state, + "cursor": _WorkloadCursor(bridge, self.env.robot, assignment.seed), + "start": None, + } + + def _finish(self, row: int) -> None: + from .program import ProgramBatchResult + + state = self.live[row] + assignment, cursor = state["assignment"], state["cursor"] + end = self.control_steps if cursor.length else None + if cursor.length and end - state["start"] + 1 != cursor.length: + raise RuntimeError("an active trajectory missed a World control boundary") + result = ProgramBatchResult( + (row,), + (cursor.length,), + (cursor.accepted,), + (cursor.reason,), + (tuple(cursor.results),), + ) + receipt = self.source.complete( + WorkloadRowResult(assignment, result, state["start"], end) + ) + if receipt is not None: + if ( + not isinstance(receipt, CommitReceipt) + or not receipt.confirmed + or not cursor.accepted + or not cursor.length + ): + raise ValueError( + "only confirmed qualified native receipts earn workload quota" + ) + identity = receipt.episode_id, receipt.candidate_id, receipt.commit_id + if any( + identity[i] == known[i] for known in self.receipts for i in range(3) + ): + raise ValueError( + "a repeated episode/candidate/commit cannot earn quota twice" + ) + self.receipts.add(identity) + self.counts[assignment.workload_id]["committed"] += 1 + cursor.close() + del self.live[row] + + def run(self) -> Mapping[str, object]: + """Collect until every group meets quota or exhausts its original budget. + + Returns: + Owned counts, completeness and actual World control/planning stats. + Failed quotas remain explicit; partial results never imply success. + """ + if get_ident() != self.owner or self._used: + raise RuntimeError( + "a workload executor is single-use on its World owner thread" + ) + self._used = True + from tensordict import TensorDict + from embodichain.lab.gym.envs.types import ControllerAction + from ._planning_batches import batched_planning + + env, torch = self.env, self.torch + previous_auto = getattr(env, "_demo_no_auto_reset", False) + had_mask = hasattr(env, "_demo_active_mask") + previous_mask = getattr(env, "_demo_active_mask", None) + env._demo_no_auto_reset = True + primary = None + planning = ( + batched_planning(env.robot, self.cfg.seed) + if self.cfg.batch_planning + else nullcontext() + ) + try: + with planning as planner: + while True: + if self.checkpoint is not None: + self.checkpoint() + self._fill() + self._activate() + cursors = {row: state["cursor"] for row, state in self.live.items()} + commands = ( + planner.gather(cursors) + if planner is not None + else { + row: command + for row, cursor in cursors.items() + if (command := cursor.next()) is not None + } + ) + for row in tuple(self.live): + if row not in commands: + self._finish(row) + if not commands and not self.warming: + if any(self._eligible(name) for name in self.workloads): + continue + break + qpos, qvel = self.hold.clone(), torch.zeros_like(self.hold) + velocity = ( + env.expert_action_spec.joint_command_mode == "position_velocity" + ) + for row in tuple(commands): + cursor = self.live[row]["cursor"] + if ( + cursor.length >= self.cfg.max_steps + and cursor.emergency is None + ): + cursor.abort("step_budget_exhausted") + replacement = cursor.next() + if replacement is None: + del commands[row] + self._finish(row) + continue + commands[row] = replacement + value = commands[row].value + fields = ( + {"qpos": value} + if isinstance(value, torch.Tensor) + else value + ) + if "qpos" not in fields or set(fields) - {"qpos", "qvel"}: + raise ValueError( + "workload rows require explicit position controller commands" + ) + if velocity and "qvel" not in fields: + raise ValueError( + "position-velocity evidence requires explicit velocity commands" + ) + for name, target in (("qpos", qpos), ("qvel", qvel)): + if name not in fields: + continue + data = fields[name] + if data.ndim != 2 or data.shape[0] != 1: + raise ValueError( + "row commands must retain one physical batch row" + ) + columns = ( + list(env.active_joint_ids) + if data.shape[1] == len(env.active_joint_ids) + else list(range(env.robot.dof)) + ) + if data.shape[1] != len(columns): + raise ValueError("row command joint layout differs") + target[row, columns] = data[0] + velocity |= name == "qvel" + if not commands and not self.warming: + continue + payload = {"qpos": qpos} + if velocity: + payload["qvel"] = qvel + action = ControllerAction( + TensorDict(payload, batch_size=[env.num_envs]), + metadata={ + "row_actions": { + str(row): dict(command.metadata) + for row, command in commands.items() + } + }, + ) + mask = torch.zeros( + env.num_envs, dtype=torch.bool, device=env.device + ) + mask[list(commands)] = True + env._demo_active_mask = mask + before = env.sim.physics_step_index + self.observation, _, terminated, truncated, _ = env.step(action) + advanced = env.sim.physics_step_index - before + if advanced <= 0: + raise RuntimeError( + "the owning World did not advance physical time" + ) + self.control_steps += 1 + self.active_control_steps += len(commands) + self.hold = qpos + for state in self.warming.values(): + state["settled"] += advanced + for row in commands: + state, cursor = self.live[row], self.live[row]["cursor"] + if cursor.length == 0: + state["start"] = self.control_steps + self.counts[state["assignment"].workload_id][ + "rollouts" + ] += 1 + self.source.started(state["assignment"], self.control_steps) + cursor.length += 1 + cursor.pending_consumed = True + assignments = tuple( + self.live[row]["assignment"] for row in commands + ) + rejected = ( + tuple( + self.source.observe(assignments, action, self.observation) + ) + if commands + else () + ) + if ( + len(set(rejected)) != len(rejected) + or set(rejected) - commands.keys() + ): + raise ValueError( + "source rejection must select unique active physical rows" + ) + for row in commands: + if row in rejected: + self.live[row]["cursor"].abort("measured_validation_failed") + elif bool(terminated[row]) or bool(truncated[row]): + self.live[row]["cursor"].abort("environment_terminated") + if planner is not None: + self.planning_stats = dict(planner.stats) + for name in self.workloads: + self._group_done(name) + return { + "complete": all( + self.counts[name]["committed"] == spec.target_episodes + for name, spec in self.workloads.items() + ), + "num_envs": self.cfg.num_envs, + "workload_count": self.cfg.workload_count, + "batch_size": self.cfg.batch_size, + "counts": {name: dict(values) for name, values in self.counts.items()}, + "control_steps": self.control_steps, + "active_control_steps": self.active_control_steps, + "refills": self.refills, + "planning": dict(self.planning_stats), + "world": self.world_info, + } + except BaseException as error: + primary = error + raise + finally: + cleanup = None + for state in self.live.values(): + try: + state["cursor"].close() + except BaseException as error: + cleanup = error + env._demo_no_auto_reset = previous_auto + if had_mask: + env._demo_active_mask = previous_mask + elif hasattr(env, "_demo_active_mask"): + delattr(env, "_demo_active_mask") + if primary is None and cleanup is not None: + raise cleanup diff --git a/embodichain/lab/gym/envs/_json.py b/embodichain/lab/gym/envs/_json.py index 660c687214..90ed8e7487 100644 --- a/embodichain/lab/gym/envs/_json.py +++ b/embodichain/lab/gym/envs/_json.py @@ -27,6 +27,10 @@ def json_safe_copy(value: Any, *, field_name: str) -> Any: """Return an owned JSON value without implicit type coercion.""" + from embodichain.utils._owned_json import _FrozenMap + + if type(value) is _FrozenMap and value._strict: + return value.mutable_copy() if value is None or type(value) in {bool, int, str}: return value if type(value) is float: diff --git a/embodichain/lab/sim/atomic_skills/_command_batches.py b/embodichain/lab/sim/atomic_skills/_command_batches.py new file mode 100644 index 0000000000..5aa5d41d1c --- /dev/null +++ b/embodichain/lab/sim/atomic_skills/_command_batches.py @@ -0,0 +1,188 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + + +"""Validate owned CUDA command snapshots in bounded CPU slabs.""" + +from __future__ import annotations + +from collections import defaultdict +import torch +from .bindings import JointPositionTarget +from .runtime_commands import ( + EndpointCommand, + JointPositionPayload, + RuntimeCommandFrame, + TimedCommandSequence, +) + +__all__: list[str] = [] + + +def _transfer(tensors, device): + """Own one slab per dtype; preserve bits, shapes and independent field spans.""" + import torch + + grouped = defaultdict(dict) + for tensor in tensors: + grouped[tensor.dtype][id(tensor)] = tensor + copied = {} + for group in grouped.values(): + values = tuple(group.values()) + packed = torch.cat([value.detach().reshape(-1) for value in values]).to(device) + for value, part in zip(values, packed.split([v.numel() for v in values])): + copied[id(value)] = part.reshape(value.shape) + return copied + + +def leaves(frame): + if type(frame) is not RuntimeCommandFrame or type(frame.commands) not in ( + list, + tuple, + ): + return None + tensors = [frame.active_mask, frame.env_ids, frame.hold_duration] + for command in frame.commands: + if ( + type(command) is not EndpointCommand + or type(command.target) is not JointPositionTarget + or type(command.payload) is not JointPositionPayload + ): + return None + tensors.append(command.payload.positions) + if command.payload.velocities is not None: + tensors.append(command.payload.velocities) + if any(type(value) is not torch.Tensor or value.requires_grad for value in tensors): + return None + if frame.env_ids.device.type != "cuda" or any( + value.device != frame.env_ids.device for value in tensors + ): + return None + return tensors + + +def cpu_frame(frame, copied): + commands = tuple( + EndpointCommand( + target=command.target, + payload=JointPositionPayload( + positions=copied[id(command.payload.positions)], + velocities=( + copied[id(command.payload.velocities)] + if command.payload.velocities is not None + else None + ), + ), + ) + for command in frame.commands + ) + return RuntimeCommandFrame( + commands=commands, + active_mask=copied[id(frame.active_mask)], + env_ids=copied[id(frame.env_ids)], + hold_duration=copied[id(frame.hold_duration)], + ) + + +def own(cls, **fields): + value = object.__new__(cls) + for name, field in fields.items(): + object.__setattr__(value, name, field) + return value + + +def lift_frame(frame, copied): + return own( + RuntimeCommandFrame, + commands=tuple( + own( + EndpointCommand, + target=command.target, + payload=own( + JointPositionPayload, + positions=copied[id(command.payload.positions)], + velocities=( + copied[id(command.payload.velocities)] + if command.payload.velocities is not None + else None + ), + ), + ) + for command in frame.commands + ), + active_mask=copied[id(frame.active_mask)], + env_ids=copied[id(frame.env_ids)], + hold_duration=copied[id(frame.hold_duration)], + ) + + +def numeric(frame): + # Already passed all native CPU checks; collect owned validated fields. + values = [frame.active_mask, frame.env_ids, frame.hold_duration] + for command in frame.commands: + values.append(command.payload.positions) + if command.payload.velocities is not None: + values.append(command.payload.velocities) + return values + + +def validate_frame(frame): + values = leaves(frame) + if values is None: + return False + checked = cpu_frame(frame, _transfer(values, "cpu")) + result = lift_frame(checked, _transfer(numeric(checked), frame.env_ids.device)) + for name in ("commands", "active_mask", "env_ids", "hold_duration"): + object.__setattr__(frame, name, getattr(result, name)) + return True + + +def validate_sequence(sequence): + if ( + type(sequence.frames) not in (list, tuple) + or type(sequence.env_ids) is not torch.Tensor + or sequence.env_ids.device.type != "cuda" + or sequence.env_ids.requires_grad + ): + return False + tensors = [sequence.env_ids] + for frame in sequence.frames: + values = leaves(frame) + if values is None or frame.env_ids.device != sequence.env_ids.device: + return False + tensors.extend(values) + copied = _transfer(tensors, "cpu") + # Preserve native sequence-before-frame rejection order, including + # malformed mutable IDs in an otherwise previously valid snapshot. + TimedCommandSequence(frames=(), env_ids=copied[id(sequence.env_ids)]) + for index, frame in enumerate(sequence.frames): + if not torch.equal(copied[id(frame.env_ids)], copied[id(sequence.env_ids)]): + raise ValueError(f"Frame {index} env_ids do not match the sequence.") + checked = TimedCommandSequence( + frames=tuple(cpu_frame(frame, copied) for frame in sequence.frames), + env_ids=copied[id(sequence.env_ids)], + ) + tensors = [checked.env_ids] + for frame in checked.frames: + tensors.extend(numeric(frame)) + copied = _transfer(tensors, sequence.env_ids.device) + object.__setattr__( + sequence, + "frames", + tuple(lift_frame(frame, copied) for frame in checked.frames), + ) + object.__setattr__(sequence, "env_ids", copied[id(checked.env_ids)]) + return True diff --git a/embodichain/lab/sim/atomic_skills/runtime_commands.py b/embodichain/lab/sim/atomic_skills/runtime_commands.py index 606438a584..de474be3f9 100644 --- a/embodichain/lab/sim/atomic_skills/runtime_commands.py +++ b/embodichain/lab/sim/atomic_skills/runtime_commands.py @@ -260,6 +260,10 @@ class RuntimeCommandFrame: hold_duration: torch.Tensor def __post_init__(self) -> None: + from ._command_batches import validate_frame + + if validate_frame(self): + return if isinstance(self.commands, (str, bytes)): raise TypeError("commands must be an iterable of EndpointCommand values.") try: @@ -410,6 +414,10 @@ class TimedCommandSequence: env_ids: torch.Tensor def __post_init__(self) -> None: + from ._command_batches import validate_sequence + + if validate_sequence(self): + return if not isinstance(self.env_ids, torch.Tensor): raise TypeError("env_ids must be a torch.Tensor.") if ( diff --git a/embodichain/lab/sim/cfg/physics.py b/embodichain/lab/sim/cfg/physics.py index c04440966d..c75ee86548 100644 --- a/embodichain/lab/sim/cfg/physics.py +++ b/embodichain/lab/sim/cfg/physics.py @@ -133,6 +133,15 @@ class PhysicsBackendCfg: class DefaultPhysicsCfg(PhysicsBackendCfg): """Configuration selector for the Default physics backend.""" + direct_gpu_api: bool = True + """Use Direct GPU state access when dynamics run on CUDA. + + False retains GPU dynamics with ordinary state/contact reporting. The + pinned Direct GPU contact API cannot identify static actors, so consumers + requiring complete contact identities must explicitly select False. + This option does not select the physics device or change contact sampling. + """ + cache_material: bool = False """Reuse identical physics materials across objects and environment clones. @@ -168,6 +177,8 @@ class DefaultPhysicsCfg(PhysicsBackendCfg): def __post_init__(self) -> None: """Decode mapping input for the nested GPU-memory configuration.""" + if type(self.direct_gpu_api) is not bool: + raise TypeError("direct_gpu_api must be a bool.") if isinstance(self.gpu_memory, Mapping): self.gpu_memory = GPUMemoryCfg(**self.gpu_memory) if not isinstance(self.gpu_memory, GPUMemoryCfg): diff --git a/embodichain/lab/sim/motion/motion_generator.py b/embodichain/lab/sim/motion/motion_generator.py index 88bae56c6a..e4c0b7d87c 100644 --- a/embodichain/lab/sim/motion/motion_generator.py +++ b/embodichain/lab/sim/motion/motion_generator.py @@ -761,6 +761,13 @@ def generate( Raises: ValueError: If targets or options violate the motion contract. """ + # The optional expansion scope collects requests on the World owner. + # It is inert for ordinary callers and preserves native budget scopes. + from embodichain.lab.expansion._planning_batches import _try_motion + + handled, result = _try_motion(self, target_states, options, defer_recovery) + if handled: + return result if not target_states: raise ValueError("target_states must contain at least one waypoint.") options = MotionGenOptions() if options is None else options diff --git a/embodichain/lab/sim/motion/planners/base_planner.py b/embodichain/lab/sim/motion/planners/base_planner.py index 99ff6a1d79..a0daf18f50 100644 --- a/embodichain/lab/sim/motion/planners/base_planner.py +++ b/embodichain/lab/sim/motion/planners/base_planner.py @@ -184,8 +184,10 @@ def wrapper(self, *args, **kwargs): target_states = kwargs.get("target_states", args[0] if args else None) if target_states is not None and hasattr(self, "robot"): robot_num = getattr(self.robot, "num_instances", None) + selected = getattr(self, "_planning_env_ids", None) + expected = len(selected) if selected is not None else robot_num _check_batch_consistency( - target_states, expected_b=robot_num, robot_num_instances=robot_num + target_states, expected_b=expected, robot_num_instances=expected ) return func(self, *args, **kwargs) diff --git a/embodichain/lab/sim/motion/solvers/base_solver.py b/embodichain/lab/sim/motion/solvers/base_solver.py index d80e0979cc..54e5cbe6e0 100644 --- a/embodichain/lab/sim/motion/solvers/base_solver.py +++ b/embodichain/lab/sim/motion/solvers/base_solver.py @@ -697,9 +697,15 @@ def get_jacobian( # Ensure locations is a tensor if provided if locations is not None: + from pytorch_kinematics import Transform3d + locations = torch.as_tensor( locations, dtype=torch.float32, device=self.device ) + # PK 0.10 constructs a CPU Transform3d for raw offset tensors. + # Supply its declared device instead of mixing CPU homogeneous + # padding with CUDA offsets inside the dependency. + locations = Transform3d(pos=locations, dtype=qpos.dtype, device=qpos.device) # Compute the Jacobian using the kinematics chain J = self.pk_serial_chain.jacobian(th=qpos, locations=locations) diff --git a/embodichain/lab/sim/motion/solvers/opw_solver.py b/embodichain/lab/sim/motion/solvers/opw_solver.py index 7d06e3e561..53e38a1ae1 100644 --- a/embodichain/lab/sim/motion/solvers/opw_solver.py +++ b/embodichain/lab/sim/motion/solvers/opw_solver.py @@ -186,7 +186,8 @@ def get_fk_warp(self, qpos: torch.tensor, **kwargs) -> torch.tensor: qpos_batch = qpos n_sample = qpos_batch.shape[0] # Retimed trajectories retain float64; the OPW kernel consumes float32. - qpos_wp = wp.from_torch(qpos_batch.to(dtype=torch.float32).reshape(-1)) + qpos_batch = qpos_batch.to(device=self.device, dtype=torch.float32).contiguous() + qpos_wp = wp.from_torch(qpos_batch.reshape(-1)) xpos_wp = wp.zeros( n_sample * 16, dtype=float, device=standardize_device_string(self.device) ) diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index 221bb22ea7..eccc9f96f6 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -2027,8 +2027,8 @@ def set_state( ) if ( (root_pose is not None or qpos is not None) - and self.device.type == "cpu" and not self._data.is_newton_backend + and not self._world.get_physics_scene().is_direct_gpu() ): # Direct root/joint writes update native kinematics, but CPU link # transforms are published by World.update. Do not advance physics diff --git a/embodichain/lab/sim/physics/default.py b/embodichain/lab/sim/physics/default.py index 22d29c8d27..078c82f00c 100644 --- a/embodichain/lab/sim/physics/default.py +++ b/embodichain/lab/sim/physics/default.py @@ -44,7 +44,7 @@ def configure_world(self, world_config, sim_config: "SimulationManagerCfg") -> N world_config.speed_tolerance = cfg.speed_tolerance if self._manager.device.type == "cuda": world_config.enable_gpu_sim = True - world_config.direct_gpu_api = True + world_config.direct_gpu_api = cfg.direct_gpu_api def activate(self, sim_config: "SimulationManagerCfg") -> None: cfg = sim_config.physics_cfg diff --git a/embodichain/lab/sim/scene/feasibility/_collision_batches.py b/embodichain/lab/sim/scene/feasibility/_collision_batches.py new file mode 100644 index 0000000000..6ffe907fc9 --- /dev/null +++ b/embodichain/lab/sim/scene/feasibility/_collision_batches.py @@ -0,0 +1,335 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + + +"""Batch the pinned Newton convex query without touching the live World.""" + +from __future__ import annotations + +import math + +import numpy as np + +__all__: list[str] = [] + + +class _CollisionBatches: + """Keep native shapes, precision, pair exemptions and halfspace supports. + + The query owns one numerical model containing independent replicas. It is + never stepped and creates no simulation World. Explicit pairs prohibit + cross-environment contacts; only the small per-row summaries are read back. + """ + + def __init__(self, checker, count, allowed, moving, all_pairs=False): + import newton + import warp as wp + + self.checker, self.count, self.wp = checker, count, wp + self.graph = None + self.queries = 0 + self.mask, self.planes, self.has_native = checker._mask( + allowed, moving, all_pairs + ) + self.locals = np.asarray([body.local_pose for body in checker.bodies]) + self.indices = [ + checker._frame_uids.index(body.frame_uid) for body in checker.bodies + ] + self.model = self.state = self.pipeline = self.contacts = None + self.meshes = [] + if not self.has_native: + return + builder = newton.ModelBuilder() + cfg = newton.ModelBuilder.ShapeConfig( + gap=checker.detection_gap_m / 2, + margin=0.0, + density=0.0, + collision_filter_parent=False, + ) + for index in checker._native: + body = checker.bodies[index] + owner = builder.add_body( + mass=1.0, + inertia=wp.mat33(np.eye(3, dtype=np.float32)), + is_kinematic=True, + ) + options = {"body": owner, "cfg": cfg} + if body.kind == "box": + shape = builder.add_shape_box( + hx=body.dimensions[0] / 2, + hy=body.dimensions[1] / 2, + hz=body.dimensions[2] / 2, + **options, + ) + elif body.kind == "sphere": + shape = builder.add_shape_sphere(radius=body.dimensions[0], **options) + elif body.kind in ("capsule", "cylinder"): + shape = getattr(builder, "add_shape_" + body.kind)( + radius=body.dimensions[0], + half_height=body.dimensions[1] / 2, + **options, + ) + else: + mesh = newton.Mesh( + np.asarray(body.vertices, dtype=np.float32), + np.asarray(body.triangles, dtype=np.int32).reshape(-1), + compute_inertia=False, + maxhullvert=len(body.vertices), + ) + self.meshes.append(mesh) + shape = builder.add_shape_convex_hull(mesh=mesh, **options) + if shape != owner: + raise RuntimeError("Collision query requires one shape per owned body") + batch = newton.ModelBuilder() + batch.replicate(builder, count) + self.model = batch.finalize(device=checker.device) + self.state = self.model.state() + width = len(checker._native) + pairs = [ + (checker._native_lookup[i], checker._native_lookup[j]) + for i, j in checker._pairs + if i in checker._native_lookup and j in checker._native_lookup + ] + explicit = np.asarray( + [ + (a + row * width, b + row * width) + for row in range(count) + for a, b in pairs + ], + dtype=np.int32, + ) + self.explicit = wp.array(explicit, dtype=wp.vec2i, device=checker.device) + self.capacity = checker._capacity * count + self.pipeline = newton.CollisionPipeline( + self.model, + broad_phase="explicit", + shape_pairs_filtered=self.explicit, + include_static_kinematic_pairs=True, + rigid_contact_max=self.capacity, + soft_contact_max=0, + ) + if self.pipeline.narrow_phase.global_contact_reducer is not None: + raise RuntimeError("Unsupported batched Newton narrow-phase route") + self.contacts = self.pipeline.contacts() + self.active = wp.array(self.mask, dtype=wp.int32, device=checker.device) + self.tolerance = wp.empty(count, dtype=float, device=checker.device) + self.output = wp.empty((count, 3), dtype=float, device=checker.device) + self.empty_count = wp.zeros(1, dtype=wp.int32, device=checker.device) + self.stream = ( + wp.Stream(checker.device) if checker.device.startswith("cuda") else None + ) + if self.stream is not None: + wp.synchronize_device(checker.device) + + def _run(self): + from embodichain.compute.geometry._warp.batch_contact_query import ( + contact_summary, + ) + + checker, wp = self.checker, self.wp + self.output.fill_(math.inf) + self.pipeline.collide(self.state, self.contacts) + phase, contacts = self.pipeline.narrow_phase, self.contacts + counters = [] + for count_name, storage_name in ( + ("gjk_candidate_pairs_count", "gjk_candidate_pairs"), + ("split_gjk_work_count", "split_gjk_work_items"), + ("split_manifold_work_count", "split_manifold_work_items"), + ): + value, storage = getattr(phase, count_name), getattr(phase, storage_name) + counters.extend( + ( + value if value is not None else self.empty_count, + storage.shape[0] if storage is not None else 0, + ) + ) + wp.launch( + contact_summary, + self.capacity, + inputs=[ + contacts.rigid_contact_count, + contacts.rigid_contact_shape0, + contacts.rigid_contact_shape1, + contacts.rigid_contact_point0, + contacts.rigid_contact_point1, + contacts.rigid_contact_normal, + contacts.rigid_contact_margin0, + contacts.rigid_contact_margin1, + self.model.shape_body, + self.state.body_q, + self.active, + len(checker._native), + self.count, + checker.clearance_m, + self.tolerance, + self.capacity, + self.pipeline.broad_phase_pair_count, + len(self.explicit), + *counters, + self.output, + ], + device=checker.device, + ) + + def check(self, frame_poses, rows): + from scipy.spatial.transform import Rotation + from embodichain.lab.sim.scene.feasibility import UnsupportedFeasibilityError + from embodichain.lab.sim.scene.feasibility.contracts import FeasibilityCheck + from embodichain.lab.sim.scene.feasibility.collision import _query_float32 + + checker = self.checker + if checker._closed: + raise RuntimeError("collision checker is closed") + frames = np.stack([frame_poses[uid] for uid in checker._frame_uids], axis=1) + if frames.shape != (self.count, len(checker._frame_uids), 4, 4): + raise ValueError("Collision frames must preserve every physical row") + if frames.dtype.kind not in "fiu" or not np.isfinite(frames).all(): + raise ValueError("arena_pose must contain finite real numbers") + frames = frames.astype(np.float64) + rotation = frames[..., :3, :3] + if ( + np.any(np.abs(frames[..., 3, :] - (0, 0, 0, 1)) > 1e-6) + or np.any( + np.abs(np.swapaxes(rotation, -1, -2) @ rotation - np.eye(3)) > 1e-6 + ) + or np.any(np.abs(np.linalg.det(rotation) - 1) > 1e-6) + ): + raise ValueError("arena_pose must be a proper rigid transform") + frames[..., 3, :] = (0, 0, 0, 1) + frames[..., :3, 3] -= frames[:, :1, :3, 3].copy() + poses = frames[:, self.indices] @ self.locals + if not np.isfinite(poses).all(): + raise UnsupportedFeasibilityError( + "Nonfinite composed physical collision pose" + ) + failures = {} + for i, j in self.planes: + plane, shape = (i, j) if checker.bodies[i].kind == "halfspace" else (j, i) + a, b = checker.bodies[plane], checker.bodies[shape] + normal = poses[:, plane, :3, :3] @ np.asarray(a.dimensions[:3]) + local = np.einsum("bji,bj->bi", poses[:, shape, :3, :3], normal) + if b.kind == "convex": + support = (local @ np.asarray(b.vertices).T).min(axis=-1) + elif b.kind == "sphere": + support = -b.dimensions[0] + elif b.kind == "capsule": + support = -b.dimensions[0] - np.abs(local[:, 2]) * b.dimensions[1] / 2 + elif b.kind == "cylinder": + support = ( + -b.dimensions[0] * np.linalg.norm(local[:, :2], axis=-1) + - np.abs(local[:, 2]) * b.dimensions[1] / 2 + ) + elif b.kind == "box": + support = -np.abs(local) @ (np.asarray(b.dimensions) / 2) + else: + raise UnsupportedFeasibilityError("Unsupported analytic halfspace pair") + distance = (normal * (poses[:, shape, :3, 3] - poses[:, plane, :3, 3])).sum( + -1 + ) + distance += support - a.dimensions[3] + if not np.isfinite(distance).all(): + raise UnsupportedFeasibilityError( + "Nonfinite analytic halfspace separation" + ) + for row in rows: + if distance[row] < checker.clearance_m - 1e-8 and row not in failures: + failures[row] = (i, j) + if self.has_native: + matrices = poses[:, checker._native] + relative = matrices[..., :3, 3] - matrices[:, :1, :3, 3] + rotations = Rotation.from_matrix(matrices[..., :3, :3].reshape(-1, 3, 3)) + quaternions = rotations.as_quat().reshape(self.count, -1, 4) + packed = _query_float32( + np.concatenate((relative, quaternions), axis=-1), + "Packed physical collision pose", + ) + recovered = rotations.as_matrix().reshape(matrices[..., :3, :3].shape) + loss = np.linalg.norm(recovered - matrices[..., :3, :3], axis=(-2, -1)).max( + -1 + ) + tolerance = ( + checker.numeric_tolerance_m + + 2 * (checker._geometry_error + loss * checker._geometry_radius) + + 32 + * np.finfo(np.float32).eps + * (np.abs(relative).max(axis=(-2, -1)) + checker._geometry_radius) + ) + if not np.isfinite(tolerance).all() or np.any( + tolerance >= checker.detection_gap_m - checker.clearance_m + ): + raise UnsupportedFeasibilityError( + "Candidate exceeds the contact-band precision budget" + ) + checker._max_effective_tolerance = max( + checker._max_effective_tolerance, float(tolerance.max()) + ) + # Solvers may temporarily borrow external CUDA streams. Own this + # query's stream through assignment, capture, launch and readback. + # Inputs have already crossed a measured CPU boundary above. + with self.wp.ScopedStream(self.stream, sync_enter=False): + self.state.body_q.assign(packed.reshape(-1, 7)) + self.tolerance.assign(tolerance.astype(np.float32)) + if self.graph is None: + self._run() + if self.stream is not None and checker._use_cuda_graph: + self.wp.synchronize_stream(self.stream) + with self.wp.ScopedCapture(stream=self.stream) as capture: + self._run() + self.graph = capture.graph + self.wp.capture_launch(self.graph, stream=self.stream) + else: + self.wp.capture_launch(self.graph, stream=self.stream) + summary = self.output.numpy() + for row in rows: + if ( + math.isfinite(summary[row, 2]) + or math.isnan(summary[row, 0]) + or summary[row, 0] == -math.inf + ): + raise UnsupportedFeasibilityError( + "Batched Newton query overflowed or returned invalid evidence" + ) + if row not in failures and math.isfinite(summary[row, 1]): + a, b = divmod(int(summary[row, 1]), len(checker._native)) + failures[row] = (checker._native[a], checker._native[b]) + self.queries += 1 + result = [] + for row in rows: + if row in failures: + i, j = failures[row] + pair = checker.bodies[i].collision_id, checker.bodies[j].collision_id + report = FeasibilityCheck( + "collision", + "failed", + f"{pair[0]} / {pair[1]}: forbidden collision or insufficient clearance", + ) + else: + pair = None + report = FeasibilityCheck( + "collision", + "passed", + "Discrete Newton convex/physical-halfspace query; batched physical rows", + ) + result.append((row, report, pair)) + return result + + def close(self): + with self.wp.ScopedStream(getattr(self, "stream", None), sync_enter=False): + self.graph = self.contacts = self.pipeline = self.state = self.model = None + for name in ("explicit", "active", "tolerance", "output", "empty_count"): + setattr(self, name, None) + self.meshes.clear() + self.stream = None diff --git a/embodichain/lab/sim/scene/feasibility/collision.py b/embodichain/lab/sim/scene/feasibility/collision.py index 38159c4f20..f45478e3e0 100644 --- a/embodichain/lab/sim/scene/feasibility/collision.py +++ b/embodichain/lab/sim/scene/feasibility/collision.py @@ -296,6 +296,8 @@ def __init__( raise UnsupportedFeasibilityError("Physical collision pair budget exceeded") self._capacity = min(max_contacts, requested_capacity) self._mask_cache = OrderedDict() + self._batch_queries = OrderedDict() + self.last_batch_pairs: tuple[tuple[str, str] | None, ...] = () self._model = self._state = self._pipeline = self._contacts = None self._graph = self._active = self._output = self._empty_count = ( self._explicit @@ -703,6 +705,86 @@ def check( "Discrete Newton convex/physical-halfspace query; clearance is capped by the detection band", ), (0.0 if math.isinf(clearance) else min(self.detection_gap_m, clearance)) + def check_batch( + self, + frame_poses: Mapping[str, object], + *, + allowed_contacts: Sequence[tuple[str, str]] = (), + moving_objects: Sequence[str] = (), + all_pairs: bool = False, + env_ids: Sequence[int] | None = None, + ) -> tuple[FeasibilityCheck, ...]: + """Check independent measured rows with one bounded numerical query. + + Args: + frame_poses: Every declared frame with arena poses ``(B, 4, 4)``. + Tensor inputs are copied to the query-owned CPU boundary. + allowed_contacts: Shared explicit contact exemptions for these rows. + moving_objects: Dynamic frames requiring environment checks. + all_pairs: Also inspect static layout pairs. + env_ids: Ordered selected rows; None selects all B rows. + + Returns: + Discrete checks in selected-row order. ``last_batch_pairs`` records + each failure's physical IDs. This API does not report a distance or + overwrite the scalar check's clearance interval. It never advances + or changes the live simulation. Geometry, precision and saturation + rejection follow :meth:`check`. + """ + if self._closed: + raise RuntimeError("collision checker is closed") + from ._collision_batches import _CollisionBatches + + self.last_batch_pairs = () + frames = {} + size = None + for uid in self._frame_uids: + if uid not in frame_poses: + raise UnsupportedFeasibilityError( + f"Missing physical collision frame {uid}" + ) + value = frame_poses[uid] + if hasattr(value, "detach"): + value = value.detach().cpu().numpy() + array = np.asarray(value) + if array.ndim != 3 or array.shape[1:] != (4, 4) or not array.shape[0]: + raise ValueError("batched arena poses must have shape (B, 4, 4), B > 0") + if size is not None and size != len(array): + raise ValueError("batched collision frames must share their row count") + size = len(array) + frames[uid] = array + rows = tuple(range(size)) if env_ids is None else tuple(env_ids) + if ( + not rows + or len(set(rows)) != len(rows) + or any(type(row) is not int or not 0 <= row < size for row in rows) + ): + raise ValueError("env_ids must select unique physical rows") + if type(all_pairs) is not bool: + raise TypeError("all_pairs must be a bool") + # Resolve permissions with the same native validation before caching. + allowed = frozenset(self._expand_pairs(allowed_contacts)) + self._mask(allowed_contacts, moving_objects, all_pairs) + key = size, allowed, frozenset(moving_objects), all_pairs + query = self._batch_queries.get(key) + if query is None: + if size * self._capacity > 1048576: + raise UnsupportedFeasibilityError( + "Batched collision contact budget exceeded" + ) + query = _CollisionBatches( + self, size, allowed_contacts, moving_objects, all_pairs + ) + self._batch_queries[key] = query + while len(self._batch_queries) > 2: + _, old = self._batch_queries.popitem(last=False) + old.close() + else: + self._batch_queries.move_to_end(key) + result = query.check(frames, rows) + self.last_batch_pairs = tuple(pair for _, _, pair in result) + return tuple(report for _, report, _ in result) + def _failure(self, i: int, j: int) -> FeasibilityCheck: self.last_pair = self.bodies[i].collision_id, self.bodies[j].collision_id return FeasibilityCheck( @@ -713,6 +795,29 @@ def _failure(self, i: int, j: int) -> FeasibilityCheck: def close(self) -> None: """Release owned query arrays/graphs; repeated cleanup is harmless.""" + stream = next( + ( + query.stream + for query in self._batch_queries.values() + if getattr(query, "stream", None) is not None + ), + None, + ) + if stream is not None: + import warp as wp + + # A robot solver may have borrowed a temporary Warp stream. Keep + # query-owned arrays on a live stream through their final release. + with wp.ScopedStream(stream, sync_enter=False): + self._close_arrays() + else: + self._close_arrays() + + def _close_arrays(self) -> None: + for query in self._batch_queries.values(): + query.close() + self._batch_queries.clear() + self.last_batch_pairs = () self._graph = self._pipeline = self._contacts = self._state = self._model = None self._active = self._output = self._empty_count = None self._tolerance = None diff --git a/embodichain/lab/sim/sensors/__init__.py b/embodichain/lab/sim/sensors/__init__.py index 467fce2f80..6f87bb669b 100644 --- a/embodichain/lab/sim/sensors/__init__.py +++ b/embodichain/lab/sim/sensors/__init__.py @@ -19,6 +19,8 @@ ``Camera``, ``StereoCamera``, and ``ContactSensor`` all derive from ``BaseSensor`` and expose batched ``TensorDict`` data buffers. """ +from __future__ import annotations + from .base_sensor import BaseSensor, SensorCfg from .camera import Camera, CameraCfg from .stereo import StereoCamera, StereoCameraCfg @@ -26,4 +28,5 @@ ContactSensor, ContactSensorCfg, ArticulationContactFilterCfg, + ContactPermissionChecker, ) diff --git a/embodichain/lab/sim/sensors/contact/__init__.py b/embodichain/lab/sim/sensors/contact/__init__.py index 80a6e493ce..ed6121e479 100644 --- a/embodichain/lab/sim/sensors/contact/__init__.py +++ b/embodichain/lab/sim/sensors/contact/__init__.py @@ -48,10 +48,12 @@ from .history import ( ContactHistory, ) +from .permissions import ContactPermissionChecker __all__ = [ "ArticulationContactFilterCfg", "ContactSensor", + "ContactPermissionChecker", "ContactSensorCfg", "ContactSamplingObserver", "CONTACT_ACTOR_IDS", diff --git a/embodichain/lab/sim/sensors/contact/permissions.py b/embodichain/lab/sim/sensors/contact/permissions.py new file mode 100644 index 0000000000..831dc8f3b1 --- /dev/null +++ b/embodichain/lab/sim/sensors/contact/permissions.py @@ -0,0 +1,204 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Device-side permission checks over complete native contact samples.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from typing import TYPE_CHECKING +import torch + +if TYPE_CHECKING: + from .sensor import ContactSensor + +__all__ = ["ContactPermissionChecker"] + + +class ContactPermissionChecker: + """Classify every valid measured contact without constructing Python pairs. + + Args: + sensor: Initialized native sensor with applied-impulse validity flags. + actor_frames: Explicit actor ID to frame name mapping for every row. + A global static actor may appear in several rows with the same name. + allowed_contacts: Structural and task-owned unordered frame exemptions. + moving_frames: Robot and dynamic frames requiring permission checks. + + This checker does not infer adjacency, change filters, step physics or select + a sampling frequency. A host requiring substep validation must invoke it at + every original physical sample. Unknown actors and unavailable impulses fail + closed; cumulative native/scatter loss invalidates every row conservatively. + """ + + def __init__( + self, + sensor: ContactSensor, + actor_frames: Sequence[Mapping[int, str]], + *, + allowed_contacts: Sequence[tuple[str, str]], + moving_frames: Sequence[str], + ) -> None: + self.sensor = sensor + self.device = torch.device(sensor.device) + self.rows = sensor.num_instances + if len(actor_frames) != self.rows or any(not row for row in actor_frames): + raise ValueError("actor_frames must declare every nonempty sensor row") + if any( + type(actor) is not int + or actor < 0 + or type(name) is not str + or not name.strip() + for row in actor_frames + for actor, name in row.items() + ): + raise ValueError( + "contact actors require nonnegative IDs and explicit frame names" + ) + names = sorted({name for row in actor_frames for name in row.values()}) + if any( + len(pair) != 2 or any(name not in names for name in pair) + for pair in allowed_contacts + ): + raise ValueError("contact permissions reference an absent frame") + if set(moving_frames) - set(names): + raise ValueError("moving_frames references an absent contact frame") + codes = {name: index for index, name in enumerate(names)} + actors = {} + for row, frames in enumerate(actor_frames): + for actor, name in frames.items(): + code, rows = actors.setdefault(actor, (codes[name], set())) + if code != codes[name]: + raise ValueError("contact actor has ambiguous frame identities") + rows.add(row) + actors = sorted(actors.items()) + allowed = {frozenset(pair) for pair in allowed_contacts} + self._actors = torch.tensor( + [actor for actor, _ in actors], dtype=torch.int32, device=self.device + ) + self._memberships = torch.tensor( + [ + [row in members for row in range(self.rows)] + for _, (_, members) in actors + ], + dtype=torch.bool, + device=self.device, + ) + self._codes = torch.tensor( + [code for _, (code, _) in actors], dtype=torch.long, device=self.device + ) + self._moving = torch.tensor( + [name in moving_frames for name in names], + dtype=torch.bool, + device=self.device, + ) + self._allowed = torch.tensor( + [[frozenset((a, b)) in allowed for b in names] for a in names], + dtype=torch.bool, + device=self.device, + ) + self._first = torch.empty(self.rows, dtype=torch.int32, device=self.device) + self._drop = torch.zeros(1, dtype=torch.int32, device=self.device) + self._output = torch.empty( + (self.rows, 3), dtype=torch.int32, device=self.device + ) + self._ready = torch.cuda.Event() if self.device.type == "cuda" else None + if self._ready is not None: + self._ready.record(torch.cuda.current_stream(self.device)) + self._actors_token = id(sensor._query.actors) + + def check(self) -> torch.Tensor: + """Return owned per-row ``(reason, actor0, actor1)`` integer summaries. + + Returns: + Tensor ``(B, 3)`` on the sensor device. Reason zero passes; one means + unavailable applied impulse, two unknown/cross-row actor, three + forbidden moving contact, four dropped evidence. The earliest bad + buffer slot supplies actor IDs. No valid contact is subsampled. + + Raises: + RuntimeError: Sensor topology changed after permission binding. + """ + import warp as wp + from embodichain.compute.geometry._warp.contact_permissions import ( + classify_contacts, + contact_failures, + ) + + if self._ready is not None: + torch.cuda.current_stream(self.device).wait_event(self._ready) + data = self.sensor.get_data() + if id(self.sensor._query.actors) != self._actors_token: + raise RuntimeError( + "contact topology changed; bind a new permission checker" + ) + pairs = data["user_ids"] + self._first.fill_(pairs.shape[1] * 8) + self._drop.zero_() + # Use the current Torch stream on every call, rather than assuming that + # the stream used at construction is still the caller's producer. + stream = ( + wp.stream_from_torch(torch.cuda.current_stream(self.device)) + if self.device.type == "cuda" + else None + ) + wp.launch( + classify_contacts, + pairs.shape[:2], + inputs=[ + wp.from_torch(pairs), + wp.from_torch(data["is_valid"]), + wp.from_torch(data["impulse_valid"]), + wp.from_torch(self._actors), + wp.from_torch(self._memberships), + wp.from_torch(self._codes), + wp.from_torch(self._moving), + wp.from_torch(self._allowed), + wp.from_torch(self.sensor.dropped_contacts_mask), + wp.from_torch(self._first), + wp.from_torch(self._drop), + ], + device=str(self.device), + stream=stream, + ) + wp.launch( + contact_failures, + self.rows, + inputs=[ + wp.from_torch(pairs), + wp.from_torch(self._first), + wp.from_torch(self._drop), + wp.from_torch(self._output), + ], + device=str(self.device), + stream=stream, + ) + result = self._output.clone() + if self._ready is not None: + current = torch.cuda.current_stream(self.device) + self._ready.record(current) + for value in ( + self._actors, + self._memberships, + self._codes, + self._moving, + self._allowed, + self._first, + self._drop, + self._output, + ): + value.record_stream(current) + return result diff --git a/embodichain/lab/task_program/integrations/_measured_batch.py b/embodichain/lab/task_program/integrations/_measured_batch.py new file mode 100644 index 0000000000..a63057a10f --- /dev/null +++ b/embodichain/lab/task_program/integrations/_measured_batch.py @@ -0,0 +1,133 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + + +"""Batch measured predicate inputs while retaining native event state machines.""" + +from __future__ import annotations + +from contextlib import contextmanager +import json + +import numpy as np + +__all__: list[str] = [] + + +class _MeasuredChecks: + """Read one packed physical boundary instead of synchronizing per predicate. + + Native monitors still own ordered events, consecutive counts, receipts and + terminal reports. Only their predicate arithmetic is shared across rows; + unsupported fast paths continue through the original native predicate. + The cache exists solely inside an explicit measured-boundary scope. + """ + + def __init__(self, env, configuration, monitors): + self.env, self.monitors = env, monitors + conditions = [*configuration["initial_checks"], *configuration["final_checks"]] + for event in configuration["events"]: + conditions.extend(event["conditions"]) + self.conditions = {self.key(c): c for c in conditions} + self.entities = tuple( + dict.fromkeys( + c[name] for c in conditions for name in ("entity", "other") if name in c + ) + ) + self.values = None + self.reads = 0 + + @staticmethod + def key(condition): + return json.dumps(condition, sort_keys=True, separators=(",", ":")) + + @contextmanager + def boundary(self): + import torch + + env = self.env + parts = [] + for uid in self.entities: + obj = env.sim.get_rigid_object(uid) + if obj is None: + raise ValueError("Predicate entity is absent from the physical scene") + parts.append( + torch.cat( + ( + obj.get_local_pose()[:, :3], + obj.body_state[:, 7:10].norm(dim=-1, keepdim=True), + obj.body_state[:, 10:13].norm(dim=-1, keepdim=True), + ), + dim=-1, + ) + ) + parts.append(env.robot.get_qpos()) + # One D2H boundary, independent of the number of predicates and rows. + packed = torch.cat(parts, dim=-1).detach().cpu().numpy() + self.reads += 1 + states = { + uid: packed[:, index * 5 : (index + 1) * 5] + for index, uid in enumerate(self.entities) + } + joints = packed[:, len(self.entities) * 5 :] + values = {} + for key, condition in self.conditions.items(): + kind = condition["kind"] + if kind == "joint_range": + index = env.robot.joint_names.index(condition["joint"]) + value = joints[:, index].astype(np.float64) + result = (value >= condition["lower"]) & (value <= condition["upper"]) + elif kind in ("pose_near", "lifted", "speed_below", "axis_relation"): + state = states[condition["entity"]] + position = state[:, :3].astype(np.float64) + if kind == "pose_near": + result = ( + np.linalg.norm(position - condition["position"], axis=-1) + <= condition["tolerance_m"] + ) + elif kind == "lifted": + initial = np.full(env.num_envs, np.nan) + for row, monitor in self.monitors.items(): + initial[row] = monitor.initial_heights.get( + condition["entity"], np.nan + ) + result = position[:, 2] - initial >= condition["height_m"] + elif kind == "axis_relation": + axis = condition["axis"] + delta = position[:, axis] - states[condition["other"]][:, axis] + result = (delta >= condition["lower"]) & ( + delta <= condition["upper"] + ) + else: + # Native scalar comparisons convert the float32 norms to + # Python floats before comparing authored thresholds. + result = ( + state[:, 3].astype(np.float64) <= condition["linear_m_s"] + ) & (state[:, 4].astype(np.float64) <= condition["angular_rad_s"]) + else: + continue + values[key] = ~result if condition.get("negate", False) else result + previous = { + row: getattr(monitor, "_batch_predicates", None) + for row, monitor in self.monitors.items() + } + for monitor in self.monitors.values(): + monitor._batch_predicates = values + try: + yield + finally: + for row, monitor in self.monitors.items(): + monitor._batch_predicates = previous[row] diff --git a/embodichain/lab/task_program/integrations/feasibility.py b/embodichain/lab/task_program/integrations/feasibility.py index f8887fe207..8f688a15da 100644 --- a/embodichain/lab/task_program/integrations/feasibility.py +++ b/embodichain/lab/task_program/integrations/feasibility.py @@ -176,6 +176,13 @@ def predicate(self, env: Any, condition: Mapping[str, object]) -> bool: Returns: Physical truth value; unknown entities/joints are configuration errors. """ + cached = getattr(self, "_batch_predicates", None) + if cached is not None: + from ._measured_batch import _MeasuredChecks + + key = _MeasuredChecks.key(condition) + if key in cached: + return bool(cached[key][self.env_id]) kind = condition["kind"] if kind == "joint_range": index = env.robot.joint_names.index(condition["joint"]) @@ -274,6 +281,47 @@ def predicate(self, env: Any, condition: Mapping[str, object]) -> bool: ) return bool(not passed if condition.get("negate", False) else passed) + @staticmethod + def batch_scope( + env: Any, + monitors: Sequence[MeasuredTaskMonitor], + *, + extra_conditions: Sequence[Mapping[str, object]] = (), + ) -> object: + """Share one measured numeric boundary across independent event monitors. + + Args: + env: Live environment or complete measured-state facade. + monitors: Distinct physical-row monitors, already reset for this episode. + extra_conditions: Additional validated initial checks to read in this scope. + + Returns: + Context manager. Inside it, predicate arithmetic uses one packed + readback; ordered events, counters and reports still belong to each + native monitor. Unsupported fast paths keep the scalar predicate. + No cached physical values survive the scope. + """ + from ._measured_batch import _MeasuredChecks + + selected = {monitor.env_id: monitor for monitor in monitors} + if not selected or len(selected) != len(monitors): + raise ValueError( + "batch_scope requires unique nonempty physical-row monitors" + ) + if any(not 0 <= row < env.num_envs for row in selected): + raise ValueError("monitor row is outside the measured environment") + first = next(iter(selected.values())) + conditions = [first._validate(c) for c in extra_conditions] + final, events = [], [] + for monitor in selected.values(): + final.extend(monitor.final_checks) + events.extend({"conditions": values} for _, values, _ in monitor.events) + return _MeasuredChecks( + env, + {"initial_checks": conditions, "final_checks": final, "events": events}, + selected, + ).boundary() + def observe(self, env: Any, action: Any) -> None: """Record consecutive event predicates at actual control boundaries.""" self._steps += 1 diff --git a/embodichain/lab/task_program/integrations/simulation/policies.py b/embodichain/lab/task_program/integrations/simulation/policies.py index 339d6185ba..6d23062c36 100644 --- a/embodichain/lab/task_program/integrations/simulation/policies.py +++ b/embodichain/lab/task_program/integrations/simulation/policies.py @@ -727,6 +727,7 @@ def _hold_actions(self, policy, *, segment, active_mask): context, attached, previous_target = self._hold_observer( cfg.object, accepted ) + attached = attached.to(device=active_mask.device) self.validate_observation_scope(clock) if clock.step_index != start + tick: raise ValueError("Hold observation changed its execution clock.") @@ -752,6 +753,7 @@ def _hold_actions(self, policy, *, segment, active_mask): name="hold.at", ) ) + target = target.to(device=active_mask.device) pose = context.scene.entities[cfg.object].pose.to(target) if pose.ndim == 2: pose = pose.unsqueeze(0).expand(active_mask.numel(), -1, -1) @@ -1883,7 +1885,14 @@ def _measure_pose_delta_settle_target( target.native_entity, entity_id=target.canonical_id, ) - pose = pose.index_select(0, row_indices.to(pose.device)) + # _read_pose already selects this port's bound physical rows. Convert + # the requested World IDs to local offsets instead of selecting twice + # with the original IDs (for example row 7 in a B=1 policy port). + matches = row_indices[:, None] == self._row_indices[None, :] + if not bool((matches.sum(dim=1) == 1).all()): + raise ValueError("Pose-delta settling requested an unbound physical row") + local = matches.long().argmax(dim=1).to(pose.device) + pose = pose.index_select(0, local) row_count = row_indices.numel() if previous_pose is None: speed = torch.full( diff --git a/embodichain/utils/_owned_json.py b/embodichain/utils/_owned_json.py new file mode 100644 index 0000000000..851a39a93d --- /dev/null +++ b/embodichain/utils/_owned_json.py @@ -0,0 +1,115 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + + +"""Owned immutable finite JSON nodes and defensive mutable projections.""" + +from __future__ import annotations + +from collections.abc import Mapping +import json +import math +from types import MappingProxyType + +__all__: list[str] = [] + +_ATOMS = frozenset((str, bool, int, type(None))) + + +class _FrozenTuple(tuple): + __slots__ = () + + +class _FrozenMap(Mapping): + """Own validated immutable children; expose no mutable backing dictionary.""" + + __slots__ = ("_data", "_strict", "_encoded") + + def __init__(self, values, strict): + object.__setattr__(self, "_data", MappingProxyType(values)) + object.__setattr__(self, "_strict", strict) + object.__setattr__(self, "_encoded", None) + + def __setattr__(self, name, value): + raise TypeError("Frozen metadata cannot be modified") + + def __getitem__(self, key): + return self._data[key] + + def __iter__(self): + return iter(self._data) + + def __len__(self): + return len(self._data) + + def __deepcopy__(self, memo): + return self + + def __repr__(self): + return repr(self._data) + + def __str__(self): + return str(self._data) + + def mutable_copy(self): + if self._encoded is None: + encoded = json.dumps( + self._data, + default=dict, + ensure_ascii=False, + allow_nan=False, + ) + object.__setattr__(self, "_encoded", encoded) + # Every call returns new mutable containers. No caller can modify the + # validated immutable source through the returned tree. + return json.loads(self._encoded) + + +def _key_valid(key): + return type(key) is str and bool(key) and key == key.strip() + + +def _strict_keys(value): + if type(value) is _FrozenMap: + return value._strict + if type(value) is _FrozenTuple: + return all(_strict_keys(item) for item in value) + return True + + +def freeze_metadata(value: object) -> object: + kind = type(value) + if kind in _ATOMS or kind in (_FrozenMap, _FrozenTuple): + return value + if kind is float: + if not math.isfinite(value): + raise ValueError("metadata must contain only finite JSON values") + return value + if isinstance(value, (list, tuple)): + return _FrozenTuple(freeze_metadata(item) for item in value) + if isinstance(value, Mapping): + result = {} + strict = True + for key, item in value.items(): + # The expansion contract accepts whitespace around nonempty keys; + # the Gym JSON-copy contract is stricter. Retain both behaviours. + if not isinstance(key, str) or not key.strip(): + raise ValueError("metadata key must be a nonempty string") + owned = freeze_metadata(item) + result[key] = owned + strict &= _key_valid(key) and _strict_keys(owned) + return _FrozenMap(result, strict) + raise ValueError("metadata must contain only finite JSON values") diff --git a/pyproject.toml b/pyproject.toml index b20997c6df..8c6882821a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -41,6 +41,7 @@ dependencies = [ "ortools", "prettytable", "psutil>=5.9", + "greenlet>=3.2,<4", "black==26.10.1", "fvcore", "h5py", diff --git a/tests/lab/expansion/test_export.py b/tests/lab/expansion/test_export.py index ce9b2e5046..eddf3b32e1 100644 --- a/tests/lab/expansion/test_export.py +++ b/tests/lab/expansion/test_export.py @@ -29,6 +29,61 @@ from embodichain.lab.expansion import export as module +def test_per_episode_export_uses_real_sdk_and_preserves_portable_boundaries(tmp_path): + from embodichain.lab.expansion import ( + export_expansion_episodes, + load_expansion_episode, + ) + from lerobot.datasets.lerobot_dataset import LeRobotDataset + + sink = ArtifactEpisodeSink(tmp_path) + for ordinal in range(2): + parent = episode() + parent = replace( + parent, + identity=replace( + parent.identity, + candidate_id=f"candidate-{ordinal}", + geometry_family_id=f"family-{ordinal}", + ), + episode_id=f"episode-{ordinal}", + commit_id=f"commit-{ordinal}", + observations={ + "joint_positions": torch.cat( + ( + parent.observations["joint_positions"], + parent.observations["joint_positions"] + 0.03, + ), + dim=1, + ), + "joint_velocities": torch.zeros(3, 2), + "joint_efforts": torch.zeros(3, 2), + }, + actions=torch.cat((parent.actions, parent.actions + 0.03), dim=1), + metadata={ + "expert_action_spec": { + "joint_names": ["joint_0", "joint_1"], + "joint_command_mode": "position", + "step_dt": 0.1, + }, + "source_episode_uuids": ["source"], + }, + ) + sink.submit(parent, submission_id=0) + result = export_expansion_episodes(tmp_path) + assert result["episodes"] == 2 + assert export_expansion_episodes(tmp_path) == result + for row in result["datasets"]: + path = Path(result["directory"]) / row["dataset_path"] + data = LeRobotDataset(path.name, root=path, download_videos=False) + assert data.num_episodes == 1 and len(data) == 2 + receipts = ArtifactEpisodeSink(path / "embodichain_artifacts").inspect() + assert len(receipts) == 1 and receipts[0].episode_id == row["episode_id"] + saved = load_expansion_episode(receipts[0].storage_id) + assert len(saved.timestamps) == len(saved.actions) + 1 == 3 + torch.testing.assert_close(data[0]["action"], saved.actions[0], rtol=0, atol=0) + + def test_export_includes_visual_children_without_new_physical_receipts( tmp_path, monkeypatch ): diff --git a/tests/lab/expansion/test_persistence.py b/tests/lab/expansion/test_persistence.py index 074f02cfa6..81bcd6d803 100644 --- a/tests/lab/expansion/test_persistence.py +++ b/tests/lab/expansion/test_persistence.py @@ -30,7 +30,10 @@ ValidationCheck, ValidationResult, ) -from embodichain.lab.expansion.persistence import ArtifactEpisodeSink +from embodichain.lab.expansion.persistence import ( + ArtifactEpisodeSink, + load_expansion_episode, +) from embodichain.data_pipeline.recording.media import MediaSpool @@ -63,6 +66,34 @@ def test_staging_has_no_credit_and_recovery_never_reruns_physics(tmp_path): assert len(sink.inspect()) == 1 +def test_verified_episode_reads_own_tensors_and_invalidate_changed_bytes(tmp_path): + sink = ArtifactEpisodeSink(tmp_path) + data = replace(episode(), metadata={"nested": {"values": [1, 2]}}) + receipt = sink.submit(data, submission_id=0) + first = load_expansion_episode(receipt.storage_id) + second = load_expansion_episode(receipt.storage_id) + first.actions.fill_(99) + first.observations["joint_positions"].fill_(88) + third = load_expansion_episode(receipt.storage_id) + torch.testing.assert_close(second.actions, data.actions, rtol=0, atol=0) + torch.testing.assert_close(third.actions, data.actions, rtol=0, atol=0) + torch.testing.assert_close( + third.observations["joint_positions"], + data.observations["joint_positions"], + rtol=0, + atol=0, + ) + from embodichain.lab.gym.envs._json import json_safe_copy + + projected = json_safe_copy(third.metadata, field_name="metadata") + projected["nested"]["values"][0] = 55 + assert third.metadata["nested"]["values"][0] == 1 + path = Path(receipt.storage_id) + path.write_bytes(b"invalid replaced evidence") + with pytest.raises((ValueError, OSError)): + load_expansion_episode(path) + + def test_media_alignment_is_required_before_receipt(tmp_path): sink = ArtifactEpisodeSink(tmp_path) data = episode() diff --git a/tests/lab/expansion/test_workloads_sim.py b/tests/lab/expansion/test_workloads_sim.py new file mode 100644 index 0000000000..516f540eb1 --- /dev/null +++ b/tests/lab/expansion/test_workloads_sim.py @@ -0,0 +1,364 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import os +from pathlib import Path +import subprocess +import sys + +import pytest + + +def _probe(output: str) -> None: + import hashlib + import json + import torch + from embodichain.lab.expansion import ( + ArtifactEpisodeSink, + ExpansionEngine, + ExpansionJobCfg, + ExpansionSession, + ExpansionSessionCfg, + EpisodeCandidate, + PreparedEpisodeWorkItem, + ExpansionWorkloadCfg, + WorkloadExecutionCfg, + WorkloadProgramExecutor, + load_expansion_episode, + ) + from embodichain.lab.expansion.contracts import ( + ExpertEpisode, + SceneCase, + ValidationCheck, + ValidationResult, + ) + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + discover_task_packages, + execute_init_hooks, + ) + from embodichain.lab.sim import SimulationManager + from embodichain.utils.utility import load_config + + torch.set_num_threads(1) + root = Path(__file__).resolve().parents[3] + path = ( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task.ur5.yaml" + ) + discover_task_packages() + execute_init_hooks() + cfg = config_to_cfg(load_config(path), source_path=path) + cfg.num_envs = cfg.sim_cfg.num_envs = 4 + cfg.sim_cfg.device = "cpu" + cfg.sim_cfg.headless = True + cfg.sim_cfg.render_cfg.renderer = "no-render" + cfg.sensor, cfg.enable_sensor, cfg.dataset = [], False, None + cfg.init_rollout_buffer = cfg.record_trajectory = False + cfg.task_program.program.count = 1 + env = None + try: + env = REGISTERED_ENVS[load_config(path)["id"]].make(cfg=cfg) + env.reset(seed=0) + robot = env.robot + program = env.compile_task_program(cfg.task_program) + from embodichain.lab.sim.motion.motion_generator import ( + MotionGenerator, + MotionGenCfg, + MotionGenOptions, + ) + from embodichain.lab.sim.motion.planners.trapezoidal_planner import ( + TrapezoidalPlannerCfg, + ) + from embodichain.lab.sim.motion.planners.utils import PlanState + from embodichain.lab.expansion._planning_batches import batched_planning + + requests = {} + for row in (0, 3): + generator = MotionGenerator( + MotionGenCfg( + planner_cfg=TrapezoidalPlannerCfg( + robot_uid=robot.uid, sim_instance_id=env.sim.instance_id + ) + ), + env_ids=(row,), + ) + start = robot.get_qpos(name="arm")[row : row + 1].clone() + goal = start.clone() + goal[:, 0] += 0.01 * (row + 1) + states = [PlanState.from_qpos(goal)] + options = MotionGenOptions( + strategy="ik_interp", + interpolation_mode="fixed_grid", + ik_seed_mode="sequential", + sample_count=12, + interpolation_dt=0.04, + start_qpos=start, + control_part="arm", + ) + requests[row] = (generator, states, options) + reference = { + row: generator.generate(states, options) + for row, (generator, states, options) in requests.items() + } + + class Request: + def __init__(self, row): + self.row = row + + def next(self): + generator, states, options = requests[self.row] + return generator.generate(states, options) + + before = env.sim.physics_step_index + with batched_planning(robot, 0) as planning: + planned = planning.gather({row: Request(row) for row in requests}) + assert ( + planning.stats["motion_calls"] == 1 + and planning.stats["max_motion_group"] == 2 + ), planning.stats + for row, original in reference.items(): + torch.testing.assert_close( + planned[row].success, original.success, rtol=0, atol=0 + ) + torch.testing.assert_close( + planned[row].positions, original.positions, rtol=0, atol=1e-6 + ) + torch.testing.assert_close(planned[row].dt, original.dt, rtol=0, atol=0) + assert env.sim.physics_step_index == before + from embodichain.lab.expansion.collision import _MeasuredCollision + + checks = _MeasuredCollision( + env, + str(root / "examples/expansion/scene_robot_feasibility/search.ur5.yaml"), + Path(output) / "checks", + ) + + class Host: + capacity = env.num_envs + + def preflight(self, items): + assert all(item.compatibility_key == "one-world" for item in items) + + def validate_plan(self, item): + assert torch.isfinite(robot.get_qpos()[item.env_id]).all() + return ValidationResult( + ( + ValidationCheck( + "initial_geometry", + "passed" if checks.initial[item.env_id] else "failed", + ), + ) + ) + + sink = ArtifactEpisodeSink(output) + session = ExpansionSession( + ExpansionSessionCfg.from_mapping( + { + "collection": { + "target_committed_episodes": 4, + "max_proposals": 8, + "max_rollout_attempts": 8, + }, + "execution": {"ready_high_watermark": 4, "ready_low_watermark": 0}, + "persistence": { + "pending_episode_limit": 4, + "pending_max_bytes": 32 * 1024 * 1024, + }, + } + ) + ) + engine = ExpansionEngine( + ExpansionJobCfg(batch_size=4), session=session, host=Host(), sink=sink + ) + + class Source: + def __init__(self): + self.buffers, self.assignments, self.adapters = {}, {}, [] + self.finished = [] + + def prepare(self, assignments): + assert len(SimulationManager._instances) == 1 + + def activate(self, assignment, observation): + row = assignment.env_id + checks.prepare(env_ids=(row,)) + from copy import deepcopy + + self.scalar_monitors = getattr(self, "scalar_monitors", {}) + self.scalar_monitors[row] = deepcopy(checks.monitors[row]) + key = f"{assignment.workload_id}-{assignment.ordinal}" + state = robot.get_qpos()[row].detach().cpu().clone() + initial = hashlib.sha256(state.numpy().tobytes()).hexdigest() + session.register_case( + SceneCase(key, initial, "scene", program.program_id, robot.uid), + robot.get_qpos_limits()[row].cpu(), + joint_names=tuple(robot.joint_names), + ) + identity, _ = session.propose( + key, + initial, + source_id=program.program_id, + source_revision="v1", + template_id=key, + operator_id="episode", + ) + item = PreparedEpisodeWorkItem( + EpisodeCandidate(identity, "{}"), "one-world", row + ) + native = engine.admit_row(item) + assert native is not None + self.assignments[row] = native + self.buffers[row] = {"actions": [], "states": [state]} + adapter = env.task_program_adapter.for_env_ids((row,)) + self.adapters.append(adapter) + return adapter.create_bridge(program, isolate_policy_sessions=True) + + def started(self, assignment, boundary): + engine.mark_row_started(self.assignments[assignment.env_id]) + + def observe(self, assignments, action, observation): + rejected = [] + checks.sample(tuple(a.env_id for a in assignments), action) + for assignment in assignments: + scalar = self.scalar_monitors[assignment.env_id] + scalar.observe(env, action) + assert scalar.validation(env) == checks.terminal[assignment.env_id] + for assignment in assignments: + row = assignment.env_id + buffer = self.buffers[row] + values = ( + action.value + if isinstance(action.value, torch.Tensor) + else action.value["qpos"] + ) + buffer["actions"].append(values[row].detach().cpu().clone()) + buffer["states"].append( + robot.get_qpos()[row].detach().cpu().clone() + ) + if ( + assignment.workload_id == "first" + and assignment.ordinal == 0 + and len(buffer["actions"]) == 12 + ): + rejected.append(row) + return rejected + + def complete(self, result): + row = result.assignment.env_id + native = self.assignments.pop(row) + data = self.buffers.pop(row) + count = result.program.lengths[0] + assert ( + len(data["actions"]) == count and len(data["states"]) == count + 1 + ) + assert ( + result.world_control_end - result.world_control_start + 1 == count + ) + self.finished.append( + ( + result.assignment.workload_id, + result.assignment.ordinal, + count, + result.program.accepted[0], + ) + ) + if not result.program.accepted[0]: + return engine.complete_row(native, None) + episode = ExpertEpisode( + native.item.candidate.identity, + {"joint_positions": torch.stack(data["states"])}, + torch.stack(data["actions"]), + torch.arange(count + 1, dtype=torch.float64) * env.step_dt, + "qpos", + ValidationResult( + ( + ValidationCheck("task_success", "passed"), + ValidationCheck( + "path_collision", checks.results[row].status + ), + *checks.terminal[row].checks, + ) + ), + native.episode_id, + native.commit_id, + ) + return engine.complete_row(native, episode) + + source = Source() + owner = id(env.sim.get_world()) + done = [] + executor = WorkloadProgramExecutor( + env, + [ + ExpansionWorkloadCfg("first", 2, 4), + ExpansionWorkloadCfg("second", 1, 2), + ExpansionWorkloadCfg("queued", 1, 2), + ], + source, + WorkloadExecutionCfg( + workload_count=2, batch_size=2, max_steps=2500, settling_steps=5 + ), + on_workload_done=lambda name, counts: done.append((name, counts)), + ) + result = executor.run() + assert result["complete"], (result, source.finished) + assert result["num_envs"] == 4 and result["refills"] > 0 + assert result["counts"]["first"] == { + "attempts": 3, + "rollouts": 3, + "committed": 2, + } + assert len(done) == 3 and len(sink.inspect()) == 4 + assert session.snapshot()["counts"]["committed"] == 4 + assert ( + id(env.sim.get_world()) == owner and len(SimulationManager._instances) == 1 + ) + assert next(entry for entry in source.finished if entry[:2] == ("first", 0))[ + 2: + ] == (13, False) + for receipt in sink.inspect(): + episode = load_expansion_episode(receipt.storage_id) + assert len(episode.timestamps) == len(episode.actions) + 1 + assert episode.validation.accepted + with pytest.raises(RuntimeError, match="single-use"): + executor.run() + for adapter in source.adapters: + adapter.close() + checks.close() + (Path(output) / "workload-result.json").write_text(json.dumps(result, indent=2)) + finally: + if env is not None: + env.close(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +@pytest.mark.requires_sim +def test_rolling_workloads_keep_native_receipts_and_world_boundaries(tmp_path): + result = subprocess.run( + [sys.executable, __file__, str(tmp_path)], + cwd=Path(__file__).resolve().parents[3], + env={**os.environ, "SDL_VIDEODRIVER": "offscreen", "OMP_NUM_THREADS": "1"}, + capture_output=True, + text=True, + timeout=300, + ) + assert result.returncode == 0, result.stdout[-4000:] + result.stderr[-5000:] + + +if __name__ == "__main__": + _probe(sys.argv[1]) diff --git a/tests/sim/atomic_skills/test_runtime_commands.py b/tests/sim/atomic_skills/test_runtime_commands.py index 6bb50b4768..14ad1a673c 100644 --- a/tests/sim/atomic_skills/test_runtime_commands.py +++ b/tests/sim/atomic_skills/test_runtime_commands.py @@ -141,6 +141,40 @@ def _frame( ) +@pytest.mark.gpu +def test_cuda_sequence_snapshots_keep_ownership_and_revalidate_mutated_values() -> None: + target = JointPositionTarget("arm", (0, 1)) + ids = torch.tensor([1, 5], device="cuda:0") + frames = tuple( + RuntimeCommandFrame( + commands=( + EndpointCommand( + target, + JointPositionPayload( + torch.full((2, 2), 0.1 * (i + 1), device="cuda:0") + ), + ), + ), + active_mask=torch.tensor([True, False], device="cuda:0"), + env_ids=ids, + hold_duration=torch.full((2,), 0.04, device="cuda:0"), + ) + for i in range(3) + ) + original = TimedCommandSequence(frames, ids) + copied = original.snapshot() + original.frames[0].commands[0].payload.positions.fill_(99) + assert copied.frames[0].commands[0].payload.positions[0, 0] == 0.1 + copied.frames[0].commands[0].payload.positions.fill_(55) + assert copied.frames[1].commands[0].payload.positions[0, 0] == 0.2 + original.frames[0].commands[0].payload.positions[0, 0] = float("nan") + with pytest.raises(ValueError, match="finite"): + original.snapshot() + copied.frames[0].hold_duration[0] = -0.1 + with pytest.raises(ValueError, match="non-negative"): + copied.snapshot() + + def test_runtime_command_payload_is_abstract() -> None: with pytest.raises(TypeError): RuntimeCommandPayload() # type: ignore[abstract] diff --git a/tests/sim/motion/solvers/test_base_solver.py b/tests/sim/motion/solvers/test_base_solver.py index c5f260493e..745cfde5f4 100644 --- a/tests/sim/motion/solvers/test_base_solver.py +++ b/tests/sim/motion/solvers/test_base_solver.py @@ -45,6 +45,38 @@ } +@pytest.mark.parametrize( + "device", ["cpu", pytest.param("cuda:0", marks=pytest.mark.gpu)] +) +def test_tcp_offset_jacobian_uses_the_solver_device(tmp_path, device): + path = tmp_path / "one_joint.urdf" + path.write_text(""" + + + + + + + + """) + solver = PytorchSolverCfg( + urdf_path=str(path), + root_link_name="base", + end_link_name="tool", + joint_names=["joint"], + ).init_solver(device=device) + expected = torch.tensor( + [[[-0.2], [1.0], [0.0], [0.0], [0.0], [1.0]]], device=device + ) + for locations in ( + torch.tensor([0.0, 0.2, 0.0]), + torch.tensor([[0.0, 0.2, 0.0]], device=device), + ): + actual = solver.get_jacobian(torch.zeros(1, 1), locations=locations) + torch.testing.assert_close(actual, expected, rtol=0, atol=1e-6) + assert actual.device.type == torch.device(device).type + + @pytest.mark.parametrize("dtype", [np.float16, np.float32, np.float64]) def test_nearest_weight_numpy_dtypes_update_tensor_in_place(dtype) -> None: solver = object.__new__(OPWSolver) diff --git a/tests/sim/scene/feasibility/test_collision_planning.py b/tests/sim/scene/feasibility/test_collision_planning.py index 899164d3aa..12b028b1ee 100644 --- a/tests/sim/scene/feasibility/test_collision_planning.py +++ b/tests/sim/scene/feasibility/test_collision_planning.py @@ -69,6 +69,67 @@ def pose(x): checker.check({"left": pose(0)}) +@pytest.mark.parametrize( + "device", ["cpu", pytest.param("cuda:0", marks=pytest.mark.gpu)] +) +def test_batched_geometry_matches_scalar_rows_and_rebinds_permissions(device): + bodies = ( + CollisionBody("arm", "arm", "box", (0.2, 0.2, 0.2), robot=True), + CollisionBody("payload", "payload", "sphere", (0.1,)), + CollisionBody("obstacle", "obstacle", "box", (0.2, 0.2, 0.2)), + CollisionBody("ground", "ground", "halfspace", (0, 0, 1, 0)), + ) + frames = { + name: np.repeat(np.eye(4)[None], 3, axis=0) + for name in ("arm", "payload", "obstacle", "ground") + } + for name, x in (("arm", 0.0), ("payload", 1.0), ("obstacle", 2.0)): + frames[name][:, 0, 3] = x + frames[name][:, 2, 3] = 0.5 + frames["payload"][1, 0, 3] = 0.1 + frames["payload"][2, 2, 3] = -0.1 + checker = NewtonCollisionChecker(bodies, device=device) + try: + for allowed in ((), (("arm", "payload"),)): + expected, pairs = [], [] + for row in range(3): + report, _ = checker.check( + {key: value[row] for key, value in frames.items()}, + allowed_contacts=allowed, + moving_objects=("payload",), + ) + expected.append(report.status) + pairs.append(checker.last_pair) + result = checker.check_batch( + frames, allowed_contacts=allowed, moving_objects=("payload",) + ) + assert [report.status for report in result] == expected + assert checker.last_batch_pairs == tuple(pairs) + selected = checker.check_batch( + frames, + allowed_contacts=allowed, + moving_objects=("payload",), + env_ids=(2, 0), + ) + assert [report.status for report in selected] == [expected[2], expected[0]] + assert expected == ["passed", "passed", "failed"] + # Changing permissions cannot reuse the previous mask, and invalid row + # IDs or malformed physical transforms fail before querying geometry. + assert ( + checker.check_batch(frames, moving_objects=("payload",))[1].status + == "failed" + ) + with pytest.raises(ValueError, match="unique"): + checker.check_batch(frames, env_ids=(1, 1)) + assert checker.last_batch_pairs == () + malformed = {key: value.copy() for key, value in frames.items()} + malformed["arm"][0, 3, 0] = 1 + with pytest.raises(ValueError, match="rigid"): + checker.check_batch(malformed) + finally: + checker.close() + + def test_phase_branches_must_connect_and_dual_targets_stay_synchronized(): identity = np.eye(4).tolist() mode = TaskOperationMode( diff --git a/tests/sim/sensors/test_contact_default_e2e.py b/tests/sim/sensors/test_contact_default_e2e.py index ba43dc0e80..69997e0e36 100644 --- a/tests/sim/sensors/test_contact_default_e2e.py +++ b/tests/sim/sensors/test_contact_default_e2e.py @@ -19,6 +19,7 @@ import pytest import warp as wp +import torch from embodichain.lab.sim.cfg import ( DefaultPhysicsCfg, @@ -26,7 +27,7 @@ RigidBodyPhysicsCfg, RigidObjectCfg, ) -from embodichain.lab.sim.sensors import ContactSensorCfg +from embodichain.lab.sim.sensors import ContactSensorCfg, ContactPermissionChecker from embodichain.lab.sim.shapes import CubeCfg from embodichain.lab.sim.sim_manager import SimulationManager, SimulationManagerCfg @@ -34,15 +35,19 @@ @pytest.mark.parametrize( - ("device", "expected_contacts_per_env"), + ("device", "expected_contacts_per_env", "direct_gpu_api"), [ - pytest.param("cpu", 4, id="cpu"), - pytest.param("cuda:0", 4, marks=pytest.mark.gpu, id="cuda"), + pytest.param("cpu", 4, True, id="cpu"), + pytest.param("cuda:0", 4, True, marks=pytest.mark.gpu, id="cuda"), + pytest.param( + "cuda:0", 4, False, marks=pytest.mark.gpu, id="cuda-native-reports" + ), ], ) def test_default_contact_sensor_preserves_contact_multiplicity( device: str, expected_contacts_per_env: int, + direct_gpu_api: bool, ) -> None: wp.init() if device.startswith("cuda") and not wp.is_cuda_available(): @@ -53,7 +58,7 @@ def test_default_contact_sensor_preserves_contact_multiplicity( headless=True, num_envs=2, arena_space=2.0, - physics_cfg=DefaultPhysicsCfg(device=device), + physics_cfg=DefaultPhysicsCfg(device=device, direct_gpu_api=direct_gpu_api), ) ) try: @@ -110,6 +115,51 @@ def test_default_contact_sensor_preserves_contact_multiplicity( sensor.get_actor_info(actor_id).path.endswith("/cube") for actor_id in cube_actor_ids ) + assert sim.is_use_gpu_physics == device.startswith("cuda") + assert sim.get_world().get_physics_scene().is_direct_gpu() == ( + device.startswith("cuda") and direct_gpu_api + ) + if device == "cpu" or not direct_gpu_api: + maps = [{} for _ in range(2)] + for uid in ("cube", "ground", "default_plane"): + values = sensor.get_actor_ids(uid)[:, 0].cpu().tolist() + for row, actor in enumerate(values): + maps[row][actor] = uid + checker = ContactPermissionChecker( + sensor, + maps, + allowed_contacts=(("cube", "ground"),), + moving_frames=("cube",), + ) + accepted = checker.check() + assert accepted[:, 0].cpu().tolist() == [0, 0] + refused = ContactPermissionChecker( + sensor, maps, allowed_contacts=(), moving_frames=("cube",) + ) + assert refused.check()[:, 0].cpu().tolist() == [3, 3] + assert accepted[:, 0].cpu().tolist() == [0, 0] + if device.startswith("cuda"): + other = torch.cuda.Stream() + with torch.cuda.stream(other): + repeated = checker.check() + other.synchronize() + assert repeated[:, 0].cpu().tolist() == [0, 0] + bounded = sim.add_sensor( + ContactSensorCfg( + uid="bounded", + rigid_uid_list=["cube"], + filter_need_both_actor=False, + max_contacts_per_env=1, + ) + ) + bounded.update() + overflow = ContactPermissionChecker( + bounded, + maps, + allowed_contacts=(("cube", "ground"),), + moving_frames=("cube",), + ) + assert overflow.check()[:, 0].cpu().tolist() == [4, 4] finally: sim.destroy(exit_process=False) SimulationManager.flush_cleanup_queue() From 2cbe6a59fac51685d29dcd61a4ae86459bfa6037 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 14 Nov 2026 09:12:02 +0000 Subject: [PATCH 05/45] fix(expansion): own validated metadata inputs --- docs/source/api_reference/public_api.rst | 8 +++--- embodichain/utils/_owned_json.py | 33 ++++++++++++++---------- tests/lab/expansion/test_persistence.py | 8 ++++++ 3 files changed, 31 insertions(+), 18 deletions(-) diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 9620eac1cb..c1fd609325 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -1777,7 +1777,7 @@ attempts at episode boundaries. Native failures invalidate the active host. PreparingExpansionHost embodichain.lab.expansion.episode_export ---------------------------------------- +---------------------------------------- .. currentmodule:: embodichain.lab.expansion.episode_export @@ -1946,7 +1946,7 @@ Apply one native visual profile per compatible World cohort. Render complete rec inspect_observation_variants embodichain.lab.expansion.workloads ----------------------------------- +----------------------------------- .. currentmodule:: embodichain.lab.expansion.workloads @@ -3706,7 +3706,7 @@ embodichain.lab.sim.sensors.camera CameraCfg embodichain.lab.sim.sensors.contact ----------------------------------- +----------------------------------- .. currentmodule:: embodichain.lab.sim.sensors.contact @@ -3715,7 +3715,7 @@ embodichain.lab.sim.sensors.contact ContactPermissionChecker embodichain.lab.sim.sensors.contact.permissions ----------------------------------------------- +----------------------------------------------- .. currentmodule:: embodichain.lab.sim.sensors.contact.permissions diff --git a/embodichain/utils/_owned_json.py b/embodichain/utils/_owned_json.py index 851a39a93d..12460f8477 100644 --- a/embodichain/utils/_owned_json.py +++ b/embodichain/utils/_owned_json.py @@ -32,14 +32,29 @@ class _FrozenTuple(tuple): __slots__ = () + def __new__(cls, values): + return super().__new__(cls, (freeze_metadata(value) for value in values)) + class _FrozenMap(Mapping): """Own validated immutable children; expose no mutable backing dictionary.""" __slots__ = ("_data", "_strict", "_encoded") - def __init__(self, values, strict): - object.__setattr__(self, "_data", MappingProxyType(values)) + def __init__(self, values): + if not isinstance(values, Mapping): + raise ValueError("metadata must contain only finite JSON values") + owned, strict = {}, True + for key, value in values.items(): + if not isinstance(key, str) or not key.strip(): + raise ValueError("metadata key must be a nonempty string") + item = freeze_metadata(value) + owned[key] = item + strict &= _key_valid(key) and _strict_keys(item) + # Validate and detach even direct construction of a node's public type. + # Reusing a supposedly immutable wrapper over a caller-owned dictionary + # would permit later mutation to invalidate its finite-JSON proof. + object.__setattr__(self, "_data", MappingProxyType(owned)) object.__setattr__(self, "_strict", strict) object.__setattr__(self, "_encoded", None) @@ -99,17 +114,7 @@ def freeze_metadata(value: object) -> object: raise ValueError("metadata must contain only finite JSON values") return value if isinstance(value, (list, tuple)): - return _FrozenTuple(freeze_metadata(item) for item in value) + return _FrozenTuple(value) if isinstance(value, Mapping): - result = {} - strict = True - for key, item in value.items(): - # The expansion contract accepts whitespace around nonempty keys; - # the Gym JSON-copy contract is stricter. Retain both behaviours. - if not isinstance(key, str) or not key.strip(): - raise ValueError("metadata key must be a nonempty string") - owned = freeze_metadata(item) - result[key] = owned - strict &= _key_valid(key) and _strict_keys(owned) - return _FrozenMap(result, strict) + return _FrozenMap(value) raise ValueError("metadata must contain only finite JSON values") diff --git a/tests/lab/expansion/test_persistence.py b/tests/lab/expansion/test_persistence.py index 81bcd6d803..cb8e043b04 100644 --- a/tests/lab/expansion/test_persistence.py +++ b/tests/lab/expansion/test_persistence.py @@ -88,6 +88,14 @@ def test_verified_episode_reads_own_tensors_and_invalidate_changed_bytes(tmp_pat projected = json_safe_copy(third.metadata, field_name="metadata") projected["nested"]["values"][0] = 55 assert third.metadata["nested"]["values"][0] == 1 + external = {"nested": [3]} + owned = type(third.metadata)(external) + external["nested"][0] = 99 + assert owned["nested"][0] == 3 + with pytest.raises(ValueError, match="finite"): + type(third.metadata)({"invalid": float("nan")}) + with pytest.raises(ValueError, match="finite"): + type(third.metadata["nested"]["values"])((float("inf"),)) path = Path(receipt.storage_id) path.write_bytes(b"invalid replaced evidence") with pytest.raises((ValueError, OSError)): From 61f922d38e6161ec3a7b19137f7f043f8fe8192e Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 14 Nov 2026 10:24:38 +0000 Subject: [PATCH 06/45] fix(sensors): wait for contact sample before permission checks Order permission queries after the latest sensor sample on the caller's CUDA stream without resampling or advancing histories. Cover pending forbidden contacts and dropped evidence in standalone and history sampling paths. Validation: 75 targeted tests passed; Black 26.10.1, public API documentation coverage and agent context checks passed. --- .../topics/sensor-system/sensor-system.md | 2 ++ .../lab/sim/sensors/contact/permissions.py | 5 +++ .../sim/sensors/test_contact_sensor_graph.py | 36 ++++++++++++++++--- 3 files changed, 38 insertions(+), 5 deletions(-) diff --git a/agent_context/topics/sensor-system/sensor-system.md b/agent_context/topics/sensor-system/sensor-system.md index 82e9440ed1..91601b5073 100644 --- a/agent_context/topics/sensor-system/sensor-system.md +++ b/agent_context/topics/sensor-system/sensor-system.md @@ -128,6 +128,8 @@ valid; sensor adaptation does not synthesize a common contact manifold. `ContactPermissionChecker` consumes complete native samples with explicit actor/frame permissions, returning owned per-row device summaries. It does not choose the host's sampling frequency or relax unavailable-impulse/loss gates. +Eager checks wait for the latest sensor sample on their CUDA stream without +refreshing contacts or advancing histories. Global static actors retain their explicit row memberships. Rebind after query topology changes. Default CUDA dynamics can select ordinary reporting through `DefaultPhysicsCfg.direct_gpu_api=False` when static actor identity is required; diff --git a/embodichain/lab/sim/sensors/contact/permissions.py b/embodichain/lab/sim/sensors/contact/permissions.py index 831dc8f3b1..5270daac22 100644 --- a/embodichain/lab/sim/sensors/contact/permissions.py +++ b/embodichain/lab/sim/sensors/contact/permissions.py @@ -123,6 +123,9 @@ def __init__( def check(self) -> torch.Tensor: """Return owned per-row ``(reason, actor0, actor1)`` integer summaries. + Eager CUDA checks wait for the latest sensor sample on the caller's + stream without refreshing contacts or advancing histories. + Returns: Tensor ``(B, 3)`` on the sensor device. Reason zero passes; one means unavailable applied impulse, two unknown/cross-row actor, three @@ -140,6 +143,8 @@ def check(self) -> torch.Tensor: if self._ready is not None: torch.cuda.current_stream(self.device).wait_event(self._ready) + # The checker event orders its own buffers, not the sensor's producer. + self.sensor._wait_for_sample() data = self.sensor.get_data() if id(self.sensor._query.actors) != self._actors_token: raise RuntimeError( diff --git a/tests/sim/sensors/test_contact_sensor_graph.py b/tests/sim/sensors/test_contact_sensor_graph.py index 2241692cbb..297039b4e0 100644 --- a/tests/sim/sensors/test_contact_sensor_graph.py +++ b/tests/sim/sensors/test_contact_sensor_graph.py @@ -23,7 +23,11 @@ import warp as wp from dexsim.scene import ContactActorInfo, ContactBuffer, ContactQueryCapabilities -from embodichain.lab.sim.sensors import ContactSensor, ContactSensorCfg +from embodichain.lab.sim.sensors import ( + ContactPermissionChecker, + ContactSensor, + ContactSensorCfg, +) pytestmark = [pytest.mark.gpu, pytest.mark.no_sim] @@ -209,8 +213,14 @@ def assert_air_time(value: float) -> None: @pytest.mark.parametrize("history_sample", [False, True]) -def test_cached_sample_is_ready_on_another_cuda_stream(history_sample: bool) -> None: - """A cached update must wait for the producer's outstanding device work.""" +@pytest.mark.parametrize( + "dropped_count", + [pytest.param(0, id="forbidden-contact"), pytest.param(3, id="dropped-evidence")], +) +def test_cached_sample_is_ready_on_another_cuda_stream( + history_sample: bool, dropped_count: int +) -> None: + """Permission checks and cached updates wait for the pending sensor sample.""" wp.init() device = torch.device("cuda:0") query = _CudaQuery(device) @@ -238,6 +248,12 @@ def test_cached_sample_is_ready_on_another_cuda_stream(history_sample: bool) -> if history_sample else None ) + checker = ContactPermissionChecker( + sensor, + ({0: "ground", 10: "cube"}, {0: "ground", 20: "cube"}), + allowed_contacts=(), + moving_frames=("cube",), + ) def sample() -> None: if history_sample: @@ -246,7 +262,10 @@ def sample() -> None: sensor.update() sample() # Compile kernels before arranging the delayed producer. + checker.check() sensor.reset() + query.buffer.device_counts[0] = 0 if dropped_count else 2 + query.buffer.device_counts[1] = dropped_count torch.cuda.synchronize(device) owner.physics_step_index += 1 producer = torch.cuda.Stream(device=device) @@ -255,14 +274,21 @@ def sample() -> None: torch.cuda._sleep(200_000_000) sample() with torch.cuda.stream(consumer): + # Check before a cached update can independently wait for the producer. + permissions = checker.check() sample() observed = sensor.get_data()["is_valid"].clone() force = history.force.clone() if history is not None else None consumer.synchronize() - assert observed.tolist() == [[True, False], [True, False]] + assert permissions.tolist() == ( + [[4, 0, 0], [4, 0, 0]] if dropped_count else [[3, 0, 10], [3, 0, 20]] + ) + assert observed.tolist() == [[not dropped_count, False]] * 2 if force is not None: torch.testing.assert_close( force, - torch.tensor([[[0.0, 0.0, 2.0]], [[0.0, 0.0, 2.0]]], device=device), + torch.tensor( + [[[0.0, 0.0, 0.0 if dropped_count else 2.0]]] * 2, device=device + ), ) producer.synchronize() From 4723cba69c505c3b33315efcbdee51a6b3c9d946 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 14 Nov 2026 14:02:08 +0000 Subject: [PATCH 07/45] feat(expansion): expose task ports and measured ghost comparison --- agent_context/MAP.yaml | 2 +- .../expansion-engine/expansion-engine.md | 20 + .../topics/motion-planning/motion-planning.md | 7 + .../sim-visualization/sim-visualization.md | 9 + .../embodichain.lab.task_program.rst | 4 + docs/source/api_reference/public_api.rst | 92 +++ embodichain/lab/expansion/__init__.py | 14 + embodichain/lab/expansion/_gym_context.py | 4 + embodichain/lab/expansion/_gym_execution.py | 83 +++ embodichain/lab/expansion/acceptance.py | 611 ++++++++++++++++ embodichain/lab/expansion/comparison.py | 681 ++++++++++++++++++ embodichain/lab/expansion/host.py | 32 +- embodichain/lab/expansion/host_extensions.py | 112 +++ embodichain/lab/sim/atomic_skills/policies.py | 27 +- .../sim/atomic_skills/primitives/pick_up.py | 7 + .../lab/sim/atomic_skills/primitives/place.py | 7 + embodichain/lab/sim/motion/_phase_paths.py | 110 +++ .../lab/sim/motion/motion_generator.py | 30 + .../integrations/_native_plan_expansion.py | 455 ++++++++++++ .../task_program/integrations/configured.py | 2 + .../task_program/integrations/expansion.py | 72 +- .../lab/task_program/runtime/results.py | 1 + .../lab/visualization/markers/__init__.py | 2 + .../lab/visualization/markers/_native.py | 43 +- .../lab/visualization/markers/frame.py | 86 +++ .../lab/visualization/markers/group.py | 6 +- tests/lab/expansion/test_comparison.py | 76 ++ .../lab/expansion/test_extension_ports_sim.py | 193 +++++ 28 files changed, 2775 insertions(+), 13 deletions(-) create mode 100644 embodichain/lab/expansion/acceptance.py create mode 100644 embodichain/lab/expansion/comparison.py create mode 100644 embodichain/lab/expansion/host_extensions.py create mode 100644 embodichain/lab/sim/motion/_phase_paths.py create mode 100644 embodichain/lab/task_program/integrations/_native_plan_expansion.py create mode 100644 embodichain/lab/visualization/markers/frame.py create mode 100644 tests/lab/expansion/test_comparison.py create mode 100644 tests/lab/expansion/test_extension_ports_sim.py diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 0e27617b7f..c5b8dc0e3e 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -198,7 +198,7 @@ topics: - id: expansion-engine title: Expansion Engine aliases: [Expansion Engine, offline expansion, 扩增引擎, 批量专家采集] - keywords: [ExpansionEngine, ExpansionRunCfg, ExpansionJobCfg, ExpansionSession, ExpansionSessionCfg, ExpansionCoverage, EpisodeCoverage, GymExpansionHost, ArtifactEpisodeSink, MediaSpool, render_observation_variants, WorkloadProgramExecutor, WorkloadExecutionCfg, workload_count] + keywords: [ExpansionEngine, ExpansionRunCfg, ExpansionJobCfg, ExpansionSession, ExpansionSessionCfg, ExpansionCoverage, EpisodeCoverage, GymExpansionHost, ArtifactEpisodeSink, MediaSpool, render_observation_variants, WorkloadProgramExecutor, WorkloadExecutionCfg, workload_count, GymExpansionHostExtensions, MeasuredExpansionValidator, MeasuredTrajectory, preview_expansion_comparison, 扩增虚影] paths: [topics/expansion-engine/expansion-engine.md] source_of_truth: - embodichain/lab/expansion/ diff --git a/agent_context/topics/expansion-engine/expansion-engine.md b/agent_context/topics/expansion-engine/expansion-engine.md index 88c2794c38..cb2fe4abab 100644 --- a/agent_context/topics/expansion-engine/expansion-engine.md +++ b/agent_context/topics/expansion-engine/expansion-engine.md @@ -13,6 +13,9 @@ algorithms stay in `sim.motion.expansion`; scene and workspace algorithms stay i | Rolling workload × B execution in one caller-owned World | `embodichain/lab/expansion/workloads.py` | | Job assembly, stage recovery and inspect | `embodichain/lab/expansion/workflow.py`, `stages.py` | | Native World ownership and compatibility facade | `embodichain/lab/expansion/host.py` | +| Task-owned render/source/transform/acceptance ports | `embodichain/lab/expansion/host_extensions.py` | +| Measured geometry, native substeps and sustained-contact acceptance | `embodichain/lab/expansion/acceptance.py` | +| Physics-free measured reference/ghost comparison | `embodichain/lab/expansion/comparison.py` | | Native recipes, cohort preparation, step observation and episode assembly | `embodichain/lab/expansion/_gym_recipe.py`, `_gym_prepare.py`, `_gym_execution.py`, `_gym_frames.py`, `_gym_episode.py` | | Durable recipes, attempts and invocation budgets | `embodichain/lab/expansion/journal.py` | | Task Program execution with native or prototype motion | `embodichain/lab/expansion/program.py`, `real2sim.py` | @@ -73,6 +76,23 @@ sampling, whole-cohort preparation, active-row frame buffering and measured epis assembly have separate owners. Preserve the host constructor, port methods and existing readout properties used by benchmark/showcase instrumentation. +`GymExpansionHostExtensions` supplies non-owning callbacks for collection renderer, +scene source, immutable recipe transformation, measured validator and row plan +factory. `ExpansionRowContext` carries pinned identity without exposing mutable +host buffers. Hosts retain preparation, control clocks, T+1 capture, attachment +archival and confirmed receipts. Do not replace configured nominal sources with +an extension transform. `MeasuredExpansionValidator` presents the same lifecycle +for independent geometry and native contacts, including task-declared sustained +finger contact and drift limits. Every original physical substep remains required; +missing impulses, unknown actors and dropped samples reject acceptance. + +`preview_expansion_comparison` consumes a caller-owned environment factory and +qualified measured episodes. Shared-time pause/switch controls retain the selected +time; physics stays disabled and render updates do not mutate saved evidence. +`MarkerFramePublisher` owns resident robot/object groups and publishes a coherent +cross-group frame through the public marker layer. Task assets and any collection +process orchestration remain outside the replay core. + `TaskProgramDemoBridge.program_id` and `env_ids` expose binding identity without runtime traversal. `EmbodiedEnv.task_program_bridge_scope()` restores the borrowed bridge even after failure. `Real2SimTabletopSource` and explicit diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index df16f69a42..527adf63f0 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -53,6 +53,13 @@ positions `(B, N, dof)`. Use `PlanState.single()` for one environment. duration is their sum. Failed results may omit positions. Planning timing never changes simulator `physics_dt`. +`MotionGenOptions.waypoint_modes` explicitly selects joint/Cartesian paths to +individual keyframes under fixed-grid sequential IK. Adjacent equal modes share +ordinary native requests; composition retains the total control grid and row +failure masks. No robot UID or waypoint count enables the behavior. Atomic +`MotionPolicy.phase_paths` names the PickUp/Place phases that opt into this path; +ordinary calls and other primitives retain their own planning semantics. + `motion_gen` delegates timing to its backend. `ik_interp` needs explicit start/count/dt, rejects backend-specific options and uses minimum-jerk segments when limits apply. Adaptive timing and batched IK remain defaults. Explicit diff --git a/agent_context/topics/sim-visualization/sim-visualization.md b/agent_context/topics/sim-visualization/sim-visualization.md index 2f5a29ad49..4abc6bcca5 100644 --- a/agent_context/topics/sim-visualization/sim-visualization.md +++ b/agent_context/topics/sim-visualization/sim-visualization.md @@ -80,6 +80,15 @@ with visual origins. URDF FK accepts all `joint_names`, including caller-resolve mimic joints. Multiple poses can be displayed in one environment. Tasks own motion loading, sampling and alignment. +`MarkerFramePublisher` stages caller-owned groups for one cross-group native +publication. Initial preload retains hidden resources; subsequent frames upload +visible instances and change real visibility without moving hidden geometry. +`MarkerGroup.snapshot(visible_only=True)` preserves original instance IDs while +omitting hidden meshes. Close the publisher before its World. The measured +reference/ghost timeline and keyboard playback live in +`lab/expansion/comparison.py`; task/environment and collection ownership stay +with the caller. + `RobotMarkersAppearanceCfg` selects source appearance or a base-color override, with adjustable opacity. Source textures and material parameters are preserved; opaque visuals use scene lighting. Transparent shading may differ by renderer. diff --git a/docs/source/api_reference/embodichain/embodichain.lab.task_program.rst b/docs/source/api_reference/embodichain/embodichain.lab.task_program.rst index 6eeee2f284..97394708b1 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.task_program.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.task_program.rst @@ -404,6 +404,7 @@ template contract. TaskProgramMappedRetimeRecord TaskProgramSameGridExpansionFactory TaskProgramSameGridExpansionRecord + TaskProgramNativeExpansionRecord .. autoclass:: TaskProgramSourceAdapter :members: @@ -426,6 +427,9 @@ template contract. .. autoclass:: TaskProgramSameGridExpansionRecord :members: +.. autoclass:: TaskProgramNativeExpansionRecord + :members: + .. currentmodule:: embodichain.lab.task_program MLLM frontend diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index c1fd609325..15ba57ac10 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -1672,6 +1672,33 @@ and durable episode-boundary recovery. See the offline expansion example. WorkloadProgramExecutor export_expansion_episodes +.. autosummary:: + + GymExpansionHostExtensions + ExpansionRowContext + ExpansionValidator + MeasuredExpansionValidator + MeasuredTrajectory + record_reference_trajectory + preview_expansion_comparison + +embodichain.lab.expansion.acceptance +------------------------------------ + +Measured task and physical acceptance share a host port. Geometry uses independent +Newton queries; native contacts inspect every physical sample and optionally +require sustained finger contact and bounded object drift. Tasks declare all +permissions and thresholds; missing or dropped evidence rejects the trajectory. + +.. currentmodule:: embodichain.lab.expansion.acceptance + +.. autosummary:: + + MeasuredExpansionValidator + +.. autoclass:: MeasuredExpansionValidator + :members: + embodichain.lab.expansion.artifacts ----------------------------------- @@ -1685,6 +1712,29 @@ Typed content-addressed numeric, media and source references. Paths remain conta EpisodeArtifactManifest resolve_artifact_path +embodichain.lab.expansion.comparison +------------------------------------ + +Reference and accepted expansion recordings share a measured timeline. The +native viewer retains robot/object ghost resources, supports pause and selection, +and publishes render states with physics disabled. Collection is an optional +caller-owned callback, and the environment factory retains task/asset ownership. + +.. currentmodule:: embodichain.lab.expansion.comparison + +.. autosummary:: + + MeasuredTrajectory + record_reference_trajectory + preview_expansion_comparison + +.. autoclass:: MeasuredTrajectory + :members: + +.. autofunction:: record_reference_trajectory + +.. autofunction:: preview_expansion_comparison + embodichain.lab.expansion.configured ------------------------------------ @@ -1813,6 +1863,30 @@ The native Gym adapter owns World preparation, parameter readback and Task Progr GymExpansionHost +embodichain.lab.expansion.host_extensions +----------------------------------------- + +Runtime extension callbacks select collection rendering, a scene source, measured +acceptance, immutable recipe transforms and per-row Task Program plan factories. +The native host continues to own preparation, clocks, state capture and receipts. + +.. currentmodule:: embodichain.lab.expansion.host_extensions + +.. autosummary:: + + ExpansionValidator + ExpansionRowContext + GymExpansionHostExtensions + +.. autoclass:: ExpansionValidator + :members: + +.. autoclass:: ExpansionRowContext + :members: + +.. autoclass:: GymExpansionHostExtensions + :members: + embodichain.lab.expansion.journal --------------------------------- @@ -4370,6 +4444,8 @@ native simulation windows and browser visualization. MarkerPrototypeCfg MarkerGroup + MarkerFramePublisher + embodichain.lab.visualization.markers.cfg ---------------------------------------- @@ -4380,6 +4456,22 @@ embodichain.lab.visualization.markers.cfg MarkerPrototypeCfg MarkerGroupCfg +embodichain.lab.visualization.markers.frame +------------------------------------------- + +The frame publisher stages multiple marker groups and submits one native visual +batch. Hidden instances retain their resources and actual visibility; later +frames transform only visible instances. Publishing never advances physics. + +.. currentmodule:: embodichain.lab.visualization.markers.frame + +.. autosummary:: + + MarkerFramePublisher + +.. autoclass:: MarkerFramePublisher + :members: + embodichain.lab.visualization.markers.group ------------------------------------------ diff --git a/embodichain/lab/expansion/__init__.py b/embodichain/lab/expansion/__init__.py index 5a32997b25..fd6dd78f38 100644 --- a/embodichain/lab/expansion/__init__.py +++ b/embodichain/lab/expansion/__init__.py @@ -67,6 +67,13 @@ "WorkloadSource", "WorkloadProgramExecutor", "export_expansion_episodes", + "GymExpansionHostExtensions", + "ExpansionRowContext", + "ExpansionValidator", + "MeasuredExpansionValidator", + "MeasuredTrajectory", + "record_reference_trajectory", + "preview_expansion_comparison", ] @@ -112,6 +119,13 @@ def __getattr__(name: str) -> object: "WorkloadSource": "workloads", "WorkloadProgramExecutor": "workloads", "export_expansion_episodes": "episode_export", + "GymExpansionHostExtensions": "host_extensions", + "ExpansionRowContext": "host_extensions", + "ExpansionValidator": "host_extensions", + "MeasuredExpansionValidator": "acceptance", + "MeasuredTrajectory": "comparison", + "record_reference_trajectory": "comparison", + "preview_expansion_comparison": "comparison", }.get(name, "engine") module = f"{__name__}.{owner}" value = getattr(import_module(module), name) diff --git a/embodichain/lab/expansion/_gym_context.py b/embodichain/lab/expansion/_gym_context.py index 1bfd210f86..0ae951c4af 100644 --- a/embodichain/lab/expansion/_gym_context.py +++ b/embodichain/lab/expansion/_gym_context.py @@ -27,6 +27,7 @@ from .journal import ExpansionJournal, RecipeAttempt from .recipes import BatchCompatibility, OperatorCapability, ExpansionRecipe from .contracts import CandidateIdentity, ValidationResult +from .host_extensions import GymExpansionHostExtensions if TYPE_CHECKING: from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv, EmbodiedEnvCfg @@ -90,3 +91,6 @@ class GymHostContext: services: dict[int, _EpisodeSource] = field(default_factory=dict) bridges: dict[int, TaskProgramDemoBridge] = field(default_factory=dict) recipe: Callable[[int], ExpansionRecipe] | None = None + extensions: GymExpansionHostExtensions = field( + default_factory=GymExpansionHostExtensions + ) diff --git a/embodichain/lab/expansion/_gym_execution.py b/embodichain/lab/expansion/_gym_execution.py index 8c45786af6..5ced90c201 100644 --- a/embodichain/lab/expansion/_gym_execution.py +++ b/embodichain/lab/expansion/_gym_execution.py @@ -18,10 +18,13 @@ from __future__ import annotations from collections.abc import Callable +from dataclasses import replace import time from embodichain.data_pipeline.recording.media import _atomic_json from embodichain.lab.expansion.program import TaskProgramBatchExecutor from .contracts import CandidateIdentity, ExpertEpisode +from .contracts import SceneCase +from .host_extensions import ExpansionRowContext from embodichain.lab.sim.scene.physics import capture_physics_properties from .engine import ExpansionAssignment @@ -65,6 +68,14 @@ def execute_batch( for assignment in assigned } bridges, services = {}, {} + factories, adapters = {}, [] + extension = ctx.extensions + if extension.plan_transform_factory is not None: + ctx.source_artifacts = { + key: value + for key, value in ctx.source_artifacts.items() + if not key.startswith("plan_") + } ctx.bridges, ctx.services = bridges, services phase_started = time.monotonic() prototype_profile = None @@ -84,6 +95,44 @@ def execute_batch( record, ) in assignments.items(): ordinal = ctx.requests[row].recipe.context.ordinal + if extension.plan_transform_factory is not None: + if ( + profile is not None + or env.task_program_adapter.configured_nominal_sources + or motion_profile is not None + ): + raise ValueError( + "an extension transform cannot replace a configured nominal/motion source" + ) + request = ctx.requests[row] + factory = extension.plan_transform_factory( + env, + ExpansionRowContext( + row, + identity, + ordinal, + request.attempt_id, + directory, + SceneCase(**record["case"]), + request.recipe, + ), + ) + if factory is None: + raise ValueError("plan_transform_factory returned no factory") + factories[row] = factory + adapter = ( + env.task_program_adapter + if cfg.batch_size == 1 + else env.task_program_adapter.for_env_ids((row,)) + ) + if cfg.batch_size != 1: + adapters.append(adapter) + bridges[row] = adapter.create_bridge( + program, + plan_transform_factory=factory, + isolate_policy_sessions=True, + ) + continue if profile is None: from embodichain.lab.task_program.integrations.demo_skill import ( _ConfiguredDemoSkillSource, @@ -168,9 +217,43 @@ def mark_started(row): ), ) timings["execute"] += time.monotonic() - phase_started + if factories: + expansions, accepted, reasons = ( + [], + list(result.accepted), + list(result.reasons), + ) + for index, row in enumerate(result.env_ids): + factory = factories[row] + records = tuple(factory.records) + assignments[row][4]["expansion"] = [ + record.to_metadata() for record in records + ] + portable = [] + for number, record in enumerate(records): + trace = record.to_metadata() + if "plan_artifact" in trace: + key = f"plan_{row}_{number}" + ctx.source_artifacts[key] = trace.pop("plan_artifact") + trace["plan_artifact_key"] = key + portable.append(trace) + expansions.append(tuple(portable)) + if extension.require_plan_transform and accepted[index] and not records: + accepted[index], reasons[index] = ( + False, + "requested_operator_not_applied", + ) + result = replace( + result, + expansion_records=tuple(expansions), + accepted=tuple(accepted), + reasons=tuple(reasons), + ) physics_after = capture_physics_properties(view) episodes = GymEpisodeAssembler(ctx).assemble( assignments, collector, services, result, physics_after ) ctx.services, ctx.bridges = services, bridges + for adapter in adapters: + adapter.close() return tuple(episodes[assignment.env_id] for assignment in assigned) diff --git a/embodichain/lab/expansion/acceptance.py b/embodichain/lab/expansion/acceptance.py new file mode 100644 index 0000000000..71e8220df4 --- /dev/null +++ b/embodichain/lab/expansion/acceptance.py @@ -0,0 +1,611 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Shared measured geometry/contact acceptance without task-owned World control.""" + +from __future__ import annotations + +from collections import deque +from pathlib import Path +from typing import Any, TYPE_CHECKING +from collections.abc import Mapping, Sequence +from types import SimpleNamespace +import xml.etree.ElementTree as ET +import numpy as np +import torch +from embodichain.data_pipeline.recording.media import _atomic_json as atomic_json + +if TYPE_CHECKING: + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv + from embodichain.lab.gym.envs.types import ControllerAction + from embodichain.lab.sim.scene import SceneState + +__all__ = ["MeasuredExpansionValidator"] + + +class _GeometryRow: + """Independently check measured task predicates and complete physical geometry.""" + + def _initialize_task_checks(self, env, configuration, output): + from embodichain.lab.task_program.integrations.feasibility import ( + MeasuredTaskMonitor, + ) + + self.env, self.configuration, self.output = env, configuration, output + self.monitor = MeasuredTaskMonitor( + configuration["final_checks"], configuration["events"], env_id=self.env_id + ) + for condition in configuration["initial_checks"]: + self.monitor._validate(condition) + self.initial, self.terminal, self.results = {}, {}, {} + + @staticmethod + def contact_permissions(env, configuration, known): + """Resolve authored contacts and mechanically adjacent robot assemblies.""" + # Collapse fixed URDF assemblies before deciding mechanical adjacency. + model = ET.parse(env.robot.cfg.fpath).getroot() + joints = model.findall("joint") + links = {link.get("name") for link in model.findall("link")} + groups = {link: link for link in links} + + def group(link): + while groups[link] != link: + link = groups[link] + return link + + for joint in joints: + if joint.get("type") == "fixed": + parent, child = joint.find("parent").get("link"), joint.find( + "child" + ).get("link") + groups[group(child)] = group(parent) + adjacent = { + frozenset( + ( + group(j.find("parent").get("link")), + group(j.find("child").get("link")), + ) + ) + for j in joints + } + physical_links = sorted( + link for link in links if f"{env.robot.uid}/{link}" in known + ) + ignored = [ + (f"{env.robot.uid}/{left}", f"{env.robot.uid}/{right}") + for i, left in enumerate(physical_links) + for right in physical_links[i + 1 :] + if group(left) == group(right) + or frozenset((group(left), group(right))) in adjacent + ] + children = {j.get("name"): j.find("child").get("link") for j in joints} + root_groups = {group(link) for link in links - set(children.values())} + roots = {link for link in physical_links if group(link) in root_groups} + + def expand(name): + if name == "@root": + return [ + f"{env.robot.uid}/{link}" + for link in sorted(roots) + if f"{env.robot.uid}/{link}" in known + ] + if name.startswith("@"): + return [ + f"{env.robot.uid}/{children[joint]}" + for joint in env.robot.control_parts[name[1:]] + ] + return [name] + + def pairs(values): + return tuple( + (a, b) + for left, right in values + for a in expand(left) + for b in expand(right) + ) + + ignored.extend(pairs(configuration["mounting_contacts"])) + allowed = pairs(configuration["allowed_contacts"]) + if any(not set(pair) <= known for pair in (*ignored, *allowed)): + raise ValueError("Contact permissions reference an absent physical frame") + # Match the physical model's authored self-collision policy. Keeping + # robot/scene checks does not require all internal robot shape pairs. + if env.robot.cfg.root_props.self_collision_enabled is False: + robot_frames = sorted( + frame for frame in known if frame.startswith(env.robot.uid + "/") + ) + ignored.extend( + (a, b) + for i, a in enumerate(robot_frames) + for b in robot_frames[i + 1 :] + ) + return ignored, allowed + + def __init__( + self, env, configuration: dict, output: Path, *, device="cpu", env_id=0 + ): + self.env_id = env_id + from embodichain.lab.task_program.integrations.simulation.feasibility_geometry import ( + collision_bodies_from_simulation, + ) + from embodichain.lab.sim.scene.feasibility import NewtonCollisionChecker + + self._initialize_task_checks(env, configuration, output) + bodies = collision_bodies_from_simulation(env, env_id=self.env_id) + known = {body.frame_uid for body in bodies} + ignored, self.allowed_contacts = self.contact_permissions( + env, configuration, known + ) + checker = NewtonCollisionChecker( + bodies, ignored_pairs=ignored, device=device, max_shapes=512 + ) + self.checkers = [(checker, self.allowed_contacts, known)] + atomic_json( + output / "collision-geometry.json", + { + "shape_count": len(bodies), + "frames": sorted(known), + "query_count": len(self.checkers), + "self_collision_enabled": env.robot.cfg.root_props.self_collision_enabled, + }, + ) + self.moving = tuple( + uid + for uid in env.sim.get_rigid_object_uid_list() + if env.sim.get_rigid_object(uid).body_type == "dynamic" + ) + + def prepare(self, snapshot=None): + self.monitor.reset(self.env) + self.initial = { + self.env_id: all( + self.monitor.predicate(self.env, c) + for c in self.configuration["initial_checks"] + ) + and not all( + self.monitor.predicate(self.env, c) + for c in self.configuration["final_checks"] + ) + } + self.terminal.clear() + self.results.clear() + + def sample(self, ids, action, snapshot=None): + from embodichain.utils.math import matrix_from_quat + + self.monitor.observe(self.env, action) + self.terminal[self.env_id] = self.monitor.validation(self.env) + # Read the measured link buffer once and convert the whole arm in one + # batch. get_link_pose() otherwise repeats these allocations and + # quaternion conversions separately for every physical/visual link. + robot = self.env.robot + poses = robot.body_data.body_link_pose[self.env_id] + matrices = torch.eye(4, dtype=poses.dtype, device=poses.device).repeat( + len(robot.link_names), 1, 1 + ) + matrices[:, :3, :3] = matrix_from_quat(poses[:, 3:]) + matrices[:, :3, 3] = poses[:, :3] + matrices = matrices.detach().cpu().numpy() + frames = { + f"{robot.uid}/{link}": matrix + for link, matrix in zip(robot.link_names, matrices) + } + frames.update( + { + uid: self.env.sim.get_rigid_object(uid) + .get_local_pose(to_matrix=True)[self.env_id] + .detach() + .cpu() + .numpy() + for uid in self.env.sim.get_rigid_object_uid_list() + } + ) + frames["default_plane"] = np.eye(4) + for checker, contacts, aliases in self.checkers: + report, _ = checker.check( + frames, + allowed_contacts=contacts, + moving_objects=tuple(uid for uid in self.moving if uid in aliases), + ) + if self.env_id not in self.results or report.status != "passed": + self.results[self.env_id] = SimpleNamespace( + status=report.status, + reason=f"{report.reason}; pair={checker.last_pair}", + ) + if report.status != "passed": + metadata = action.metadata.get("row_actions", {}).get( + str(self.env_id), action.metadata + ) + atomic_json( + self.output / "collision-failure.json", + { + "reason": self.results[self.env_id].reason, + "action": { + key: str(value) + for key, value in metadata.items() + if isinstance(value, (str, int, float, bool)) + }, + }, + ) + break + + def close(self): + for checker, _, _ in self.checkers: + checker.close() + self.checkers.clear() + + +class _ContactRow(_GeometryRow): + """Inspect actual physics contacts, including every control substep. + + Unlike the independent Newton query, this backend retains native triangle + meshes. Unknown actors, unavailable impulses and dropped rows fail closed. + The optional final hold audit additionally requires both physical fingers + throughout the final measured control samples and bounds object drift. + """ + + def __init__(self, env, configuration, output, *, sensor, env_id=0): + self.env_id = env_id + self._initialize_task_checks(env, configuration, output) + self.active, self._bad, self.substeps, self._interval_samples = ( + False, + None, + 0, + 0, + ) + self.hold = configuration.get("contact_hold") + count = ( + round(self.hold["duration_s"] * env.control_frequency) + 1 + if self.hold + else 1 + ) + self.samples = deque(maxlen=count) + self.sensor = sensor + self.frames = {} + for link, actor in zip( + env.robot.link_names, + self.sensor.get_actor_ids(env.robot.uid, env.robot.link_names)[ + self.env_id + ].tolist(), + ): + self.frames[actor] = f"{env.robot.uid}/{link}" + for uid in (*env.sim.get_rigid_object_uid_list(), "default_plane"): + self.frames[int(self.sensor.get_actor_ids(uid)[self.env_id, 0])] = uid + ignored, allowed = self.contact_permissions( + env, configuration, set(self.frames.values()) + ) + self.allowed = {frozenset(pair) for pair in (*ignored, *allowed)} + self.moving = {f"{env.robot.uid}/{link}" for link in env.robot.link_names} | { + uid + for uid in env.sim.get_rigid_object_uid_list() + if env.sim.get_rigid_object(uid).body_type == "dynamic" + } + atomic_json( + output / "collision-geometry.json", + { + "backend": "native_contacts", + "geometry": "native physical shapes, including triangle meshes", + "frames": sorted(self.frames.values()), + "substep_sampling": True, + "max_contacts_per_env": 8192, + "self_collision_enabled": env.robot.cfg.root_props.self_collision_enabled, + }, + ) + + def reset(self, env_ids=None): + self.active, self._bad, self.substeps, self._interval_samples = ( + False, + None, + 0, + 0, + ) + self.samples.clear() + + def prepare(self, snapshot=None): + super().prepare(snapshot) + self.reset() + self.active = True + + def begin_control_step(self): + self._bad = None + self._interval_samples = 0 + + def before_sample(self): + return None + + def _hold_report(self): + report = { + "samples": len(self.samples), + "required_samples": self.samples.maxlen, + "physics_substeps": self.substeps, + } + if len(self.samples) != self.samples.maxlen: + return {**report, "passed": False} + poses = np.asarray([sample[0] for sample in self.samples]) + from scipy.spatial.transform import Rotation + + rotations = Rotation.from_quat(poses[:, 3:]) + report.update( + both_fingers_samples=sum(all(sample[1]) for sample in self.samples), + max_translation_drift_m=float( + np.linalg.norm(poses[:, :3] - poses[0, :3], axis=1).max() + ), + max_rotation_drift_rad=float( + (rotations[0].inv() * rotations).magnitude().max() + ), + ) + report["passed"] = ( + report["both_fingers_samples"] == self.samples.maxlen + and report["max_translation_drift_m"] + <= self.hold["max_translation_drift_m"] + and report["max_rotation_drift_rad"] <= self.hold["max_rotation_drift_rad"] + ) + return report + + def sample(self, ids, action, snapshot=None): + from embodichain.lab.expansion.contracts import ( + ValidationCheck, + ValidationResult, + ) + + if not self._interval_samples: + self._bad = "native contact substeps were not sampled" + if self.env_id not in self.results or self._bad: + self.results[self.env_id] = SimpleNamespace( + status="failed" if self._bad else "passed", + reason=self._bad or "all measured native contacts are permitted", + ) + if self._bad: + atomic_json(self.output / "collision-failure.json", {"reason": self._bad}) + self.monitor.observe(self.env, action) + terminal = self.monitor.validation(self.env) + if self.hold: + data = self.sensor.get_data() + pairs = data["user_ids"][self.env_id][ + data["is_valid"][self.env_id] & data["impulse_valid"][self.env_id] + ] + book = int(self.sensor.get_actor_ids(self.hold["entity"])[self.env_id, 0]) + fingers = self.sensor.get_actor_ids(self.env.robot.uid, self.hold["links"])[ + self.env_id + ].tolist() + touching = [ + bool( + ((pairs == book).any(dim=-1) & (pairs == finger).any(dim=-1)).any() + ) + for finger in fingers + ] + pose = ( + self.env.sim.get_rigid_object(self.hold["entity"]) + .get_local_pose()[self.env_id] + .tolist() + ) + self.samples.append((pose, touching)) + report = ( + self._hold_report() + if terminal.accepted + else {"passed": False, "reason": "awaiting terminal hold"} + ) + terminal = ValidationResult( + ( + *terminal.checks, + ValidationCheck( + "task.contact_hold", + "passed" if report["passed"] else "failed", + str(report), + ), + ) + ) + if terminal.accepted: + atomic_json(self.output / "contact-hold.json", report) + self.terminal[self.env_id] = terminal + + def close(self): + if self.hold and self.samples: + obj = self.env.sim.get_rigid_object(self.hold["entity"]) + atomic_json( + self.output / "terminal-diagnostics.json", + { + "pose": obj.get_local_pose()[self.env_id].tolist(), + "body_state": obj.body_state[self.env_id].tolist(), + "predicates": [ + { + "condition": condition, + "passed": self.monitor.predicate(self.env, condition), + } + for condition in self.configuration["final_checks"] + ], + "hold": self._hold_report(), + "events": self.monitor.receipts, + }, + ) + self.active = False + + +class MeasuredExpansionValidator: + """Use one measured acceptance port for geometry or complete native contacts. + + Args: + environment: Caller-owned native Gym environment. + configuration: Task-owned initial_checks/final_checks/events, declared + allowed_contacts/mounting_contacts, and optional contact_hold with + entity, finger links, duration and translation/rotation drift limits. + output: Artifact directory for diagnostic and terminal evidence. + backend: geometry (independent Newton query) or native_contacts (actual + physical substeps including triangle meshes). Unavailable data rejects. + device: Numerical geometry-query device; never changes physics. + + The World, stepping and acceptance thresholds remain with the host/task. + Selected-row preparation leaves live peers' monitors and evidence intact. + """ + + def __init__( + self, + environment: EmbodiedEnv, + configuration: Mapping[str, Any], + output: str | Path, + *, + backend: str = "geometry", + device: str = "cpu", + ) -> None: + if backend not in ("geometry", "native_contacts"): + raise ValueError("backend must be geometry or native_contacts") + from embodichain.lab.gym.envs._json import json_safe_copy + + configuration = json_safe_copy( + configuration, field_name="measured acceptance configuration" + ) + self.env, self.rows, self.sensor = environment, {}, None + self._observer_registered = False + output = Path(output) + output.mkdir(parents=True, exist_ok=True) + self.initial, self.terminal, self.results = {}, {}, {} + self._closed = False + try: + if backend == "native_contacts": + from embodichain.lab.sim.sensors import ( + ContactSensorCfg, + ArticulationContactFilterCfg, + ContactPermissionChecker, + ) + + self.sensor_uid = "expansion_native_contacts" + if self.sensor_uid in environment.sensors: + raise ValueError( + "the measured contact validator already owns a sensor" + ) + self.sensor = environment.sim.add_sensor( + ContactSensorCfg( + uid=self.sensor_uid, + rigid_uid_list=environment.sim.get_rigid_object_uid_list(), + articulation_cfg_list=[ + ArticulationContactFilterCfg( + articulation_uid=environment.robot.uid + ) + ], + filter_need_both_actor=False, + max_contacts_per_env=8192, + ) + ) + environment.sensors[self.sensor_uid] = self.sensor + for row in range(environment.num_envs): + path = output if environment.num_envs == 1 else output / f"row-{row}" + path.mkdir(parents=True, exist_ok=True) + self.rows[row] = ( + _GeometryRow( + environment, configuration, path, device=device, env_id=row + ) + if self.sensor is None + else _ContactRow( + environment, configuration, path, sensor=self.sensor, env_id=row + ) + ) + if self.sensor is not None: + permissions = self.rows[0].allowed + self.checker = ContactPermissionChecker( + self.sensor, + [self.rows[row].frames for row in range(environment.num_envs)], + allowed_contacts=tuple( + ( + tuple(pair) + if len(pair) == 2 + else (next(iter(pair)), next(iter(pair))) + ) + for pair in permissions + ), + moving_frames=tuple(self.rows[0].moving), + ) + self.sensor.add_sampling_observer("expansion", self) + self._observer_registered = True + except BaseException: + self.close() + raise + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Reset only the sensor-selected monitor rows.""" + ids = tuple(self.rows) if env_ids is None else tuple(int(i) for i in env_ids) + for row in ids: + self.rows[row].reset() if self.sensor is not None else None + + def prepare( + self, + snapshot: SceneState | None = None, + *, + env_ids: Sequence[int] | None = None, + ) -> None: + """Capture initial qualification after physical preparation/settling.""" + ids = tuple(self.rows) if env_ids is None else tuple(env_ids) + if ( + not ids + or len(set(ids)) != len(ids) + or any(type(row) is not int or row not in self.rows for row in ids) + ): + raise ValueError("prepare requires unique physical rows") + for row in ids: + check = self.rows[row] + check.prepare(snapshot) + self.initial[row] = check.initial[row] + self.terminal.pop(row, None) + self.results.pop(row, None) + + def before_sample(self): + return None + + def after_sample(self, dt: float, token: object) -> None: + """Inspect every physical sample once, reducing all rows together.""" + active = [check for check in self.rows.values() if check.active] + if not active: + return + summaries = self.checker.check().cpu().tolist() + for check in active: + check.substeps += 1 + check._interval_samples += 1 + code, actor_a, actor_b = summaries[check.env_id] + if code: + check._bad = f"native contact evidence rejected: code={code}; actors={actor_a},{actor_b}" + + def begin_control_step(self) -> None: + """Start a physical interval without discarding prior failure results.""" + for check in self.rows.values(): + if self.sensor is not None and check.active: + check.begin_control_step() + + def sample( + self, + ids: Sequence[int], + action: ControllerAction, + snapshot: SceneState | None = None, + ) -> None: + """Check measured task state and retain each row's first physical failure.""" + for row in ids: + check = self.rows[row] + check.sample((row,), action, snapshot) + self.terminal[row] = check.terminal[row] + self.results[row] = check.results[row] + if self.sensor is not None: + check._interval_samples = 0 + + def close(self) -> None: + """Detach owned observers before releasing resources; safe on failure.""" + if self._closed: + return + self._closed = True + for check in self.rows.values(): + check.close() + if self.sensor is not None: + if self._observer_registered: + self.sensor.remove_sampling_observer("expansion", expected=self) + self.env.sensors.pop(self.sensor_uid, None) + self.env.sim.remove_asset(self.sensor_uid) diff --git a/embodichain/lab/expansion/comparison.py b/embodichain/lab/expansion/comparison.py new file mode 100644 index 0000000000..e0c0540c52 --- /dev/null +++ b/embodichain/lab/expansion/comparison.py @@ -0,0 +1,681 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Measured reference/expansion comparison on a shared physics-free timeline.""" + +from __future__ import annotations + +from contextlib import ExitStack, AbstractContextManager +from collections.abc import Callable +from dataclasses import dataclass +from datetime import datetime, timezone +from pathlib import Path +from types import MappingProxyType +import time +import numpy as np +from embodichain.data_pipeline.recording.media import _atomic_json as atomic_json +from embodichain.lab.expansion.persistence import _digest as file_digest +from embodichain.lab.visualization.markers import MarkerFramePublisher + +__all__ = [ + "MeasuredTrajectory", + "record_reference_trajectory", + "preview_expansion_comparison", +] + +GHOST_COLOR = (0.15, 0.8, 0.95, 0.35) + + +def _utc_now() -> str: + return datetime.now(timezone.utc).isoformat() + + +@dataclass(frozen=True) +class MeasuredTrajectory: + """One arena's measured poses; all frames include the initial boundary.""" + + name: str + robot_uid: str + joint_names: tuple[str, ...] + timestamps: np.ndarray + qpos: np.ndarray + root_poses: np.ndarray + object_poses: dict[str, np.ndarray] + + def __post_init__(self) -> None: + for name in ("timestamps", "qpos", "root_poses"): + object.__setattr__(self, name, np.array(getattr(self, name), copy=True)) + object.__setattr__( + self, + "object_poses", + MappingProxyType( + { + key: np.array(value, copy=True) + for key, value in self.object_poses.items() + } + ), + ) + count = len(self.timestamps) + if ( + count < 2 + or self.timestamps.shape != (count,) + or self.timestamps[0] != 0 + or not np.all(np.diff(self.timestamps) > 0) + or not self.joint_names + or len(set(self.joint_names)) != len(self.joint_names) + or self.qpos.shape != (count, len(self.joint_names)) + ): + raise ValueError("Preview requires a nonempty, ordered measured trajectory") + arrays = ( + self.timestamps, + self.qpos, + self.root_poses, + *self.object_poses.values(), + ) + if any(not np.isfinite(array).all() for array in arrays): + raise ValueError("Preview trajectory must contain finite measurements") + for poses in (self.root_poses, *self.object_poses.values()): + if poses.shape != (count, 7) or not np.allclose( + np.linalg.norm(poses[:, 3:], axis=1), 1, atol=1e-5, rtol=0 + ): + raise ValueError("Preview poses must be xyz + normalized xyzw") + for array in ( + self.timestamps, + self.qpos, + self.root_poses, + *self.object_poses.values(), + ): + array.flags.writeable = False + + def frame_at(self, elapsed: float) -> int: + """Select the latest measured boundary, holding the final pose at the end.""" + return int( + np.clip( + np.searchsorted(self.timestamps, elapsed, side="right") - 1, + 0, + len(self.timestamps) - 1, + ) + ) + + @classmethod + def load_reference(cls, path: Path) -> MeasuredTrajectory: + """Read a pickle-free reference captured by record_reference_trajectory.""" + with np.load(path, allow_pickle=False) as data: + return cls( + name="original", + robot_uid=str(data["robot_uid"]), + joint_names=tuple(data["joint_names"].tolist()), + timestamps=data["timestamps"].copy(), + qpos=data["qpos"].copy(), + root_poses=data["root_poses"].copy(), + object_poses={ + str(uid): data[f"object_{index}"].copy() + for index, uid in enumerate(data["object_uids"]) + }, + ) + + @classmethod + def load_episode(cls, storage_id: str, *, robot_uid: str) -> MeasuredTrajectory: + """Read only an accepted native episode, preserving full joint order.""" + from embodichain.lab.expansion import load_expansion_episode + + episode = load_expansion_episode(storage_id) + if not episode.validation.accepted: + raise ValueError("comparison requires a qualified native episode") + metadata, observations = episode.metadata, episode.observations + robot = next( + item for item in metadata["initial_state"] if item["uid"] == robot_uid + ) + root_key = next( + field["key"] + for field in metadata["scene_state_fields"] + if field["entity_uid"] == robot_uid and field["field"] == "pose" + ) + + def array(value): + return value.detach().cpu().numpy().copy() + + return cls( + name=episode.episode_id, + robot_uid=robot_uid, + joint_names=tuple(robot["joint_names"]), + timestamps=array(episode.timestamps), + qpos=array(observations["full_joint_positions"]), + root_poses=array(observations[root_key]), + object_poses={ + key[len("rigid/") : -len("/state")]: array(value)[:, :7] + for key, value in observations.items() + if key.startswith("rigid/") and key.endswith("/state") + }, + ) + + +def record_reference_trajectory(env: object, path: Path) -> None: + """Execute the unmodified expert once and archive its measured scene poses.""" + from embodichain.lab.gym.envs.demo import execute_demo_episode + from embodichain.lab.sim.scene import SceneView, capture_state + + view = SceneView(env.sim, env_ids=(0,)) + frames = [capture_state(view).batches[0]] + + def observe(*_args): + frames.append(capture_state(view).batches[0]) + + episode = execute_demo_episode(env, step_observer=observe) + atomic_json(path.with_suffix(".json"), episode.to_metadata()) + if not episode.completed or not episode.all_success: + raise RuntimeError(f"Nominal preview expert failed: {episode.terminal_reason}") + robot_index = next( + index for index, item in enumerate(frames[0]) if item.uid == env.robot.uid + ) + robot = frames[0][robot_index] + objects = [ + (index, item.uid) + for index, item in enumerate(frames[0]) + if item.kind == "rigid_object" + ] + arrays = { + "robot_uid": np.asarray(robot.uid), + "joint_names": np.asarray(robot.joint_names), + "timestamps": np.arange(len(frames), dtype=np.float64) * env.step_dt, + "qpos": np.asarray([frame[robot_index].qpos for frame in frames]), + "root_poses": np.asarray([frame[robot_index].pose for frame in frames]), + "object_uids": np.asarray([uid for _, uid in objects]), + } + arrays.update( + { + f"object_{ordinal}": np.asarray([frame[index].pose for frame in frames]) + for ordinal, (index, _) in enumerate(objects) + } + ) + np.savez_compressed(path, **arrays) + # Validate the exact archive before another World is created. + MeasuredTrajectory.load_reference(path) + + +def _object_prototype(obj): + """Use the physical object's actual visual shape and scale exactly once.""" + from embodichain.lab.visualization.markers import MarkerPrototypeCfg + from embodichain.lab.sim.shapes import CubeCfg, MeshCfg, SphereCfg + + shape, scale = obj.cfg.shape, np.asarray(obj.cfg.body_scale) + options = {"color": GHOST_COLOR, "scene_lighting": True, "unlit": False} + if isinstance(shape, CubeCfg): + return MarkerPrototypeCfg( + shape="box", scale=tuple(scale * shape.size), **options + ) + if isinstance(shape, SphereCfg): + return MarkerPrototypeCfg( + shape="sphere", scale=tuple(scale * 2 * shape.radius), **options + ) + if isinstance(shape, MeshCfg) or obj.cfg.source is not None: + # Use the simulator's materialized visual geometry: GLB loading may + # convert axes, while USD sources can author their own mesh transforms. + vertices = obj.get_vertices(env_ids=[0], scale=False)[0].cpu().numpy() + faces = obj.get_triangles(env_ids=[0])[0].cpu().numpy() + return MarkerPrototypeCfg( + shape="mesh", vertices=vertices, faces=faces, scale=tuple(scale), **options + ) + raise ValueError(f"Unsupported preview object shape for {obj.uid}") + + +def _precompute_link_poses(clip, robot, asset_path): + """Compute complete-arm FK once; browsing never runs FK or loads episodes.""" + import torch + import pytorch_kinematics as pk + from scipy.spatial.transform import Rotation + + chain = pk.build_chain_from_urdf(Path(asset_path).read_bytes()) + indices = [clip.joint_names.index(name) for name in robot.joint_names] + poses = [] + with torch.no_grad(): + for start in range(0, len(clip.timestamps), 256): + qpos = torch.as_tensor( + clip.qpos[start : start + 256, indices], dtype=torch.float32 + ) + fk = chain.forward_kinematics(qpos) + poses.append( + torch.stack( + [fk[name].get_matrix() for name in robot.link_names], dim=1 + ).numpy() + ) + roots = np.broadcast_to(np.eye(4), (len(clip.timestamps), 4, 4)).copy() + roots[:, :3, :3] = Rotation.from_quat(clip.root_poses[:, 3:]).as_matrix() + roots[:, :3, 3] = clip.root_poses[:, :3] + return roots[:, None] @ np.concatenate(poses) + + +class _ReferencePlayer: + """Publish measured entities through the native render-state owner.""" + + def __init__(self, env, reference): + from embodichain.lab.sim.scene import SceneView, capture_state + + if tuple(env.robot.joint_names) != reference.joint_names: + raise ValueError("Preview robot joint order differs from recording") + self.view = SceneView(env.sim, env_ids=(0,)) + self.initial = capture_state(self.view) + self.reference = reference + + def queue(self, frame): + from dataclasses import replace + from embodichain.lab.sim.scene import restore_render_state + + reference = self.reference + entities = [] + for state in self.initial.batches[0]: + if state.uid == reference.robot_uid: + state = replace( + state, + pose=tuple(map(float, reference.root_poses[frame])), + qpos=tuple(map(float, reference.qpos[frame])), + ) + elif state.uid in reference.object_poses: + state = replace( + state, + pose=tuple(map(float, reference.object_poses[state.uid][frame])), + ) + entities.append(state) + restore_render_state( + self.view, replace(self.initial, batches=(tuple(entities),)) + ) + + def close(self): + from embodichain.lab.sim.scene import restore_render_state + + restore_render_state(self.view, self.initial) + + +def _accepted_candidates(operators): + from embodichain.lab.expansion import ArtifactEpisodeSink + + return [ + { + "operator": name, + "storage_id": receipt.storage_id, + "episode_id": receipt.episode_id, + } + for name, report in operators.items() + if "directory" in report + for receipt in ArtifactEpisodeSink(report["directory"]).inspect() + ] + + +def preview_expansion_comparison( + environment_factory: Callable[[], AbstractContextManager], + reference_path: Path, + operators: dict[str, dict] | None, + *, + renderer: str, + scene_entities: tuple[str, ...], + output: Path, + collect: Callable | None = None, + max_frames: int | None = None, + frame_observer: Callable | None = None, + expected_trajectories: int | None = None, +) -> dict: + """Browse accepted trajectories; N advances, Space pauses and Q closes. + + With a collection callback, warm model resources before opening the window, + then delegate collection and load only its qualified measured trajectories. + Only the selected expansion is visible; N preserves playback time and + pause state. Rendering cannot alter episodes or advance physics. + + Args: + environment_factory: Context manager factory for the caller's native + environment. The viewer displays physical row zero; other rows are + retained with World-wide physics disabled during playback. + reference_path: Pickle-free measured reference produced by the recorder. + operators: Named artifact directories containing accepted native receipts. + renderer: Renderer label in viewer evidence; the factory configures it. + scene_entities: Recorded objects to display with the selected robot ghost. + output: Viewer manifest and interaction evidence directory. + collect: Optional callback receiving simulation, window, output and status; + returns named artifact directories. Collection ownership stays external. + max_frames: Optional display-frame budget for bounded playback. + frame_observer: Optional callback receiving environment, frame number, + selected index and shared elapsed time after publication. + expected_trajectories: Required preload count for early-window collection. + + Returns: + Playback outcome, selection/frame counts and actual physics-step delta. + """ + from collections import deque + from dexsim.engine import InputControl + from dexsim.types import InputKey + from embodichain.lab.visualization.markers import ( + RobotMarkers, + RobotMarkersAppearanceCfg, + RobotMarkersCfg, + VisualizationMarkers, + VisualizationMarkersCfg, + ) + import os + + output = Path(output) + output.mkdir(parents=True, exist_ok=True) + if max_frames is not None and (type(max_frames) is not int or max_frames < 1): + raise ValueError("max_frames must be a positive integer") + if collect is not None and ( + type(expected_trajectories) is not int or expected_trajectories < 1 + ): + raise ValueError("Early preview requires the expected trajectory count") + with environment_factory() as env: + sim = env.sim + physics_enabled = sim.physics_enabled + sim.enable_physics(False) + physics_steps_before = sim.physics_step_index + with ExitStack() as cleanup: + cleanup.callback(sim.enable_physics, physics_enabled) + players = cleanup.enter_context(ExitStack()) + markers = MarkerFramePublisher(sim, xray=True) + cleanup.callback(markers.close) + robot = RobotMarkers( + RobotMarkersCfg( + name="expansion_robot", + asset_path=env.robot.cfg.fpath, + coordinate_space="env", + appearance=RobotMarkersAppearanceCfg( + color=GHOST_COLOR[:3], opacity=GHOST_COLOR[3] + ), + ), + markers, + ) + objects = VisualizationMarkers( + VisualizationMarkersCfg( + prim_path="expansion_objects", + coordinate_space="env", + markers={ + uid: _object_prototype(sim.get_rigid_object(uid)) + for uid in scene_entities + }, + ), + markers, + ) + + def open_window(): + if not sim.open_window(): + raise RuntimeError( + "The requested native expansion preview did not open" + ) + cleanup.callback(sim.close_window) + window = sim.get_world().get_windows() + center = np.mean( + [env.robot.get_local_pose()[0, :3].detach().cpu().numpy()] + + [ + sim.get_rigid_object(uid) + .get_local_pose()[0, :3] + .detach() + .cpu() + .numpy() + for uid in scene_entities + ], + axis=0, + ) + center[2] += 0.2 + window.set_look_at( + center + [1.6, -1.8, 1.1], center, np.array([0.0, 0.0, 1.0]) + ) + return window + + window = None + window_opened_at = None + if collect is not None: + # Allocate all native mesh instances before the render loop. + # Their hidden placeholder poses require no recorded data/FK. + robot.visualize_link_poses( + np.broadcast_to( + np.eye(4), + (1, expected_trajectories, len(robot.link_names), 4, 4), + ), + env_ids=(0,), + ) + count = expected_trajectories * len(scene_entities) + objects.visualize( + translations=np.zeros((count, 3)), + orientations=np.tile([0.0, 0.0, 0.0, 1.0], (count, 1)), + marker_indices=np.tile( + np.arange(len(scene_entities)), expected_trajectories + ), + ) + for name, width in ( + ("expansion_robot", robot.num_visuals), + ("expansion_objects", len(scene_entities)), + ): + markers.groups[name].update( + visible=np.zeros(expected_trajectories * width, dtype=bool), + env_ids=(0,), + ) + markers.publish() + sim.sync_render_state() + window = open_window() + window_opened_at = _utc_now() + status = { + "state": "collecting", + "renderer": renderer, + "pid": os.getpid(), + "window_opened_at": window_opened_at, + "window_open": True, + "expected_trajectories": expected_trajectories, + "preloaded_robots": expected_trajectories, + } + atomic_json(output / "window.json", status) + window.set_window_title("Expansion preparing | reference | Q cancel") + operators = collect(sim, window, output, status) + atomic_json(output / "status.json", {**status, "state": "loading"}) + + reference = MeasuredTrajectory.load_reference(reference_path) + candidates = _accepted_candidates(operators) + atomic_json( + output / "manifest.json", + { + "reference": str(reference_path), + "reference_sha256": file_digest(reference_path), + "candidates": candidates, + }, + ) + if not candidates: + result = { + "state": "empty", + "trajectories": 0, + "reason": "no_accepted_episodes", + } + atomic_json(output / "status.json", result) + return result + clips = [ + MeasuredTrajectory.load_episode( + candidate["storage_id"], robot_uid=reference.robot_uid + ) + for candidate in candidates + ] + if any( + set(clip.joint_names) != set(reference.joint_names) for clip in clips + ): + raise ValueError("Preview expansion has incompatible robot joints") + duration = max(clip.timestamps[-1] for clip in [reference, *clips]) + 0.5 + player = _ReferencePlayer(env, reference) + players.callback(player.close) + selected = 0 + links = [ + _precompute_link_poses(clip, robot, env.robot.cfg.fpath) + for clip in clips + ] + last_frame = None + + def show_frame(elapsed): + nonlocal last_frame + original_frame = reference.frame_at(elapsed) + expanded_frames = [clip.frame_at(elapsed) for clip in clips] + current = (original_frame, *expanded_frames, selected) + if current == last_frame: + sim.get_world().update(0.0) + return original_frame, expanded_frames + player.queue(original_frame) + robot.visualize_link_poses( + np.stack( + [poses[frame] for poses, frame in zip(links, expanded_frames)] + )[None], + env_ids=(0,), + ) + poses = np.stack( + [ + clip.object_poses[uid][frame] + for clip, frame in zip(clips, expanded_frames) + for uid in scene_entities + ] + ) + objects.visualize( + translations=poses[:, :3], + orientations=poses[:, 3:], + marker_indices=np.tile(np.arange(len(scene_entities)), len(clips)), + ) + for name, width in ( + ("expansion_robot", robot.num_visuals), + ("expansion_objects", len(scene_entities)), + ): + markers.groups[name].update( + visible=np.repeat(np.arange(len(clips)) == selected, width), + env_ids=(0,), + ) + markers.publish() + # Publish once, after full-arm, object and reference state writes. + sim.sync_render_state() + # Publication only flushes render uploads in the pinned SDK; + # poll window input and present even when every frame changes. + sim.get_world().update(0.0) + last_frame = current + return original_frame, expanded_frames + + # Early windows reuse the warmed meshes; recorded-only previews + # create their first instances before starting the render loop. + show_frame(0) + if window is None: + window = open_window() + window_opened_at = _utc_now() + switches, frames, paused = 0, 0, False + elapsed, previous = 0.0, time.monotonic() + running = True + pressed, held, polled = deque(), set(), set() + preview_keys = { + key.value + for key in ( + InputKey.SCANCODE_Q, + InputKey.SCANCODE_ESCAPE, + InputKey.SCANCODE_N, + InputKey.SCANCODE_SPACE, + ) + } + + class PreviewInput(InputControl): + def on_key_down(self, key): + if key in preview_keys and key not in held: + held.add(key) + pressed.append(key) + + def on_key_up(self, key): + held.discard(key) + + control = PreviewInput() + window.add_input_control(control) + cleanup.callback(window.remove_input_control, control) + result = { + "state": "playing", + "trajectories": len(candidates), + "renderer": renderer, + "robot_layers": 1, + "object_instances": len(scene_entities), + "preloaded_robots": len(clips), + "preloaded_object_instances": len(clips) * len(scene_entities), + "marker_render_mode": "overlay", + "pid": os.getpid(), + "world_object_id": id(sim.get_world()), + "num_envs": sim.num_envs, + "window_opened_at": window_opened_at, + } + + def publish(): + result.update( + selected_index=selected, + operator=candidates[selected]["operator"], + episode_id=candidates[selected]["episode_id"], + switches=switches, + frames=frames, + state="paused" if paused else "playing", + original_frame=original_frame, + expanded_frame=expanded_frames[selected], + elapsed_s=elapsed, + expanded_frames=expanded_frames, + marker_publications=markers.publications, + solid_qpos=env.robot.get_qpos()[0].detach().cpu().tolist(), + ) + atomic_json(output / "status.json", result) + window.set_window_title( + f"Expansion {selected + 1}/{len(candidates)} | {candidates[selected]['operator']} | N next | Space pause | Q close" + ) + + while ( + running + and sim.get_world().is_window_initialized() + and (max_frames is None or frames < max_frames) + ): + started = time.monotonic() + changed = frames == 0 + # Queue native key edges so a short tap survives a slow render + # frame. A newly opened viewport may not dispatch control + # callbacks before its first click, so retain held-key edges too. + down = {key for key in preview_keys if sim.get_world().key_state(key)} + for key in down - polled: + if key not in held: + held.add(key) + pressed.append(key) + held.difference_update(polled - down) + polled = down + while pressed: + key = pressed.popleft() + if key in ( + InputKey.SCANCODE_Q.value, + InputKey.SCANCODE_ESCAPE.value, + ): + running = False + elif key == InputKey.SCANCODE_N.value: + selected = (selected + 1) % len(candidates) + switches += 1 + changed = True + elif key == InputKey.SCANCODE_SPACE.value: + paused, changed = not paused, True + if not running: + break + if not paused: + elapsed += started - previous + previous = started + elapsed %= duration + original_frame, expanded_frames = show_frame(elapsed) + frames += 1 + if frame_observer is not None: + frame_observer(env, frames, selected, elapsed) + if not sim.get_world().is_window_initialized(): + break + if changed: + publish() + time.sleep(max(0.0, 1 / 30 - (time.monotonic() - started))) + result.update( + state="closed", + frames=frames, + switches=switches, + physics_steps=sim.physics_step_index - physics_steps_before, + ) + atomic_json(output / "status.json", result) + return result diff --git a/embodichain/lab/expansion/host.py b/embodichain/lab/expansion/host.py index 7c705325c7..2bb2bcfd5b 100644 --- a/embodichain/lab/expansion/host.py +++ b/embodichain/lab/expansion/host.py @@ -48,6 +48,7 @@ from ._gym_recipe import GymRecipeBuilder from ._gym_prepare import GymCohortPreparer from ._gym_execution import GymCohortExecutor +from .host_extensions import GymExpansionHostExtensions __all__ = ["GymExpansionHost"] @@ -93,12 +94,17 @@ def __init__( root: Path, journal: ExpansionJournal, source_artifacts: Mapping[str, object], + *, + extensions: GymExpansionHostExtensions | None = None, ) -> None: self._context = GymHostContext( cfg, report, root, journal, source_artifacts, cfg.batch_size ) + self._context.extensions = extensions or GymExpansionHostExtensions() + if not isinstance(self._context.extensions, GymExpansionHostExtensions): + raise TypeError("extensions must be GymExpansionHostExtensions") self._recipe_builder = GymRecipeBuilder(self._context) - self._context.recipe = self._recipe_builder.recipe + self._context.recipe = self.recipe self._preparer = GymCohortPreparer(self._context) self._executor = GymCohortExecutor(self._context) try: @@ -123,6 +129,9 @@ def _open(self) -> None: environment_cfg.record_trajectory = False environment_cfg.sim_cfg.headless = True _configure_sensor_capture(environment_cfg, capture_media=cfg.capture_media) + extensions = self._context.extensions + if extensions.collection_renderer is not None: + environment_cfg.sim_cfg.render_cfg.renderer = extensions.collection_renderer from embodichain.lab.sim.profiler import ProfilerCfg environment_cfg.profiler = ProfilerCfg( @@ -170,6 +179,17 @@ def _open(self) -> None: env, cfg.collision_profile, root / "collision" ) self._context.collision = collision + if extensions.validator_factory is not None: + if collision is not None: + raise ValueError("choose a collision profile or an extension validator") + self._context.collision = collision = extensions.validator_factory( + env, root + ) + if collision is None or not all( + callable(getattr(collision, method, None)) + for method in ("prepare", "sample", "close") + ): + raise TypeError("validator_factory must return a measured validator") profile = None if cfg.source == "real2sim": from embodichain.lab.task_program.integrations.simulation.real2sim_collection import ( @@ -208,6 +228,12 @@ def _open(self) -> None: self._context.collision = collision self._context.profile = profile self._context.scene_source = scene_source + if extensions.scene_source_factory is not None: + if scene_source is not None: + raise ValueError("choose a scene atlas or an extension scene source") + self._context.scene_source = extensions.scene_source_factory(env) + if not callable(getattr(self._context.scene_source, "candidate", None)): + raise TypeError("scene_source_factory must return a candidate source") self._context.motion_profile = motion_profile self._context.motion_recipes = motion_recipes self._context.names = names @@ -448,7 +474,9 @@ def profile(self, value: Any) -> None: def recipe(self, ordinal: int) -> ExpansionRecipe: """Sample a pinned recipe without mutating the World.""" - return self._recipe_builder.recipe(ordinal) + recipe = self._recipe_builder.recipe(ordinal) + transform = self._context.extensions.recipe_transform + return recipe if transform is None else transform(recipe) def prepare_batch( self, requests: tuple[RecipeAttempt, ...], session: ExpansionSession diff --git a/embodichain/lab/expansion/host_extensions.py b/embodichain/lab/expansion/host_extensions.py new file mode 100644 index 0000000000..5f4e54a0c4 --- /dev/null +++ b/embodichain/lab/expansion/host_extensions.py @@ -0,0 +1,112 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Non-owning public extension ports for the native Gym expansion host.""" + +from __future__ import annotations + +from dataclasses import dataclass +from collections.abc import Callable, Mapping +from pathlib import Path +from typing import Any, Protocol +from .contracts import CandidateIdentity, SceneCase, ValidationResult +from .recipes import ExpansionRecipe + +__all__ = ["ExpansionValidator", "ExpansionRowContext", "GymExpansionHostExtensions"] + + +class ExpansionValidator(Protocol): + """Measured validation port shared by geometry and native-contact checks. + + Unavailable evidence must reject. Implementations never step/reset the World; + the host calls prepare after settling and sample on every measured boundary. + """ + + initial: Mapping[int, bool] + terminal: Mapping[int, ValidationResult] + results: Mapping[int, Any] + + def prepare(self, snapshot: Any = None) -> None: + """Reset measured evidence after the host has settled its cohort.""" + ... + + def sample(self, ids: tuple[int, ...], action: Any, snapshot: Any = None) -> None: + """Observe the original control boundary and its physical substeps.""" + ... + + def close(self) -> None: + """Release only owned observers/resources; the host retains its World.""" + ... + + +@dataclass(frozen=True) +class ExpansionRowContext: + """Immutable identity supplied to a per-row plan-transform factory.""" + + env_id: int + identity: CandidateIdentity + ordinal: int + attempt_id: int + directory: Path + scene_case: SceneCase + recipe: ExpansionRecipe + + +@dataclass(frozen=True) +class GymExpansionHostExtensions: + """Runtime callbacks; construction, collection and persistence stay native. + + Args: + collection_renderer: Explicit headless collection renderer, or None for + the native sensor/media default. + scene_source_factory: Receives the live environment and returns a source + exposing candidate(ordinal); it cannot own another World. + validator_factory: Receives environment and artifact root; returns the + same measured port for either physical geometry or native contacts. + recipe_transform: Receives the native immutable recipe and returns its + task-owned operations. Preparation verifies it again before mutation. + plan_transform_factory: Receives environment and immutable row identity; + returns an ordinary Task Program transform factory. Its record metadata + and hashed plan attachments are archived by the native episode owner. + require_plan_transform: Reject completed programs without transform records. + """ + + collection_renderer: str | None = None + scene_source_factory: Callable[[Any], Any] | None = None + validator_factory: Callable[[Any, Path], ExpansionValidator] | None = None + recipe_transform: Callable[[ExpansionRecipe], ExpansionRecipe] | None = None + plan_transform_factory: Callable[[Any, ExpansionRowContext], Any] | None = None + require_plan_transform: bool = False + + def __post_init__(self) -> None: + if self.collection_renderer is not None and ( + type(self.collection_renderer) is not str + or not self.collection_renderer.strip() + ): + raise ValueError("collection_renderer must be a nonempty renderer name") + for name in ( + "scene_source_factory", + "validator_factory", + "recipe_transform", + "plan_transform_factory", + ): + value = getattr(self, name) + if value is not None and not callable(value): + raise TypeError(f"{name} must be callable") + if type(self.require_plan_transform) is not bool or ( + self.require_plan_transform and self.plan_transform_factory is None + ): + raise ValueError("required plan transforms need an explicit factory") diff --git a/embodichain/lab/sim/atomic_skills/policies.py b/embodichain/lab/sim/atomic_skills/policies.py index 22fb610e65..1a8170108f 100644 --- a/embodichain/lab/sim/atomic_skills/policies.py +++ b/embodichain/lab/sim/atomic_skills/policies.py @@ -19,7 +19,7 @@ from __future__ import annotations import math -from collections.abc import Callable +from collections.abc import Callable, Mapping from copy import deepcopy from dataclasses import dataclass, field, fields, is_dataclass from enum import Enum @@ -63,6 +63,15 @@ class MotionPolicy: strategy: Literal["motion_gen", "ik_interp"] = "ik_interp" """Motion strategy: ``motion_gen`` or ``ik_interp``.""" + phase_paths: Mapping[str, Literal["joint", "cartesian"]] = field( + default_factory=dict, kw_only=True + ) + """Explicit primitive phase paths; empty preserves each skill's defaults. + + PickUp reads pre_grasp/grasp/lift (and optional initial_lift); Place reads + pre_place/place/retreat. Other primitives retain their own declared paths. + """ + sample_count: int = 50 """Minimum trajectory sample count when the backend does not preserve samples.""" @@ -143,6 +152,22 @@ class MotionPolicy: """ def __post_init__(self) -> None: + if not isinstance(self.phase_paths, Mapping) or any( + type(name) is not str + or not name.strip() + or mode not in ("joint", "cartesian") + for name, mode in self.phase_paths.items() + ): + raise ValueError("phase_paths must map phase names to joint/cartesian") + if self.phase_paths and ( + self.strategy != "ik_interp" + or self.interpolation_mode != "fixed_grid" + or self.ik_seed_mode != "sequential" + ): + raise ValueError("phase_paths requires fixed-grid sequential ik_interp") + from embodichain.utils._owned_json import freeze_metadata + + object.__setattr__(self, "phase_paths", freeze_metadata(self.phase_paths)) if self.final_velocity_rejection not in {"hold", "mask_only"}: raise ValueError("final_velocity_rejection must be hold or mask_only.") segment_retries = self.segment_sample_count_retries diff --git a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py index d582f6acda..c4cacf8801 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py @@ -404,6 +404,7 @@ def _get_full_pickup_trajectory( # planner options were supplied. motion_options.sample_count = None waypoints = [pre_grasp_xpos, grasp_xpos, lift_xpos] + phase_names = ["pre_grasp", "grasp", "lift"] if options.initial_lift_height > 0.0: start_xpos = self.robot.compute_fk( qpos=start_arm_qpos, @@ -420,6 +421,12 @@ def _get_full_pickup_trajectory( start_xpos.new_tensor([0.0, 0.0, options.initial_lift_height]), ) waypoints.insert(0, initial_lift) + phase_names.insert(0, "initial_lift") + + if any(name in motion_policy.phase_paths for name in phase_names): + motion_options.waypoint_modes = tuple( + motion_policy.phase_paths.get(name, "joint") for name in phase_names + ) motion_result = self.motion_generator.generate( build_pose_plan_states(torch.stack(waypoints, dim=1)), diff --git a/embodichain/lab/sim/atomic_skills/primitives/place.py b/embodichain/lab/sim/atomic_skills/primitives/place.py index 8780f9e473..5e86696a95 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/place.py +++ b/embodichain/lab/sim/atomic_skills/primitives/place.py @@ -307,6 +307,13 @@ def _plan( # motion generator resolves backend defaults when no explicit # planner options were supplied. motion_options.sample_count = None + paths = request.motion_policy.phase_paths + if any(name in paths for name in ("pre_place", "place", "retreat")): + motion_options.waypoint_modes = ( + paths.get("pre_place", "joint"), + *(paths.get("place", "joint"),) * (down_xpos.shape[1] - 1), + *(paths.get("retreat", "joint"),) * back_xpos.shape[1], + ) motion_result = self.motion_generator.generate( build_pose_plan_states(torch.cat([down_xpos, back_xpos], dim=1)), options=motion_options, diff --git a/embodichain/lab/sim/motion/_phase_paths.py b/embodichain/lab/sim/motion/_phase_paths.py new file mode 100644 index 0000000000..d675fcb710 --- /dev/null +++ b/embodichain/lab/sim/motion/_phase_paths.py @@ -0,0 +1,110 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Explicit waypoint phase paths composed through the ordinary motion facade.""" + +from __future__ import annotations + +from dataclasses import replace +from itertools import groupby +from typing import TYPE_CHECKING +import torch +from .planners.utils import MoveType, PlanResult, RejectedMotionCandidate + +if TYPE_CHECKING: + from .motion_generator import MotionGenerator, MotionGenOptions + from .planners.utils import PlanState + +__all__: list[str] = [] + + +def generate_phase_paths( + generator: MotionGenerator, + targets: list[PlanState], + options: MotionGenOptions, + *, + defer_recovery: bool, +) -> PlanResult: + """Retain one declared grid while grouping consecutive path modes.""" + modes = options.waypoint_modes + if len(modes) != len(targets) or not all( + t.move_type is MoveType.EEF_MOVE for t in targets + ): + raise ValueError( + f"waypoint_modes must describe every Cartesian waypoint: modes={modes}, target_types={[state.move_type for state in targets]}" + ) + if ( + options.start_qpos is None + or options.sample_count is None + or options.sample_count <= len(targets) + ): + raise ValueError( + "phase paths require a start and at least one interval per waypoint" + ) + if type(defer_recovery) is not bool or ( + defer_recovery and "cartesian" not in modes + ): + raise ValueError("deferred phase recovery requires a Cartesian phase") + pieces, start, offset = [], options.start_qpos, 0 + for mode, group in groupby(modes): + stop = offset + len(tuple(group)) + intervals = (options.sample_count - 1) * stop // len(targets) - ( + options.sample_count - 1 + ) * offset // len(targets) + result = generator.generate( + targets[offset:stop], + options=replace( + options, + waypoint_modes=(), + start_qpos=start, + sample_count=intervals + 1, + is_linear=mode == "cartesian", + ), + defer_recovery=defer_recovery and mode == "cartesian", + ) + if result.positions is None or result.dt is None: + raise ValueError("phase path returned no timed trajectory") + pieces.append(result) + start = result.positions[:, -1] + offset = stop + success = torch.stack([piece.success for piece in pieces]).all(dim=0) + positions = torch.cat( + [p.positions if i == 0 else p.positions[:, 1:] for i, p in enumerate(pieces)], + dim=1, + ) + dt = torch.cat( + [p.dt if i == 0 else p.dt[:, 1:] for i, p in enumerate(pieces)], dim=1 + ) + rejected = None + if any(p.rejected_candidate is not None for p in pieces): + mask = torch.zeros_like(success) + candidates = [] + for i, p in enumerate(pieces): + points = p.positions + if p.rejected_candidate is not None: + mask |= p.rejected_candidate.env_mask + points = p.rejected_candidate.positions + candidates.append(points if i == 0 else points[:, 1:]) + rejected = RejectedMotionCandidate(torch.cat(candidates, dim=1), dt, mask) + return PlanResult( + success=success, + positions=torch.where( + success[:, None, None], positions, options.start_qpos[:, None] + ), + dt=dt, + rejected_candidate=rejected, + recovery=pieces[-1].recovery, + ) diff --git a/embodichain/lab/sim/motion/motion_generator.py b/embodichain/lab/sim/motion/motion_generator.py index e4c0b7d87c..0da837274a 100644 --- a/embodichain/lab/sim/motion/motion_generator.py +++ b/embodichain/lab/sim/motion/motion_generator.py @@ -207,8 +207,32 @@ class MotionGenOptions: retains its own Torch-only fork; no outer action counter is reused. """ + waypoint_modes: tuple[Literal["joint", "cartesian"], ...] = field( + default=(), kw_only=True + ) + """Explicit path mode from the preceding pose to each supplied waypoint. + + Supported for fixed-grid sequential ``ik_interp``. Empty retains ordinary + planning. Consecutive equal modes share a native request; the total sample + count and interval remain unchanged. No robot or target-name inference occurs. + """ + def __post_init__(self) -> None: """Validate backend-neutral motion generation options.""" + if type(self.waypoint_modes) is not tuple or any( + mode not in ("joint", "cartesian") for mode in self.waypoint_modes + ): + raise ValueError("waypoint_modes must be a tuple of joint/cartesian modes") + if self.waypoint_modes and ( + self.strategy != "ik_interp" + or self.interpolation_mode != "fixed_grid" + or self.ik_seed_mode != "sequential" + or self.is_linear + or self.preserve_cartesian_samples + ): + raise ValueError( + "waypoint_modes requires unsampled fixed-grid sequential ik_interp" + ) segment_retries = self.segment_sample_count_retries if ( type(segment_retries) is not tuple @@ -773,6 +797,12 @@ def generate( options = MotionGenOptions() if options is None else options # Options are mutable config objects; revalidate at the call boundary. options.__post_init__() + if options.waypoint_modes: + from ._phase_paths import generate_phase_paths + + return generate_phase_paths( + self, target_states, options, defer_recovery=defer_recovery + ) if type(defer_recovery) is not bool: raise TypeError("defer_recovery must be a boolean.") linear_keyframes = ( diff --git a/embodichain/lab/task_program/integrations/_native_plan_expansion.py b/embodichain/lab/task_program/integrations/_native_plan_expansion.py new file mode 100644 index 0000000000..66e3a72e27 --- /dev/null +++ b/embodichain/lab/task_program/integrations/_native_plan_expansion.py @@ -0,0 +1,455 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Native measured-plan branch of the shared same-grid expansion factory.""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +import hashlib +from pathlib import Path +from typing import Any + +__all__: list[str] = [] + + +@dataclass(frozen=True) +class TaskProgramNativeExpansionRecord: + """Owned measured-plan expansion provenance and hashed numeric attachment.""" + + env_id: int + payload: dict[str, Any] + + def __post_init__(self) -> None: + from embodichain.utils._owned_json import freeze_metadata + + object.__setattr__(self, "payload", freeze_metadata(self.payload)) + + def to_metadata(self) -> dict[str, Any]: + from embodichain.lab.gym.envs._json import json_safe_copy + + return { + "env_id": self.env_id, + **json_safe_copy(self.payload, field_name="expansion record"), + } + + +class _NativePlanExpansion: + """Select native grasp branches or adapt explicitly editable free phases.""" + + def __init__( + self, + operator: str, + options: dict, + *, + seed: int, + scene_case, + revision: str, + output: Path | None = None, + episode_id: int = 0, + ): + if operator not in ( + "object_pose", + "affordance", + "joint_residual", + "via_points", + "nullspace_residual", + ): + raise ValueError("unsupported native-plan expansion operator") + self.operator = operator + from embodichain.lab.gym.envs._json import json_safe_copy + + self.options = json_safe_copy(options, field_name="same-grid options") + self.seed = seed + self.scene_case, self.revision = scene_case, revision + self.output = output + self.episode_id = episode_id + self._records: list[TaskProgramNativeExpansionRecord] = [] + + @property + def records(self) -> tuple[TaskProgramNativeExpansionRecord, ...]: + return tuple(self._records) + + def create_plan_transform(self, request, *, engine): + if self.operator == "object_pose": + return None + if self.operator == "affordance": + if ( + request.analysis_call_index + not in self.options["affordance"]["call_indices"] + ): + return None + + def record(resolved, context, plan): + return self._record_affordance(request, context, plan) + + return record + + calls = self.options.get("call_indices") + if calls is not None and request.analysis_call_index not in calls: + return None + + def transform(resolved, context, plan): + return self._transform(request, engine, resolved, context, plan) + + return transform + + def prepare_planning_context(self, request, context, *, engine): + """Select a non-nominal native Affordance branch before planning.""" + if self.operator != "affordance": + return context + settings = self.options["affordance"] + if request.analysis_call_index not in settings["call_indices"]: + return context + if context.batch_size != 1: + raise ValueError("Native affordance expansion requires batch_size=1") + from embodichain.lab.sim.atomic_actions.affordance_sampling import ( + AffordanceSamplingContext, + ) + + return replace( + context, + affordance_sampling=AffordanceSamplingContext( + count=settings["branches"], + seed=self.seed, + episode_id=self.episode_id, + branch_override=1 + self.episode_id % (settings["branches"] - 1), + ), + ) + + def _record_affordance(self, call, context, plan): + """Require distinct native grasp geometry, retaining physical validation.""" + if not bool(plan.plan_success.all()): + return plan + samples = plan.diagnostics.metadata.get("affordance_sample", {}) + sampling = context.affordance_sampling + branch = sampling.branch_override + distinct = any( + sample.get("success") == [True] + and sample.get("unique_candidate_counts", [0])[0] > branch + and sample.get("reused") == [False] + for sample in samples.values() + ) + if not distinct: + raise ValueError("affordance produced no distinct feasible grasp branch") + evidence = { + "operator": "affordance", + "skill": plan.skill_id, + "workflow_call_index": call.analysis_call_index, + "sampling": sampling.metadata(), + "samples": samples, + "mode": "native_affordance_replanning", + } + if self.output is not None: + import numpy as np + from embodichain.lab.expansion.persistence import _digest as file_digest + + self.output.mkdir(parents=True, exist_ok=True) + path = ( + self.output + / f"call-{call.analysis_call_index}-{len(self._records)}.npz" + ) + with path.open("wb") as stream: + np.savez_compressed( + stream, + candidate=plan.joint_trajectory.positions[0].detach().cpu().numpy(), + dt=plan.joint_trajectory.dt[0].detach().cpu().numpy(), + ) + evidence["plan_artifact"] = {"path": str(path), "sha256": file_digest(path)} + metadata = dict(plan.diagnostics.metadata, expansion=evidence) + self._records.append( + TaskProgramNativeExpansionRecord(int(context.env_ids[0]), evidence) + ) + return replace(plan, diagnostics=replace(plan.diagnostics, metadata=metadata)) + + def _transform(self, call, engine, request, context, plan): + import torch + from embodichain.lab.sim.atomic_actions.plans import TimedTrajectory + from embodichain.lab.sim.atomic_actions.trajectory_adapter import ( + ActionPlanTemplateAdapter, + ) + from embodichain.lab.sim.motion.expansion import SourceContext + from embodichain.lab.sim.motion.expansion.operators import ( + ProposalRejected, + joint_residual, + nullspace_residual, + validate_motion_samples, + via_points, + ) + + free = set(self.options["free_phases"].get(plan.skill_id, ())) + if ( + not free + or plan.joint_trajectory is None + or not bool(plan.plan_success.all()) + ): + return plan + if context.batch_size != 1: + raise ValueError("Native same-grid transforms require batch_size=1") + endpoints = tuple( + e for e in request.binding.endpoints if e.endpoint_id == "motion" + ) + motion_ids = tuple(dict.fromkeys(i for e in endpoints for i in e.joint_ids)) + permissions = { + segment.name: (self.operator,) if segment.name in free else () + for segment in plan.segments + } + adapter = ActionPlanTemplateAdapter( + tuple(engine.robot.joint_names), + permissions, + { + name: "free" if value else "contact" + for name, value in permissions.items() + }, + controlled_joint_indices=motion_ids, + ) + source = SourceContext( + self.scene_case.task_id, + self.revision, + f"{call.workflow_id}:{call.analysis_call_index}", + self.scene_case, + context.require_control_dt(), + ) + template = adapter.export_template(plan, context=source) + reference_phases = template.phases + # Keep the first/last two samples, including discrete derivative stencils. + phases = tuple( + ( + replace(p, start_index=p.start_index + 1, stop_index=p.stop_index - 1) + if p.allowed_operators and p.stop_index - p.start_index >= 5 + else replace(p, allowed_operators=()) + ) + for p in template.phases + ) + if not any(p.allowed_operators for p in phases): + return plan + template = replace(template, phases=phases) + limits = engine.robot.get_qpos_limits().detach().cpu() + if limits.ndim == 3: + limits = limits[int(context.env_ids[0])] + active = list(template.controlled_joint_indices) + velocity = engine.robot.get_qvel_limits().detach().cpu() + if velocity.ndim == 2: + velocity = velocity[int(context.env_ids[0])] + velocity = velocity[active].clamp(max=self.options["velocity_limit"]) + acceleration = torch.full_like(velocity, self.options["acceleration_limit"]) + seed = int.from_bytes( + hashlib.sha256( + f"{self.seed}/{source.unit_id}/{self.operator}".encode() + ).digest()[:8], + "little", + ) % (2**63) + generator = torch.Generator().manual_seed(seed) + jacobians, kinematics = self._kinematics(engine.robot, endpoints, template) + + def motion_checks(trajectory): + checks = [] + for phase in phases: + if not phase.allowed_operators: + continue + # Operators keep their endpoints exact. The narrowed phase + # leaves two original samples at each boundary, covering every + # second difference affected by an edit. + start, stop = max(0, phase.start_index - 1), min( + len(trajectory.dt), phase.stop_index + 1 + ) + intervals = trajectory.dt[start:stop].clone() + intervals[0] = 0 + checks.append( + validate_motion_samples( + trajectory.positions[start:stop, active], + intervals, + velocity_limits=velocity, + acceleration_limits=acceleration, + ) + ) + return checks + + reference_checks = motion_checks(template) + if any(not check.accepted for check in reference_checks): + raise ValueError( + f"Reference free phases already exceed sampled motion bounds: " + f"{reference_checks}" + ) + reason = "no distinct candidate" + for attempt in range(self.options["max_proposals"]): + arguments = dict( + joint_limits=limits, + normalized_scale=self.options["joint_offset_scale"], + generator=generator, + ) + try: + if self.operator == "joint_residual": + candidate = joint_residual(template, **arguments) + elif self.operator == "via_points": + candidate = via_points( + template, via_count=self.options["via_count"], **arguments + ) + elif self.operator == "nullspace_residual": + candidate = nullspace_residual( + template, task_jacobians=jacobians, **arguments + ) + else: + raise ValueError(f"Unsupported operator: {self.operator}") + except ProposalRejected as exc: + reason = str(exc) + continue + delta = float((candidate.positions - template.positions).abs().max()) + if delta < self.options["minimum_joint_delta"]: + continue + checks = motion_checks(candidate) + if any(not check.accepted for check in checks): + reason = f"edited-phase motion limits: {checks}" + continue + fk = self._check_fk(template, candidate, kinematics) + if self.operator == "nullspace_residual" and ( + fk["max_translation_m"] + > self.options["nullspace_translation_tolerance_m"] + or fk["max_rotation_rad"] + > self.options["nullspace_rotation_tolerance_rad"] + ): + reason = f"nullspace FK error: {fk}" + continue + positions = adapter.restore_positions(plan, candidate.positions).unsqueeze( + 0 + ) + metadata = dict(plan.diagnostics.metadata) + evidence = { + "operator": self.operator, + "skill": plan.skill_id, + "workflow_call_index": call.analysis_call_index, + "phases": [p.phase_id for p in phases if p.allowed_operators], + "reference_phases": [ + {"name": p.phase_id, "start": p.start_index, "stop": p.stop_index} + for p in reference_phases + ], + "controlled_joint_names": [template.joint_names[i] for i in active], + "task_rows": self.options["nullspace_task_rows"], + "max_joint_delta": delta, + "proposal": attempt, + "same_time_grid": True, + "motion_validation_scope": "edited_phases_and_boundary_stencils", + "motion": { + "reference": [ + dict(c.metrics) for r in reference_checks for c in r.checks + ], + "candidate": [dict(c.metrics) for r in checks for c in r.checks], + }, + "fk": fk, + } + if self.output is not None: + import numpy as np + from embodichain.lab.expansion.persistence import _digest as file_digest + + self.output.mkdir(parents=True, exist_ok=True) + path = ( + self.output + / f"call-{call.analysis_call_index}-{len(self._records)}.npz" + ) + editable = torch.zeros_like(template.positions, dtype=torch.bool) + for phase in phases: + if phase.allowed_operators: + editable[phase.start_index : phase.stop_index, active] = True + with path.open("wb") as stream: + np.savez_compressed( + stream, + reference=template.positions.numpy(), + candidate=candidate.positions.numpy(), + dt=template.dt.numpy(), + editable=editable.numpy(), + ) + evidence["plan_artifact"] = { + "path": str(path), + "sha256": file_digest(path), + } + metadata.update( + expansion=evidence, + collision_evidence="unverified", + collision_scope="not_checked", + ) + rebuilt = engine.rebuild_plan_from_trajectory( + request, + context, + replace(plan, diagnostics=replace(plan.diagnostics, metadata=metadata)), + TimedTrajectory.from_positions( + positions, + env_ids=context.env_ids, + dt=plan.joint_trajectory.dt.clone(), + ), + ) + self._records.append( + TaskProgramNativeExpansionRecord(int(context.env_ids[0]), evidence) + ) + return rebuilt + raise ValueError( + f"{self.operator} produced no qualified {plan.skill_id} variant: {reason}" + ) + + def _kinematics(self, robot, endpoints, template): + import torch + + active = list(template.controlled_joint_indices) + blocks, models = [], [] + for endpoint in endpoints: + indices = list(endpoint.joint_ids) + if not set(indices) & set(active): + continue + solver = robot.get_solver(endpoint.target.control_part) + if solver is None or solver.pk_serial_chain is None: + raise ValueError( + "Trajectory augmentation requires the actual robot kinematic chain" + ) + chain_ids = [ + robot.joint_names.index(n) + for n in solver.pk_serial_chain.get_joint_parameter_names() + ] + qpos = template.positions[:, chain_ids] + rows = self.options["nullspace_task_rows"] + jacobian = ( + solver.get_jacobian( + qpos, + locations=torch.as_tensor(solver.tcp_xpos)[:3, 3], + ) + .detach() + .cpu()[:, rows, :] + ) + block = torch.zeros(len(qpos), len(rows), len(active), dtype=jacobian.dtype) + for column, joint in enumerate(chain_ids): + if joint in active: + block[:, :, active.index(joint)] = jacobian[:, :, column] + blocks.append(block) + models.append((solver, chain_ids)) + if not blocks: + raise ValueError("No kinematic chain covers the edited motion joints") + return torch.cat(blocks, dim=1), models + + @staticmethod + def _check_fk(reference, candidate, models): + import torch + + translation, rotation = 0.0, 0.0 + for solver, indices in models: + a = solver.get_fk(reference.positions[:, indices]).detach().cpu().double() + b = solver.get_fk(candidate.positions[:, indices]).detach().cpu().double() + translation = max( + translation, float((a[:, :3, 3] - b[:, :3, 3]).norm(dim=-1).max()) + ) + relative = a[:, :3, :3].transpose(-1, -2) @ b[:, :3, :3] + cosine = ((relative.diagonal(dim1=-2, dim2=-1).sum(-1) - 1) / 2).clamp( + -1, 1 + ) + rotation = max(rotation, float(torch.acos(cosine).max())) + return {"max_translation_m": translation, "max_rotation_rad": rotation} diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index 9af6c9ecd6..a8b50a4376 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -1706,6 +1706,7 @@ def _decode_motion_policy(value: object, *, path: str) -> MotionPolicy: "plan_opts", "interpolation_mode", "ik_seed_mode", + "phase_paths", "sample_count_retries", "segment_sample_count_retries", "segment_continuous_ik_recovery", @@ -1718,6 +1719,7 @@ def _decode_motion_policy(value: object, *, path: str) -> MotionPolicy: config["sample_count"], path=f"{path}.sample_count", minimum=2 ) kwargs: dict[str, object] = { + "phase_paths": dict(config.get("phase_paths", {})), "strategy": _identifier( config.get("strategy", "ik_interp"), path=f"{path}.strategy", diff --git a/embodichain/lab/task_program/integrations/expansion.py b/embodichain/lab/task_program/integrations/expansion.py index 543c680ff6..7e296f757c 100644 --- a/embodichain/lab/task_program/integrations/expansion.py +++ b/embodichain/lab/task_program/integrations/expansion.py @@ -24,6 +24,7 @@ import hashlib from collections.abc import Callable, Mapping, Sequence import math +from pathlib import Path from typing import ClassVar import torch @@ -54,6 +55,7 @@ ) from embodichain.lab.sim.atomic_skills.state import PlanningContext from embodichain.lab.task_program.runtime.expansion import TaskProgramPlanRequest +from ._native_plan_expansion import TaskProgramNativeExpansionRecord __all__ = [ "TaskProgramSourceAdapter", @@ -63,6 +65,7 @@ "TaskProgramMappedRetimeRecord", "TaskProgramSameGridExpansionFactory", "TaskProgramSameGridExpansionRecord", + "TaskProgramNativeExpansionRecord", ] @@ -193,6 +196,63 @@ class TaskProgramSameGridExpansionFactory: still pass the unchanged full-path motion and collision checks. """ + @classmethod + def from_native_plan( + cls, + operator: str, + options: Mapping[str, object], + *, + seed: int, + scene_case: SceneCase, + revision: str, + output: Path | None = None, + episode_id: int = 0, + ) -> TaskProgramSameGridExpansionFactory: + """Expand live Atomic plans on their existing measured control grid. + + Args: + operator: object_pose, affordance, joint_residual, via_points or nullspace_residual. + options: Task-owned phase allowlists, call selection, native operator + magnitudes, sampled motion/FK limits and proposal budget. + seed: Logical candidate seed, independent of physical row. + scene_case: Measured scene and initial-state identity. + revision: Pinned task/source revision. + output: Optional directory for hashed reference/candidate attachments. + episode_id: Logical affordance branch identity. + + This source path retains native operators and Atomic reconstruction, + protects phase-boundary derivative stencils and never claims physical + success. The host must still perform measured collision/task acceptance. + """ + from ._native_plan_expansion import _NativePlanExpansion + + instance = object.__new__(cls) + instance._native_delegate = _NativePlanExpansion( + operator, + options, + seed=seed, + scene_case=scene_case, + revision=revision, + output=output, + episode_id=episode_id, + ) + return instance + + def prepare_planning_context( + self, + request: TaskProgramPlanRequest, + context: PlanningContext, + *, + engine: AtomicSkillEngine, + ) -> PlanningContext: + """Bind optional native affordance sampling before the ordinary planner.""" + delegate = getattr(self, "_native_delegate", None) + return ( + context + if delegate is None + else delegate.prepare_planning_context(request, context, engine=engine) + ) + def __init__( self, *, @@ -306,8 +366,15 @@ def __init__( self._records: list[TaskProgramSameGridExpansionRecord] = [] @property - def records(self) -> tuple[TaskProgramSameGridExpansionRecord, ...]: + def records( + self, + ) -> tuple[ + TaskProgramSameGridExpansionRecord | TaskProgramNativeExpansionRecord, ... + ]: """Return successfully rebuilt call-level candidates.""" + delegate = getattr(self, "_native_delegate", None) + if delegate is not None: + return delegate.records return tuple( replace(record, template=replace(record.template)) for record in self._records @@ -317,6 +384,9 @@ def create_plan_transform( self, request: TaskProgramPlanRequest, *, engine: AtomicSkillEngine ) -> PlanTransform | None: """Select the spatial transform for one compiled call occurrence.""" + delegate = getattr(self, "_native_delegate", None) + if delegate is not None: + return delegate.create_plan_transform(request, engine=engine) if not isinstance(request, TaskProgramPlanRequest) or not isinstance( engine, AtomicSkillEngine ): diff --git a/embodichain/lab/task_program/runtime/results.py b/embodichain/lab/task_program/runtime/results.py index 61d288c669..4851fc16b3 100644 --- a/embodichain/lab/task_program/runtime/results.py +++ b/embodichain/lab/task_program/runtime/results.py @@ -423,6 +423,7 @@ def _motion_policy_to_metadata(policy: MotionPolicy) -> dict[str, object]: "velocity_targets": policy.velocity_targets, "interpolation_mode": policy.interpolation_mode, "ik_seed_mode": policy.ik_seed_mode, + **({"phase_paths": dict(policy.phase_paths)} if policy.phase_paths else {}), "sample_count_retries": list(policy.sample_count_retries), "require_final_velocity": policy.require_final_velocity, "final_velocity_rejection": policy.final_velocity_rejection, diff --git a/embodichain/lab/visualization/markers/__init__.py b/embodichain/lab/visualization/markers/__init__.py index 2f65927085..4172846aaf 100644 --- a/embodichain/lab/visualization/markers/__init__.py +++ b/embodichain/lab/visualization/markers/__init__.py @@ -22,6 +22,7 @@ from .group import MarkerGroup from .visualization_markers import VisualizationMarkers, VisualizationMarkersCfg from .robot_markers import RobotMarkers, RobotMarkersCfg, RobotMarkersAppearanceCfg +from .frame import MarkerFramePublisher __all__ = [ "RobotMarkers", @@ -30,6 +31,7 @@ "MarkerGroupCfg", "MarkerPrototypeCfg", "MarkerGroup", + "MarkerFramePublisher", "VisualizationMarkers", "VisualizationMarkersCfg", ] diff --git a/embodichain/lab/visualization/markers/_native.py b/embodichain/lab/visualization/markers/_native.py index 49dfd66940..a5ff8df9c6 100644 --- a/embodichain/lab/visualization/markers/_native.py +++ b/embodichain/lab/visualization/markers/_native.py @@ -86,7 +86,9 @@ class NativeMarkerRenderer: isolate them with the camera's render-category filter. """ - def __init__(self, scene: Scene, *, use_scene_for_overlays: bool = False) -> None: + def __init__( + self, scene: Scene, *, use_scene_for_overlays: bool = False, xray: bool = False + ) -> None: try: spawn = import_module("dexsim.spawn") required = ( @@ -111,6 +113,7 @@ def __init__(self, scene: Scene, *, use_scene_for_overlays: bool = False) -> Non self._spawn = spawn self._scene = scene self._use_scene_for_overlays = use_scene_for_overlays + self._xray = xray self._groups: dict[str, dict[str, _Entry]] = {} self._texture_directory = TemporaryDirectory( prefix="embodichain-marker-textures-" @@ -124,7 +127,10 @@ def _create( """Declare one render-only object through its lifetime-owning Scene.""" name = f"__embodichain_marker_{uuid4().hex}" if template is not None and hasattr(self._scene, "clone_mesh_object"): - return self._scene.clone_mesh_object(template.handle.path, name) + handle = self._scene.clone_mesh_object(template.handle.path, name) + if self._xray: + handle.get_material().set_depth_culling(False) + return handle scene_lighting = self._scene_lit(marker) use_scene = scene_lighting or self._use_scene_for_overlays parameters = ( @@ -235,7 +241,10 @@ def _create( physics=None, per_env=False, ) - return self._scene.add_mesh_object(desc, arena_name="default") + handle = self._scene.add_mesh_object(desc, arena_name="default") + if self._xray: + handle.get_material().set_depth_culling(False) + return handle def _discard(self, handle: SpawnedRigidBody) -> None: # Scene.close() invalidates the selected handles. @@ -288,7 +297,13 @@ def _apply( if previous is None or previous.visible != marker.visible: handle.set_visible(marker.visible) - def publish(self, name: str, markers: tuple[MeshMarkerOverlay, ...]) -> None: + def publish( + self, + name: str, + markers: tuple[MeshMarkerOverlay, ...], + *, + retain_omitted: bool = False, + ) -> None: """Apply a complete group, staging replacements before releasing old meshes. Args: @@ -296,7 +311,7 @@ def publish(self, name: str, markers: tuple[MeshMarkerOverlay, ...]) -> None: markers: Complete detached group snapshots in world coordinates. """ previous = self._groups.get(name, {}) - pending: dict[str, _Entry] = {} + pending: dict[str, _Entry] = dict(previous) if retain_omitted else {} created = [] touched = [] batch_enabled = hasattr(self._scene, "create_visual_object_batch") @@ -304,6 +319,20 @@ def publish(self, name: str, markers: tuple[MeshMarkerOverlay, ...]) -> None: for entry in previous.values(): templates.setdefault(id(entry.snapshot.vertices), entry) try: + if retain_omitted: + selected = {marker.overlay_id for marker in markers} + for key, entry in previous.items(): + if key not in selected and entry.snapshot.visible: + touched.append(entry) + hidden = replace( + entry.snapshot, + visible=False, + _shared_geometry=entry.snapshot._geometry, + ) + self._apply( + entry.handle, hidden, entry.snapshot, transform=False + ) + pending[key] = _Entry(entry.handle, hidden) for marker in markers: entry = previous.get(marker.overlay_id) if entry is not None and self._same_geometry(entry.snapshot, marker): @@ -329,9 +358,9 @@ def publish(self, name: str, markers: tuple[MeshMarkerOverlay, ...]) -> None: ) pending[marker.overlay_id] = _Entry(handle, marker) templates.setdefault(id(marker.vertices), pending[marker.overlay_id]) - if batch_enabled and pending: + if batch_enabled and markers: transforms = _transforms(markers) - handles = tuple(entry.handle for entry in pending.values()) + handles = tuple(pending[marker.overlay_id].handle for marker in markers) cached = self._batches.get(name) if ( cached is None diff --git a/embodichain/lab/visualization/markers/frame.py b/embodichain/lab/visualization/markers/frame.py new file mode 100644 index 0000000000..95093135d4 --- /dev/null +++ b/embodichain/lab/visualization/markers/frame.py @@ -0,0 +1,86 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Resident marker resources and one cross-group native frame publication.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING +from .group import MarkerGroup +from .cfg import MarkerGroupCfg +from ._native import NativeMarkerRenderer + +if TYPE_CHECKING: + from embodichain.lab.sim import SimulationManager + +__all__ = ["MarkerFramePublisher"] + + +class MarkerFramePublisher: + """Stage robot/object groups and publish one coherent render-only frame. + + Args: + simulation: Prepared simulation owning the native render resources. + xray: Disable depth culling for translucent comparison overlays. + + Groups belong to this publisher, independently of automatic simulation + markers. Initial publication uploads all declared resources, including hidden + instances. Later publications transform only visible instances, retain hidden + resources, and use actual native visibility rather than moving/scaling objects. + Publishing neither prepares nor steps the simulation. Close before the World. + """ + + def __init__(self, simulation: SimulationManager, *, xray: bool = False) -> None: + self.sim = simulation + self.groups: dict[str, MarkerGroup] = {} + self.native = NativeMarkerRenderer(simulation.spawn_result, xray=xray) + self.publications = 0 + self._closed = False + + def add_marker_group(self, cfg: MarkerGroupCfg) -> MarkerGroup: + """Declare a group compatible with RobotMarkers/VisualizationMarkers.""" + if self._closed: + raise RuntimeError("marker frame publisher is closed") + if cfg.name in self.groups: + raise ValueError(f"duplicate marker group: {cfg.name}") + group = MarkerGroup( + cfg, num_envs=self.sim.num_envs, origins=self.sim.arena_offsets + ) + self.groups[group.name] = group + return group + + def publish(self, *, preload: bool = False) -> None: + """Submit all staged groups together, retaining hidden GPU resources.""" + if self._closed: + raise RuntimeError("marker frame publisher is closed") + initial = self.publications == 0 or preload + meshes = tuple( + mesh + for group in self.groups.values() + for mesh in group.snapshot(visible_only=not initial) + ) + self.native.publish("frame", meshes, retain_omitted=True) + self.publications += 1 + + def close(self) -> None: + """Release resident batches, meshes and groups; repeated close is safe.""" + if self._closed: + return + self._closed = True + self.native.close() + for group in self.groups.values(): + group.remove() + self.groups.clear() diff --git a/embodichain/lab/visualization/markers/group.py b/embodichain/lab/visualization/markers/group.py index c499d2d5be..03dbaa7033 100644 --- a/embodichain/lab/visualization/markers/group.py +++ b/embodichain/lab/visualization/markers/group.py @@ -391,7 +391,7 @@ def _poses(self, env_id: int) -> tuple[np.ndarray, np.ndarray]: xyzw = (rotation * Rotation.from_quat(xyzw)).as_quat() return xyz + self._origins[env_id], xyzw - def snapshot(self) -> tuple[MeshMarkerOverlay, ...]: + def snapshot(self, *, visible_only: bool = False) -> tuple[MeshMarkerOverlay, ...]: """Return detached world-space snapshots without querying parent assets. Returns: @@ -408,6 +408,10 @@ def snapshot(self) -> tuple[MeshMarkerOverlay, ...]: ): raise ValueError("World positions must be finite float32 values.") for index, prototype_index in enumerate(state["prototype_indices"]): + if visible_only and not ( + self._visible[env_id] and state["visible"][index] + ): + continue prototype = prototypes[prototype_index] rgba = ( prototype.color diff --git a/tests/lab/expansion/test_comparison.py b/tests/lab/expansion/test_comparison.py new file mode 100644 index 0000000000..016cd793c9 --- /dev/null +++ b/tests/lab/expansion/test_comparison.py @@ -0,0 +1,76 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import numpy as np +import pytest +from embodichain.lab.expansion import MeasuredTrajectory +from embodichain.lab.visualization.markers import ( + MarkerGroup, + MarkerGroupCfg, + MarkerPrototypeCfg, +) + + +def test_measured_comparison_owns_boundaries_and_preserves_marker_identity(): + times = np.array([0.0, 0.1, 0.3]) + poses = np.tile([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], (3, 1)) + clip = MeasuredTrajectory( + "original", + "robot", + ("joint",), + times, + np.array([[1.0], [2.0], [3.0]]), + poses, + {"object": poses}, + ) + times[1] = 99 + poses[0, 0] = 99 + assert [clip.frame_at(t) for t in (-1.0, 0.0, 0.1, 0.2, 0.3, 5.0)] == [ + 0, + 0, + 1, + 1, + 2, + 2, + ] + assert clip.root_poses[0, 0] == clip.object_poses["object"][0, 0] == 0 + with pytest.raises(ValueError, match="ordered"): + MeasuredTrajectory( + "bad", + "robot", + ("joint",), + np.array([0.0, 0.0]), + np.zeros((2, 1)), + np.zeros((2, 7)), + {}, + ) + group = MarkerGroup( + MarkerGroupCfg( + name="ghost", prototypes={"box": MarkerPrototypeCfg(shape="box")} + ), + num_envs=1, + origins=np.zeros((1, 3)), + ) + group.update(translations=np.zeros((3, 3)), visible=np.array([False, True, False])) + complete = group.snapshot() + selected = group.snapshot(visible_only=True) + assert len(complete) == 3 and len(selected) == 1 + assert selected[0].overlay_id == complete[1].overlay_id + group.update(visible=np.array([False, False, True])) + assert group.snapshot(visible_only=True)[0].overlay_id == complete[2].overlay_id + group.remove() diff --git a/tests/lab/expansion/test_extension_ports_sim.py b/tests/lab/expansion/test_extension_ports_sim.py new file mode 100644 index 0000000000..2312257ab8 --- /dev/null +++ b/tests/lab/expansion/test_extension_ports_sim.py @@ -0,0 +1,193 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import os +from pathlib import Path +import subprocess +import sys +import pytest + + +def _probe(output): + import torch + from embodichain.lab.expansion import MeasuredExpansionValidator + from embodichain.lab.expansion.program import TaskProgramBatchExecutor + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + discover_task_packages, + execute_init_hooks, + ) + from embodichain.lab.sim import SimulationManager + from embodichain.lab.sim.motion.motion_generator import ( + MotionGenerator, + MotionGenCfg, + MotionGenOptions, + ) + from embodichain.lab.sim.motion.planners.trapezoidal_planner import ( + TrapezoidalPlannerCfg, + ) + from embodichain.lab.sim.motion.planners.utils import PlanState + from embodichain.lab.task_program.integrations.simulation.feasibility_geometry import ( + collision_bodies_from_simulation, + ) + from embodichain.utils.utility import load_config + + torch.set_num_threads(1) + root = Path(__file__).resolve().parents[3] + path = ( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task.ur5.yaml" + ) + discover_task_packages() + execute_init_hooks() + cfg = config_to_cfg(load_config(path), source_path=path) + cfg.num_envs = cfg.sim_cfg.num_envs = 2 + cfg.sim_cfg.device, cfg.sim_cfg.headless = "cpu", True + cfg.sim_cfg.render_cfg.renderer = "no-render" + cfg.sensor, cfg.enable_sensor, cfg.dataset = [], False, None + cfg.init_rollout_buffer = cfg.record_trajectory = False + cfg.task_program.program.count = 1 + env = REGISTERED_ENVS[load_config(path)["id"]].make(cfg=cfg) + try: + env.reset(seed=0) + generator = MotionGenerator( + MotionGenCfg( + planner_cfg=TrapezoidalPlannerCfg( + robot_uid=env.robot.uid, sim_instance_id=env.sim.instance_id + ) + ), + env_ids=(0,), + ) + start = env.robot.get_qpos(name="arm")[0:1].clone() + states = [] + for index in range(3): + goal = start.clone() + goal[:, 0] += 0.001 * (index + 1) + pose = env.robot.compute_fk( + qpos=goal, name="arm", to_matrix=True, env_ids=(0,) + ) + states.append(PlanState.from_xpos(pose)) + base = dict( + strategy="ik_interp", + interpolation_mode="fixed_grid", + ik_seed_mode="sequential", + interpolation_dt=0.04, + start_qpos=start, + control_part="arm", + ) + ordinary = generator.generate(states, MotionGenOptions(sample_count=25, **base)) + first = generator.generate(states[:1], MotionGenOptions(sample_count=9, **base)) + tail = generator.generate( + states[1:], + MotionGenOptions( + sample_count=17, + **{**base, "start_qpos": first.positions[:, -1], "is_linear": True}, + ), + ) + before = env.sim.physics_step_index + mixed = generator.generate( + states, + MotionGenOptions( + sample_count=25, + waypoint_modes=("joint", "cartesian", "cartesian"), + **base, + ), + ) + assert mixed.success.all() and first.success.all() and tail.success.all() + torch.testing.assert_close( + mixed.positions, + torch.cat((first.positions, tail.positions[:, 1:]), dim=1), + rtol=0, + atol=1e-6, + ) + torch.testing.assert_close( + mixed.dt, torch.cat((first.dt, tail.dt[:, 1:]), dim=1), rtol=0, atol=0 + ) + after = generator.generate(states, MotionGenOptions(sample_count=25, **base)) + torch.testing.assert_close( + ordinary.positions, after.positions, rtol=0, atol=1e-6 + ) + assert env.sim.physics_step_index == before + program = env.compile_task_program(cfg.task_program) + frames = sorted({b.frame_uid for b in collision_bodies_from_simulation(env)}) + value = float(env.robot.get_qpos()[0, 0]) + name = env.robot.joint_names[0] + configuration = { + "initial_checks": [ + { + "kind": "joint_range", + "joint": name, + "lower": value - 0.1, + "upper": value + 0.1, + } + ], + "final_checks": [ + { + "kind": "joint_range", + "joint": name, + "lower": value + 1, + "upper": value + 2, + } + ], + "events": [], + "mounting_contacts": [], + "allowed_contacts": [ + (a, b) for i, a in enumerate(frames) for b in frames[i + 1 :] + ], + } + for backend in ("geometry", "native_contacts"): + check = MeasuredExpansionValidator( + env, configuration, Path(output) / backend, backend=backend + ) + try: + check.prepare() + TaskProgramBatchExecutor(env, program).run( + (0, 1), max_steps=3, observe_step=check.sample + ) + assert set(check.results) == set(check.terminal) == {0, 1} + assert all( + row.status == "passed" for row in check.results.values() + ), check.results + peer = check.terminal[1] + check.prepare(env_ids=(0,)) + assert check.terminal[1] is peer and 0 not in check.terminal + if backend == "native_contacts": + assert check.rows[1].substeps > 0 and check.rows[0].substeps == 0 + finally: + check.close() + finally: + env.close(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +@pytest.mark.subprocess_sim +def test_explicit_phase_paths_and_selected_row_acceptance(tmp_path): + result = subprocess.run( + [sys.executable, __file__, str(tmp_path)], + cwd=Path(__file__).resolve().parents[3], + env={**os.environ, "SDL_VIDEODRIVER": "offscreen", "OMP_NUM_THREADS": "1"}, + capture_output=True, + text=True, + timeout=300, + ) + assert result.returncode == 0, result.stdout[-3000:] + result.stderr[-5000:] + + +if __name__ == "__main__": + _probe(sys.argv[1]) From ab04d972b5b75042434498db9a889b4edd03b6fe Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 14 Nov 2026 14:24:14 +0000 Subject: [PATCH 08/45] feat(graspkit): scope pose NMS memory budgets per task --- .../motion-planning/trajectory-expansion.md | 4 ++ embodichain/utils/nms.py | 40 ++++++++++++-- tests/utils/test_nms_scope.py | 52 +++++++++++++++++++ 3 files changed, 91 insertions(+), 5 deletions(-) create mode 100644 tests/utils/test_nms_scope.py diff --git a/agent_context/topics/motion-planning/trajectory-expansion.md b/agent_context/topics/motion-planning/trajectory-expansion.md index 6a626491df..5d8dcb9fe5 100644 --- a/agent_context/topics/motion-planning/trajectory-expansion.md +++ b/agent_context/topics/motion-planning/trajectory-expansion.md @@ -65,6 +65,10 @@ position search reuses its pool; approach search draws one pool per direction. Each successful row freezes independently. Per-row resets or filtering before ranking/NMS change populations. Freeze grasp, direction and position before comparing planners and review population fingerprints after selection changes. +`utils.nms.pose_nms_scope` supplies a nested, task-local pairwise tile budget; +explicit call arguments override it. It preserves threshold arithmetic and +selection order without changing an imported backend function. Copied planning +contexts inherit the budget while independent threads keep their own default. Explicit goal-conditioned endpoint searches also change the population and retain rejected goals; they establish neither paths nor physical success. diff --git a/embodichain/utils/nms.py b/embodichain/utils/nms.py index a43a5be3c8..8befa8028d 100644 --- a/embodichain/utils/nms.py +++ b/embodichain/utils/nms.py @@ -17,6 +17,9 @@ from __future__ import annotations import math +from contextlib import contextmanager +from contextvars import ContextVar +from collections.abc import Iterator import numpy as np import torch @@ -24,9 +27,35 @@ from embodichain.utils import logger from embodichain.utils.math import quat_from_matrix -__all__ = ["pose_nms", "pose_nms_indices"] +__all__ = ["pose_nms", "pose_nms_indices", "pose_nms_scope"] _POSE_NMS_CHUNK_SIZE = 2048 +_POSE_NMS_SCOPE: ContextVar[int] = ContextVar( + "pose_nms_chunk_size", default=_POSE_NMS_CHUNK_SIZE +) + + +@contextmanager +def pose_nms_scope(*, chunk_size: int | None = None) -> Iterator[int]: + """Select a task-local NMS memory budget without replacing any function. + + Args: + chunk_size: Positive pairwise tile bound; None inherits the active scope + (2048 outside a scope). Explicit NMS call arguments take precedence. + + Yields: + Active tile bound. Nested scopes restore their parent on success/failure; + threads and copied cooperative contexts retain their own selection. + Threshold arithmetic, candidate ordering and output values are unchanged. + """ + size = _POSE_NMS_SCOPE.get() if chunk_size is None else chunk_size + if type(size) is not int or size < 1: + raise ValueError("NMS chunk_size must be a positive integer") + token = _POSE_NMS_SCOPE.set(size) + try: + yield size + finally: + _POSE_NMS_SCOPE.reset(token) def _poses_to_components(poses: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]: @@ -219,7 +248,7 @@ def pose_nms_indices( angle_th: float = np.pi / 36, dist_th: float = 0.003, preserve_order: bool = False, - chunk_size: int = _POSE_NMS_CHUNK_SIZE, + chunk_size: int | None = None, ) -> torch.Tensor: """Return pose indices after removing poses that are too close. @@ -237,7 +266,7 @@ def pose_nms_indices( ``False``, poses with fewer close neighbors are selected first. Defaults to ``False``. chunk_size: Maximum size of either dimension of a pairwise tile. - Defaults to 2048. + None uses the active pose_nms_scope, defaulting to 2048. Returns: Indices of selected poses. Shape is ``(M,)``, where ``M <= N``. @@ -246,6 +275,7 @@ def pose_nms_indices( ValueError: If ``poses`` is not shaped as ``(N, 4, 4)``, is not on a CPU or CUDA device, or ``chunk_size`` is not positive. """ + chunk_size = _POSE_NMS_SCOPE.get() if chunk_size is None else chunk_size if poses.ndim != 3 or poses.shape[-2:] != (4, 4): raise ValueError(f"Invalid input shape {poses.shape}, expected (N, 4, 4).") if poses.device.type not in {"cpu", "cuda"}: @@ -316,7 +346,7 @@ def pose_nms( poses: torch.Tensor, angle_th: float = np.pi / 36, dist_th: float = 0.003, - chunk_size: int = _POSE_NMS_CHUNK_SIZE, + chunk_size: int | None = None, ) -> tuple[torch.Tensor, torch.Tensor]: """Remove poses that are too close in translation and rotation. @@ -325,7 +355,7 @@ def pose_nms( angle_th: Rotation threshold in radians. Defaults to pi / 36. dist_th: Translation threshold. Defaults to 0.003. chunk_size: Maximum size of either dimension of a pairwise tile. - Defaults to 2048. + None uses the active pose_nms_scope, defaulting to 2048. Returns: A tuple containing the filtered pose matrices in input order and their diff --git a/tests/utils/test_nms_scope.py b/tests/utils/test_nms_scope.py new file mode 100644 index 0000000000..412b75cbef --- /dev/null +++ b/tests/utils/test_nms_scope.py @@ -0,0 +1,52 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from concurrent.futures import ThreadPoolExecutor +from contextvars import copy_context +import pytest +import torch +from embodichain.utils.nms import pose_nms, pose_nms_scope + + +def test_nms_memory_scope_is_owned_and_preserves_selection(): + def current(): + with pose_nms_scope() as size: + return size + + poses = torch.eye(4).repeat(8, 1, 1) + poses[:, 0, 3] = torch.tensor([0.0, 0.001, 0.002, 0.01, 0.012, 0.02, 0.021, 0.03]) + expected = pose_nms(poses, chunk_size=2) + with pose_nms_scope(chunk_size=2): + inherited = copy_context() + with pytest.raises(RuntimeError): + with pose_nms_scope(chunk_size=3): + assert current() == 3 + raise RuntimeError("scope interrupted") + assert current() == 2 + actual = pose_nms(poses) + assert torch.equal(actual[0], expected[0]) and torch.equal( + actual[1], expected[1] + ) + explicit = pose_nms(poses, chunk_size=1) + assert torch.equal(explicit[1], expected[1]) + with ThreadPoolExecutor(max_workers=1) as pool: + assert pool.submit(current).result() == 2048 + assert current() == 2048 and inherited.run(current) == 2 + with pytest.raises(ValueError): + with pose_nms_scope(chunk_size=0): + pass From bc5ae3f2ba53422a0c41f136b36ddbd6ee07df5a Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sun, 15 Nov 2026 06:52:59 +0000 Subject: [PATCH 09/45] perf: reduce single-world planning and row execution overhead Share Default CPU numeric observations within bounded command-gather scopes, reconstruct immutable standard targets, and transfer fresh internal command ownership while preserving public snapshots and validation. Keep native multi-chunk grasp geometry and reductions resident on CUDA within each query, preserving samples, thresholds, stream ordering and independent results. Add focused regression coverage and a reproducible runtime comparison driver. Validation: 201 focused tests including CUDA and 42 context tests passed; Black 26.10.1, API docs 4924/4924 and diff checks passed. Sources match the final benchmark manifest. Forward/reverse sensor-free UR5 execution versus main 0119171b (seconds): small main 10.34-10.45, off 9.07-9.31, on 8.31-9.08; medium main 49.24-57.12, off 47.19-50.90, on 48.66-49.57; large main 251.77-260.14, off 189.57-196.42, on 192.07-200.29. All 492 program attempts accepted. Default CPU physics retains legacy CUDA grasp classification; these shared-host single-seed results are not physical qualification. --- .../topics/expansion-engine/workloads.md | 10 + .../topics/motion-planning/motion-planning.md | 6 + embodichain/lab/expansion/workloads.py | 22 +- .../lab/sim/atomic_skills/_command_batches.py | 6 + embodichain/lab/sim/atomic_skills/bindings.py | 18 + embodichain/lab/sim/atomic_skills/core.py | 14 +- .../lab/sim/atomic_skills/runtime_commands.py | 113 ++++- .../objects/backends/_numeric_observations.py | 165 +++++++ embodichain/lab/sim/objects/backends/scene.py | 9 + embodichain/lab/sim/objects/rigid_object.py | 19 + .../pg_grasp/gripper_collision_checker.py | 147 ++++-- .../benchmark/expansion/runtime_comparison.py | 449 ++++++++++++++++++ tests/sim/atomic_skills/test_core.py | 38 +- .../atomic_skills/test_runtime_commands.py | 29 ++ tests/sim/objects/test_scene_backend.py | 102 ++++ tests/toolkits/test_fixed_gripper_geometry.py | 61 +++ 16 files changed, 1150 insertions(+), 58 deletions(-) create mode 100644 embodichain/lab/sim/objects/backends/_numeric_observations.py create mode 100644 scripts/benchmark/expansion/runtime_comparison.py diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index 6dcdf270f1..6ca15ed63a 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -56,6 +56,16 @@ The optional cooperative planner merges compatible fixed-grid motion requests and pure FK/IK/Jacobian calls. Budget-bearing whole-motion requests keep their original execution so charge semantics are preserved. Context variables and candidate random streams remain row-local; results are independently owned. +Default CPU command gathering shares native numeric reads through a bounded +World-owner scope in `sim/objects/backends/_numeric_observations.py`. It ends +before completion callbacks and physical stepping, and starts after reset and +activation. Checked native writes, physics steps and binding topology changes +invalidate samples. Derived dynamic rigid poses share the same boundary and invalidation, with +separate CPU autocast representations. Consumers receive independent output +buffers; semantic +state, episode clocks and validation remain row-owned. CUDA and other backends +retain native reads. Raw SDK mutation or concurrent World writes inside the +read boundary are outside this internal side-effect-free planning contract. Semantic programs and some trajectory processing still run per row. Per-control planning readiness is a barrier; this API does not claim complete vectorization or universal throughput improvement. diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index 527adf63f0..fe98a0d836 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -93,6 +93,12 @@ unchanged physics substeps; Gym experts use their environment's command period. prepared actions without retiming again. See [retiming and playback](planner-details.md#retiming-and-playback). +Gripper surface queries with CPU inputs still classify convex distances on CUDA. +Native multi-chunk calls own one query-local geometry snapshot and bounded scratch, +retain CPU point/ground transforms and all samples, and return owned reduced CPU +results. This is distinct from the simulation physics device. Visualization, +custom point-query overrides and already-device-resident callers keep their paths. + GraspKit owns sampled/native geometric qualification from explicit hand inputs; Scene captures geometry and Robot adapters provide FK/calibration. GraspKit performs no hardware lookup. Sampling uses the contact frame and maps to runtime diff --git a/embodichain/lab/expansion/workloads.py b/embodichain/lab/expansion/workloads.py index 3f575924ba..dfbd37b65e 100644 --- a/embodichain/lab/expansion/workloads.py +++ b/embodichain/lab/expansion/workloads.py @@ -463,6 +463,9 @@ def run(self) -> Mapping[str, object]: from tensordict import TensorDict from embodichain.lab.gym.envs.types import ControllerAction from ._planning_batches import batched_planning + from embodichain.lab.sim.objects.backends._numeric_observations import ( + numeric_observation_scope, + ) env, torch = self.env, self.torch previous_auto = getattr(env, "_demo_no_auto_reset", False) @@ -483,15 +486,16 @@ def run(self) -> Mapping[str, object]: self._fill() self._activate() cursors = {row: state["cursor"] for row, state in self.live.items()} - commands = ( - planner.gather(cursors) - if planner is not None - else { - row: command - for row, cursor in cursors.items() - if (command := cursor.next()) is not None - } - ) + with numeric_observation_scope(env.sim, env.robot): + commands = ( + planner.gather(cursors) + if planner is not None + else { + row: command + for row, cursor in cursors.items() + if (command := cursor.next()) is not None + } + ) for row in tuple(self.live): if row not in commands: self._finish(row) diff --git a/embodichain/lab/sim/atomic_skills/_command_batches.py b/embodichain/lab/sim/atomic_skills/_command_batches.py index 5aa5d41d1c..492357f672 100644 --- a/embodichain/lab/sim/atomic_skills/_command_batches.py +++ b/embodichain/lab/sim/atomic_skills/_command_batches.py @@ -140,6 +140,12 @@ def numeric(frame): def validate_frame(frame): + if ( + type(frame) is not RuntimeCommandFrame + or type(frame.env_ids) is not torch.Tensor + or frame.env_ids.device.type != "cuda" + ): + return False values = leaves(frame) if values is None: return False diff --git a/embodichain/lab/sim/atomic_skills/bindings.py b/embodichain/lab/sim/atomic_skills/bindings.py index e2bd6cd1e1..3f57133224 100644 --- a/embodichain/lab/sim/atomic_skills/bindings.py +++ b/embodichain/lab/sim/atomic_skills/bindings.py @@ -203,6 +203,24 @@ def __post_init__(self) -> None: raise ValueError("JointPositionTarget.joint_ids must be unique.") object.__setattr__(self, "joint_ids", joint_ids) + def snapshot(self) -> JointPositionTarget: + """Return an independently owned target. + + Standard immutable fields are reconstructed and validated; subclasses + and extended scalar types retain generic deep-snapshot semantics. + + Returns: + A new target of the same concrete type. + """ + if ( + type(self) is not JointPositionTarget + or type(self.control_part) is not str + or type(self.joint_ids) is not tuple + or any(type(joint) is not int for joint in self.joint_ids) + ): + return deepcopy(self) + return JointPositionTarget(self.control_part, self.joint_ids) + @property def transport_id(self) -> str: """Return the built-in joint-position transport identifier.""" diff --git a/embodichain/lab/sim/atomic_skills/core.py b/embodichain/lab/sim/atomic_skills/core.py index 20587e026b..fcebdab5d4 100644 --- a/embodichain/lab/sim/atomic_skills/core.py +++ b/embodichain/lab/sim/atomic_skills/core.py @@ -61,6 +61,8 @@ ) from .requirements import GRASP_CAPABILITY, SkillBindingContract from .runtime_commands import ( + _sequence_from_fresh_frames, + _frame_from_fresh_commands, EndpointCommand, JointPositionPayload, RuntimeCommandFrame, @@ -1358,8 +1360,8 @@ def _authorize_command_targets( EndpointCommand(target=target, payload=command.payload) ) frames.append( - RuntimeCommandFrame( - commands=tuple(endpoint_commands), + _frame_from_fresh_commands( + endpoint_commands, active_mask=( frame.active_mask if active_mask is None @@ -1369,7 +1371,7 @@ def _authorize_command_targets( hold_duration=frame.hold_duration, ) ) - return TimedCommandSequence(frames=tuple(frames), env_ids=commands.env_ids) + return _sequence_from_fresh_frames(frames, commands.env_ids) def _joint_command_sequence( self, @@ -1420,14 +1422,14 @@ def _joint_command_sequence( # dispatching it, wait for the next arrival interval; the terminal # frame reuses its own interval as the action's settling window. frames.append( - RuntimeCommandFrame( - commands=tuple(endpoint_commands), + _frame_from_fresh_commands( + endpoint_commands, active_mask=active_mask, env_ids=trajectory.env_ids, hold_duration=trajectory.dt[:, next_waypoint_index], ) ) - return TimedCommandSequence(frames=tuple(frames), env_ids=trajectory.env_ids) + return _sequence_from_fresh_frames(frames, trajectory.env_ids) @staticmethod def _build_segments( diff --git a/embodichain/lab/sim/atomic_skills/runtime_commands.py b/embodichain/lab/sim/atomic_skills/runtime_commands.py index de474be3f9..2a05935f3f 100644 --- a/embodichain/lab/sim/atomic_skills/runtime_commands.py +++ b/embodichain/lab/sim/atomic_skills/runtime_commands.py @@ -70,6 +70,18 @@ def _snapshot_target(target: RuntimeEndpointTarget) -> RuntimeEndpointTarget: return snapshot +def _unique_env_ids(env_ids: torch.Tensor) -> bool: + """Avoid a data-dependent kernel for a provably unique dense CPU singleton.""" + if ( + type(env_ids) is torch.Tensor + and env_ids.device.type == "cpu" + and env_ids.layout == torch.strided + and env_ids.numel() == 1 + ): + return True + return torch.unique(env_ids).numel() == env_ids.numel() + + class RuntimeCommandPayload(ABC): """Immutable-by-ownership payload submitted to one runtime transport.""" @@ -264,6 +276,10 @@ def __post_init__(self) -> None: if validate_frame(self): return + RuntimeCommandFrame._initialize(self, owned_commands=False) + + def _initialize(self, *, owned_commands: bool) -> None: + """Validate every frame field, optionally transferring private commands.""" if isinstance(self.commands, (str, bytes)): raise TypeError("commands must be an iterable of EndpointCommand values.") try: @@ -284,7 +300,7 @@ def __post_init__(self) -> None: ): raise ValueError("env_ids must be int64 with shape (batch_size,).") batch_size = int(self.env_ids.shape[0]) - if torch.unique(self.env_ids).numel() != batch_size: + if not _unique_env_ids(self.env_ids): raise ValueError("env_ids must be unique.") if not isinstance(self.active_mask, torch.Tensor): raise TypeError("active_mask must be a torch.Tensor.") @@ -306,7 +322,11 @@ def __post_init__(self) -> None: if self.hold_duration.device != self.env_ids.device: raise ValueError("hold_duration and env_ids must share a device.") - snapshots = tuple(command.snapshot() for command in commands) + snapshots = ( + commands + if owned_commands + else tuple(command.snapshot() for command in commands) + ) destinations: set[tuple[str, str]] = set() joint_owners: dict[int, tuple[str, str]] = {} for command in snapshots: @@ -426,7 +446,7 @@ def __post_init__(self) -> None: or self.env_ids.shape[0] < 1 ): raise ValueError("env_ids must be int64 with shape (batch_size,).") - if torch.unique(self.env_ids).numel() != self.env_ids.numel(): + if not _unique_env_ids(self.env_ids): raise ValueError("env_ids must be unique.") if isinstance(self.frames, (str, bytes)): raise TypeError("frames must be an iterable of RuntimeCommandFrame values.") @@ -481,6 +501,93 @@ def snapshot(self) -> TimedCommandSequence: return TimedCommandSequence(frames=self.frames, env_ids=self.env_ids) +def _canonical_cpu_command(command: EndpointCommand) -> bool: + """Identify standard commands without user-defined snapshot callbacks.""" + return ( + type(command) is EndpointCommand + and type(command.target) is JointPositionTarget + and type(command.target.control_part) is str + and type(command.target.joint_ids) is tuple + and all(type(joint) is int for joint in command.target.joint_ids) + and not getattr(command.target, "__dict__", {}) + and type(command.payload) is JointPositionPayload + and type(command.payload.positions) is torch.Tensor + and command.payload.positions.device.type == "cpu" + and ( + command.payload.velocities is None + or type(command.payload.velocities) is torch.Tensor + ) + ) + + +def _frame_from_fresh_commands( + commands: list[EndpointCommand], + *, + active_mask: torch.Tensor, + env_ids: torch.Tensor, + hold_duration: torch.Tensor, +) -> RuntimeCommandFrame: + """Transfer unexposed engine-built commands while retaining frame checks. + + Only fresh commands from the two canonical engine builders may enter this + path. Extended targets, payloads and tensor types use public construction. + Public frame constructors and snapshots never transfer caller ownership. + """ + fields = dict( + commands=tuple(commands), + active_mask=active_mask, + env_ids=env_ids, + hold_duration=hold_duration, + ) + if any( + type(v) is not torch.Tensor or v.device.type != "cpu" + for v in (active_mask, env_ids, hold_duration) + ) or not all(_canonical_cpu_command(c) for c in commands): + return RuntimeCommandFrame(**fields) + frame = object.__new__(RuntimeCommandFrame) + for name, value in fields.items(): + object.__setattr__(frame, name, value) + RuntimeCommandFrame._initialize(frame, owned_commands=True) + commands.clear() + return frame + + +def _sequence_from_fresh_frames( + frames: list[RuntimeCommandFrame], env_ids: torch.Tensor +) -> TimedCommandSequence: + """Transfer private, freshly validated canonical frames into a sequence. + + Only engine builders may pass their newly constructed, unexposed CPU frames. + CUDA retains packed sequence validation rather than synchronizing each ID check. + Public constructors and snapshots always retain defensive revalidation and + copying. Custom targets/payloads retain that path as their callbacks may + carry aliases or side effects. The input list is consumed on transfer. + """ + if ( + type(env_ids) is not torch.Tensor + or env_ids.device.type != "cpu" + or any( + type(frame) is not RuntimeCommandFrame + or any( + type(value) is not torch.Tensor + for value in (frame.active_mask, frame.env_ids, frame.hold_duration) + ) + or not all(_canonical_cpu_command(command) for command in frame.commands) + for frame in frames + ) + ): + return TimedCommandSequence(frames=tuple(frames), env_ids=env_ids) + sequence = TimedCommandSequence(frames=(), env_ids=env_ids) + for index, frame in enumerate(frames): + if frame.device != sequence.device: + raise ValueError(f"Frame {index} must share the sequence device.") + if not torch.equal(frame.env_ids, sequence.env_ids): + raise ValueError(f"Frame {index} env_ids do not match the sequence.") + object.__setattr__(sequence, "frames", tuple(frames)) + frames.clear() + return sequence + + __all__ = [ "EndpointCommand", "JointPositionPayload", diff --git a/embodichain/lab/sim/objects/backends/_numeric_observations.py b/embodichain/lab/sim/objects/backends/_numeric_observations.py new file mode 100644 index 0000000000..2c9e3ab47c --- /dev/null +++ b/embodichain/lab/sim/objects/backends/_numeric_observations.py @@ -0,0 +1,165 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Bounded shared native reads during a single World-owner planning boundary. + +Only Default CPU full-batch numeric reads are shared. Each consumer still +receives its own output buffer. Checked native writes invalidate all samples; +physics/topology revisions and scope exit prevent reuse across boundaries. +Raw SDK mutations and concurrent World writers are outside this internal +read-boundary contract. +""" + +from __future__ import annotations + +from collections.abc import Callable +from contextlib import contextmanager +from contextvars import ContextVar +from threading import get_ident +from typing import Any, Iterator, TYPE_CHECKING +import torch + +if TYPE_CHECKING: + from embodichain.lab.sim.sim_manager import SimulationManager + from embodichain.lab.sim.objects.robot import Robot + +__all__: list[str] = [] +_READS = frozenset( + ( + "fetch_pose", + "fetch_root_pose", + "fetch_link_pose", + "fetch_joint_position", + "fetch_joint_velocity", + "fetch_joint_force", + "fetch_linear_velocity", + "fetch_angular_velocity", + "fetch_root_linear_velocity", + "fetch_root_angular_velocity", + ) +) +_ACTIVE: ContextVar[_NumericReads | None] = ContextVar( + "native_numeric_reads", default=None +) + + +class _NumericReads: + def __init__( + self, batches: tuple[object, ...], revision: Callable[[], object] + ) -> None: + self.batches = {id(batch): batch for batch in batches} + self.revision: Callable[[], object] | None = revision + self.stamp = revision() + self.owner = get_ident() + self.closed = False + self.values: dict[tuple, tuple[torch.Tensor, Any]] = {} + + def refresh(self) -> bool: + """Check the owner/revision before accessing protected samples.""" + if self.closed or self.owner != get_ident() or self.revision is None: + return False + stamp = self.revision() + if stamp != self.stamp: + self.values.clear() + self.stamp = stamp + return True + + def key( + self, batch: object, method: str, args: tuple, kwargs: dict + ) -> tuple | None: + if not self.refresh(): + return None + if not method.startswith("fetch_"): + # Invalidate before a write, including partially failing writes. + self.values.clear() + return None + if ( + method not in _READS + or id(batch) not in self.batches + or len(args) != 1 + or kwargs + ): + return None + out = args[0] + if ( + type(out) is not torch.Tensor + or out.device.type != "cpu" + or out.requires_grad + or not out.is_contiguous() + ): + return None + return (id(batch), method, out.shape, out.dtype) + + +def shared_pose( + batch: object, to_matrix: bool, read: Callable[[], torch.Tensor] +) -> torch.Tensor: + """Own one derived pose representation and clone it for every consumer.""" + reads = _ACTIVE.get() + if reads is None or not reads.refresh() or id(batch) not in reads.batches: + return read() + key = ( + id(batch), + "derived_pose", + to_matrix, + torch.is_autocast_enabled("cpu"), + torch.get_autocast_dtype("cpu"), + ) + cached = reads.values.get(key) + if cached is not None: + return cached[0].clone() + value = read() + if ( + type(value) is torch.Tensor + and value.device.type == "cpu" + and not value.requires_grad + ): + reads.values[key] = (value.detach().clone(), None) + return value + + +@contextmanager +def numeric_observation_scope(sim: SimulationManager, robot: Robot) -> Iterator[None]: + """Share numeric samples only while this owner is gathering row commands.""" + if sim.physics_backend != "default" or sim.device.type != "cpu": + yield + return + batches = [robot._data.articulation_view.batch] + for uid in sim.get_rigid_object_uid_list(): + data = sim.get_rigid_object(uid)._data + if data is not None: + batches.append(data.body_view.batch) + reads = _NumericReads( + tuple(batches), + lambda: ( + sim.physics_step_index, + id(robot._data.articulation_view.scene), + robot._data.articulation_view.scene.topology_revision, + ), + ) + previous = _ACTIVE.get() + if previous is not None: + previous.values.clear() + token = _ACTIVE.set(reads) + try: + yield + finally: + reads.values.clear() + reads.closed = True + reads.batches.clear() + reads.revision = None + _ACTIVE.reset(token) + if previous is not None: + previous.values.clear() diff --git a/embodichain/lab/sim/objects/backends/scene.py b/embodichain/lab/sim/objects/backends/scene.py index 1a7967773b..3ea1daf78c 100644 --- a/embodichain/lab/sim/objects/backends/scene.py +++ b/embodichain/lab/sim/objects/backends/scene.py @@ -34,6 +34,7 @@ import torch from .base import ArticulationViewBase, RigidBodyViewBase +from ._numeric_observations import _ACTIVE if TYPE_CHECKING: from dexsim.scene import ( @@ -57,12 +58,20 @@ def _checked_batch_call( **kwargs: Any, ) -> Any: """Call one Scene batch operation and reject native failure statuses.""" + reads = _ACTIVE.get() + key = None if reads is None else reads.key(batch, method_name, args, kwargs) + if key is not None and key in reads.values: + saved, status = reads.values[key] + args[0].copy_(saved) + return status status = getattr(batch, method_name)(*args, **kwargs) if isinstance(status, Integral) and status < 0: raise RuntimeError( f"DexSim Scene batch operation {method_name!r} failed with " f"status {status}." ) + if key is not None: + reads.values[key] = (args[0].detach().clone(), status) return status diff --git a/embodichain/lab/sim/objects/rigid_object.py b/embodichain/lab/sim/objects/rigid_object.py index edb21f9eb7..c38d10eebd 100644 --- a/embodichain/lab/sim/objects/rigid_object.py +++ b/embodichain/lab/sim/objects/rigid_object.py @@ -36,6 +36,10 @@ is_newton_scene, ) from embodichain.lab.sim.objects.backends.base import RigidBodyViewBase +from embodichain.lab.sim.objects.backends._numeric_observations import ( + _ACTIVE as _NUMERIC_READS, + shared_pose, +) from embodichain.lab.sim.objects.backends.rigid_physics import ( _set_default_spawn_friction, apply_legacy_physical_attr, @@ -853,6 +857,21 @@ def get_local_pose_cpu( if self.is_static: return get_local_pose_cpu(self._entities, to_matrix).to(self.device) + if ( + type(self) is RigidObject + and type(to_matrix) is bool + and self._data is not None + and _NUMERIC_READS.get() is not None + ): + return shared_pose( + self._data.body_view.batch, + to_matrix, + lambda: RigidObject._read_dynamic_local_pose(self, to_matrix), + ) + return RigidObject._read_dynamic_local_pose(self, to_matrix) + + def _read_dynamic_local_pose(self, to_matrix: bool) -> torch.Tensor: + """Convert the current native pose without sharing caller-owned storage.""" pose = self.body_data.pose.clone() if to_matrix: xyz = pose[:, :3] diff --git a/embodichain/toolkits/graspkit/pg_grasp/gripper_collision_checker.py b/embodichain/toolkits/graspkit/pg_grasp/gripper_collision_checker.py index 7c2f41a69a..2dd6230673 100644 --- a/embodichain/toolkits/graspkit/pg_grasp/gripper_collision_checker.py +++ b/embodichain/toolkits/graspkit/pg_grasp/gripper_collision_checker.py @@ -16,6 +16,9 @@ from __future__ import annotations +from contextlib import contextmanager, nullcontext +from collections.abc import Iterator, Callable + import numpy as np import open3d as o3d import torch @@ -28,6 +31,60 @@ __all__ = ["GripperCollisionCfg", "GripperCollisionChecker"] +_NATIVE_POINT_QUERY = ConvexCollisionChecker.query_batch_points + + +@contextmanager +def _cpu_query_device( + checker: ConvexCollisionChecker, +) -> Iterator[tuple[torch.device, Callable]]: + """Keep a query's existing CUDA computation resident with bounded scratch. + + CPU transforms, every surface sample and the signed-distance kernel are + unchanged. Inputs are copied afresh per query; no mutable geometry is cached + across calls. Scratch is released on its owning stream before scope exit. + """ + import warp as wp + from embodichain.compute.geometry._warp.convex_query import ( + convex_signed_distance_kernel, + ) + + device = torch.device("cuda", torch.cuda.current_device()) + planes = checker.plane_equations["plane_equations"].to(device).clone() + counts = checker.plane_equations["plane_equation_counts"].to(device).clone() + workspace = None + + def query( + points: torch.Tensor, *, collision_threshold: float + ) -> tuple[torch.Tensor, torch.Tensor]: + nonlocal workspace + gpu_points = points.to(device) + shape = (*points.shape[:2], planes.shape[0]) + if workspace is None or workspace.shape != shape: + workspace = None + workspace = wp.empty(shape, dtype=wp.float32, device=str(device)) + workspace.fill_(-float("inf")) + wp.launch( + convex_signed_distance_kernel, + dim=shape, + inputs=( + wp.from_torch(gpu_points), + wp.from_torch(planes), + wp.from_torch(counts), + ), + outputs=(workspace,), + device=str(device), + ) + distance = wp.to_torch(workspace).min(dim=-1).values + return distance <= collision_threshold, distance + + with wp.ScopedStream(wp.stream_from_torch(torch.cuda.current_stream(device))): + try: + yield device, query + finally: + workspace = None + + @configclass class GripperCollisionCfg: """Target-object decomposition settings, independent of gripper geometry.""" @@ -76,6 +133,10 @@ def query( ) -> tuple[torch.Tensor, torch.Tensor]: """Reduce finite mesh-surface queries at canonical grasp poses. + CPU inputs retain CPU transforms and the existing CUDA classification. + Native multi-chunk queries keep geometry and reductions on that device + for one call; every original sample and chunk bound is preserved. + Args: obj_pose: World pose of the target mesh, shape ``(4, 4)``. grasp_poses: World canonical contact poses, shape ``(B, 4, 4)``. @@ -111,44 +172,62 @@ def _query_chunks( """Reduce all samples with bounded candidate/point/hull intermediates.""" relative = torch.linalg.inv(obj_pose).unsqueeze(0) @ grasp_poses count = len(grasp_poses) - collided = torch.zeros(count, dtype=torch.bool, device=self.device) - distance = torch.full( - (count,), float("inf"), device=self.device, dtype=grasp_poses.dtype - ) ground = ( self.get_ground_height(obj_pose) if is_filter_ground_collision else None ) # Visualization holds at most one pose's surface and colors. batch_size = 1 if is_visual else candidate_chunk_size samples = self.surface_points.to(dtype=grasp_poses.dtype) - for start in range(0, count, batch_size): - end = min(start + batch_size, count) - display_points, display_flags = [], [] - for points in samples.split(point_chunk_size): - local = transform_points_mat(points, relative[start:end]) - flags, distances = self._checker.query_batch_points( - local, collision_threshold=collision_threshold - ) - if ground is not None: - world = transform_points_mat(points, grasp_poses[start:end]) - ground_distance = world[:, :, 2] - ground - flags = flags | (ground_distance < collision_threshold) - distances = torch.minimum(distances, ground_distance) - collided[start:end] |= flags.any(dim=1) - distance[start:end] = torch.minimum( - distance[start:end], distances.min(dim=1).values - ) + native_cpu = ( + count > 0 + and ( + count > candidate_chunk_size + or len(self.surface_points) > point_chunk_size + ) + and self.device.type == "cpu" + and not is_visual + and type(self._checker) is ConvexCollisionChecker + and getattr(self._checker.query_batch_points, "__func__", None) + is _NATIVE_POINT_QUERY + ) + scope = ( + _cpu_query_device(self._checker) + if native_cpu + else nullcontext((self.device, self._checker.query_batch_points)) + ) + with scope as (query_device, point_query): + collided = torch.zeros(count, dtype=torch.bool, device=query_device) + distance = torch.full( + (count,), float("inf"), device=query_device, dtype=grasp_poses.dtype + ) + for start in range(0, count, batch_size): + end = min(start + batch_size, count) + display_points, display_flags = [], [] + for points in samples.split(point_chunk_size): + local = transform_points_mat(points, relative[start:end]) + flags, distances = point_query( + local, collision_threshold=collision_threshold + ) + if ground is not None: + world = transform_points_mat(points, grasp_poses[start:end]) + ground_distance = (world[:, :, 2] - ground).to(query_device) + flags = flags | (ground_distance < collision_threshold) + distances = torch.minimum(distances, ground_distance) + collided[start:end] |= flags.any(dim=1) + distance[start:end] = torch.minimum( + distance[start:end], distances.min(dim=1).values + ) + if is_visual: + display_points.append(local[0].cpu().numpy()) + display_flags.append(flags[0].cpu().numpy()) if is_visual: - display_points.append(local[0].cpu().numpy()) - display_flags.append(flags[0].cpu().numpy()) - if is_visual: - cloud = o3d.geometry.PointCloud() - displayed = np.concatenate(display_points) - flags_np = np.concatenate(display_flags) - cloud.points = o3d.utility.Vector3dVector(displayed) - colors = np.zeros_like(displayed) - colors[flags_np, 0] = 1 - colors[~flags_np, 1] = 1 - cloud.colors = o3d.utility.Vector3dVector(colors) - o3d.visualization.draw_geometries([self._checker.mesh, cloud]) - return collided, distance + cloud = o3d.geometry.PointCloud() + displayed = np.concatenate(display_points) + flags_np = np.concatenate(display_flags) + cloud.points = o3d.utility.Vector3dVector(displayed) + colors = np.zeros_like(displayed) + colors[flags_np, 0] = 1 + colors[~flags_np, 1] = 1 + cloud.colors = o3d.utility.Vector3dVector(colors) + o3d.visualization.draw_geometries([self._checker.mesh, cloud]) + return collided.to(self.device), distance.to(self.device) diff --git a/scripts/benchmark/expansion/runtime_comparison.py b/scripts/benchmark/expansion/runtime_comparison.py new file mode 100644 index 0000000000..8d8c00d0cd --- /dev/null +++ b/scripts/benchmark/expansion/runtime_comparison.py @@ -0,0 +1,449 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Compare sensor-free Task Program execution on pinned source trees. + +Run with --help. Results measure execution, not qualified dataset production. +""" + +from __future__ import annotations +import argparse +import json +import os +from pathlib import Path +import sys +import time +import resource +import subprocess +import cProfile +import pstats + + +def write_report(path: Path, rows: list[dict]) -> None: + """Write three tables, explicitly separating program acceptance from quality.""" + text = [ + "# Sensor-free execution diagnostic", + "", + ( + "Profiler enabled: these times diagnose hotspots and must not be compared as benchmark latency." + if any(row.get("profiled") for row in rows) + else "Profiler disabled." + ), + "", + "Cold execution; CPU Default physics and solvers, with legacy CUDA grasp classification included. Program acceptance is not physical qualification. No sensors, recording, external collision checks or persistence. Process RSS peaks include startup; their sum is an upper bound, not a simultaneous sampled peak.", + "", + "## Time & Memory", + "", + "| Scale | Mode | cost_time_ms | Action rows/s | Sum env.step s | Sum sim.update s | Sum peak RSS MiB | cpu_delta_mb | gpu_delta_mb | peak_gpu_mb |", + "|---|---|---:|---:|---:|---:|---:|---|---|---|", + ] + for row in rows: + text.append( + f"| {row['scale']} | {row['mode']} | {row['execution_s']*1000:.3f} | {row['action_rows']/row['execution_s']:.1f} | {row['env_step_s']:.3f} | {row['sim_update_s']:.3f} | {row['peak_rss_mib']:.1f} | N/A | N/A | N/A |" + ) + text += [ + "", + "## Success & Other Metrics", + "", + "| Scale | Mode | Attempts | Program accepted | Action rows |", + "|---|---|---:|---:|---:|", + ] + for row in rows: + text.append( + f"| {row['scale']} | {row['mode']} | {row['attempts']} | {row['accepted']} | {row['action_rows']} |" + ) + text += [ + "", + "## Leaderboard", + "", + "Ranked by program acceptance fraction only; modes with different action counts are not equal-work latency comparisons.", + "", + "| Rank | Scale | Mode | Program acceptance |", + "|---:|---|---|---:|", + ] + for i, row in enumerate( + sorted(rows, key=lambda r: r["accepted"] / max(r["attempts"], 1), reverse=True), + 1, + ): + text.append( + f"| {i} | {row['scale']} | {row['mode']} | {row['accepted']/max(row['attempts'],1):.3f} |" + ) + path.write_text("\n".join(text) + "\n") + + +def matrix_main() -> None: + """Run variants sequentially, with concurrent main workers per scale.""" + p = argparse.ArgumentParser() + p.add_argument("--matrix", action="store_true") + p.add_argument("--baseline-root", type=Path, required=True) + p.add_argument("--pr-root", type=Path, required=True) + p.add_argument("--output-dir", type=Path, required=True) + p.add_argument("--waves", type=int, default=2) + p.add_argument("--seeds", default="7") + p.add_argument("--scales", default="small,medium,large") + p.add_argument("--order", choices=("forward", "reverse"), default="forward") + a = p.parse_args() + a.output_dir.mkdir(parents=True, exist_ok=False) + cpus = sorted(os.sched_getaffinity(0))[:8] + os.sched_setaffinity(0, cpus) + settings = {"small": (1, 1), "medium": (2, 4), "large": (4, 8)} + rows = [] + for seed_index, seed in enumerate(map(int, a.seeds.split(","))): + for scale in a.scales.split(","): + w, b = settings[scale] + modes = ["cohort", "rolling-off", "rolling-on"] + if (seed_index % 2) ^ (a.order == "reverse"): + modes.reverse() + for mode in modes: + processes = [] + for worker in range(w if mode == "cohort" else 1): + name = f"{scale}-{mode}-seed{seed}-worker{worker}" + output = a.output_dir / (name + ".json") + log = (a.output_dir / (name + ".log")).open("w") + command = [ + sys.executable, + str(Path(__file__).resolve()), + "--root", + str(a.baseline_root if mode == "cohort" else a.pr_root), + "--mode", + mode, + "--rows", + str(b if mode == "cohort" else w * b), + "--groups", + str(1 if mode == "cohort" else w), + "--episodes", + str(a.waves * b if mode == "cohort" else a.waves * w * b), + "--seed", + str(seed), + "--output", + str(output), + ] + process = subprocess.Popen( + command, + stdout=log, + stderr=subprocess.STDOUT, + env={ + **os.environ, + "OMP_NUM_THREADS": "1", + "MKL_NUM_THREADS": "1", + "PYTHONHASHSEED": "0", + "SDL_VIDEODRIVER": "offscreen", + }, + ) + processes.append((process, log, output)) + failed = [] + for process, log, output in processes: + code = process.wait() + log.close() + if code: + failed.append(str(output.with_suffix(".log"))) + if failed: + raise RuntimeError(f"Workers failed: {failed}") + reports = [json.loads(output.read_text()) for _, _, output in processes] + episodes = [e for report in reports for e in report["episodes"]] + row = { + "scale": scale, + "mode": mode, + "seed": seed, + "execution_s": max(r["execution_end"] for r in reports) + - min(r["execution_start"] for r in reports), + "env_step_s": sum(r["env_step_s"] for r in reports), + "sim_update_s": sum(r["sim_update_s"] for r in reports), + "peak_rss_mib": sum(r["peak_rss_mib"] for r in reports), + "attempts": len(episodes), + "accepted": sum(e["program_accepted"] for e in episodes), + "action_rows": sum(e["steps"] for e in episodes), + } + rows.append(row) + (a.output_dir / "results.json").write_text( + json.dumps( + { + "cpu_affinity": cpus, + "initial_order": a.order, + "waves": a.waves, + "baseline_root": str(a.baseline_root), + "pr_root": str(a.pr_root), + "rows": rows, + }, + indent=2, + ) + ) + write_report(a.output_dir / "report.md", rows) + print(json.dumps(row), flush=True) + + +def main() -> None: + """Execute a bounded real-physics diagnostic in one isolated process.""" + p = argparse.ArgumentParser(description=__doc__) + p.add_argument("--root", type=Path, required=True) + p.add_argument( + "--mode", choices=["cohort", "rolling-off", "rolling-on"], required=True + ) + p.add_argument("--rows", type=int, default=1) + p.add_argument("--groups", type=int, default=1) + p.add_argument("--episodes", type=int, default=1) + p.add_argument("--seed", type=int, default=7) + p.add_argument("--output", type=Path, required=True) + p.add_argument( + "--profile", + action="store_true", + help="Profile execution only; these times are not benchmark results.", + ) + a = p.parse_args() + root = a.root.resolve() + sys.path[:0] = [ + str(root), + str(root / "extensions/embodichain_tasks"), + str(root / "extensions/embodichain_robots"), + ] + import torch + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + discover_task_packages, + execute_init_hooks, + ) + from embodichain.lab.sim import SimulationManager + from embodichain.utils.utility import load_config + from embodichain.lab.expansion.program import collect_program_batch + + torch.set_num_threads(1) + path = ( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task.ur5.yaml" + ) + discover_task_packages() + execute_init_hooks() + cfg = config_to_cfg(load_config(path), source_path=path) + cfg.num_envs = cfg.sim_cfg.num_envs = a.rows + cfg.sim_cfg.device = "cpu" + cfg.sim_cfg.headless = True + cfg.sim_cfg.render_cfg.renderer = "no-render" + cfg.sensor = [] + cfg.enable_sensor = False + cfg.dataset = None + cfg.init_rollout_buffer = cfg.record_trajectory = False + timings = {"env_step_s": 0.0, "reset_s": 0.0} + sim_time = [0.0] + episodes = [] + adapters = {} + env = None + start = time.perf_counter() + try: + env = REGISTERED_ENVS[load_config(path)["id"]].make(cfg=cfg) + assert env.sim.physics_backend == "default" + assert not env.sim.get_sensor_uid_list(), env.sim.get_sensor_uid_list() + env.reset(seed=a.seed) + program = env.compile_task_program(cfg.task_program) + step = env.step + reset = env.reset + + def timed_step(*args, **kwargs): + tick = time.perf_counter() + try: + return step(*args, **kwargs) + finally: + timings["env_step_s"] += time.perf_counter() - tick + + def timed_reset(*args, **kwargs): + tick = time.perf_counter() + try: + return reset(*args, **kwargs) + finally: + timings["reset_s"] += time.perf_counter() - tick + + env.step = timed_step + env.reset = timed_reset + update = env.sim.update + + def timed_update(*args, **kwargs): + tick = time.perf_counter() + try: + return update(*args, **kwargs) + finally: + sim_time[0] += time.perf_counter() - tick + + env.sim.update = timed_update + setup = time.perf_counter() - start + + def bridge(row): + adapter = env.task_program_adapter.for_env_ids((row,)) + adapters[row] = adapter + return adapter.create_bridge(program, isolate_policy_sessions=True) + + def completed(result): + for row, length, accepted, reason in zip( + result.env_ids, result.lengths, result.accepted, result.reasons + ): + episodes.append( + { + "row": row, + "steps": length, + "program_accepted": accepted, + "reason": reason, + } + ) + adapters.pop(row).close() + + profiler = cProfile.Profile() if a.profile else None + cpu_before = resource.getrusage(resource.RUSAGE_SELF) + if profiler is not None: + profiler.enable() + begin = time.perf_counter() + before = env.sim.physics_step_index + planning = {} + if a.mode == "cohort": + remaining = a.episodes + while remaining: + n = min(a.rows, remaining) + env.reset(seed=a.seed, options={"save_data": False}) + # Same minimum settling, at ordinary environment cadence. + from embodichain.lab.gym.envs.types import ControllerAction + + physical = env.sim.physics_step_index + while env.sim.physics_step_index - physical < 5: + env.step(ControllerAction(env.robot.get_qpos(target=True).clone())) + env._elapsed_steps[:] = 0 + env._reset_physical_objective(tuple(range(n))) + result = collect_program_batch( + env, + {r: bridge(r) for r in range(n)}, + max_steps=1200, + observe_step=lambda *args: None, + ) + completed(result) + remaining -= n + else: + from embodichain.lab.expansion.workloads import ( + WorkloadProgramExecutor, + WorkloadExecutionCfg, + ExpansionWorkloadCfg, + ) + + class Source: + def prepare(self, assignments): + pass + + def activate(self, assignment, observation): + return bridge(assignment.env_id) + + def started(self, assignment, boundary): + pass + + def observe(self, assignments, action, observation): + return () + + def complete(self, result): + completed(result.program) + # No persistence in this diagnostic: never fabricate receipts. + return None + + assert a.rows % a.groups == 0 and a.episodes % a.groups == 0 + count = a.episodes // a.groups + executor = WorkloadProgramExecutor( + env, + [ + ExpansionWorkloadCfg(f"g{i}", count, count, 3600) + for i in range(a.groups) + ], + Source(), + WorkloadExecutionCfg( + workload_count=a.groups, + batch_size=a.rows // a.groups, + max_steps=1200, + settling_steps=5, + seed=a.seed, + batch_planning=a.mode == "rolling-on", + ), + ) + outcome = executor.run() + planning = outcome["planning"] + elapsed = time.perf_counter() - begin + if profiler is not None: + profiler.disable() + cpu_after = resource.getrusage(resource.RUSAGE_SELF) + report = { + "mode": a.mode, + "rows": a.rows, + "seed": a.seed, + "setup_s": setup, + "execution_s": elapsed, + "profiled": a.profile, + "physics_backend": env.sim.physics_backend, + "physics_device": str(env.sim.device), + "solver_devices": { + name: str(solver.device) for name, solver in env.robot._solvers.items() + }, + "grasp_collision_device_policy": "legacy current CUDA device", + "cuda_visible_devices": os.environ.get("CUDA_VISIBLE_DEVICES"), + "cuda_device_after_execution": ( + torch.cuda.current_device() if torch.cuda.is_initialized() else None + ), + "process_cpu_s": cpu_after.ru_utime + + cpu_after.ru_stime + - cpu_before.ru_utime + - cpu_before.ru_stime, + "execution_start": begin, + "execution_end": begin + elapsed, + "sim_update_s": sim_time[0], + "peak_rss_mib": resource.getrusage(resource.RUSAGE_SELF).ru_maxrss / 1024, + **timings, + "remaining_execution_s": elapsed - sum(timings.values()), + "physics_substeps": env.sim.physics_step_index - before, + "episodes": episodes, + "planning": planning, + "sensors": env.sim.get_sensor_uid_list(), + "scope": "cold execution including planning, preparation and env.step; no sensors, external collision qualification, media, artifact persistence or export; program acceptance is not physical qualification", + } + a.output.parent.mkdir(parents=True, exist_ok=True) + if profiler is not None: + profiler.dump_stats(str(a.output.with_suffix(".pstats"))) + with a.output.with_suffix(".profile.txt").open("w") as stream: + stream.write( + "Profiled execution only. Cumulative times overlap; greenlet switching can distort caller attribution. Use leaf self-times and call counts, not sums of cumulative times.\n" + ) + stats = pstats.Stats(profiler, stream=stream) + stats.sort_stats("tottime").print_stats(70) + stats.sort_stats("cumulative").print_stats(70) + a.output.write_text(json.dumps(report, indent=2)) + write_report( + a.output.with_suffix(".md"), + [ + { + **report, + "scale": str(a.rows), + "attempts": len(episodes), + "accepted": sum(e["program_accepted"] for e in episodes), + "action_rows": sum(e["steps"] for e in episodes), + } + ], + ) + print( + json.dumps( + {k: v for k, v in report.items() if k not in ("episodes", "planning")} + ), + flush=True, + ) + finally: + for adapter in adapters.values(): + adapter.close() + if env is not None: + env.close(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +if __name__ == "__main__": + matrix_main() if "--matrix" in sys.argv else main() diff --git a/tests/sim/atomic_skills/test_core.py b/tests/sim/atomic_skills/test_core.py index 26cc228128..7c8f36ebc2 100644 --- a/tests/sim/atomic_skills/test_core.py +++ b/tests/sim/atomic_skills/test_core.py @@ -1039,8 +1039,12 @@ def test_public_plan_authorizes_raw_action_plan_destinations() -> None: action.plan(request, context) -def test_command_target_authorization_rejects_altered_joint_claims() -> None: +@pytest.mark.parametrize( + "device", ["cpu", pytest.param("cuda:0", marks=pytest.mark.gpu)] +) +def test_command_target_authorization_rejects_altered_joint_claims(device: str) -> None: context = _context() + env_ids = context.env_ids.to(device) request = ResolvedSkillRequest( skill_id="dependency_test", goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), @@ -1065,20 +1069,42 @@ def test_command_target_authorization_rejects_altered_joint_claims() -> None: commands=( EndpointCommand( target=JointPositionTarget("arm", (2, 3)), - payload=JointPositionPayload(torch.ones(2, 2)), + payload=JointPositionPayload(torch.ones(2, 2, device=device)), ), ), - active_mask=torch.ones(2, dtype=torch.bool), - env_ids=context.env_ids, - hold_duration=torch.full((2,), 0.1), + active_mask=torch.ones(2, dtype=torch.bool, device=device), + env_ids=env_ids, + hold_duration=torch.full((2,), 0.1, device=device), ) with pytest.raises(ValueError, match="bound joint-position target"): _DependencyAction._authorize_command_targets( request, - TimedCommandSequence(frames=(frame,), env_ids=context.env_ids), + TimedCommandSequence(frames=(frame,), env_ids=env_ids), ) + commands = _command_sequence(env_ids=env_ids, frame_count=2) + owned = _DependencyAction._authorize_command_targets(request, commands) + commands.frames[0].commands[0].payload.positions.fill_(99) + torch.testing.assert_close( + owned.frames[0].commands[0].payload.positions, torch.ones(2, 2, device=device) + ) + owned.frames[0].commands[0].payload.positions.fill_(55) + torch.testing.assert_close( + owned.frames[1].commands[0].payload.positions, + torch.full((2, 2), 2.0, device=device), + ) + owned.frames[0].commands[0].payload.positions[0, 0] = float("nan") + with pytest.raises(ValueError, match="finite"): + owned.snapshot() + commands.frames[1].commands[0].payload.positions[0, 0] = float("nan") + with pytest.raises(ValueError, match="finite"): + _DependencyAction._authorize_command_targets(request, commands) + commands.frames[1].commands[0].payload.positions.fill_(2) + commands.frames[0].hold_duration.fill_(-1) + with pytest.raises(ValueError, match="non-negative"): + _DependencyAction._authorize_command_targets(request, commands) + def test_command_target_authorization_rejects_custom_claim_conflicts() -> None: context = _context() diff --git a/tests/sim/atomic_skills/test_runtime_commands.py b/tests/sim/atomic_skills/test_runtime_commands.py index 14ad1a673c..45c3ed4b95 100644 --- a/tests/sim/atomic_skills/test_runtime_commands.py +++ b/tests/sim/atomic_skills/test_runtime_commands.py @@ -411,3 +411,32 @@ def test_timed_command_sequence_requires_nonempty_int64_batch() -> None: TimedCommandSequence(frames=(), env_ids=torch.tensor([0.0])) with pytest.raises(ValueError, match="unique"): TimedCommandSequence(frames=(), env_ids=torch.tensor([2, 2])) + + +def test_joint_target_snapshot_revalidates_and_preserves_custom_ownership() -> None: + original = JointPositionTarget("arm", (0, 1)) + copied = original.snapshot() + assert copied == original and copied is not original + object.__setattr__(original, "joint_ids", (0, 0)) + with pytest.raises(ValueError, match="unique"): + original.snapshot() + + @dataclass(frozen=True, slots=True) + class ExtendedTarget(JointPositionTarget): + metadata: list[int] + + custom = ExtendedTarget("arm", (0, 1), [3]) + snapshot = custom.snapshot() + assert type(snapshot) is ExtendedTarget + snapshot.metadata[0] = 7 + assert custom.metadata == [3] + + class AnnotatedName(str): + pass + + name = AnnotatedName("arm") + name.annotations = [1] + named = JointPositionTarget(name, (0, 1)) + named_copy = named.snapshot() + named_copy.control_part.annotations.append(2) + assert named.control_part.annotations == [1] diff --git a/tests/sim/objects/test_scene_backend.py b/tests/sim/objects/test_scene_backend.py index ede7d95d4e..52697d00b4 100644 --- a/tests/sim/objects/test_scene_backend.py +++ b/tests/sim/objects/test_scene_backend.py @@ -1400,3 +1400,105 @@ class RebuiltScene: rigid.get_friction([0]) assert factory.call_count == 2 assert factory.call_args.args[0] is rebuilt + + +def test_numeric_boundary_owns_samples_and_invalidates_writes_and_revisions() -> None: + from contextvars import copy_context + from embodichain.lab.sim.objects.backends._numeric_observations import ( + numeric_observation_scope, + shared_pose, + ) + from embodichain.lab.sim.objects.backends.scene import _checked_batch_call + + class Batch: + def __init__(self): + self.value = torch.ones(2, 3) + self.reads = 0 + + def fetch_joint_position(self, out): + self.reads += 1 + out.copy_(self.value) + return 2 + + def apply_joint_position(self, value): + self.value.copy_(value) + return 2 + + def apply_failed(self): + self.value.fill_(9) + raise RuntimeError("partial native write") + + batch = Batch() + sim = SimpleNamespace( + physics_backend="default", + device=torch.device("cpu"), + physics_step_index=0, + _spawn_scene=SimpleNamespace(topology_revision=0), + get_rigid_object_uid_list=lambda: (), + ) + robot = SimpleNamespace( + _data=SimpleNamespace( + articulation_view=SimpleNamespace(batch=batch, scene=sim._spawn_scene) + ) + ) + + def read(): + out = torch.empty(2, 3) + assert _checked_batch_call(batch, "fetch_joint_position", out) == 2 + return out + + with numeric_observation_scope(sim, robot): + escaped = copy_context() + first = read() + first.fill_(77) + torch.testing.assert_close(copy_context().run(read), torch.ones(2, 3)) + assert batch.reads == 1 + derived_calls = [] + + def derive(): + derived_calls.append(True) + return batch.value @ torch.eye(3) + 10 + + derived = shared_pose(batch, True, derive) + derived.fill_(88) + torch.testing.assert_close( + shared_pose(batch, True, derive), torch.full((2, 3), 11.0) + ) + assert len(derived_calls) == 1 + torch.testing.assert_close( + shared_pose(batch, False, lambda: batch.value.clone()), torch.ones(2, 3) + ) + _checked_batch_call(batch, "apply_joint_position", torch.full((2, 3), 2.0)) + torch.testing.assert_close(read(), torch.full((2, 3), 2.0)) + assert batch.reads == 2 + torch.testing.assert_close( + shared_pose(batch, True, derive), torch.full((2, 3), 12.0) + ) + assert len(derived_calls) == 2 + with torch.autocast("cpu", dtype=torch.bfloat16): + torch.testing.assert_close( + shared_pose(batch, True, derive), + torch.full((2, 3), 12.0, dtype=torch.bfloat16), + ) + assert len(derived_calls) == 3 + sim.physics_step_index += 1 + batch.value.fill_(3) + assert read()[0, 0] == 3 and batch.reads == 3 + sim._spawn_scene.topology_revision += 1 + batch.value.fill_(4) + assert read()[0, 0] == 4 and batch.reads == 4 + with pytest.raises(RuntimeError, match="partial native"): + _checked_batch_call(batch, "apply_failed") + assert read()[0, 0] == 9 and batch.reads == 5 + with numeric_observation_scope(sim, robot): + _checked_batch_call(batch, "apply_joint_position", torch.full((2, 3), 6.0)) + assert read()[0, 0] == 6 + assert read()[0, 0] == 6 and batch.reads == 7 + # Same physics tick, new boundary: reset/prepare writes are never reused. + batch.value.fill_(8) + with numeric_observation_scope(sim, robot): + assert read()[0, 0] == 8 and batch.reads == 8 + batch.value.fill_(10) + assert escaped.run(read)[0, 0] == 10 and batch.reads == 9 + batch.value.fill_(11) + assert escaped.run(read)[0, 0] == 11 and batch.reads == 10 diff --git a/tests/toolkits/test_fixed_gripper_geometry.py b/tests/toolkits/test_fixed_gripper_geometry.py index 27f1d5b1c4..260da1014e 100644 --- a/tests/toolkits/test_fixed_gripper_geometry.py +++ b/tests/toolkits/test_fixed_gripper_geometry.py @@ -232,3 +232,64 @@ def test_authored_collision_box_preserves_physical_pad_dimensions( assert geometry.provenance["parts"][0]["geometry_kind"] == "urdf_box" assert geometry.provenance["parts"][0]["mesh_path"] is None assert len(geometry.provenance["files_sha256"]) == 1 + + +@pytest.mark.gpu +def test_native_cpu_chunks_keep_all_samples_and_owned_results_across_streams() -> None: + import warp as wp + + wp.init() + from types import SimpleNamespace + from embodichain.toolkits.graspkit.pg_grasp.collision_checker import ( + ConvexCollisionChecker, + ) + + convex = object.__new__(ConvexCollisionChecker) + convex.device = torch.device("cpu") + normals = torch.cat((torch.eye(3), -torch.eye(3))) + convex.plane_equations = { + "plane_equations": torch.cat((normals, torch.full((6, 1), -0.1)), dim=1)[None], + "plane_equation_counts": torch.tensor([6], dtype=torch.int32), + } + checker = object.__new__(GripperCollisionChecker) + checker.device = torch.device("cpu") + checker._checker = convex + checker.obj_mesh_verts = torch.zeros(1, 3, dtype=torch.float64) + x = torch.linspace(-0.3, 0.3, 1025) + checker.surface_points = torch.stack((x, x.flip(0), x * 0.5), dim=-1) + poses = torch.eye(4).repeat(258, 1, 1)[::2] + poses[:, 0, 3] = torch.linspace(0, 0.5, len(poses)) + stream = torch.cuda.Stream() + for ground in (False, True): + for threshold in (0.0, -0.02, 0.02): + checker._checker = SimpleNamespace( + query_batch_points=convex.query_batch_points + ) + expected = checker.query( + torch.eye(4), + poses, + collision_threshold=threshold, + is_filter_ground_collision=ground, + ) + checker._checker = convex + with torch.cuda.stream(stream): + actual = checker.query( + torch.eye(4), + poses, + collision_threshold=threshold, + is_filter_ground_collision=ground, + ) + for value, reference in zip(actual, expected): + assert value.device.type == "cpu" + torch.testing.assert_close(value, reference, rtol=0, atol=0) + # Each query observes new geometry, and later scratch reuse cannot alter + # already returned results. Expand the box to encompass a distant pose. + old = checker.query(torch.eye(4), poses, is_filter_ground_collision=False) + saved = tuple(value.clone() for value in old) + convex.plane_equations["plane_equations"][..., 3] = -1.0 + changed = checker.query(torch.eye(4), poses, is_filter_ground_collision=False) + assert changed[0].all() and not old[0].all() + for value, reference in zip(old, saved): + torch.testing.assert_close(value, reference, rtol=0, atol=0) + empty = checker.query(torch.eye(4), poses[:0], is_filter_ground_collision=False) + assert empty[0].numel() == empty[1].numel() == 0 From 504f44f7152cfdf67024f6f8cd2f6f6737e5f79f Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sun, 15 Nov 2026 08:45:13 +0000 Subject: [PATCH 10/45] docs(expansion): document native ghost preview and WebRTC follow-up --- examples/expansion/engine/README.md | 8 ++ examples/expansion/engine/ghost-preview.md | 122 +++++++++++++++++++++ 2 files changed, 130 insertions(+) create mode 100644 examples/expansion/engine/ghost-preview.md diff --git a/examples/expansion/engine/README.md b/examples/expansion/engine/README.md index 1a01cc0565..2b902ff70a 100644 --- a/examples/expansion/engine/README.md +++ b/examples/expansion/engine/README.md @@ -24,6 +24,14 @@ for measured 3D/RGB-D comparisons, per-capability checks, recovery diagnostics a portable dataset verification. `--mode quick` samples a smaller subset; unrun capabilities remain explicit. `--reuse` rebuilds the offline page without a World. +## Native ghost preview + +See [the native ghost-preview notes](ghost-preview.md) for the existing SDK +window comparison, the separately validated eight-result browser prototype, +linked spatial/affordance/trajectory filters, and the native WebRTC follow-up. +The prototype uses native offscreen rendering with HTTP JPEG transport; it is +not a WebRTC entry point or a released general-purpose viewer. + ## Run a configured job From the repository root, with the simulation dependencies installed: diff --git a/examples/expansion/engine/ghost-preview.md b/examples/expansion/engine/ghost-preview.md new file mode 100644 index 0000000000..3d538376d0 --- /dev/null +++ b/examples/expansion/engine/ghost-preview.md @@ -0,0 +1,122 @@ +# 扩增分布的原生虚影预览 + +本文记录空间、affordance 与轨迹分布的交互预览方案,以及 2026-11-15 完成的 +小规模原型验收。它补充 [Expansion Engine](README.md) 的结果查看流程。 + +## 当前能力与边界 + +| 路径 | 渲染与交互 | 状态 | +| --- | --- | --- | +| SDK 原生窗口对照 | `preview_expansion_comparison` 加载已验收轨迹,显示参考与选中虚影;N 切换、Space 暂停、Q 关闭 | 仓库已有能力,见 [comparison.py](../../../embodichain/lab/expansion/comparison.py) | +| 独立浏览器原型 | EmbodiChain 原生离屏渲染完整模型;HTTP JPEG 传图,HTTP 控制筛选与回放 | 已完成本地验收,尚未整理为仓库通用入口 | +| 原生 WebRTC 推流 | 复用 EmbodiChain 的原生推流生命周期与控制接口 | TODO,尚未接入本预览 | + +“原生渲染”指场景由 `SimulationManager`、`RobotMarkers`、 +`VisualizationMarkers` 和 DexSim Hybrid 渲染器生成,**不表示采用 WebRTC**。 +浏览器原型没有自行绘制机器人骨架,也不使用 Viser 重建场景;它显示原生离屏 +相机输出的 JPEG。本次使用 OptiX 去噪。展示灯光、视角和透明材质与采集时可以不同, +不能把预览图片当作采集传感器的原始观测。 + +独立原型使用采集时冻结的源码 `ab04d972b5b75042434498db9a889b4edd03b6fe`。 +本文新增的是说明与验收记录,不把本地脚本、机器路径或录制产物包装成已发布 SDK API。 +[showcase](showcase/README.md) 的离线三维/RGB-D 页面也是另一条查看路径。 + +## 配方与真实结果 + +使用已有 `task.default.yaml` 的单轮 UR5 + DH PGI 抓取放置任务和 default 物理后端。 +原型关闭传感器媒体采集与数据集导出,保留原有 lift/place/release 等独立校验。 + +| 维度 | 小规模设置 | +| --- | --- | +| 参考 | 方块初始位置 `[-0.42, -0.08, 0.025]` m,1 条轨迹 | +| 空间 S-A | `[-0.435, -0.08, 0.025]` m | +| 空间 S-B | `[-0.405, -0.08, 0.025]` m | +| Affordance | 每个空间 2 个抓取配方分支;`max_proposals: 8` | +| 轨迹 | 每个分支 2 个变体:名义规划和 `joint_residual`,扰动尺度 `0.002` | +| 扩增总数 | 2 × 2 × 2 = 8 条,另加 1 条参考 | +| 执行 | 扩增批大小 2,参考批大小 1,随机种子 7 | + +两个空间位置通过分别冻结的环境配置定义;本次没有启用场景随机化算子, +也没有验证场景扩增引擎的分布采样。2 个位置不能代表连续空间覆盖率。 + +9 条轨迹均通过原有校验并确认回执,共保留 3642 个原始状态帧。 +该结果只代表这个固定小配方,不代表更大规模、其他任务或其他后端的资格结论。 + +## 数据到画面的运行逻辑 + +1. 读取 `ArtifactEpisodeSink` 的确认回执,并通过 `load_expansion_episode` + 加载已验收的真实结果;不重新规划或执行扩增。 +2. 核对 episode 身份、机器人模型、关节名称与顺序,以及动作和状态的 T+1 对齐。 + 加载逐帧完整关节位置、机器人根位姿、物体位姿和时间戳。 +3. 使用与记录匹配的 URDF 和实测关节计算 FK,驱动 `RobotMarkers` 的完整视觉网格。 + 选中样本显示原始模型外观,其余可见候选显示半透明虚影;参考单独开关。 +4. 用真实物体位姿驱动物体模型,并通过 `VisualizationMarkers` 显示初始位置、 + 抓取目标坐标轴和末端路径。 +5. 在仿真/渲染所有者线程应用筛选与回放状态,调用原生相机渲染。 + HTTP 线程只排队控制消息和读取已发布画面,不直接访问仿真对象。 +6. 页面读取 JPEG 并展示对应记录信息。关闭服务时释放相机、场景和监听端口; + 已提交轨迹与校验结果保持不变。 + +原型回放不调用物理推进,验收记录的物理步数为 0。回放不是新物理成功样本, +也不生成新的成功回执。 + +## 联动筛选与时间语义 + +空间、affordance、轨迹三个筛选器取交集,同时作用于候选列表、虚影、物体、 +初始位置、抓取目标和轨迹线。选中样本被筛掉时,选择交集内的一个有效样本。 +这个固定配方的组合完整,因此没有空交集;通用化时必须显式处理空结果。 +参考轨迹是独立对照,不计入 8 条候选数量,也不受候选筛选限制。 + +- **空间**:显示实际初始位置,不给样本添加人为排列偏移。 +- **Affordance**:A0/A1 是配方分支编号,不是底层候选 ID。读取规划诊断时按实际 + `env_id` 选择批内对应行,再取得抓取目标与候选 ID,不能默认使用第 0 行。 + 先用 `inverse(reference_pose) @ selected_pose` 得到物体局部抓取目标,再用 + 实际初始物体位姿转换到世界坐标。坐标轴展示规划目标,不是实测接触点; + 原型把它固定在初始抓取位置,不随搬运中的方块移动。 +- **轨迹**:V0/V1 来自执行元数据中的实际选择。末端路径由实测关节 FK 推导, + 原型轨迹线每 4 帧取点并保留末帧;机器人和物体回放仍可访问全部原始状态帧。 +- **时间**:支持公共真实时间或六个记录动作阶段的归一化对齐。每个样本取目标 + 时刻之前最近的已记录帧,不插值生成新姿态;短轨迹到末帧后保持。 + 阶段对齐只改变浏览进度映射,不改变原始时间戳。页面保留实际秒数和源帧号。 + +## 验收与当前限制 + +本地浏览器验收覆盖:筛选数量 8 → 4 → 2 → 1,筛选后原生画面改变, +时间拖动与播放、虚影开关、相机视角切换、窄屏布局和非法控制值拒绝。 +浏览器无脚本错误,回放全过程未推进物理。验收后服务已关闭,结果保留。 + +原型脚本及结果保存在本次实验的 `real-demo` 目录: + +| 文件 | 用途 | +| --- | --- | +| `recipe.yaml`、`configs/`、`results/` | 固定配方、环境设置、真实产物与回执 | +| `build_data.py`、`preview-data.json` | 从产物提取显示数据并验证对应关系 | +| `native_preview.py`、`native-preview.html` | 原生渲染服务和浏览器交互页面 | +| `native-assets/` | 对应采集模型的 URDF 与视觉资产副本 | +| `native-acceptance.json`、`native-preview-acceptance.png` | 浏览器验收结果和原生渲染截图 | + +这些文件不是仓库已安装入口。已有实验目录中可用兼容的 EmbodiChain/DexSim +Python 环境运行 `python native_preview.py --gpu GPU_ID --port PORT`,然后访问该机器的 +loopback 地址;远程访问需要端口转发。Ctrl+C 关闭服务。原型默认端口 8769, +默认渲染 GPU 4,设备编号需按部署环境调整;不要从仓库根目录直接执行此命令。 +原型仍依赖实验目录中的冻结源码、配置与产物路径,不是便携式数据查看器。 + +HTTP JPEG 轮询不提供 WebRTC 的编码、拥塞控制、连接恢复或低延迟保证。 +页面是单个服务的共享回放会话,多人操作会改变同一状态;尚未提供独立会话、 +认证、任意配方加载、大规模分页或通用资产恢复。当前不应对外公开监听。 + +## TODO:接入 EmbodiChain 原生 WebRTC 推流 + +后续直接复用 EmbodiChain 原生 WebRTC 能力,替换 HTTP JPEG 轮询;先核对实际 +推流入口、信令与关闭接口,不在本示例另建一套独立 WebRTC 栈。 +保留真实结果加载、原生场景、三类筛选联动、播放、时间轴及视角控制。 +控制通道的实现需按原生接口确定,不能预先宣称已经通过 DataChannel 接入。 + +验收条件: + +- 浏览器建立真实 WebRTC 连接并接收原生相机视频,确认画面不再依赖 JPEG 轮询。 +- 沿用这 8 条扩增结果和参考轨迹,复测筛选、拖动、暂停、切换视角及重新连接。 +- 控制状态、源帧和视频画面的对应关系可检查;显示延迟不冒充记录时间。 +- 回放物理步数仍为 0,不触发重采集、不修改原始证据或成功计数。 +- 关闭预览后释放推流、相机、端口及场景资源;记录端到端延迟与实际帧率, + 再据此报告改善,不能用传输协议名称推断性能。 From bddf4d72f821e3f0d68e615ee2e21669a5de436f Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sun, 15 Nov 2026 08:53:53 +0000 Subject: [PATCH 11/45] fix(physics): remove direct GPU API configuration override Restore the existing Default CUDA backend policy. Remove the newly introduced public selector and its ordinary-reporting test variant, retaining CPU contact permission coverage and documenting Direct GPU static-identity limits. Validation: 53 configuration/contact tests including CUDA and 42 context tests passed. Black 26.10.1, API docs and diff checks passed. --- .../topics/expansion-engine/workloads.md | 9 ++++----- .../topics/sensor-system/sensor-system.md | 5 ++--- embodichain/lab/sim/cfg/physics.py | 11 ----------- embodichain/lab/sim/physics/default.py | 2 +- tests/sim/sensors/test_contact_default_e2e.py | 16 ++++++---------- 5 files changed, 13 insertions(+), 30 deletions(-) diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index 6ca15ed63a..5c47c5d1a4 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -79,11 +79,10 @@ each native event state machine and receipts. `ContactPermissionChecker` checks all valid native contact slots with the original applied-impulse and loss gates; hosts still own sampling at every required physics substep. -Default GPU dynamics may use ordinary reporting with -`DefaultPhysicsCfg(direct_gpu_api=False)` when complete static contact identities -are needed. It retains GPU dynamics; the Direct GPU buffer initialization flag -alone is not a physics-device measurement. Physics devices and task qualification -must be reported separately. Existing CPU deployments do not switch devices. +Default GPU dynamics use Direct GPU reporting, whose static contact identity +limitations remain explicit admission constraints. Physics devices and task +qualification must be reported separately. Existing CPU deployments do not +switch devices. ## Focused validation diff --git a/agent_context/topics/sensor-system/sensor-system.md b/agent_context/topics/sensor-system/sensor-system.md index 91601b5073..f7ea125af8 100644 --- a/agent_context/topics/sensor-system/sensor-system.md +++ b/agent_context/topics/sensor-system/sensor-system.md @@ -131,9 +131,8 @@ choose the host's sampling frequency or relax unavailable-impulse/loss gates. Eager checks wait for the latest sensor sample on their CUDA stream without refreshing contacts or advancing histories. Global static actors retain their explicit row memberships. Rebind after query -topology changes. Default CUDA dynamics can select ordinary reporting through -`DefaultPhysicsCfg.direct_gpu_api=False` when static actor identity is required; -this is independent of the physics device and leaves CPU defaults unchanged. +topology changes. Default CUDA dynamics use Direct GPU reporting; its static +actor identity limitations remain explicit admission constraints. The contact scatter kernel remains sensor-owned because its columns, IDs and quota are integration contracts. Generic image tiling belongs to diff --git a/embodichain/lab/sim/cfg/physics.py b/embodichain/lab/sim/cfg/physics.py index c75ee86548..c04440966d 100644 --- a/embodichain/lab/sim/cfg/physics.py +++ b/embodichain/lab/sim/cfg/physics.py @@ -133,15 +133,6 @@ class PhysicsBackendCfg: class DefaultPhysicsCfg(PhysicsBackendCfg): """Configuration selector for the Default physics backend.""" - direct_gpu_api: bool = True - """Use Direct GPU state access when dynamics run on CUDA. - - False retains GPU dynamics with ordinary state/contact reporting. The - pinned Direct GPU contact API cannot identify static actors, so consumers - requiring complete contact identities must explicitly select False. - This option does not select the physics device or change contact sampling. - """ - cache_material: bool = False """Reuse identical physics materials across objects and environment clones. @@ -177,8 +168,6 @@ class DefaultPhysicsCfg(PhysicsBackendCfg): def __post_init__(self) -> None: """Decode mapping input for the nested GPU-memory configuration.""" - if type(self.direct_gpu_api) is not bool: - raise TypeError("direct_gpu_api must be a bool.") if isinstance(self.gpu_memory, Mapping): self.gpu_memory = GPUMemoryCfg(**self.gpu_memory) if not isinstance(self.gpu_memory, GPUMemoryCfg): diff --git a/embodichain/lab/sim/physics/default.py b/embodichain/lab/sim/physics/default.py index 078c82f00c..22d29c8d27 100644 --- a/embodichain/lab/sim/physics/default.py +++ b/embodichain/lab/sim/physics/default.py @@ -44,7 +44,7 @@ def configure_world(self, world_config, sim_config: "SimulationManagerCfg") -> N world_config.speed_tolerance = cfg.speed_tolerance if self._manager.device.type == "cuda": world_config.enable_gpu_sim = True - world_config.direct_gpu_api = cfg.direct_gpu_api + world_config.direct_gpu_api = True def activate(self, sim_config: "SimulationManagerCfg") -> None: cfg = sim_config.physics_cfg diff --git a/tests/sim/sensors/test_contact_default_e2e.py b/tests/sim/sensors/test_contact_default_e2e.py index 69997e0e36..9ba29a1d3c 100644 --- a/tests/sim/sensors/test_contact_default_e2e.py +++ b/tests/sim/sensors/test_contact_default_e2e.py @@ -35,19 +35,15 @@ @pytest.mark.parametrize( - ("device", "expected_contacts_per_env", "direct_gpu_api"), + ("device", "expected_contacts_per_env"), [ - pytest.param("cpu", 4, True, id="cpu"), - pytest.param("cuda:0", 4, True, marks=pytest.mark.gpu, id="cuda"), - pytest.param( - "cuda:0", 4, False, marks=pytest.mark.gpu, id="cuda-native-reports" - ), + pytest.param("cpu", 4, id="cpu"), + pytest.param("cuda:0", 4, marks=pytest.mark.gpu, id="cuda"), ], ) def test_default_contact_sensor_preserves_contact_multiplicity( device: str, expected_contacts_per_env: int, - direct_gpu_api: bool, ) -> None: wp.init() if device.startswith("cuda") and not wp.is_cuda_available(): @@ -58,7 +54,7 @@ def test_default_contact_sensor_preserves_contact_multiplicity( headless=True, num_envs=2, arena_space=2.0, - physics_cfg=DefaultPhysicsCfg(device=device, direct_gpu_api=direct_gpu_api), + physics_cfg=DefaultPhysicsCfg(device=device), ) ) try: @@ -117,9 +113,9 @@ def test_default_contact_sensor_preserves_contact_multiplicity( ) assert sim.is_use_gpu_physics == device.startswith("cuda") assert sim.get_world().get_physics_scene().is_direct_gpu() == ( - device.startswith("cuda") and direct_gpu_api + device.startswith("cuda") ) - if device == "cpu" or not direct_gpu_api: + if device == "cpu": maps = [{} for _ in range(2)] for uid in ("cube", "ground", "default_plane"): values = sensor.get_actor_ids(uid)[:, 0].cpu().tolist() From 24c21fbbdec3356e7d4f2fdbf53f0a5964df9552 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sun, 15 Nov 2026 09:01:37 +0000 Subject: [PATCH 12/45] feat(expansion): preview confirmed results with native ghost rendering --- .../expansion-engine/expansion-engine.md | 10 + embodichain/cli/expansion.py | 70 +++ embodichain/lab/expansion/_preview.html | 34 ++ embodichain/lab/expansion/_preview_data.py | 474 ++++++++++++++++++ embodichain/lab/expansion/_preview_render.py | 385 ++++++++++++++ embodichain/lab/expansion/_preview_server.py | 212 ++++++++ examples/expansion/engine/README.md | 16 +- examples/expansion/engine/ghost-preview.md | 79 ++- setup.py | 1 + tests/lab/expansion/test_preview.py | 316 ++++++++++++ 10 files changed, 1578 insertions(+), 19 deletions(-) create mode 100644 embodichain/lab/expansion/_preview.html create mode 100644 embodichain/lab/expansion/_preview_data.py create mode 100644 embodichain/lab/expansion/_preview_render.py create mode 100644 embodichain/lab/expansion/_preview_server.py create mode 100644 tests/lab/expansion/test_preview.py diff --git a/agent_context/topics/expansion-engine/expansion-engine.md b/agent_context/topics/expansion-engine/expansion-engine.md index cb2fe4abab..1155740b83 100644 --- a/agent_context/topics/expansion-engine/expansion-engine.md +++ b/agent_context/topics/expansion-engine/expansion-engine.md @@ -93,6 +93,16 @@ time; physics stays disabled and render updates do not mutate saved evidence. cross-group frame through the public marker layer. Task assets and any collection process orchestration remain outside the replay core. +`embodichain expansion preview` reads confirmed job outputs through `_preview_data.py`, +then replays full measured states using `_preview_render.py` and a loopback-only +`_preview_server.py`. Inputs retain their original evidence; no collection or +physics stepping occurs. The first version accepts one URDF robot and Cube rigid +objects with fixed layouts, and rejects unsupported geometry. Space filters group +input directories; missing motion annotations remain explicit. HTTP publishes a +single immutable image/state snapshot; WebRTC is follow-up work. FK and episode +budgets bound preload, controls are queued for the render owner, and native +resources/ports close on exit. See the example's `ghost-preview.md` for usage. + `TaskProgramDemoBridge.program_id` and `env_ids` expose binding identity without runtime traversal. `EmbodiedEnv.task_program_bridge_scope()` restores the borrowed bridge even after failure. `Real2SimTabletopSource` and explicit diff --git a/embodichain/cli/expansion.py b/embodichain/cli/expansion.py index 12166044e3..5c2ace7a7a 100644 --- a/embodichain/cli/expansion.py +++ b/embodichain/cli/expansion.py @@ -45,7 +45,77 @@ def main(argv: Sequence[str] | None = None) -> int: command.add_argument("--stage", choices=("all", "export"), default="all") command = commands.add_parser("inspect") command.add_argument("--output", required=True) + command = commands.add_parser( + "preview", help="Replay confirmed results with native ghost rendering" + ) + command.add_argument( + "--output", + action="append", + required=True, + help="Result directory with job.json; repeat for spatial groups", + ) + command.add_argument( + "--reference", + help="Optional result directory containing exactly one reference episode", + ) + command.add_argument( + "--robot-asset", help="Matching URDF override if recorded asset paths moved" + ) + command.add_argument( + "--eef-link", + help="URDF link used for measured FK paths; inferred from the solver when unique", + ) + command.add_argument("--gpu", type=int, default=0) + command.add_argument( + "--port", type=int, default=8769, help="Loopback port; 0 selects a free port" + ) + command.add_argument("--max-episodes", type=int, default=32) + command.add_argument( + "--duration", + type=float, + default=0, + help="Exit after these seconds; 0 waits for Ctrl+C", + ) + command.add_argument( + "--open", + action="store_true", + help="Open a browser after the first complete render", + ) args = parser.parse_args(argv) + if args.operation == "preview": + import math + from pathlib import Path + + if ( + not 0 <= args.port <= 65535 + or args.gpu < 0 + or not math.isfinite(args.duration) + or args.duration < 0 + ): + parser.error("invalid preview port, GPU or duration") + from embodichain.lab.expansion._preview_data import load_preview + from embodichain.lab.expansion._preview_server import serve_preview + + try: + data = load_preview( + [Path(p) for p in args.output], + Path(args.reference) if args.reference else None, + robot_asset=Path(args.robot_asset) if args.robot_asset else None, + eef_link=args.eef_link, + max_episodes=args.max_episodes, + ) + serve_preview( + data, + port=args.port, + gpu=args.gpu, + open_browser=args.open, + duration=args.duration, + ) + except KeyboardInterrupt: + return 0 + except (ValueError, OSError) as exc: + parser.error(str(exc)) + return 0 from embodichain.lab.expansion.configured import ( load_expansion_job, ) diff --git a/embodichain/lab/expansion/_preview.html b/embodichain/lab/expansion/_preview.html new file mode 100644 index 0000000000..ad53361468 --- /dev/null +++ b/embodichain/lab/expansion/_preview.html @@ -0,0 +1,34 @@ +EmbodiChain · 扩增结果回放 + +

EmbodiChain · 扩增结果回放

完整模型虚影 · 已确认的真实扩增结果

正在连接原生渲染器
+
拖动画面旋转 · 滚轮缩放
EmbodiChain 原生场景渲染画面
正在加载完整机器人模型…
——
+
+
EmbodiChain SimulationManager / RobotMarkers / VisualizationMarkers → DexSim Hybrid 原生渲染。仅回放,物理推进 0 步。机器人与物体使用逐帧记录;轨迹线每 4 帧取点并连接。视角、灯光与透明材质仅用于验收展示。
+ \ No newline at end of file diff --git a/embodichain/lab/expansion/_preview_data.py b/embodichain/lab/expansion/_preview_data.py new file mode 100644 index 0000000000..0dfeb76d8c --- /dev/null +++ b/embodichain/lab/expansion/_preview_data.py @@ -0,0 +1,474 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Read-only inputs and controls for the native expansion-result browser.""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import Any +import json +import math +import numpy as np +from scipy.spatial.transform import Rotation + +from .comparison import MeasuredTrajectory +from .persistence import ArtifactEpisodeSink, load_expansion_episode + +__all__: list[str] = [] + + +def pose_matrix(pose: np.ndarray) -> np.ndarray: + result = np.eye(4) + result[:3, :3] = Rotation.from_quat(pose[3:]).as_matrix() + result[:3, 3] = pose[:3] + return result + + +@dataclass +class PreviewClip: + measured: MeasuredTrajectory + space: str + affordance: str + variant: str + targets: np.ndarray + phase_names: tuple[str, ...] + anchors: np.ndarray + candidate_ids: tuple[str, ...] + label: str = "" + links: np.ndarray | None = None + tips: np.ndarray | None = None + + +def annotations(metadata: Any, measured: MeasuredTrajectory) -> dict[str, Any]: + records = metadata.get("expansion", ()) + env_ids = {r["env_id"] for r in records if "env_id" in r} + if len(env_ids) > 1: + raise ValueError("Episode expansion annotations refer to multiple rows") + env_id = next(iter(env_ids), None) + branches, candidates, targets, phases = [], [], [], [] + for segment in metadata.get("program_segments", ()): + for call in segment.get("metadata", {}).get("runtime", {}).get("calls", ()): + for event in call.get("events", ()): + if event.get("kind") == "trajectory_segment_entered": + phases.append( + ( + str(call["call"]["call_kind"]) + + "/" + + str(event["segment_name"]), + float(event["timestamp"]), + ) + ) + # Only the successful final planning attempt describes the executed target. + attempts = call.get("plan_attempts", ()) + if not attempts: + continue + sample = ( + attempts[-1] + .get("planner_diagnostics", {}) + .get("metadata", {}) + .get("affordance_sample", {}) + .get("grasp") + ) + if not sample: + continue + rows = list(sample["env_ids"]) + row = ( + rows.index(env_id) + if env_id in rows + else 0 if env_id is None and len(rows) == 1 else None + ) + if row is None: + raise ValueError( + "Cannot bind grasp annotation to the recorded environment row" + ) + if not sample["success"][row]: + continue + sampling = sample.get("sampling") or {} + overrides = sampling.get("branch_overrides") + branch = overrides[row] if overrides else sampling.get("branch_override") + branches.append("unrecorded" if branch is None else str(branch)) + candidates.append(str(sample["candidate_ids"][row])) + uid = ( + call.get("call", {}) + .get("arguments", {}) + .get("object", {}) + .get("entity_id") + ) + if uid not in measured.object_poses: + continue + reference = np.asarray(sample["reference_poses"][row], dtype=float) + selected = np.asarray(sample["selected_poses"][row], dtype=float) + if ( + reference.shape != (4, 4) + or selected.shape != (4, 4) + or not np.isfinite([reference, selected]).all() + ): + raise ValueError("Invalid recorded grasp transform") + targets.append( + pose_matrix(measured.object_poses[uid][0]) + @ np.linalg.inv(reference) + @ selected + ) + phases = [v for i, v in enumerate(phases) if i == 0 or v != phases[i - 1]] + anchors = np.array([p[1] for p in phases] + [measured.timestamps[-1]]) + if ( + not phases + or anchors[0] < 0 + or not np.isfinite(anchors).all() + or not np.all(np.diff(anchors) > 0) + ): + phases = [] + anchors = np.empty(0) + variants = [ + str(r["trajectory_selected"]) + for r in records + if r.get("trajectory_requested") is not None + ] + return dict( + affordance="/".join(branches) or "unrecorded", + variant="/".join(variants) or "unrecorded", + targets=np.asarray(targets, dtype=float).reshape(-1, 4, 4), + phase_names=tuple(p[0] for p in phases), + anchors=anchors, + candidate_ids=tuple(candidates), + ) + + +def _scene(job: dict[str, Any]) -> tuple[dict, dict[str, tuple[float, ...]]]: + cfg = job.get("resolved_deployment") + if not isinstance(cfg, dict) or not isinstance(cfg.get("robot"), dict): + raise ValueError( + "Preview requires job.json with a resolved single-robot deployment" + ) + unsupported = ( + "articulation", + "rigid_object_group", + "background", + "cable_object", + "fluid_object", + "soft_object", + "cloth_object", + "deformable_object", + ) + if any(cfg.get(k) for k in unsupported) or job.get("config", {}).get("scene_atlas"): + raise ValueError( + "Preview currently supports a fixed layout, one URDF robot and Cube rigid objects only" + ) + robot = cfg["robot"] + if robot.get("visuals") or robot.get("link_visuals"): + raise ValueError("Preview requires the URDF-owned robot visual geometry") + if not np.allclose(robot.get("body_scale", [1, 1, 1]), [1, 1, 1]): + raise ValueError("Scaled robot assets are not supported by this preview") + boxes = {} + for obj in cfg.get("rigid_object", []): + shape = obj.get("shape") or {} + if ( + shape.get("shape_type") != "Cube" + or obj.get("source") + or obj.get("visuals") + or obj.get("visual_shape") + ): + raise ValueError( + "Preview requires Cube rigid geometry; no asset substitution is performed" + ) + size = np.asarray(shape["size"], float) * np.asarray( + obj.get("body_scale", [1, 1, 1]), float + ) + if size.shape != (3,) or not np.isfinite(size).all() or np.any(size <= 0): + raise ValueError("Invalid recorded Cube dimensions") + boxes[obj["uid"]] = tuple(size) + return robot, boxes + + +@dataclass +class PreviewData: + clips: list[PreviewClip] + reference: PreviewClip | None + asset: Path + eef_link: str + boxes: dict[str, tuple[float, ...]] + ground_visible: bool = True + + @property + def all_clips(self) -> list[PreviewClip]: + return self.clips + ([self.reference] if self.reference is not None else []) + + @property + def phase_alignment(self) -> bool: + signature = self.clips[0].phase_names + return bool(signature) and all( + c.phase_names == signature for c in self.all_clips + ) + + +def load_preview( + outputs: list[Path], + reference: Path | None = None, + *, + robot_asset: Path | None = None, + eef_link: str | None = None, + max_episodes: int = 32, +) -> PreviewData: + if not outputs or not 1 <= max_episodes <= 64: + raise ValueError("Provide result directories and max_episodes in [1,64]") + paths = [Path(p).expanduser().resolve() for p in outputs] + if len(set(paths)) != len(paths): + raise ValueError("Duplicate output directory") + if reference is not None: + paths.append(Path(reference).expanduser().resolve()) + clips = [] + reference_clip = None + asset = None + boxes = None + robot_spec = None + end = None + ground = True + seen = set() + frames = 0 + for group, path in enumerate(paths): + job_path = path / "job.json" + if not job_path.is_file(): + raise ValueError(f"Not an expansion job output: {path} (missing job.json)") + job = json.loads(job_path.read_text()) + robot, shapes = _scene(job) + spec = {k: robot.get(k) for k in ("uid", "control_parts", "solver_cfg")} + if robot_spec is not None and (spec != robot_spec or shapes != boxes): + raise ValueError( + "Result directories must share robot/control topology and Cube geometry" + ) + robot_spec = spec + boxes = shapes + path_value = robot.get("fpath") or (robot.get("urdf_cfg") or {}).get("fpath") + resolved_asset = Path(robot_asset or path_value or "").expanduser().resolve() + if not resolved_asset.is_file() or resolved_asset.suffix.lower() != ".urdf": + raise ValueError( + "Recorded URDF is unavailable; supply --robot-asset with matching visuals" + ) + # URDF assembly comments may differ; compare the actual XML model. + import xml.etree.ElementTree as ET + + if asset is not None and ET.tostring(ET.parse(asset).getroot()) != ET.tostring( + ET.parse(resolved_asset).getroot() + ): + raise ValueError("Output directories reference different robot models") + asset = resolved_asset + ends = { + s.get("end_link_name") + for s in robot.get("solver_cfg", {}).values() + if s.get("end_link_name") + } + inferred = eef_link or (next(iter(ends)) if len(ends) == 1 else None) + if not inferred: + raise ValueError( + "Specify --eef-link when the recorded solver endpoint is ambiguous" + ) + end = inferred + if group == 0: + ground = job["resolved_deployment"].get("ground_plane_visible") is not False + elif ground != ( + job["resolved_deployment"].get("ground_plane_visible") is not False + ): + raise ValueError("Outputs disagree about ground visibility") + receipts = [r for r in ArtifactEpisodeSink(path).inspect() if r.confirmed] + is_reference = reference is not None and group == len(outputs) + if not receipts or (is_reference and len(receipts) != 1): + raise ValueError( + "Each output needs confirmed episodes; reference must contain exactly one" + ) + if len(clips) + len(receipts) > (max_episodes + int(is_reference)): + raise ValueError( + "Preview episode budget exceeded; select smaller output directories" + ) + for receipt in receipts: + episode = load_expansion_episode(receipt.storage_id) + if not episode.validation.accepted: + raise ValueError("Unqualified episode cannot be previewed") + if episode.episode_id in seen: + raise ValueError("Duplicate episode across inputs/reference") + seen.add(episode.episode_id) + measured = MeasuredTrajectory.load_episode( + receipt.storage_id, robot_uid=robot["uid"] + ) + if len(measured.timestamps) != len(episode.actions) + 1 or set( + measured.object_poses + ) != set(boxes): + raise ValueError( + "Incomplete T+1 scene state or unsupported scene entities" + ) + if ( + any( + x["kind"] not in ("robot", "rigid_object") + for x in episode.metadata["initial_state"] + ) + or sum(x["kind"] == "robot" for x in episode.metadata["initial_state"]) + != 1 + ): + raise ValueError( + "Preview supports one robot and recorded rigid objects only" + ) + frames += len(measured.timestamps) + if frames > 100_000: + raise ValueError("Preview exceeds the 100000 measured-state budget") + clip = PreviewClip( + measured=measured, + space=f"S{group+1} · {path.name}", + **annotations(episode.metadata, measured), + ) + if is_reference: + reference_clip = clip + clip.label = "Reference" + else: + clips.append(clip) + clip.label = f"C-{len(clips):03d}" + return PreviewData(clips, reference_clip, asset, end, boxes, ground) + + +class PreviewState: + def __init__(self, data: PreviewData) -> None: + self.data = data + self.values = dict( + space="all", + aff="all", + variant="all", + selected=0, + progress=0.0, + playing=False, + reference=data.reference is not None, + ghosts=True, + paths=True, + targets=True, + positions=True, + align="phase" if data.phase_alignment else "time", + yaw=-2.15, + elevation=0.55, + distance=1.65, + ) + self.options = { + "space": sorted({c.space for c in data.clips}), + "aff": sorted({c.affordance for c in data.clips}), + "variant": sorted({c.variant for c in data.clips}), + "align": ["time"] + (["phase"] if data.phase_alignment else []), + } + + def validate(self, update: Any) -> dict[str, Any]: + if ( + not isinstance(update, dict) + or not update + or set(update) - self.values.keys() + ): + raise ValueError("Invalid preview controls") + bounds = { + "selected": (0, len(self.data.clips) - 1), + "progress": (0, 1), + "yaw": (-20, 20), + "elevation": (0.1, 1.5), + "distance": (0.2, 20), + } + for key, value in update.items(): + if key in self.options: + if value not in self.options[key] + ([] if key == "align" else ["all"]): + raise ValueError(key) + elif key in bounds: + if ( + type(value) not in (float, int) + or not math.isfinite(value) + or not bounds[key][0] <= value <= bounds[key][1] + or (key == "selected" and type(value) is not int) + ): + raise ValueError(key) + elif type(value) is not bool: + raise ValueError(key) + return dict(update) + + def apply(self, update: dict[str, Any]) -> None: + self.values.update(self.validate(update)) + visible = self.visible() + if visible and self.values["selected"] not in visible: + self.values["selected"] = visible[0] + if not visible: + self.values["playing"] = False + + def visible(self) -> list[int]: + v = self.values + return [ + i + for i, c in enumerate(self.data.clips) + if all( + v[k] == "all" or v[k] == actual + for k, actual in [ + ("space", c.space), + ("aff", c.affordance), + ("variant", c.variant), + ] + ) + ] + + def frame_at(self, clip: PreviewClip) -> int: + p = self.values["progress"] + if self.values["align"] == "phase": + n = len(clip.anchors) - 1 + segment = min(n - 1, int(p * n)) + t = np.interp(p * n - segment, [0, 1], clip.anchors[segment : segment + 2]) + else: + t = p * self.duration + return clip.measured.frame_at(t) + + @property + def duration(self) -> float: + return max(float(c.measured.timestamps[-1]) for c in self.data.all_clips) + + def snapshot(self) -> dict[str, Any]: + visible = self.visible() + c = self.data.clips[self.values["selected"]] + f = self.frame_at(c) + return { + **self.values, + "visible": visible, + "count": len(visible), + "total": len(self.data.clips), + "options": self.options, + "has_reference": self.data.reference is not None, + "label": c.label if visible else "No matches", + "source_frame": f if visible else None, + "source_frames": len(c.measured.timestamps) if visible else 0, + "time": float(c.measured.timestamps[f]) if visible else None, + "episode": c.measured.name if visible else "", + "candidate": ", ".join(c.candidate_ids) or "unrecorded", + "phase": ( + next( + ( + name + for name, t in reversed(list(zip(c.phase_names, c.anchors))) + if t <= c.measured.timestamps[f] + ), + "unrecorded", + ) + if visible + else "" + ), + "clips": [ + dict( + index=i, + label=c.label, + space=c.space, + aff=c.affordance, + variant=c.variant, + ) + for i, c in enumerate(self.data.clips) + ], + } diff --git a/embodichain/lab/expansion/_preview_render.py b/embodichain/lab/expansion/_preview_render.py new file mode 100644 index 0000000000..eb75338930 --- /dev/null +++ b/embodichain/lab/expansion/_preview_render.py @@ -0,0 +1,385 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Bounded native mesh replay without constructing a task or advancing physics.""" + +from __future__ import annotations + +from contextlib import contextmanager +from collections.abc import Iterator +from typing import Any +import io +import math +import numpy as np +from scipy.spatial.transform import Rotation +from PIL import Image +from ._preview_data import PreviewData, PreviewState, pose_matrix + +__all__: list[str] = [] + + +def prepare_geometry(data: PreviewData) -> tuple[list[str], np.ndarray]: + import torch + import pytorch_kinematics as pk + + chain = pk.build_chain_from_urdf(data.asset.read_bytes()) + names = chain.get_link_names() + joints = chain.get_joint_parameter_names() + if data.eef_link not in names: + raise ValueError(f"Unknown end-effector link: {data.eef_link}") + total = ( + sum(len(c.measured.timestamps) for c in data.all_clips) * len(names) * 16 * 4 + ) + if total > 256 * 1024 * 1024: + raise ValueError( + "Preview FK exceeds the 256 MiB budget; select smaller result directories" + ) + for c in data.all_clips: + m = c.measured + if set(joints) != set(m.joint_names): + raise ValueError( + "Recorded joints do not match the supplied URDF (including mimic joints)" + ) + order = [m.joint_names.index(n) for n in joints] + c.links = np.empty((len(m.timestamps), len(names), 4, 4), np.float32) + for start in range(0, len(m.timestamps), 256): + stop = min(len(m.timestamps), start + 256) + q = torch.tensor(m.qpos[start:stop][:, order], dtype=torch.float32) + with torch.no_grad(): + fk = chain.forward_kinematics(q) + roots = np.stack([pose_matrix(p) for p in m.root_poses[start:stop]]) + c.links[start:stop] = roots[:, None] @ np.stack( + [fk[n].get_matrix().numpy() for n in names], axis=1 + ) + c.tips = c.links[:, names.index(data.eef_link), :3, 3] + if not np.isfinite(c.links).all(): + raise ValueError("FK produced nonfinite poses") + points = np.concatenate([c.links[0, :, :3, 3] for c in data.all_clips]) + center = (points.min(axis=0) + points.max(axis=0)) / 2 + return names, center + + +class NativeRenderer: + def __init__( + self, + data: PreviewData, + sim: Any, + publisher: Any, + names: list[str], + center: np.ndarray, + ) -> None: + from dexsim import spawn + from embodichain.lab.visualization.markers import ( + RobotMarkers, + RobotMarkersCfg, + RobotMarkersAppearanceCfg, + VisualizationMarkers, + VisualizationMarkersCfg, + MarkerPrototypeCfg, + ) + + self.data, self.sim, self.publisher, self.center = data, sim, publisher, center + scene = sim.spawn_result + self.step = sim.physics_step_index + self.previous = None + palette = [ + (0.18, 0.58, 1), + (0.15, 0.9, 0.5), + (0.9, 0.55, 0.2), + (0.7, 0.4, 0.95), + ] + variants = sorted({c.variant for c in data.clips}) + self.colors = {v: palette[i % len(palette)] for i, v in enumerate(variants)} + for name, pos, intensity in [ + ("preview_key", [-1, -2, 3], 35), + ("preview_fill", [1, 1, 2], 18), + ]: + pose = np.eye(4, dtype=np.float32) + pose[:3, 3] = np.asarray(pos) + center + scene.add_light( + spawn.LightDesc( + name=name, light_type="POINT", pose=pose, intensity=intensity + ) + ) + self.robots = [] + self.objects = [] + + def markers(name: str, shapes: dict) -> Any: + return VisualizationMarkers( + VisualizationMarkersCfg(prim_path=name, markers=shapes), publisher + ) + + for i, c in enumerate(data.all_clips): + color = ( + self.colors.get(c.variant, (0.65, 0.7, 0.75)) + if i < len(data.clips) + else (0.65, 0.7, 0.75) + ) + robot = RobotMarkers( + RobotMarkersCfg( + name=f"preview_robot_{i}", + asset_path=str(data.asset), + appearance=RobotMarkersAppearanceCfg(color=color, opacity=0.18), + ), + publisher, + ) + self.robots.append((robot, [names.index(n) for n in robot.link_names])) + objects = ( + markers( + f"preview_objects_{i}", + { + uid: MarkerPrototypeCfg( + shape="box", scale=size, color=(*color, 0.2) + ) + for uid, size in data.boxes.items() + }, + ) + if data.boxes + else None + ) + self.objects.append(objects) + robot.visualize_link_poses( + c.links[0, [names.index(n) for n in robot.link_names]][None] + ) + robot.set_visibility(False) + if objects: + p = np.array([c.measured.object_poses[uid][0] for uid in data.boxes]) + objects.visualize( + p[:, :3], p[:, 3:], marker_indices=np.arange(len(data.boxes)) + ) + objects.set_visibility(False) + self.selected = RobotMarkers( + RobotMarkersCfg( + name="selected_model", + asset_path=str(data.asset), + appearance=RobotMarkersAppearanceCfg(mode="asset", opacity=1), + ), + publisher, + ) + self.selected_order = [names.index(n) for n in self.selected.link_names] + self.selected.visualize_link_poses( + data.clips[0].links[0, self.selected_order][None] + ) + self.selected_objects = ( + markers( + "selected_objects", + { + uid: MarkerPrototypeCfg( + shape="box", + scale=size, + color=(0.9, 0.58, 0.2, 1), + scene_lighting=True, + unlit=False, + ) + for uid, size in data.boxes.items() + }, + ) + if data.boxes + else None + ) + self.paths = markers( + "measured_paths", + { + str(i): MarkerPrototypeCfg( + shape="cylinder", color=(*self.colors[v], 0.8) + ) + for i, v in enumerate(variants) + }, + ) + self.variant_ids = {v: i for i, v in enumerate(variants)} + self.targets = markers( + "recorded_grasp_targets", + {"target": MarkerPrototypeCfg(shape="frame", scale=(0.065, 0.065, 0.065))}, + ) + self.positions = markers( + "initial_positions", + { + "point": MarkerPrototypeCfg( + shape="sphere", scale=(0.014, 0.014, 0.014), color=(1, 0.7, 0.1, 1) + ) + }, + ) + self.camera = scene.add_camera( + spawn.CameraDesc(name="expansion_preview_camera", resolution=(1100, 760)) + ) + self.camera.open_camera() + self.camera.set_visual_overlay_enabled(True) + publisher.publish(preload=True) + + def draw(self, state: PreviewState) -> bytes: + v = state.values + visible = state.visible() + if tuple(visible) != self.previous: + positions = [] + rotations = [] + scales = [] + types = [] + for i in visible: + c = self.data.clips[i] + points = c.tips[ + np.unique(np.r_[np.arange(0, len(c.tips), 4), len(c.tips) - 1]) + ] + for a, b in zip(points[:-1], points[1:]): + delta = b - a + length = np.linalg.norm(delta) + if length < 1e-6: + continue + z = delta / length + axis = np.cross([0, 0, 1], z) + norm = np.linalg.norm(axis) + rotation = ( + Rotation.from_rotvec( + axis / norm * np.arccos(np.clip(z[2], -1, 1)) + ) + if norm > 1e-8 + else Rotation.from_rotvec([np.pi if z[2] < 0 else 0, 0, 0]) + ) + positions.append((a + b) / 2) + rotations.append(rotation.as_quat()) + scales.append([0.002, 0.002, length]) + types.append(self.variant_ids[c.variant]) + self.paths.visualize( + np.array(positions).reshape(-1, 3), + np.array(rotations).reshape(-1, 4), + np.array(scales).reshape(-1, 3), + np.asarray(types, dtype=int), + ) + targets = ( + np.concatenate([self.data.clips[i].targets for i in visible]) + if visible + else np.empty((0, 4, 4)) + ) + self.targets.visualize( + targets[:, :3, 3], + ( + Rotation.from_matrix(targets[:, :3, :3]).as_quat() + if len(targets) + else np.empty((0, 4)) + ), + ) + initial = [ + p[0, :3] + for i in visible + for p in self.data.clips[i].measured.object_poses.values() + ] + self.positions.visualize(np.asarray(initial).reshape(-1, 3)) + self.previous = tuple(visible) + self.selected.set_visibility(bool(visible)) + if self.selected_objects: + self.selected_objects.set_visibility(bool(visible)) + if visible: + c = self.data.clips[v["selected"]] + f = state.frame_at(c) + self.selected.visualize_link_poses(c.links[f, self.selected_order][None]) + if self.selected_objects: + p = np.array( + [c.measured.object_poses[uid][f] for uid in self.data.boxes] + ) + self.selected_objects.visualize( + p[:, :3], p[:, 3:], marker_indices=np.arange(len(p)) + ) + ghost_count = 0 + for i, (c, (robot, order), objects) in enumerate( + zip(self.data.all_clips, self.robots, self.objects) + ): + show = ( + v["reference"] + if i == len(self.data.clips) + else v["ghosts"] and i in visible and i != v["selected"] + ) + robot.set_visibility(show) + if objects: + objects.set_visibility(show) + if show: + ghost_count += 1 + f = state.frame_at(c) + robot.visualize_link_poses(c.links[f, order][None]) + if objects: + p = np.array( + [c.measured.object_poses[uid][f] for uid in self.data.boxes] + ) + objects.visualize( + p[:, :3], p[:, 3:], marker_indices=np.arange(len(p)) + ) + self.paths.set_visibility(v["paths"]) + self.targets.set_visibility(v["targets"]) + self.positions.set_visibility(v["positions"]) + self.publisher.publish() + yaw = v["yaw"] + e = v["elevation"] + d = v["distance"] + eye = self.center + d * np.array( + [math.cos(e) * math.cos(yaw), math.cos(e) * math.sin(yaw), math.sin(e)] + ) + self.camera.look_at(eye.tolist(), self.center.tolist(), [0, 0, 1]) + with self.sim.render_frame(capture_camera_images=False): + self.sim.sync_render_state() + self.camera.render() + if self.sim.physics_step_index != self.step: + raise RuntimeError("Preview unexpectedly advanced physics") + image = Image.fromarray( + np.ascontiguousarray(self.camera.get_rgb_map()[..., :3], dtype=np.uint8) + ) + buffer = io.BytesIO() + image.save(buffer, format="JPEG", quality=88) + self.stats = { + "physics_steps": self.sim.physics_step_index - self.step, + "ghost_count": ghost_count, + "path_segments": self.paths.count if v["paths"] else 0, + "grasp_targets": self.targets.count if v["targets"] else 0, + } + return buffer.getvalue() + + +@contextmanager +def native_renderer(data: PreviewData, gpu: int = 0) -> Iterator[NativeRenderer]: + import os + + os.environ.setdefault("SDL_VIDEODRIVER", "offscreen") + import torch + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + from embodichain.lab.sim.cfg import RenderCfg, DenoisingCfg + from embodichain.lab.visualization.markers import MarkerFramePublisher + + torch.set_num_threads(1) + names, center = prepare_geometry(data) + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + device="cpu", + gpu_id=gpu, + num_envs=1, + enable_entity_gizmo=False, + startup_summary="off", + render_cfg=RenderCfg( + renderer="hybrid", + spp=4, + denoising=DenoisingCfg(window="off", offscreen="optix"), + ), + ) + ) + publisher = None + try: + sim.enable_physics(False) + sim.prepare() + sim.set_ground_plane_visibility(data.ground_visible) + publisher = MarkerFramePublisher(sim) + yield NativeRenderer(data, sim, publisher, names, center) + finally: + if publisher is not None: + publisher.close() + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() diff --git a/embodichain/lab/expansion/_preview_server.py b/embodichain/lab/expansion/_preview_server.py new file mode 100644 index 0000000000..fa8392b292 --- /dev/null +++ b/embodichain/lab/expansion/_preview_server.py @@ -0,0 +1,212 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Loopback-only native preview transport. WebRTC integration remains a TODO.""" + +from __future__ import annotations + +import base64 +import json +import queue +import threading +import time +import webbrowser +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer +from pathlib import Path +from typing import Any +from urllib.parse import urlsplit +from ._preview_data import PreviewData, PreviewState + +__all__: list[str] = [] + + +class PreviewTransport: + def __init__(self, state: PreviewState, port: int) -> None: + self.state = state + self.commands: queue.Queue[dict[str, Any]] = queue.Queue(maxsize=64) + self.lock = threading.Lock() + self.status: dict[str, Any] = {} + self.jpeg = b"" + self.snapshot = b"{}" + html = Path(__file__).with_name("_preview.html").read_bytes() + owner = self + + class Handler(BaseHTTPRequestHandler): + def setup(self) -> None: + super().setup() + self.connection.settimeout(3) + + def log_message(self, *_: object) -> None: + pass + + def do_GET(self) -> None: + if urlsplit("//" + self.headers.get("Host", "")).hostname not in { + "127.0.0.1", + "localhost", + "::1", + }: + self.send_error(403) + return + route = self.path.split("?")[0] + with owner.lock: + if route == "/": + data, kind = html, "text/html; charset=utf-8" + elif route == "/state": + data, kind = ( + json.dumps(owner.status, allow_nan=False).encode(), + "application/json", + ) + elif route == "/snapshot": + data, kind = owner.snapshot, "application/json" + elif route == "/frame.jpg": + data, kind = owner.jpeg, "image/jpeg" + else: + self.send_error(404) + return + self.send_response(200) + self.send_header("Content-Type", kind) + self.send_header("Cache-Control", "no-store") + self.send_header("X-Content-Type-Options", "nosniff") + self.send_header("Content-Length", str(len(data))) + self.end_headers() + try: + self.wfile.write(data) + except (BrokenPipeError, ConnectionResetError): + pass + + def do_POST(self) -> None: + if self.path != "/control": + self.send_error(404) + return + origin = self.headers.get("Origin") + host = self.headers.get("Host", "") + # Same-origin loopback controls also work through a different forwarded port. + if ( + urlsplit("//" + host).hostname + not in {"127.0.0.1", "localhost", "::1"} + or self.headers.get("Content-Type") != "application/json" + or (origin and origin != "http://" + host) + ): + self.send_error(403) + return + try: + size = int(self.headers.get("Content-Length", "0")) + if not 0 < size <= 4096: + raise ValueError("control size") + update = owner.state.validate(json.loads(self.rfile.read(size))) + owner.commands.put_nowait(update) + except (ValueError, TypeError, queue.Full, TimeoutError): + self.send_error(400) + return + self.send_response(204) + self.end_headers() + + self.server = ThreadingHTTPServer(("127.0.0.1", port), Handler) + self.server.daemon_threads = True + self.url = f"http://127.0.0.1:{self.server.server_port}/" + + def publish(self, status: dict[str, Any], jpeg: bytes) -> None: + snapshot = json.dumps( + { + **status, + "image": "data:image/jpeg;base64," + base64.b64encode(jpeg).decode(), + }, + allow_nan=False, + ).encode() + with self.lock: + self.status = status + self.jpeg = jpeg + self.snapshot = snapshot + + +def serve_preview( + data: PreviewData, + *, + port: int = 8769, + gpu: int = 0, + open_browser: bool = False, + duration: float = 0, +) -> None: + from ._preview_render import native_renderer + + state = PreviewState(data) + transport = PreviewTransport(state, port) + thread = None + try: + with native_renderer(data, gpu) as renderer: + frame = 0 + + def publish() -> None: + nonlocal frame + jpeg = renderer.draw(state) + transport.publish( + { + **state.snapshot(), + **renderer.stats, + "frame": frame, + "renderer": "EmbodiChain / DexSim Hybrid + OptiX", + "transport": "HTTP JPEG", + }, + jpeg, + ) + frame += 1 + + publish() + thread = threading.Thread( + target=transport.server.serve_forever, daemon=True + ) + thread.start() + print( + json.dumps( + { + "state": "ready", + "url": transport.url, + "episodes": len(data.clips), + "physics_steps": 0, + "transport": "HTTP JPEG", + } + ), + flush=True, + ) + if open_browser: + webbrowser.open(transport.url) + start = last = time.monotonic() + while not duration or time.monotonic() - start < duration: + now = time.monotonic() + elapsed = now - last + last = now + dirty = False + while True: + try: + state.apply(transport.commands.get_nowait()) + dirty = True + except queue.Empty: + break + if state.values["playing"]: + state.values["progress"] = min( + 1.0, state.values["progress"] + elapsed / state.duration + ) + if state.values["progress"] >= 1: + state.values["playing"] = False + dirty = True + if dirty: + publish() + time.sleep(max(0.01, 0.1 - (time.monotonic() - now))) + finally: + if thread is not None: + transport.server.shutdown() + thread.join(timeout=5) + transport.server.server_close() diff --git a/examples/expansion/engine/README.md b/examples/expansion/engine/README.md index 2b902ff70a..61cc1c7383 100644 --- a/examples/expansion/engine/README.md +++ b/examples/expansion/engine/README.md @@ -26,11 +26,17 @@ capabilities remain explicit. `--reuse` rebuilds the offline page without a Worl ## Native ghost preview -See [the native ghost-preview notes](ghost-preview.md) for the existing SDK -window comparison, the separately validated eight-result browser prototype, -linked spatial/affordance/trajectory filters, and the native WebRTC follow-up. -The prototype uses native offscreen rendering with HTTP JPEG transport; it is -not a WebRTC entry point or a released general-purpose viewer. +Replay confirmed results directly, without recollection or physics stepping: + +```bash +embodichain expansion preview --output /path/to/job-output --gpu 0 --open +``` + +Repeat `--output` for spatial groups and optionally add `--reference` with one +confirmed reference episode. The [native preview guide](ghost-preview.md) covers +linked spatial/affordance/trajectory filters, asset recovery, supported scene +geometry, replay semantics and the WebRTC TODO. Rendering is native; browser +transport currently uses HTTP JPEG, not WebRTC. ## Run a configured job diff --git a/examples/expansion/engine/ghost-preview.md b/examples/expansion/engine/ghost-preview.md index 3d538376d0..dd6a49ddaa 100644 --- a/examples/expansion/engine/ghost-preview.md +++ b/examples/expansion/engine/ghost-preview.md @@ -1,26 +1,67 @@ # 扩增分布的原生虚影预览 本文记录空间、affordance 与轨迹分布的交互预览方案,以及 2026-11-15 完成的 -小规模原型验收。它补充 [Expansion Engine](README.md) 的结果查看流程。 +小规模原型验收,并说明基于已有结果启动的仓库入口。它补充 [Expansion Engine](README.md) 的结果查看流程。 ## 当前能力与边界 | 路径 | 渲染与交互 | 状态 | | --- | --- | --- | | SDK 原生窗口对照 | `preview_expansion_comparison` 加载已验收轨迹,显示参考与选中虚影;N 切换、Space 暂停、Q 关闭 | 仓库已有能力,见 [comparison.py](../../../embodichain/lab/expansion/comparison.py) | -| 独立浏览器原型 | EmbodiChain 原生离屏渲染完整模型;HTTP JPEG 传图,HTTP 控制筛选与回放 | 已完成本地验收,尚未整理为仓库通用入口 | +| 浏览器结果预览 | EmbodiChain 原生离屏渲染完整模型;HTTP JPEG 传图,HTTP 控制筛选与回放 | `embodichain expansion preview`,直接读取已确认结果 | | 原生 WebRTC 推流 | 复用 EmbodiChain 的原生推流生命周期与控制接口 | TODO,尚未接入本预览 | “原生渲染”指场景由 `SimulationManager`、`RobotMarkers`、 `VisualizationMarkers` 和 DexSim Hybrid 渲染器生成,**不表示采用 WebRTC**。 -浏览器原型没有自行绘制机器人骨架,也不使用 Viser 重建场景;它显示原生离屏 +浏览器预览没有自行绘制机器人骨架,也不使用 Viser 重建场景;它显示原生离屏 相机输出的 JPEG。本次使用 OptiX 去噪。展示灯光、视角和透明材质与采集时可以不同, 不能把预览图片当作采集传感器的原始观测。 -独立原型使用采集时冻结的源码 `ab04d972b5b75042434498db9a889b4edd03b6fe`。 -本文新增的是说明与验收记录,不把本地脚本、机器路径或录制产物包装成已发布 SDK API。 +最初实验使用采集时冻结的源码 `ab04d972b5b75042434498db9a889b4edd03b6fe`。 +仓库入口直接读取原始结果,不需要该源码快照、本地预处理 JSON 或实验脚本。 [showcase](showcase/README.md) 的离线三维/RGB-D 页面也是另一条查看路径。 +## 从已有结果启动 + +在安装了兼容 EmbodiChain、DexSim 原生渲染依赖的环境中运行: + +```bash +# 单个已完成或部分完成的作业:只加载已有确认回执,不继续采集。 +embodichain expansion preview --output /path/to/job-output --gpu 0 --open + +# 复用本次配方的两个空间组,叠加一条独立参考。 +embodichain expansion preview \ + --output /path/to/results/space-a \ + --output /path/to/results/space-b \ + --reference /path/to/results/reference \ + --gpu 0 --port 8769 --open +``` + +每个输入是含 `job.json` 和 `episodes/` 的原始作业输出目录;不是导出的 LeRobot +数据集或单个 NPZ。可重复传入 `--output`,空间筛选以输入结果目录分组。 +当前只支持固定布局,不把一个 scene-atlas 作业内部的不同布局合并成同一空间组。 +`--reference` 可省略;指定时必须恰好含一个已确认参考 episode,且不能与候选重复。 + +模型从 `job.json.resolved_deployment.robot` 的 URDF 路径恢复,场景方块尺寸从 +同一快照恢复,不加载原任务配置,也不重新实例化任务/规划器。原模型及引用的视觉 +文件必须仍可读;若资产移动,使用 `--robot-asset /new/path/robot.urdf` 显式提供 +匹配模型。入口检查完整关节名称集合和跨输入模型一致性,但旧结果没有完整视觉 +资产包,不能仅凭关节名证明替换模型几何完全一致;调用者负责提供原模型。 + +默认从唯一的 solver `end_link_name` 推导末端路径,存在多个端点时必须提供 +`--eef-link LINK_NAME`。这里的路径是该 URDF link 的 FK 原点,不隐式附加工具 TCP。 +缺失 affordance/trajectory 注释显示“未记录”,不猜测为名义分支;只有所有轨迹 +具有一致、有效的动作阶段时才提供阶段对齐。 + +服务默认仅监听 `127.0.0.1:8769`,打印就绪地址后才能访问;`--port 0` 自动选空闲端口。 +`--open` 在首帧完成后尝试打开服务所在机器的浏览器。远程服务器需要端口转发后 +在客户端打开对应地址。Ctrl+C 释放服务和渲染资源;`--duration 60` 在就绪后 +运行 60 秒自动关闭。启动端口冲突、资产缺失和不支持的场景会明确失败。 + +入口不会写入输入目录、更新成功计数或重新采集。它默认最多加载 32 条候选, +`--max-episodes` 可设为 1–64,超出则报错而非静默抽样。还限制总状态帧不超过 +100000、预计算 link FK 不超过 256 MiB;这些是显式的预览规模限制,不是性能保证。 + ## 配方与真实结果 使用已有 `task.default.yaml` 的单轮 UR5 + DH PGI 抓取放置任务和 default 物理后端。 @@ -52,9 +93,10 @@ 选中样本显示原始模型外观,其余可见候选显示半透明虚影;参考单独开关。 4. 用真实物体位姿驱动物体模型,并通过 `VisualizationMarkers` 显示初始位置、 抓取目标坐标轴和末端路径。 -5. 在仿真/渲染所有者线程应用筛选与回放状态,调用原生相机渲染。 +5. 启动时分块预计算 FK,使用 `MarkerFramePublisher` 发布一致的跨组帧; + 在仿真/渲染所有者线程应用筛选与回放状态,调用原生相机渲染。 HTTP 线程只排队控制消息和读取已发布画面,不直接访问仿真对象。 -6. 页面读取 JPEG 并展示对应记录信息。关闭服务时释放相机、场景和监听端口; +6. 页面轮询同一次发布中的 JPEG 与记录信息,避免分开读取图像和状态造成帧号错配。关闭服务时释放相机、场景和监听端口; 已提交轨迹与校验结果保持不变。 原型回放不调用物理推进,验收记录的物理步数为 0。回放不是新物理成功样本, @@ -64,7 +106,7 @@ 空间、affordance、轨迹三个筛选器取交集,同时作用于候选列表、虚影、物体、 初始位置、抓取目标和轨迹线。选中样本被筛掉时,选择交集内的一个有效样本。 -这个固定配方的组合完整,因此没有空交集;通用化时必须显式处理空结果。 +没有匹配候选时显示空结果、停止播放并隐藏候选图层;参考对照仍可独立显示。 参考轨迹是独立对照,不计入 8 条候选数量,也不受候选筛选限制。 - **空间**:显示实际初始位置,不给样本添加人为排列偏移。 @@ -85,6 +127,11 @@ 时间拖动与播放、虚影开关、相机视角切换、窄屏布局和非法控制值拒绝。 浏览器无脚本错误,回放全过程未推进物理。验收后服务已关闭,结果保留。 +仓库 CLI 已使用相同的 8 条候选和 1 条参考复测:筛选后的实际虚影/目标数量同步变化, +播放与拖动正常;无需 `--robot-asset` 即可从 `job.json` 恢复资产,`--duration` +结束后端口关闭。结果读取、批内目标绑定、空交集、时间对齐、HTTP 控制/快照、 +CLI 不触发采集和安装包页面资源由生产模块的聚焦测试覆盖。 + 原型脚本及结果保存在本次实验的 `real-demo` 目录: | 文件 | 用途 | @@ -95,15 +142,19 @@ | `native-assets/` | 对应采集模型的 URDF 与视觉资产副本 | | `native-acceptance.json`、`native-preview-acceptance.png` | 浏览器验收结果和原生渲染截图 | -这些文件不是仓库已安装入口。已有实验目录中可用兼容的 EmbodiChain/DexSim -Python 环境运行 `python native_preview.py --gpu GPU_ID --port PORT`,然后访问该机器的 -loopback 地址;远程访问需要端口转发。Ctrl+C 关闭服务。原型默认端口 8769, -默认渲染 GPU 4,设备编号需按部署环境调整;不要从仓库根目录直接执行此命令。 -原型仍依赖实验目录中的冻结源码、配置与产物路径,不是便携式数据查看器。 +上述文件是原型的历史实验产物;仓库入口不再依赖它们。结果读取与控制语义由 +`embodichain/lab/expansion/_preview_data.py` 负责,原生渲染与资源释放由 +`_preview_render.py` 负责,HTTP 服务与页面由 `_preview_server.py`、`_preview.html` +负责,CLI 由 `embodichain/cli/expansion.py` 分发。 + +当前明确支持单 URDF 机器人、单位机器人缩放和 Cube 刚体场景。其他 articulated +objects、软体、流体、线缆、mesh/source 物体、背景资产和 scene-atlas 输入会被拒绝; +不会用方块代替不支持的几何。预览保持记录中的运动与尺寸,方块颜色和灯光是展示 +外观,不声称精确恢复采集时材质。新任务支持应扩展场景恢复契约并验证实测坐标。 HTTP JPEG 轮询不提供 WebRTC 的编码、拥塞控制、连接恢复或低延迟保证。 页面是单个服务的共享回放会话,多人操作会改变同一状态;尚未提供独立会话、 -认证、任意配方加载、大规模分页或通用资产恢复。当前不应对外公开监听。 +认证或通用资产包恢复。当前不应对外公开监听。 ## TODO:接入 EmbodiChain 原生 WebRTC 推流 diff --git a/setup.py b/setup.py index d2b5b24d76..a301c9939e 100644 --- a/setup.py +++ b/setup.py @@ -153,6 +153,7 @@ def get_package_data() -> dict[str, list[str]]: ] return { "embodichain": ["VERSION"], + "embodichain.lab.expansion": ["_preview.html"], "embodichain.data": ["default_materials.json"], "embodichain.compute.kinematics._warp": [ "ssik_LICENSE.txt", diff --git a/tests/lab/expansion/test_preview.py b/tests/lab/expansion/test_preview.py new file mode 100644 index 0000000000..4de3f2c108 --- /dev/null +++ b/tests/lab/expansion/test_preview.py @@ -0,0 +1,316 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Result-only preview: provenance, row selection, controls and HTTP lifecycle.""" + +from __future__ import annotations + +import base64 +from dataclasses import replace +import hashlib +import json +from pathlib import Path +import threading +import urllib.error +import urllib.request +import numpy as np +import pytest +import torch + +from embodichain.lab.expansion.contracts import ( + CandidateIdentity, + ExpertEpisode, + ValidationResult, + ValidationCheck, +) +from embodichain.lab.expansion.persistence import ArtifactEpisodeSink +from embodichain.lab.expansion._preview_data import load_preview, PreviewState +from embodichain.lab.expansion._preview_render import prepare_geometry +from embodichain.lab.expansion._preview_server import PreviewTransport + + +@pytest.fixture +def recorded_job(tmp_path): + asset = tmp_path / "robot.urdf" + asset.write_text( + '' + '' + '' + '' + '' + ) + output = tmp_path / "result" + output.mkdir() + job = { + "resolved_deployment": { + "robot": { + "uid": "robot", + "fpath": str(asset), + "solver_cfg": {"arm": {"end_link_name": "tip"}}, + }, + "rigid_object": [ + { + "uid": "box", + "shape": {"shape_type": "Cube", "size": [0.05, 0.05, 0.05]}, + } + ], + } + } + (output / "job.json").write_text(json.dumps(job)) + poses = torch.tensor([[0.0, 0, 0, 0, 0, 0, 1]] * 3) + objects = poses.clone() + objects[:, :2] = torch.tensor([10.0, 20.0]) + transforms = np.tile(np.eye(4), (2, 1, 1)) + transforms[:, 0, 3] = [100, 200] + selected = transforms.copy() + selected[:, 0, 3] += [1, 2] + sample = { + "env_ids": [3, 7], + "success": [True, True], + "sampling": {"branch_overrides": [0, 1]}, + "candidate_ids": [23, 31], + "reference_poses": transforms.tolist(), + "selected_poses": selected.tolist(), + } + metadata = { + "initial_state": [ + {"kind": "robot", "uid": "robot", "joint_names": ["joint"]}, + {"kind": "rigid_object", "uid": "box"}, + ], + "scene_state_fields": [ + {"entity_uid": "robot", "field": "pose", "key": "robot_pose"} + ], + "expansion": [ + {"env_id": 7, "trajectory_requested": 1, "trajectory_selected": 1} + ], + "program_segments": [ + { + "metadata": { + "runtime": { + "calls": [ + { + "call": { + "call_kind": "pick", + "arguments": {"object": {"entity_id": "box"}}, + }, + "plan_attempts": [ + { + "planner_diagnostics": { + "metadata": { + "affordance_sample": {"grasp": sample} + } + } + } + ], + "events": [ + { + "kind": "trajectory_segment_entered", + "timestamp": 0.0, + "segment_name": "approach", + }, + { + "kind": "trajectory_segment_entered", + "timestamp": 0.4, + "segment_name": "lift", + }, + ], + } + ] + } + } + } + ], + } + episode = ExpertEpisode( + CandidateIdentity( + "scene", "initial", "candidate", "family", "source", "v1", "template" + ), + { + "full_joint_positions": torch.tensor([[0.0], [np.pi / 2], [np.pi]]), + "robot_pose": poses, + "rigid/box/state": objects, + }, + torch.zeros((2, 1)), + torch.tensor([0.0, 0.5, 1.0], dtype=torch.float64), + "qpos", + ValidationResult((ValidationCheck("task_success", "passed"),)), + "episode", + "commit", + metadata=metadata, + ) + ArtifactEpisodeSink(output).submit(episode, submission_id=0) + return output, job, episode + + +def test_result_loading_binds_grasp_row_and_fk_without_writing_results(recorded_job): + path, _, _ = recorded_job + before = { + p.relative_to(path): hashlib.sha256(p.read_bytes()).hexdigest() + for p in path.rglob("*") + if p.is_file() + } + data = load_preview([path]) + clip = data.clips[0] + assert ( + clip.affordance == "1" and clip.candidate_ids == ("31",) and clip.variant == "1" + ) + np.testing.assert_allclose(clip.targets[0, :3, 3], [12, 20, 0]) + names, _ = prepare_geometry(data) + np.testing.assert_allclose(clip.tips, [[1, 0, 0], [0, 1, 0], [-1, 0, 0]], atol=1e-6) + assert "tip" in names and clip.links.shape[0] == 3 + after = { + p.relative_to(path): hashlib.sha256(p.read_bytes()).hexdigest() + for p in path.rglob("*") + if p.is_file() + } + assert before == after + + +def test_result_loader_rejects_incomplete_scene_and_budget(recorded_job, tmp_path): + path, job, episode = recorded_job + with pytest.raises(ValueError, match="missing job.json"): + load_preview([tmp_path / "missing"]) + assert not (tmp_path / "missing").exists() + ArtifactEpisodeSink(path).submit( + replace(episode, episode_id="episode2", commit_id="commit2"), submission_id=1 + ) + with pytest.raises(ValueError, match="budget"): + load_preview([path], max_episodes=1) + with pytest.raises(ValueError, match="reference"): + load_preview([path], reference=path) + job["resolved_deployment"]["rigid_object"][0]["shape"]["shape_type"] = "Mesh" + (path / "job.json").write_text(json.dumps(job)) + with pytest.raises(ValueError, match="Cube"): + load_preview([path]) + + +def test_filter_empty_intersection_and_phase_alignment(recorded_job): + path, _, _ = recorded_job + data = load_preview([path]) + first = data.clips[0] + data.clips = [ + replace(first, space="A", affordance="0", variant="0"), + replace(first, space="B", affordance="1", variant="1"), + ] + state = PreviewState(data) + state.apply({"space": "B"}) + assert state.visible() == [1] and state.values["selected"] == 1 + state.apply({"aff": "0", "playing": True}) + assert state.visible() == [] and not state.values["playing"] + assert state.snapshot()["source_frame"] is None + state.apply({"space": "all", "aff": "all", "progress": 0.5}) + assert state.frame_at(first) == 0 # phase boundary .4 s -> recorded frame at 0 s + state.apply({"align": "time"}) + assert state.frame_at(first) == 1 # exactly .5 s + state.apply({"progress": 1}) + assert state.frame_at(first) == 2 + for update in ( + {"progress": float("nan")}, + {"selected": True}, + {"playing": "true"}, + {"unknown": 0}, + {"aff": "missing"}, + ): + before = dict(state.values) + with pytest.raises(ValueError): + state.apply(update) + assert state.values == before + + +def test_http_snapshot_control_and_shutdown(recorded_job): + path, _, _ = recorded_job + state = PreviewState(load_preview([path])) + transport = PreviewTransport(state, 0) + transport.publish({"frame": 7, "source_frame": 2}, b"jpeg-frame-7") + thread = threading.Thread(target=transport.server.serve_forever) + thread.start() + try: + snapshot = json.load(urllib.request.urlopen(transport.url + "snapshot")) + assert snapshot["frame"] == 7 and snapshot["source_frame"] == 2 + assert base64.b64decode(snapshot["image"].split(",")[1]) == b"jpeg-frame-7" + req = urllib.request.Request( + transport.url + "control", + data=b'{"progress":0.5}', + headers={"Content-Type": "application/json"}, + method="POST", + ) + assert urllib.request.urlopen(req).status == 204 + assert ( + state.values["progress"] == 0 + ) # only the render owner applies queued commands + state.apply(transport.commands.get_nowait()) + assert state.values["progress"] == 0.5 + req.add_header("Origin", "https://untrusted.example") + with pytest.raises(urllib.error.HTTPError) as error: + urllib.request.urlopen(req) + assert error.value.code == 403 + finally: + transport.server.shutdown() + thread.join(timeout=5) + transport.server.server_close() + assert not thread.is_alive() + + +def test_preview_cli_dispatch_does_not_collect(monkeypatch, recorded_job): + from embodichain.cli.expansion import main + import embodichain.lab.expansion._preview_server as server + from embodichain.lab.expansion import ExpansionEngine + + path, _, _ = recorded_job + called = [] + monkeypatch.setattr( + server, "serve_preview", lambda data, **kw: called.append((len(data.clips), kw)) + ) + monkeypatch.setattr( + ExpansionEngine, + "from_job", + lambda *a, **kw: pytest.fail("preview must not collect"), + ) + assert main(["preview", "--output", str(path), "--gpu", "2", "--open"]) == 0 + assert ( + called[0][0] == 1 and called[0][1]["gpu"] == 2 and called[0][1]["open_browser"] + ) + + +def test_viewer_html_is_in_package_data(tmp_path): + from setup import get_package_data + from setuptools import Distribution + from setuptools.command.build_py import build_py + import shutil + + package = "embodichain.lab.expansion" + source = tmp_path / "source" + source.mkdir() + original = ( + Path(__file__).resolve().parents[3] / "embodichain/lab/expansion/_preview.html" + ) + shutil.copyfile(original, source / "_preview.html") + (source / "__init__.py").write_text("") + distribution = Distribution( + { + "packages": [package], + "package_dir": {package: str(source)}, + "package_data": get_package_data(), + "include_package_data": False, + } + ) + distribution.script_name = "setup.py" + command = build_py(distribution) + command.build_lib = str(tmp_path / "build") + command.ensure_finalized() + command.run() + packaged = Path(command.build_lib) / "embodichain/lab/expansion/_preview.html" + assert packaged.read_bytes() == original.read_bytes() From 80d2d5175ff3aefe5301f1df219a0ff6fca17a80 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sun, 15 Nov 2026 09:48:56 +0000 Subject: [PATCH 13/45] feat(expansion): collect and preview bounded parameter distributions from jobs --- .../expansion-engine/expansion-engine.md | 13 +- .../motion-planning/trajectory-expansion.md | 10 + embodichain/cli/expansion.py | 102 +++- embodichain/lab/expansion/_gym_episode.py | 1 + embodichain/lab/expansion/_preview.html | 3 +- embodichain/lab/expansion/_preview_data.py | 49 +- embodichain/lab/expansion/_preview_job.py | 550 ++++++++++++++++++ embodichain/lab/expansion/_preview_render.py | 26 + embodichain/lab/expansion/session_cfg.py | 7 + .../lab/sim/motion/expansion/_samples.py | 43 ++ embodichain/lab/sim/motion/expansion/cfg.py | 8 + .../lab/sim/motion/expansion/combined.py | 10 + .../lab/sim/motion/expansion/operators.py | 58 +- .../lab/sim/motion/expansion/variants.py | 3 + .../task_program/integrations/expansion.py | 4 + examples/expansion/engine/README.md | 23 +- examples/expansion/engine/ghost-preview.md | 101 +++- examples/expansion/engine/job.preview.yaml | 20 + examples/expansion/engine/motion.preview.yaml | 55 ++ tests/lab/expansion/test_episode_assembly.py | 1 + tests/lab/expansion/test_preview.py | 73 ++- tests/lab/expansion/test_preview_job.py | 202 +++++++ tests/sim/motion/expansion/test_operators.py | 94 +++ 23 files changed, 1423 insertions(+), 33 deletions(-) create mode 100644 embodichain/lab/expansion/_preview_job.py create mode 100644 embodichain/lab/sim/motion/expansion/_samples.py create mode 100644 examples/expansion/engine/job.preview.yaml create mode 100644 examples/expansion/engine/motion.preview.yaml create mode 100644 tests/lab/expansion/test_preview_job.py diff --git a/agent_context/topics/expansion-engine/expansion-engine.md b/agent_context/topics/expansion-engine/expansion-engine.md index 1155740b83..255bdcaa1e 100644 --- a/agent_context/topics/expansion-engine/expansion-engine.md +++ b/agent_context/topics/expansion-engine/expansion-engine.md @@ -93,7 +93,7 @@ time; physics stays disabled and render updates do not mutate saved evidence. cross-group frame through the public marker layer. Task assets and any collection process orchestration remain outside the replay core. -`embodichain expansion preview` reads confirmed job outputs through `_preview_data.py`, +`embodichain expansion preview --input` reads confirmed job outputs through `_preview_data.py`, then replays full measured states using `_preview_render.py` and a loopback-only `_preview_server.py`. Inputs retain their original evidence; no collection or physics stepping occurs. The first version accepts one URDF robot and Cube rigid @@ -103,6 +103,17 @@ single immutable image/state snapshot; WebRTC is follow-up work. FK and episode budgets bound preload, controls are queued for the render owner, and native resources/ports close on exit. See the example's `ghost-preview.md` for usage. +`preview --job` uses `_preview_job.py` to freeze lower/upper/center and stratified +interior parameter points, then invokes ordinary native collection in owned child +processes before opening the viewer. Physical ranges, atlas request domains and +explicit joint/via-point draws retain original validation. Each point enumerates +its configured motion/affordance recipes; exact-zero residuals collapse to the +unique nominal variant. Source semantics and aggregate attempts/time budgets are +checked, failed endpoints remain in the coverage manifest, and no replacement +sampling or success credit is allowed. Original job outputs remain untouched. Episodes archive `source_env_id` for +row-correct diagnostics even without a motion profile; ambiguous legacy grasp +annotations remain unavailable rather than selecting row zero. + `TaskProgramDemoBridge.program_id` and `env_ids` expose binding identity without runtime traversal. `EmbodiedEnv.task_program_bridge_scope()` restores the borrowed bridge even after failure. `Real2SimTabletopSource` and explicit diff --git a/agent_context/topics/motion-planning/trajectory-expansion.md b/agent_context/topics/motion-planning/trajectory-expansion.md index 5d8dcb9fe5..d6faa8a58f 100644 --- a/agent_context/topics/motion-planning/trajectory-expansion.md +++ b/agent_context/topics/motion-planning/trajectory-expansion.md @@ -93,3 +93,13 @@ separate from path, dynamic and task validation. Use `tests/sim/motion/expansion/` for algorithm/contracts coverage and host tests for stepping, acceptance and persistence. Examples are not production test APIs. + +## Explicit boundary draws + +Spatial profiles optionally carry `joint_samples` (named controlled-joint signed +fractions) and `knot_samples` (via-point magnitudes), each finite in [-1,1]. +The combined Task Program adapter forwards them to the ordinary operators; +absence retains local-RNG sampling. Explicit draws never bypass phase permissions, +endpoint protection, deduplication, joint limits or measured acceptance. One named +joint draw is reused across permitted phases. The Expansion Engine preview owns +parameter-domain sampling and coverage accounting, not the motion operators. diff --git a/embodichain/cli/expansion.py b/embodichain/cli/expansion.py index 5c2ace7a7a..d3774a65d8 100644 --- a/embodichain/cli/expansion.py +++ b/embodichain/cli/expansion.py @@ -48,11 +48,37 @@ def main(argv: Sequence[str] | None = None) -> int: command = commands.add_parser( "preview", help="Replay confirmed results with native ghost rendering" ) + source = command.add_mutually_exclusive_group(required=True) + source.add_argument( + "--input", + "--inpu", + dest="inputs", + action="append", + help="Existing job output or preview-plan directory; repeat for groups", + ) + source.add_argument( + "--job", + help="Compute an endpoint/interior preview from a Task Program expansion job", + ) command.add_argument( "--output", - action="append", - required=True, - help="Result directory with job.json; repeat for spatial groups", + help="New collection directory for --job; never an existing-result input", + ) + command.add_argument( + "--interior-samples", + type=int, + default=5, + help="Stratified interior parameter points, in addition to center and both extrema", + ) + command.add_argument( + "--plan-only", + action="store_true", + help="Freeze and validate the preview plan without collection/rendering", + ) + command.add_argument( + "--no-open", + action="store_true", + help="Do not open a browser after --job collection", ) command.add_argument( "--reference", @@ -97,8 +123,72 @@ def main(argv: Sequence[str] | None = None) -> int: from embodichain.lab.expansion._preview_server import serve_preview try: + if args.open and args.no_open: + parser.error("--open and --no-open are mutually exclusive") + coverage = None + if args.job: + if args.reference: + parser.error( + "--reference is only valid with existing --input results" + ) + from embodichain.lab.expansion._preview_job import ( + plan_preview, + collect_preview, + ) + + coverage = plan_preview( + Path(args.job), + Path(args.output) if args.output else None, + interior=args.interior_samples, + max_episodes=args.max_episodes, + ) + print( + json.dumps( + { + "preview_plan": str( + Path(coverage["output"]) / "preview-plan.json" + ), + "parameter_points": len(coverage["records"]), + "planned_attempts": coverage["planned_attempts"], + } + ), + flush=True, + ) + if args.plan_only: + return 0 + import torch + + if args.gpu >= torch.cuda.device_count(): + parser.error( + "preview GPU is outside CUDA_VISIBLE_DEVICES; use a visible device ordinal" + ) + coverage = collect_preview(coverage) + print( + json.dumps( + { + "confirmed": coverage["confirmed"], + "planned_attempts": coverage["planned_attempts"], + "coverage_complete": coverage["coverage_complete"], + "output": coverage["output"], + } + ), + flush=True, + ) + if not coverage["confirmed"]: + print( + "No qualified trajectories; inspect preview-plan.json and sample logs. No boundary was replaced.", + flush=True, + ) + return 2 + inputs = [Path(coverage["output"])] + else: + if args.output or args.plan_only: + parser.error( + "--output and --plan-only require --job; use --input for existing results" + ) + inputs = [Path(p) for p in args.inputs] data = load_preview( - [Path(p) for p in args.output], + inputs, Path(args.reference) if args.reference else None, robot_asset=Path(args.robot_asset) if args.robot_asset else None, eef_link=args.eef_link, @@ -108,14 +198,14 @@ def main(argv: Sequence[str] | None = None) -> int: data, port=args.port, gpu=args.gpu, - open_browser=args.open, + open_browser=(bool(args.job) or args.open) and not args.no_open, duration=args.duration, ) except KeyboardInterrupt: return 0 except (ValueError, OSError) as exc: parser.error(str(exc)) - return 0 + return 2 if coverage and not coverage["coverage_complete"] else 0 from embodichain.lab.expansion.configured import ( load_expansion_job, ) diff --git a/embodichain/lab/expansion/_gym_episode.py b/embodichain/lab/expansion/_gym_episode.py index 7d504163a7..fd686fc458 100644 --- a/embodichain/lab/expansion/_gym_episode.py +++ b/embodichain/lab/expansion/_gym_episode.py @@ -121,6 +121,7 @@ def assemble(self, assignments, collector, services, result, physics_after): observations[key] = torch.stack(values) scene_keys.append({"key": key, "entity_uid": uid, "field": field}) metadata = { + "source_env_id": row, "attempt_sequence": ctx.requests[row].sequence, "job_instance_id": ctx.journal.job_instance_id, "recipe_id": ctx.requests[row].recipe.recipe_id, diff --git a/embodichain/lab/expansion/_preview.html b/embodichain/lab/expansion/_preview.html index ad53361468..f1615652c9 100644 --- a/embodichain/lab/expansion/_preview.html +++ b/embodichain/lab/expansion/_preview.html @@ -4,7 +4,7 @@

EmbodiChain · 扩增结果回放

完整模型虚影 · 已确认的真实扩增结果

正在连接原生渲染器
拖动画面旋转 · 滚轮缩放
EmbodiChain 原生场景渲染画面
正在加载完整机器人模型…
——
-
EmbodiChain SimulationManager / RobotMarkers / VisualizationMarkers → DexSim Hybrid 原生渲染。仅回放,物理推进 0 步。机器人与物体使用逐帧记录;轨迹线每 4 帧取点并连接。视角、灯光与透明材质仅用于验收展示。
@@ -21,6 +21,7 @@ let pointer=null;$('frame').onpointerdown=e=>{if(!status)return;pointer={x:e.clientX,y:e.clientY,yaw:status.yaw,e:status.elevation};$('frame').setPointerCapture(e.pointerId)};$('frame').onpointermove=e=>{if(pointer)control({yaw:Math.max(-20,Math.min(20,pointer.yaw-(e.clientX-pointer.x)*.008)),elevation:Math.max(.1,Math.min(1.5,pointer.e+(e.clientY-pointer.y)*.006))})};$('frame').onpointerup=$('frame').onpointercancel=()=>pointer=null; $('frame').addEventListener('wheel',e=>{e.preventDefault();if(status)control({distance:Math.max(.2,Math.min(20,status.distance+e.deltaY*.001))})},{passive:false}); async function poll(){if(busy||document.hidden)return;busy=true;try{const r=await fetch('/snapshot');if(!r.ok)throw Error('连接失败');const s=await r.json();if(s.frame===undefined)return;status=s; +if(s.coverage){$('coverage-panel').hidden=false;const c=s.coverage;$('coverage-summary').textContent=`计划 ${c.planned_attempts} · 已尝试 ${c.records.reduce((n,r)=>n+r.attempts,0)} · 已确认 ${c.records.reduce((n,r)=>n+r.confirmed,0)}。${c.coverage_complete?'全部计划样本已确认':'包含未通过、回退或未执行点'}`;$('coverage-details').style.whiteSpace='pre-line';$('coverage-details').textContent='有限参数采样,不代表连续空间或全部边界组合穷举。\n'+c.axes.map(a=>`${a.name}: [${a.lower}, ${a.upper}]`).join('\n')+'\n\n'+c.records.map(r=>`#${r.index} ${r.role} · ${r.status} · ${r.confirmed}/${r.planned ?? c.recipes_per_point} 确认\n${Object.entries(r.values).map(([k,v])=>k+'='+Number(v).toPrecision(4)).join(', ')}`).join('\n\n')} if(!initialized){for(const key of ['space','aff','variant'])for(const value of s.options[key]){const o=document.createElement('option');o.value=value;o.textContent=value==='unrecorded'?'未记录':value;$(key).append(o)}if(!s.options.align.includes('phase'))$('align').querySelector('[value="phase"]').remove();$('reference').disabled=!s.has_reference;initialized=true} if(!sending){for(const k of ['space','aff','variant','align'])if(document.activeElement!==$(k))$(k).value=s[k];for(const k of ['ghosts','reference','paths','targets','positions'])$(k).checked=s[k];if(document.activeElement!==$('camera'))$('camera').value=Object.keys(cameras).find(k=>Object.keys(cameras[k]).every(p=>Math.abs(s[p]-cameras[k][p])<.001))||'custom'} $('connection').textContent='● 原生渲染 · HTTP JPEG';$('connection').classList.remove('error');$('count').textContent=s.count;$('total').textContent=` / ${s.total} 条符合筛选`;$('play').disabled=!s.count;$('play').textContent=s.playing?'暂停':'播放';if(!dragging)$('progress').value=Math.round(s.progress*1000); diff --git a/embodichain/lab/expansion/_preview_data.py b/embodichain/lab/expansion/_preview_data.py index 0dfeb76d8c..adf91221d0 100644 --- a/embodichain/lab/expansion/_preview_data.py +++ b/embodichain/lab/expansion/_preview_data.py @@ -56,15 +56,35 @@ class PreviewClip: def annotations(metadata: Any, measured: MeasuredTrajectory) -> dict[str, Any]: records = metadata.get("expansion", ()) - env_ids = {r["env_id"] for r in records if "env_id" in r} + env_ids = {r["env_id"] for r in records if r.get("env_id") is not None} + if metadata.get("source_env_id") is not None: + source_row = metadata["source_env_id"] + if type(source_row) is not int or source_row < 0: + raise ValueError("source_env_id must be a nonnegative integer") + env_ids.add(source_row) if len(env_ids) > 1: raise ValueError("Episode expansion annotations refer to multiple rows") env_id = next(iter(env_ids), None) branches, candidates, targets, phases = [], [], [], [] + phase_safe = True for segment in metadata.get("program_segments", ()): for call in segment.get("metadata", {}).get("runtime", {}).get("calls", ()): for event in call.get("events", ()): if event.get("kind") == "trajectory_segment_entered": + mask = event.get("env_mask") + if mask: + if env_id is not None: + index = 0 if len(mask) == 1 else env_id + if index >= len(mask): + phase_safe = False + continue + if not mask[index]: + continue + elif len(set(mask)) > 1: + phase_safe = False + continue + elif not any(mask): + continue phases.append( ( str(call["call"]["call_kind"]) @@ -93,9 +113,8 @@ def annotations(metadata: Any, measured: MeasuredTrajectory) -> dict[str, Any]: else 0 if env_id is None and len(rows) == 1 else None ) if row is None: - raise ValueError( - "Cannot bind grasp annotation to the recorded environment row" - ) + # Legacy numeric evidence remains usable; never guess another row's grasp. + continue if not sample["success"][row]: continue sampling = sample.get("sampling") or {} @@ -127,7 +146,8 @@ def annotations(metadata: Any, measured: MeasuredTrajectory) -> dict[str, Any]: phases = [v for i, v in enumerate(phases) if i == 0 or v != phases[i - 1]] anchors = np.array([p[1] for p in phases] + [measured.timestamps[-1]]) if ( - not phases + not phase_safe + or not phases or anchors[0] < 0 or not np.isfinite(anchors).all() or not np.all(np.diff(anchors) > 0) @@ -203,6 +223,7 @@ class PreviewData: eef_link: str boxes: dict[str, tuple[float, ...]] ground_visible: bool = True + coverage: dict[str, Any] | None = None @property def all_clips(self) -> list[PreviewClip]: @@ -226,6 +247,21 @@ def load_preview( ) -> PreviewData: if not outputs or not 1 <= max_episodes <= 64: raise ValueError("Provide result directories and max_episodes in [1,64]") + coverage = None + if len(outputs) == 1 and (Path(outputs[0]) / "preview-plan.json").is_file(): + root = Path(outputs[0]).resolve() + coverage = json.loads((root / "preview-plan.json").read_text()) + outputs = [] + for record in coverage["records"]: + path = (root / record["result"]).resolve() + if not path.is_relative_to(root): + raise ValueError("Preview result path escapes the manifest directory") + if record["confirmed"] > 0: + outputs.append(path) + if not outputs: + raise ValueError( + "Preview plan has no qualified trajectories; inspect its rejection/unrun records" + ) paths = [Path(p).expanduser().resolve() for p in outputs] if len(set(paths)) != len(paths): raise ValueError("Duplicate output directory") @@ -335,7 +371,7 @@ def load_preview( else: clips.append(clip) clip.label = f"C-{len(clips):03d}" - return PreviewData(clips, reference_clip, asset, end, boxes, ground) + return PreviewData(clips, reference_clip, asset, end, boxes, ground, coverage) class PreviewState: @@ -443,6 +479,7 @@ def snapshot(self) -> dict[str, Any]: "total": len(self.data.clips), "options": self.options, "has_reference": self.data.reference is not None, + "coverage": self.data.coverage, "label": c.label if visible else "No matches", "source_frame": f if visible else None, "source_frames": len(c.measured.timestamps) if visible else 0, diff --git a/embodichain/lab/expansion/_preview_job.py b/embodichain/lab/expansion/_preview_job.py new file mode 100644 index 0000000000..55086e7306 --- /dev/null +++ b/embodichain/lab/expansion/_preview_job.py @@ -0,0 +1,550 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Freeze endpoint/interior experiments and collect them through the native CLI.""" + +from __future__ import annotations + +from copy import deepcopy +from datetime import datetime, timezone +from pathlib import Path +from typing import Any +import hashlib +import json +import os +import signal +import subprocess +import sys +import time +from uuid import uuid4 +import numpy as np +from scipy.spatial.transform import Rotation +import yaml + +from .configured import load_expansion_job, preflight_expansion_job +from .persistence import ArtifactEpisodeSink, load_expansion_episode + +__all__: list[str] = [] + + +def boundary_samples( + dimensions: int, interior: int, seed: int +) -> tuple[np.ndarray, list[str]]: + if ( + type(dimensions) is not int + or dimensions < 0 + or type(interior) is not int + or not 1 <= interior <= 61 + ): + raise ValueError("interior samples must be in [1,61]") + if not dimensions: + return np.empty((1, 0)), ["fixed"] + rng = np.random.default_rng(seed) + points = [np.full(dimensions, 0.5), np.zeros(dimensions), np.ones(dimensions)] + # Independent Latin-hypercube strata in every varying coordinate. + inside = np.stack( + [ + (rng.permutation(interior) + rng.uniform(0.05, 0.95, interior)) / interior + for _ in range(dimensions) + ], + axis=1, + ) + points.extend(inside) + return np.asarray(points), ["center", "lower", "upper"] + [ + f"interior-{i+1}" for i in range(interior) + ] + + +def _atomic(path: Path, value: Any) -> None: + temporary = path.with_suffix(".tmp") + temporary.write_text(json.dumps(value, indent=2, allow_nan=False)) + temporary.replace(path) + + +def plan_preview( + job: Path, output: Path | None, *, interior: int = 5, max_episodes: int = 32 +) -> dict[str, Any]: + from embodichain.lab.sim.motion.expansion.profile import load_expansion_profile + from embodichain.lab.sim.motion.expansion.combined import ( + enumerate_candidate_recipes, + ) + + if type(max_episodes) is not int or not 1 <= max_episodes <= 64: + raise ValueError("max_episodes must be in [1,64]") + cfg = load_expansion_job(job) + report = preflight_expansion_job(cfg) + if cfg.source != "task_program" or report.get("motion_source") != "native": + raise ValueError( + "preview --job currently requires native Task Program planning" + ) + if cfg.visual_profile or cfg.render_profiles or cfg.source_profile: + raise ValueError( + "Distribution preview does not sample visual or prototype profiles" + ) + axes = [] + + def axis(name: str, lower: float, upper: float, kind: str, **details: Any) -> None: + if lower != upper: + axes.append( + dict( + name=name, + lower=float(lower), + upper=float(upper), + kind=kind, + **details, + ) + ) + + for kind in ( + "mass_scale", + "friction", + "joint_stiffness_scale", + "joint_damping_scale", + ): + for uid, bounds in sorted(getattr(cfg, kind).items()): + axis(f"{kind}/{uid}", *bounds, kind, uid=uid) + for uid, ranges in sorted(cfg.center_of_mass.items()): + for i, bounds in enumerate(ranges): + axis( + f"center_of_mass/{uid}/{i}", *bounds, "center_of_mass", uid=uid, index=i + ) + profile = None + motion = None + recipe_count = 1 + if cfg.motion_profile: + profile = load_expansion_profile(cfg.motion_profile) + motion = yaml.safe_load(Path(cfg.motion_profile).read_text()) + recipe_count = len(enumerate_candidate_recipes(profile)) + if profile.scene_randomization.reference_family_count != 1: + raise ValueError( + "Use a scene atlas for spatial bounds, not implicit motion families" + ) + spatial = profile.trajectory.spatial + if spatial.enabled and profile.trajectory.variants_per_family > 1: + if profile.trajectory.variants_per_family < len(spatial.operators) + 1: + raise ValueError( + "Preview needs a nominal variant plus at least one variant per configured operator" + ) + if set(spatial.operators) - {"joint_residual", "via_points"}: + raise ValueError( + "Explicit boundary draws support joint_residual and via_points" + ) + robot = report["resolved_deployment"]["robot"] + groups = robot["control_parts"] + parts = robot.get("solver_cfg", {}).keys() + joints = sorted({name for part in parts for name in groups.get(part, [])}) + if not joints or any(not isinstance(j, str) for j in joints): + raise ValueError( + "Named solver control joints are required to bound motion draws" + ) + for name in joints: + if name not in spatial.joint_samples: + axis(f"joint_residual_fraction/{name}", -1, 1, "joint", uid=name) + if "via_points" in spatial.operators and not spatial.knot_samples: + for i in range(spatial.via_count): + axis(f"via_knot_fraction/{i}", -1, 1, "knot", index=i) + prepared = None + deployment = None + if cfg.scene_atlas: + from embodichain.lab.sim.scene.feasibility import load_feasibility_atlas + from embodichain.lab.sim.scene.feasibility.search import ( + PreparedFeasibilitySearch, + ) + from embodichain.lab.sim.scene.feasibility.cfg import SceneRobotFeasibilityCfg + from embodichain.lab.gym.utils.composition import resolve_gym_components + + atlas = load_feasibility_atlas(cfg.scene_atlas) + if len(atlas.request.modes) != 1: + raise ValueError("Preview requires one task mode per scene atlas") + prepared = PreparedFeasibilitySearch( + atlas.request, SceneRobotFeasibilityCfg(seed=cfg.seed) + ) + for i, var in enumerate(atlas.request.variables): + for j, (lo, hi) in enumerate(zip(var.lower, var.upper)): + axis( + f'scene/{var.name}/{("x","y","yaw")[j]}', + lo, + hi, + "scene", + index=i * 3 + j, + ) + raw = yaml.safe_load(Path(cfg.deployment).read_text()) + deployment = resolve_gym_components( + raw, base_dir=Path(cfg.deployment).parent + ).config + for key in ("program", "integration", "execution_policy"): + value = deployment.get("task_program", {}).get(key) + if isinstance(value, str): + deployment["task_program"][key] = str( + (Path(cfg.deployment).parent / value).resolve() + ) + elif isinstance(value, dict) and isinstance(value.get("component"), str): + value["component"] = str( + (Path(cfg.deployment).parent / value["component"]).resolve() + ) + if "embodiment" in raw: + deployment.pop("robot", None) + deployment.pop("sensor", None) + embodiment = deepcopy(raw["embodiment"]) + embodiment["component"] = str( + (Path(cfg.deployment).parent / embodiment["component"]).resolve() + ) + deployment["embodiment"] = embodiment + points, roles = boundary_samples(len(axes), interior, cfg.seed) + counts = [] + for unit in points: + samples = dict(profile.trajectory.spatial.joint_samples) if profile else {} + for a, u in zip(axes, unit): + if a["kind"] == "joint": + samples[a["uid"]] = a["lower"] + float(u) * (a["upper"] - a["lower"]) + counts.append( + profile.affordance.branches_per_family + if profile and samples and all(v == 0 for v in samples.values()) + else recipe_count + ) + total = sum(counts) + if total > max_episodes or total > 64 or total > cfg.max_attempts: + raise ValueError( + f"Preview needs {len(points)} parameter points and up to {recipe_count} motion/affordance recipes per point = {total} attempts; increase --max-episodes (up to 64) and the job max_attempts budget, reduce --interior-samples or use a smaller recipe. Nothing executed." + ) + root = ( + Path(output).expanduser().resolve() + if output + else Path(cfg.output_dir).parent + / ( + Path(cfg.output_dir).name + + "-preview-" + + datetime.now(timezone.utc).strftime("%Y%m%d-%H%M%S") + + "-" + + uuid4().hex[:6] + ) + ) + if root.exists(): + raise FileExistsError( + f"Preview output must be new: {root}; use --input to replay" + ) + # Validate the renderer's supported scene before any costly collection. + from ._preview_data import _scene + + supported = deepcopy(report) + supported["config"] = dict(report.get("config", {}), scene_atlas=None) + _scene(supported) + root.mkdir(parents=True) + (root / "configs").mkdir() + (root / "logs").mkdir() + plan = { + "schema_version": 1, + "source_job": str(Path(job).resolve()), + "source_sha256": hashlib.sha256(Path(job).read_bytes()).hexdigest(), + "source_inputs": report["input_files"], + "source_atlas": cfg.scene_atlas, + "seed": cfg.seed, + "sampling": "lower + upper + center + stratified interior; not a Cartesian grid", + "axes": axes, + "planned_attempts": total, + "recipes_per_point": recipe_count, + "collection_budget_s": cfg.max_wall_time_s, + "output": str(root), + "records": [], + "complete": False, + "coverage_complete": False, + } + for index, (unit, role) in enumerate(zip(points, roles)): + values = { + a["name"]: a["lower"] + float(u) * (a["upper"] - a["lower"]) + for a, u in zip(axes, unit) + } + case = cfg.to_dict() + case.update( + output_dir=str(root / f"jobs/sample-{index:03d}"), + target_episodes=recipe_count, + max_attempts=recipe_count, + retry_interrupted=False, + ) + m = deepcopy(motion) + scene_unit = np.zeros(prepared.dimensions) if prepared else None + if m: + spatial = m["trajectory"]["spatial"] + spatial["joint_samples"] = dict(profile.trajectory.spatial.joint_samples) + knots = list( + profile.trajectory.spatial.knot_samples + or [0.0] * profile.trajectory.spatial.via_count + ) + for a, u in zip(axes, unit): + value = values[a["name"]] + kind = a["kind"] + if kind == "joint": + spatial["joint_samples"][a["uid"]] = value + elif kind == "knot": + knots[a["index"]] = value + elif kind == "scene": + scene_unit[a["index"]] = u + elif kind == "center_of_mass": + case[kind][a["uid"]] = [list(v) for v in case[kind][a["uid"]]] + case[kind][a["uid"]][a["index"]] = [value, value] + else: + case[kind][a["uid"]] = [value, value] + if m: + if "via_points" in profile.trajectory.spatial.operators: + spatial["knot_samples"] = knots + # Exact zero residuals are the nominal trajectory, not extra successes. + # Keep native deduplication intact by requesting only that unique variant. + if spatial.get("joint_samples") and all( + v == 0 for v in spatial["joint_samples"].values() + ): + m["trajectory"]["variants_per_family"] = 1 + case["target_episodes"] = case["max_attempts"] = ( + profile.affordance.branches_per_family + ) + motion_path = root / "configs" / f"motion-{index:03d}.yaml" + motion_path.write_text(yaml.safe_dump(m, sort_keys=False)) + case["motion_profile"] = str(motion_path) + requested_positions = { + obj["uid"]: obj.get("init_pos", [0, 0, 0]) + for obj in report["resolved_deployment"].get("rigid_object", []) + } + if prepared: + candidate = prepared.candidate( + scene_unit, mode_id=prepared.request.modes[0].mode_id, ordinal=index + ) + derived = deepcopy(deployment) + changes = { + p.entity_uid: np.asarray(p.arena_pose) + for p in candidate.scene.pose_changes + } + for obj in derived.get( + "rigid_object", derived.get("simulation", {}).get("rigid_object", []) + ): + if obj["uid"] in changes: + pose = changes.pop(obj["uid"]) + obj["init_pos"] = pose[:3, 3].tolist() + obj["init_rot"] = ( + Rotation.from_matrix(pose[:3, :3]) + .as_euler("xyz", degrees=True) + .tolist() + ) + requested_positions[obj["uid"]] = obj["init_pos"] + if changes: + raise ValueError( + f"Unsupported scene variable entities: {list(changes)}" + ) + robot_uid = report["resolved_deployment"]["robot"]["uid"] + for uid, pose in candidate.deployment.poses.items(): + if uid != robot_uid: + raise ValueError("Scene variable selects a different robot") + pose = np.asarray(pose) + target = ( + derived["embodiment"].setdefault("overrides", {}) + if "embodiment" in derived + else derived["robot"] + ) + target["init_pos"] = pose[:3, 3].tolist() + target["init_rot"] = ( + Rotation.from_matrix(pose[:3, :3]) + .as_euler("xyz", degrees=True) + .tolist() + ) + deployment_path = root / "configs" / f"task-{index:03d}.yaml" + deployment_path.write_text(yaml.safe_dump(derived, sort_keys=False)) + case["deployment"] = str(deployment_path) + case["scene_atlas"] = None + config_path = root / "configs" / f"job-{index:03d}.yaml" + config_path.write_text(yaml.safe_dump(case, sort_keys=False)) + # Same public preflight and acceptance configuration as ordinary collection. + pinned = preflight_expansion_job(load_expansion_job(config_path)) + plan["records"].append( + dict( + semantic_id=pinned["semantic_id"], + scene_candidate_id=candidate.candidate_id if prepared else None, + index=index, + role=role, + unit=unit.tolist(), + values=values, + requested_positions=requested_positions, + job=str(config_path.relative_to(root)), + result=str(Path(case["output_dir"]).relative_to(root)), + status="unrun", + confirmed=0, + attempts=0, + planned=case["target_episodes"], + ) + ) + plan["planned_attempts"] = sum(r["planned"] for r in plan["records"]) + _atomic(root / "preview-plan.json", plan) + return plan + + +def _exact_recipe(metadata: Any) -> bool: + records = metadata.get("expansion", ()) + if any( + r.get("fallback_count", 0) + or ( + r.get("trajectory_requested") is not None + and r.get("trajectory_requested") != r.get("trajectory_selected") + ) + for r in records + ): + return False + if not any(r.get("affordance_requested") is not None for r in records): + return True + rows = {r["env_id"] for r in records if r.get("env_id") is not None} + env_id = metadata.get("source_env_id", next(iter(rows)) if len(rows) == 1 else None) + found = False + for segment in metadata.get("program_segments", ()): + for call in segment.get("metadata", {}).get("runtime", {}).get("calls", ()): + attempts = call.get("plan_attempts", ()) + if not attempts: + continue + sample = ( + attempts[-1] + .get("planner_diagnostics", {}) + .get("metadata", {}) + .get("affordance_sample", {}) + .get("grasp") + ) + if sample is None: + continue + ids = list(sample["env_ids"]) + index = ( + ids.index(env_id) + if env_id in ids + else 0 if env_id is None and len(ids) == 1 else None + ) + if ( + index is None + or not sample["success"][index] + or sample.get("reused", [True] * len(ids))[index] + ): + return False + found = True + return found + + +def collect_preview(plan: dict[str, Any]) -> dict[str, Any]: + root = Path(plan["output"]) + started = time.monotonic() + for record in plan["records"]: + remaining = plan["collection_budget_s"] - (time.monotonic() - started) + if remaining <= 0: + record["status"] = "budget_exhausted" + continue + job = root / record["job"] + cfg = yaml.safe_load(job.read_text()) + cfg["max_wall_time_s"] = min(cfg["max_wall_time_s"], remaining) + job.write_text(yaml.safe_dump(cfg, sort_keys=False)) + current = preflight_expansion_job(load_expansion_job(job)) + if current["semantic_id"] != record["semantic_id"]: + record["status"] = "source_changed" + _atomic(root / "preview-plan.json", plan) + raise ValueError("Preview inputs changed after planning; create a new plan") + remaining = plan["collection_budget_s"] - (time.monotonic() - started) + if remaining <= 0: + record["status"] = "budget_exhausted" + _atomic(root / "preview-plan.json", plan) + continue + record["status"] = "running" + _atomic(root / "preview-plan.json", plan) + print( + f"Preview sample {record['index']+1}/{len(plan['records'])}: {record['role']}", + flush=True, + ) + env = dict(os.environ) + env.setdefault("OMP_NUM_THREADS", "1") + env.setdefault("SDL_VIDEODRIVER", "offscreen") + with (root / "logs" / f"sample-{record['index']:03d}.log").open("w") as log: + process = subprocess.Popen( + [ + sys.executable, + "-m", + "embodichain", + "expansion", + "run", + "--job", + str(job), + ], + stdout=log, + stderr=subprocess.STDOUT, + env=env, + ) + timed_out = False + try: + process.wait(timeout=remaining) + except (subprocess.TimeoutExpired, KeyboardInterrupt) as error: + process.send_signal(signal.SIGINT) + try: + process.wait(timeout=10) + except subprocess.TimeoutExpired: + process.kill() + process.wait() + record["status"] = ( + "interrupted" + if isinstance(error, KeyboardInterrupt) + else "budget_exhausted" + ) + _atomic(root / "preview-plan.json", plan) + if isinstance(error, KeyboardInterrupt): + raise + timed_out = True + result = root / record["result"] + receipts = ArtifactEpisodeSink(result).inspect() if result.is_dir() else () + attempts = [ + json.loads(p.read_text()) + for p in (result / "attempts").glob("*/attempt.json") + ] + record.update( + confirmed=sum(r.confirmed for r in receipts), + attempts=len(attempts), + returncode=process.returncode, + outcomes=[ + { + "ordinal": a.get("ordinal"), + "status": a["status"], + "reason": a.get("reason") or a.get("error"), + } + for a in attempts + ], + ) + exact = 0 + for receipt in receipts: + episode = load_expansion_episode(receipt.storage_id) + if _exact_recipe(episode.metadata): + exact += 1 + record["exact_recipes"] = exact + record["status"] = ( + "budget_exhausted" + if timed_out + else ( + "confirmed" + if record["confirmed"] == record["planned"] + and exact == record["planned"] + else ( + "partial" + if record["confirmed"] + else "rejected" if process.returncode in (0, 2) else "error" + ) + ) + ) + _atomic(root / "preview-plan.json", plan) + plan["complete"] = all( + r["attempts"] >= r["planned"] + and r["status"] in {"confirmed", "partial", "rejected"} + for r in plan["records"] + ) + plan["coverage_complete"] = all(r["status"] == "confirmed" for r in plan["records"]) + plan["confirmed"] = sum(r["confirmed"] for r in plan["records"]) + plan["elapsed_s"] = time.monotonic() - started + _atomic(root / "preview-plan.json", plan) + return plan diff --git a/embodichain/lab/expansion/_preview_render.py b/embodichain/lab/expansion/_preview_render.py index eb75338930..4bf154de4a 100644 --- a/embodichain/lab/expansion/_preview_render.py +++ b/embodichain/lab/expansion/_preview_render.py @@ -212,6 +212,31 @@ def markers(name: str, shapes: dict) -> Any: ) }, ) + self.planned = markers( + "planned_parameter_positions", + { + "confirmed": MarkerPrototypeCfg( + shape="sphere", + scale=(0.018, 0.018, 0.018), + color=(0.3, 0.8, 0.4, 0.4), + ), + "unqualified": MarkerPrototypeCfg( + shape="sphere", + scale=(0.022, 0.022, 0.022), + color=(1, 0.25, 0.15, 0.65), + ), + }, + ) + if data.coverage: + points, types = [], [] + for record in data.coverage["records"]: + for point in record.get("requested_positions", {}).values(): + points.append(point) + types.append(0 if record["status"] == "confirmed" else 1) + self.planned.visualize( + np.asarray(points).reshape(-1, 3), + marker_indices=np.asarray(types, dtype=int), + ) self.camera = scene.add_camera( spawn.CameraDesc(name="expansion_preview_camera", resolution=(1100, 760)) ) @@ -314,6 +339,7 @@ def draw(self, state: PreviewState) -> bytes: objects.visualize( p[:, :3], p[:, 3:], marker_indices=np.arange(len(p)) ) + self.planned.set_visibility(v["positions"] and v["space"] == "all") self.paths.set_visibility(v["paths"]) self.targets.set_visibility(v["targets"]) self.positions.set_visibility(v["positions"]) diff --git a/embodichain/lab/expansion/session_cfg.py b/embodichain/lab/expansion/session_cfg.py index a66a3851fb..7808c707ca 100644 --- a/embodichain/lab/expansion/session_cfg.py +++ b/embodichain/lab/expansion/session_cfg.py @@ -125,6 +125,13 @@ def _decode(cls: type, data: Mapping[str, Any], path: str = "") -> Any: ) for item_key, item_value in value.items() } + elif value_type is float: + if any( + type(v) not in (int, float) or not math.isfinite(v) + for v in value.values() + ): + raise ValueError(f"{name} must map strings to finite numbers") + decoded[key] = {k: float(v) for k, v in value.items()} elif value_type is str: if any(type(item_value) is not str for item_value in value.values()): raise ValueError(f"{name} must map strings to strings") diff --git a/embodichain/lab/sim/motion/expansion/_samples.py b/embodichain/lab/sim/motion/expansion/_samples.py new file mode 100644 index 0000000000..7011c2c35f --- /dev/null +++ b/embodichain/lab/sim/motion/expansion/_samples.py @@ -0,0 +1,43 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Validated, explicit spatial draws for reproducible boundary experiments.""" + +from __future__ import annotations +from collections.abc import Mapping +import math + +__all__: list[str] = [] + + +def validate_samples( + joints: object, knots: object, via_count: int +) -> tuple[dict[str, float], tuple[float, ...]]: + if not isinstance(joints, Mapping) or any( + not isinstance(k, str) or not k for k in joints + ): + raise ValueError("joint_samples must map joint names to signed fractions") + if not isinstance(knots, (tuple, list)): + raise ValueError("knot_samples must be a sequence") + values = tuple(joints.values()) + tuple(knots) + if any( + type(v) not in (int, float) or not math.isfinite(v) or not -1 <= v <= 1 + for v in values + ): + raise ValueError("explicit spatial samples must be finite and in [-1,1]") + if knots and len(knots) != via_count: + raise ValueError("knot_samples must match via_count") + return {k: float(v) for k, v in joints.items()}, tuple(float(v) for v in knots) diff --git a/embodichain/lab/sim/motion/expansion/cfg.py b/embodichain/lab/sim/motion/expansion/cfg.py index 1a56e362fe..6f4f74598b 100644 --- a/embodichain/lab/sim/motion/expansion/cfg.py +++ b/embodichain/lab/sim/motion/expansion/cfg.py @@ -93,8 +93,16 @@ class _SpatialCfg: method: tuple[str, ...] = ("joint_residual",) joint_offset_scale: float = 0.05 via_count: int = 1 + joint_samples: dict[str, float] = {} + knot_samples: tuple[float, ...] = () def __post_init__(self) -> None: + from ._samples import validate_samples + + self.joint_samples, self.knot_samples = validate_samples( + self.joint_samples, self.knot_samples, self.via_count + ) + _boolean(self.enabled, "spatial.enabled") methods = (self.method,) if isinstance(self.method, str) else tuple(self.method) if not methods: diff --git a/embodichain/lab/sim/motion/expansion/combined.py b/embodichain/lab/sim/motion/expansion/combined.py index 0acae6e3b6..5f526a97a2 100644 --- a/embodichain/lab/sim/motion/expansion/combined.py +++ b/embodichain/lab/sim/motion/expansion/combined.py @@ -236,8 +236,16 @@ class _SpatialCfg: joint_offset_scale: float = 0.05 dedup_tolerance: float | None = None via_count: int = 1 + joint_samples: dict[str, float] = {} + knot_samples: tuple[float, ...] = () def __post_init__(self) -> None: + from ._samples import validate_samples + + self.joint_samples, self.knot_samples = validate_samples( + self.joint_samples, self.knot_samples, self.via_count + ) + self.operators = _strings(self.operators, "trajectory.spatial.operators") if any(item not in ("joint_residual", "via_points") for item in self.operators): raise ValueError( @@ -520,6 +528,8 @@ def from_mapping(cls, data: Mapping[str, Any]) -> "CombinedExpansionProfile": "enabled", "operators", "joint_offset_scale", + "joint_samples", + "knot_samples", "dedup_tolerance", "via_count", }, diff --git a/embodichain/lab/sim/motion/expansion/operators.py b/embodichain/lab/sim/motion/expansion/operators.py index f87bceed64..5f9bd20d62 100644 --- a/embodichain/lab/sim/motion/expansion/operators.py +++ b/embodichain/lab/sim/motion/expansion/operators.py @@ -17,6 +17,7 @@ """Grasp-pose and constrained qpos operators requiring downstream validation.""" from __future__ import annotations +from collections.abc import Mapping, Sequence from dataclasses import replace import math @@ -312,6 +313,7 @@ def joint_residual( joint_limits: torch.Tensor, normalized_scale: float, generator: torch.Generator, + joint_samples: Mapping[str, float] | None = None, ) -> TrajectoryTemplate: """Add one smooth, endpoint-preserving residual per allowed free phase. @@ -324,6 +326,10 @@ def joint_residual( joint_limits: Finite lower/upper limits with shape ``(D, 2)``. normalized_scale: Maximum offset as a fraction of each joint range. generator: Explicit local generator; global RNG is never consumed. + joint_samples: Optional signed fractions in [-1,1], keyed by all + controlled joint names. Extra names must belong to the full template. + Uses the same draw in each permitted phase; endpoints and validation + remain unchanged. Omit to retain ordinary random sampling. Returns: A new template requiring path and physical validation. @@ -336,6 +342,14 @@ def joint_residual( q = template.positions.clone() limits = _resolve_joint_limits(q, joint_limits) indices = list(template.controlled_joint_indices) + from ._samples import validate_samples + + samples, _ = validate_samples({} if joint_samples is None else joint_samples, (), 1) + if samples and ( + set(samples) - set(template.joint_names) + or any(template.joint_names[i] not in samples for i in indices) + ): + raise ValueError("joint_samples must cover the controlled joint names") for phase in phases: length = phase.stop_index - phase.start_index if length < 3: @@ -344,7 +358,16 @@ def joint_residual( values = torch.rand( len(indices), generator=generator, device=generator.device, dtype=q.dtype ).to(q.device) - offsets = (2 * values - 1) * scale * (limits[indices, 1] - limits[indices, 0]) + direction = ( + torch.tensor( + [samples[template.joint_names[i]] for i in indices], + device=q.device, + dtype=q.dtype, + ) + if samples + else 2 * values - 1 + ) + offsets = direction * scale * (limits[indices, 1] - limits[indices, 0]) q[phase.start_index : phase.stop_index, indices] += envelope[:, None] * offsets _reject_limit_violations(q, limits, indices) return replace(template, positions=q) @@ -357,6 +380,8 @@ def via_points( via_count: int, normalized_scale: float, generator: torch.Generator, + joint_samples: Mapping[str, float] | None = None, + knot_samples: Sequence[float] = (), ) -> TrajectoryTemplate: """Route each allowed free phase through sampled interior joint knots. @@ -383,6 +408,11 @@ def via_points( via_count: Number of interior knots per phase, at least one. normalized_scale: Maximum knot offset as a fraction of each joint range. generator: Explicit local generator; global RNG is never consumed. + knot_samples: Optional signed knot magnitudes; length must match via_count. + joint_samples: Optional signed fractions in [-1,1], keyed by all + controlled joint names. Extra names must belong to the full template. + Uses the same draw in each permitted phase; endpoints and validation + remain unchanged. Omit to retain ordinary random sampling. Returns: A new template requiring path and physical validation. @@ -398,6 +428,16 @@ def via_points( limits = _resolve_joint_limits(q, joint_limits) indices = list(template.controlled_joint_indices) spans = limits[indices, 1] - limits[indices, 0] + from ._samples import validate_samples + + samples, knots_fixed = validate_samples( + {} if joint_samples is None else joint_samples, knot_samples, via_count + ) + if samples and ( + set(samples) - set(template.joint_names) + or any(template.joint_names[i] not in samples for i in indices) + ): + raise ValueError("joint_samples must cover the controlled joint names") for phase in phases: length = phase.stop_index - phase.start_index if length < via_count + 2: @@ -408,8 +448,20 @@ def via_points( device=generator.device, dtype=q.dtype, ).to(q.device) - magnitudes = 2 * values[:via_count] - 1 - direction = 2 * values[via_count:] - 1 + magnitudes = ( + torch.tensor(knots_fixed, device=q.device, dtype=q.dtype) + if knots_fixed + else 2 * values[:via_count] - 1 + ) + direction = ( + torch.tensor( + [samples[template.joint_names[i]] for i in indices], + device=q.device, + dtype=q.dtype, + ) + if samples + else 2 * values[via_count:] - 1 + ) knots = magnitudes[:, None] * (direction * scale * spans)[None, :] progress = torch.linspace(0, 1, length, device=q.device, dtype=q.dtype) q[phase.start_index : phase.stop_index, indices] += _clamped_hermite( diff --git a/embodichain/lab/sim/motion/expansion/variants.py b/embodichain/lab/sim/motion/expansion/variants.py index 475c8790f8..949ee76ef4 100644 --- a/embodichain/lab/sim/motion/expansion/variants.py +++ b/embodichain/lab/sim/motion/expansion/variants.py @@ -556,6 +556,7 @@ def _apply_variant_once( joint_limits=joint_limits, normalized_scale=spatial.joint_offset_scale, generator=generator, + joint_samples=spatial.joint_samples, ) elif variant.spatial_operator == "via_points": result = via_points( @@ -564,6 +565,8 @@ def _apply_variant_once( via_count=spatial.via_count, normalized_scale=spatial.joint_offset_scale, generator=generator, + joint_samples=spatial.joint_samples, + knot_samples=spatial.knot_samples, ) elif variant.spatial_operator == "nullspace_residual": if target is not None: diff --git a/embodichain/lab/task_program/integrations/expansion.py b/embodichain/lab/task_program/integrations/expansion.py index 7e296f757c..7c62629306 100644 --- a/embodichain/lab/task_program/integrations/expansion.py +++ b/embodichain/lab/task_program/integrations/expansion.py @@ -937,6 +937,10 @@ def _combined_augmentation_payload( } }, } + if spatial.joint_samples: + payload["factors"]["spatial"]["joint_samples"] = spatial.joint_samples + if spatial.knot_samples: + payload["factors"]["spatial"]["knot_samples"] = spatial.knot_samples if spatial.dedup_tolerance is not None: payload["coverage"] = { "joint_dedup_normalized_tol": spatial.dedup_tolerance, diff --git a/examples/expansion/engine/README.md b/examples/expansion/engine/README.md index 61cc1c7383..e1165c7648 100644 --- a/examples/expansion/engine/README.md +++ b/examples/expansion/engine/README.md @@ -26,17 +26,26 @@ capabilities remain explicit. `--reuse` rebuilds the offline page without a Worl ## Native ghost preview -Replay confirmed results directly, without recollection or physics stepping: +Compute Task Program expansion at both parameter bounds and stratified interior +points, validate the real results, then automatically open the native viewer: ```bash -embodichain expansion preview --output /path/to/job-output --gpu 0 --open +embodichain expansion preview --job examples/expansion/engine/job.preview.yaml ``` -Repeat `--output` for spatial groups and optionally add `--reference` with one -confirmed reference episode. The [native preview guide](ghost-preview.md) covers -linked spatial/affordance/trajectory filters, asset recovery, supported scene -geometry, replay semantics and the WebRTC TODO. Rendering is native; browser -transport currently uses HTTP JPEG, not WebRTC. +Preview existing results without any collection: + +```bash +embodichain expansion preview --input /path/to/job-output --gpu 0 --open +``` + +Repeat `--input` for groups, or point it at a generated preview-plan directory. +`--inpu` is an alias for `--input`. In job mode `--output` selects a **new** output +folder; it is no longer the existing-result input flag. `--plan-only` freezes and +validates samples without executing, and `--no-open` suppresses automatic opening. +The [preview guide](ghost-preview.md) explains budgets, failed-boundary evidence, +scene-atlas domains, finite-sampling limitations and the WebRTC TODO. Rendering is +native; browser transport remains HTTP JPEG, not WebRTC. ## Run a configured job diff --git a/examples/expansion/engine/ghost-preview.md b/examples/expansion/engine/ghost-preview.md index dd6a49ddaa..e31f38505b 100644 --- a/examples/expansion/engine/ghost-preview.md +++ b/examples/expansion/engine/ghost-preview.md @@ -21,25 +21,97 @@ 仓库入口直接读取原始结果,不需要该源码快照、本地预处理 JSON 或实验脚本。 [showcase](showcase/README.md) 的离线三维/RGB-D 页面也是另一条查看路径。 +## 基于 Task Program 重新计算并预览 + +```bash +# 一体化入口:检查配置 → 固定采样计划 → 实测扩增 → 验收 → 原生预览。 +embodichain expansion preview --job examples/expansion/engine/job.preview.yaml + +# 先查看计划。--output 必须是新目录,不会覆盖原 job 的数据。 +embodichain expansion preview --job examples/expansion/engine/job.preview.yaml \ + --output /tmp/preview-plan-check --plan-only + +# 减少区间内采样点,用于快速验证;仍包含中心和两个端点。 +embodichain expansion preview --job examples/expansion/engine/job.preview.yaml \ + --output /tmp/preview-run --interior-samples 1 --no-open --duration 10 + +# 重放整个结果分布及失败/未执行记录,不重新计算。 +embodichain expansion preview --input /tmp/preview-run --open +``` + +`--job` 使用原生 Task Program source、原始动作/物理时钟和验收配置;不支持把 +Real2Sim 或视觉 profile 静默转换为另一种扩增。计算结束后默认尝试打开浏览器, +`--no-open` 可关闭此行为。`--input` 是已有结果的只读入口,兼容别名 `--inpu`; +`--job` 与 `--input` 互斥。`--output` 只在 job 模式指定新产物目录,省略时在原 +job 输出目录旁创建唯一目录。原 job 文件和原产物不修改,也不隐式 resume。 + +### 参数空间与采样含义 + +- 连续物理轴取自 job 的质量、摩擦、关节刚度/阻尼倍率及 CoM 区间。 +- 若配置 `scene_atlas`,读取其原始 request 中物体布局与机器人安装 XY/yaw 的 + lower/upper,通过已有联合场景参数化转换为完整场景/安装姿态。保留组内关系, + 不拼接不同可行区域的边界。采样的是**原始声明范围**,不是历史成功区域证明; + 每个新点都必须重新执行并接受原来的实测验证。 +- 轨迹 `joint_residual` 的受控关节使用独立的有符号比例轴 `[-1,1]`,实际残差 + 幅值由原 `joint_offset_scale × joint range` 决定;`via_points` 另外包含节点 + 幅值轴。`joint_samples` / `knot_samples` 是可选的显式采样值,不设置时原随机 + 算法保持不变。受保护阶段、端点、非受控关节和关节限位检查均保留。 +- 每个参数点遍历配置的 affordance 分支和轨迹 recipe。全零关节残差在数学上 + 等于名义轨迹,因此该点只执行唯一名义变体,避免重复计成功或降低去重阈值。 + 轨迹数量不足以覆盖声明的全部算子时会提前拒绝,并要求补足配置。 +- 未声明范围的维度保持固定;不会把碰撞检查配置里的搜索范围自动当作启用的 + 场景扩增。需要空间扩增时,在 job 中显式设置已有 `scene_atlas`。 + +默认采样包含全下界、全上界、中心和 **5 个 Latin-hypercube 分层区间内点**。 +每个变化轴都包含其两个端点,区间内各轴独立分层。`--interior-samples N` 可调整 +区间内点数。它覆盖的是有限参数样本,**不是所有混合角点的 Cartesian 穷举**, +不是连续空间成功率估计;同一关节的显式偏移复用于各允许自由阶段,也不穷举 +独立分段方向组合。物理约束与 FK 非线性意味着参数端点不等于世界坐标轨迹包络端点。 + +示例 `job.preview.yaml` 声明 2 个抓取分支 × 2 个轨迹变体,默认 8 个参数点; +中心零残差折叠后计划 30 次实测,位于其 `max_attempts: 32` 预算内。 +所有点的总次数受原 job `max_attempts`、`--max-episodes` 和 64 条预览上限共同约束。 +超过预算在执行前报错,不静默截断端点或改窄范围;全部子作业共用原 job 的 +`max_wall_time_s` 总时间预算。`--duration` 仅控制之后的查看时长。 +每个参数点在独立子进程中运行原生 Engine,以明确释放 World;预览进程只在 +采集结束后创建渲染资源。`--gpu` 是当前 `CUDA_VISIBLE_DEVICES` 内的可见编号。 + +### 覆盖报告与失败处理 + +新目录包含 `preview-plan.json`、冻结的 `configs/`、逐点 `logs/` 和原生 `jobs/` +结果。计划记录源文件哈希、上下界、归一化坐标、请求值、目标 recipe 数和后续 +实测结果;执行前重新核对语义身份,源配置发生变化则停止。 + +不会为了填满预览而重新采样、替换失败端点、放宽验收或将轨迹回退算作边界覆盖。 +报告区分已确认、部分通过、拒绝、错误、预算耗尽和未执行。`complete` 表示计划 +尝试完成,`coverage_complete` 仅在全部唯一 recipe 被原样执行并确认时成立;抓取分支复用或轨迹回退不计作完整覆盖。 +存在有效结果时仍打开预览,最终返回 2 表示不完整覆盖;没有有效轨迹时返回 2, +保留报告和日志,不伪造成功回放。 + +页面显示所有计划点的参数和状态,候选虚影只来自确认结果;初始位置图层中, +红色计划点表示未完全确认的参数点,不代表一条失败轨迹。筛选后没有匹配样本时 +显示空结果,不用别的候选替代。WebRTC 接入仍为 TODO。 + ## 从已有结果启动 在安装了兼容 EmbodiChain、DexSim 原生渲染依赖的环境中运行: ```bash # 单个已完成或部分完成的作业:只加载已有确认回执,不继续采集。 -embodichain expansion preview --output /path/to/job-output --gpu 0 --open +embodichain expansion preview --input /path/to/job-output --gpu 0 --open # 复用本次配方的两个空间组,叠加一条独立参考。 embodichain expansion preview \ - --output /path/to/results/space-a \ - --output /path/to/results/space-b \ + --input /path/to/results/space-a \ + --input /path/to/results/space-b \ --reference /path/to/results/reference \ --gpu 0 --port 8769 --open ``` 每个输入是含 `job.json` 和 `episodes/` 的原始作业输出目录;不是导出的 LeRobot -数据集或单个 NPZ。可重复传入 `--output`,空间筛选以输入结果目录分组。 -当前只支持固定布局,不把一个 scene-atlas 作业内部的不同布局合并成同一空间组。 +数据集或单个 NPZ。可重复传入 `--input`,空间筛选以输入结果目录分组。 +已有结果直接读取仍要求固定布局;`--job` 会将 scene-atlas 的每个采样点冻结为 +独立布局子作业,再作为一个参数组加载。 `--reference` 可省略;指定时必须恰好含一个已确认参考 episode,且不能与候选重复。 模型从 `job.json.resolved_deployment.robot` 的 URDF 路径恢复,场景方块尺寸从 @@ -50,7 +122,8 @@ embodichain expansion preview \ 默认从唯一的 solver `end_link_name` 推导末端路径,存在多个端点时必须提供 `--eef-link LINK_NAME`。这里的路径是该 URDF link 的 FK 原点,不隐式附加工具 TCP。 -缺失 affordance/trajectory 注释显示“未记录”,不猜测为名义分支;只有所有轨迹 +缺失 affordance/trajectory 注释显示“未记录”,不猜测为名义分支;新记录保存 +`source_env_id`,旧批量记录无法绑定诊断行时省略抓取目标而不猜用第 0 行。只有所有轨迹 具有一致、有效的动作阶段时才提供阶段对齐。 服务默认仅监听 `127.0.0.1:8769`,打印就绪地址后才能访问;`--port 0` 自动选空闲端口。 @@ -116,7 +189,7 @@ embodichain expansion preview \ 实际初始物体位姿转换到世界坐标。坐标轴展示规划目标,不是实测接触点; 原型把它固定在初始抓取位置,不随搬运中的方块移动。 - **轨迹**:V0/V1 来自执行元数据中的实际选择。末端路径由实测关节 FK 推导, - 原型轨迹线每 4 帧取点并保留末帧;机器人和物体回放仍可访问全部原始状态帧。 + 轨迹线每 4 帧取点并保留末帧;机器人和物体回放仍可访问全部原始状态帧。 - **时间**:支持公共真实时间或六个记录动作阶段的归一化对齐。每个样本取目标 时刻之前最近的已记录帧,不插值生成新姿态;短轨迹到末帧后保持。 阶段对齐只改变浏览进度映射,不改变原始时间戳。页面保留实际秒数和源帧号。 @@ -147,7 +220,7 @@ CLI 不触发采集和安装包页面资源由生产模块的聚焦测试覆盖 `_preview_render.py` 负责,HTTP 服务与页面由 `_preview_server.py`、`_preview.html` 负责,CLI 由 `embodichain/cli/expansion.py` 分发。 -当前明确支持单 URDF 机器人、单位机器人缩放和 Cube 刚体场景。其他 articulated +当前渲染明确支持单 URDF 机器人、单位机器人缩放和 Cube 刚体场景。其他 articulated objects、软体、流体、线缆、mesh/source 物体、背景资产和 scene-atlas 输入会被拒绝; 不会用方块代替不支持的几何。预览保持记录中的运动与尺寸,方块颜色和灯光是展示 外观,不声称精确恢复采集时材质。新任务支持应扩展场景恢复契约并验证实测坐标。 @@ -156,6 +229,18 @@ HTTP JPEG 轮询不提供 WebRTC 的编码、拥塞控制、连接恢复或低 页面是单个服务的共享回放会话,多人操作会改变同一状态;尚未提供独立会话、 认证或通用资产包恢复。当前不应对外公开监听。 +## 一体化入口的实测验收 + +使用已有 UR5 抓取放置任务、default 后端和已保存的联合场景 atlas,覆盖物体 +XY、安装 XY/yaw、质量、摩擦与六个受控关节残差,共 13 个变化轴。 +中心、全下界、全上界和一个区间内点共计划 14 条非重复轨迹(中心 2 条, +其余各 4 条);14 条均完成原有验收并确认,没有轨迹回退或抓取分支复用。 +这是固定小配置的实测结果,不是对任意参数范围或总体成功率的保证。 + +浏览器复测了覆盖报告和 14 → 4 → 2 → 1 → 0 的筛选,包括无结果时隐藏候选 +图层;回放物理推进为 0,自动关闭后端口释放。另行验证了无 motion profile 的 +普通 Task Program 作业也能重新计算并进入预览;批内诊断由来源行号绑定。 + ## TODO:接入 EmbodiChain 原生 WebRTC 推流 后续直接复用 EmbodiChain 原生 WebRTC 能力,替换 HTTP JPEG 轮询;先核对实际 diff --git a/examples/expansion/engine/job.preview.yaml b/examples/expansion/engine/job.preview.yaml new file mode 100644 index 0000000000..1e40c920d6 --- /dev/null +++ b/examples/expansion/engine/job.preview.yaml @@ -0,0 +1,20 @@ +deployment: task.default.yaml +source: task_program +collision_profile: ../scene_robot_feasibility/search.ur5.yaml +output_dir: /tmp/embodichain-expansion/distribution +batch_size: 2 +target_episodes: 4 +max_attempts: 32 +max_steps: 2000 +seed: 7 +settling_steps: 5 +mass_scale: + cube: + - 0.98 + - 1.02 +friction: + cube: + - 0.95 + - 1.0 +motion_profile: motion.preview.yaml +max_wall_time_s: 1800 diff --git a/examples/expansion/engine/motion.preview.yaml b/examples/expansion/engine/motion.preview.yaml new file mode 100644 index 0000000000..88e807158d --- /dev/null +++ b/examples/expansion/engine/motion.preview.yaml @@ -0,0 +1,55 @@ +schema_version: 1 +source: + kind: task_program + source_id: repeated_cube_pick_place + source_revision: one-cycle-v1 + unit_scope: episode + template_id: task_program_call + call_count: 2 + affordance_call_indices: + - 0 + trajectory_call_indices: + - 1 +scene_randomization: + enabled: false + reference_family_count: 1 + profile: none + change_scope: none + keep_drop_targets: true +affordance: + enabled: true + branches_per_family: 2 + max_proposals: 8 + fallback: next_surviving_branch +trajectory: + variants_per_family: 2 + spatial: + enabled: true + operators: + - joint_residual + joint_offset_scale: 0.002 + via_count: 1 + dedup_tolerance: 0.0001 + timing: + enabled: false +visual: + enabled: false + profile_file: extensions/embodichain_tasks/configs/components/randomization_profiles/rgb_visual.yaml + profiles: [] + apply_per_rollout: true + balance: round_robin + restore_before_next_candidate: true +observation: + profiles: [] + capture_rgb: false + capture_video: false +scheduling: + policy: round_robin_fifo + candidate_budget: 4 + reference_family_budget: 1 +execution: + max_inflight: 1 + compatibility: strict +persistence: + sink: lerobot + output_dir: /tmp/embodichain-expansion diff --git a/tests/lab/expansion/test_episode_assembly.py b/tests/lab/expansion/test_episode_assembly.py index d2ff7d3f23..15e65eaf8a 100644 --- a/tests/lab/expansion/test_episode_assembly.py +++ b/tests/lab/expansion/test_episode_assembly.py @@ -44,6 +44,7 @@ def test_source_rejection_preserves_successful_peer(tmp_path, failed_row): assert episodes[failed_row] is None peer = episodes[1 - failed_row] assert peer is not None and peer.validation.accepted + assert peer.metadata["source_env_id"] == 1 - failed_row assert len(peer.actions) == 1 and len(peer.timestamps) == 2 record = json.loads((tmp_path / str(failed_row) / "attempt.json").read_text()) assert ( diff --git a/tests/lab/expansion/test_preview.py b/tests/lab/expansion/test_preview.py index 4de3f2c108..48b17ab489 100644 --- a/tests/lab/expansion/test_preview.py +++ b/tests/lab/expansion/test_preview.py @@ -279,7 +279,7 @@ def test_preview_cli_dispatch_does_not_collect(monkeypatch, recorded_job): "from_job", lambda *a, **kw: pytest.fail("preview must not collect"), ) - assert main(["preview", "--output", str(path), "--gpu", "2", "--open"]) == 0 + assert main(["preview", "--input", str(path), "--gpu", "2", "--open"]) == 0 assert ( called[0][0] == 1 and called[0][1]["gpu"] == 2 and called[0][1]["open_browser"] ) @@ -314,3 +314,74 @@ def test_viewer_html_is_in_package_data(tmp_path): command.run() packaged = Path(command.build_lib) / "embodichain/lab/expansion/_preview.html" assert packaged.read_bytes() == original.read_bytes() + + +def test_manifest_keeps_rejected_points_visible_in_coverage(recorded_job): + path, _, _ = recorded_job + root = path.parent + manifest = { + "records": [ + {"result": path.name, "confirmed": 1, "status": "confirmed"}, + {"result": "unqualified", "confirmed": 0, "status": "rejected"}, + ], + "coverage_complete": False, + } + (root / "preview-plan.json").write_text(json.dumps(manifest)) + data = load_preview([root]) + assert len(data.clips) == 1 + assert ( + PreviewState(data).snapshot()["coverage"]["records"][1]["status"] == "rejected" + ) + manifest["records"][1]["result"] = "../escape" + (root / "preview-plan.json").write_text(json.dumps(manifest)) + with pytest.raises(ValueError, match="escapes"): + load_preview([root]) + + +def test_job_preview_collects_then_opens_and_input_alias_stays_read_only( + monkeypatch, recorded_job +): + from embodichain.cli.expansion import main + import embodichain.lab.expansion._preview_job as job + import embodichain.lab.expansion._preview_server as server + import torch + + path, _, _ = recorded_job + calls = [] + monkeypatch.setattr( + job, + "plan_preview", + lambda *a, **kw: {"output": str(path), "records": [{}], "planned_attempts": 1}, + ) + + def collect(plan): + calls.append("collect") + return {**plan, "confirmed": 1, "coverage_complete": True} + + monkeypatch.setattr(job, "collect_preview", collect) + monkeypatch.setattr(torch.cuda, "device_count", lambda: 1) + monkeypatch.setattr( + server, + "serve_preview", + lambda data, **kw: calls.append(("serve", kw["open_browser"])), + ) + assert main(["preview", "--job", "job.yaml"]) == 0 + assert calls == ["collect", ("serve", True)] + calls.clear() + assert main(["preview", "--inpu", str(path)]) == 0 + assert calls == [("serve", False)] + + +def test_row_identity_without_motion_and_legacy_ambiguity(recorded_job): + from embodichain.lab.expansion._preview_data import annotations + from embodichain.lab.gym.envs._json import json_safe_copy + + path, _, episode = recorded_job + measured = load_preview([path]).clips[0].measured + metadata = json_safe_copy(episode.metadata, field_name="metadata") + metadata["expansion"] = [] + assert annotations(metadata, measured)["targets"].shape == (0, 4, 4) + metadata["source_env_id"] = 7 + result = annotations(metadata, measured) + np.testing.assert_allclose(result["targets"][0, :3, 3], [12, 20, 0]) + assert result["candidate_ids"] == ("31",) diff --git a/tests/lab/expansion/test_preview_job.py b/tests/lab/expansion/test_preview_job.py new file mode 100644 index 0000000000..fd995017b2 --- /dev/null +++ b/tests/lab/expansion/test_preview_job.py @@ -0,0 +1,202 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Boundary plans cover declared domains without resampling failed endpoints.""" + +from __future__ import annotations + +import json +from pathlib import Path +import numpy as np +import pytest +import yaml +from embodichain.lab.expansion._preview_job import ( + boundary_samples, + plan_preview, + collect_preview, +) + + +def test_endpoint_and_stratified_interior_sampling_is_deterministic(): + points, roles = boundary_samples(7, 5, 17) + assert roles[:3] == ["center", "lower", "upper"] + np.testing.assert_array_equal(points[1], np.zeros(7)) + np.testing.assert_array_equal(points[2], np.ones(7)) + assert np.all((points[3:] > 0) & (points[3:] < 1)) + for column in points[3:].T: + assert sorted((column * 5).astype(int)) == list(range(5)) + np.testing.assert_array_equal(points, boundary_samples(7, 5, 17)[0]) + assert boundary_samples(0, 5, 17)[0].shape == (1, 0) + + +def test_plan_keeps_job_bounds_and_produces_explicit_motion_draws(tmp_path): + from embodichain.lab.expansion.configured import load_expansion_job + + root = Path(__file__).resolve().parents[3] + original = root / "examples/expansion/engine/job.motion.yaml" + before = original.read_bytes() + plan = plan_preview(original, tmp_path / "preview", interior=1) + assert len(plan["records"]) == 4 + assert ( + plan["planned_attempts"] == 7 + ) # exact-zero center is one nominal, not two copies + low = yaml.safe_load((tmp_path / "preview" / plan["records"][1]["job"]).read_text()) + high = yaml.safe_load( + (tmp_path / "preview" / plan["records"][2]["job"]).read_text() + ) + assert low["mass_scale"]["cube"] == [0.98, 0.98] and high["mass_scale"]["cube"] == [ + 1.02, + 1.02, + ] + low_motion = yaml.safe_load(Path(low["motion_profile"]).read_text()) + assert set(low_motion["trajectory"]["spatial"]["joint_samples"].values()) == {-1.0} + assert low["collision_profile"] == load_expansion_job(original).collision_profile + assert original.read_bytes() == before + with pytest.raises(FileExistsError): + plan_preview(original, tmp_path / "preview", interior=1) + with pytest.raises(ValueError, match="Nothing executed"): + plan_preview(original, tmp_path / "too-large", max_episodes=1) + assert not (tmp_path / "too-large").exists() + + +def test_collection_retains_failed_and_unrun_points_without_retry( + monkeypatch, tmp_path +): + import embodichain.lab.expansion._preview_job as module + + (tmp_path / "configs").mkdir() + (tmp_path / "logs").mkdir() + for i in range(2): + (tmp_path / f"configs/{i}.yaml").write_text("max_wall_time_s: 10\n") + plan = dict( + output=str(tmp_path), + collection_budget_s=10, + recipes_per_point=1, + planned_attempts=2, + records=[ + dict( + semantic_id="test", + index=i, + role=role, + job=f"configs/{i}.yaml", + result=f"jobs/{i}", + confirmed=0, + attempts=0, + planned=1, + status="unrun", + ) + for i, role in enumerate(["lower", "upper"]) + ], + ) + monkeypatch.setattr(module, "load_expansion_job", lambda path: None) + monkeypatch.setattr( + module, "preflight_expansion_job", lambda cfg: {"semantic_id": "test"} + ) + calls = [] + + class FailedProcess: + returncode = 2 + + def __init__(self, args, **kw): + calls.append(args) + + def wait(self, timeout): + return 2 + + monkeypatch.setattr(module.subprocess, "Popen", FailedProcess) + result = collect_preview(plan) + assert len(calls) == 2 and all(r["status"] == "rejected" for r in result["records"]) + assert result["confirmed"] == 0 and not result["coverage_complete"] + assert ( + json.loads((tmp_path / "preview-plan.json").read_text())["records"][0]["role"] + == "lower" + ) + + +def test_source_change_stops_before_collecting(monkeypatch, tmp_path): + import embodichain.lab.expansion._preview_job as module + + (tmp_path / "configs").mkdir() + (tmp_path / "configs/job.yaml").write_text("max_wall_time_s: 10\n") + plan = { + "output": str(tmp_path), + "collection_budget_s": 10, + "records": [ + { + "index": 0, + "job": "configs/job.yaml", + "semantic_id": "pinned", + "status": "unrun", + } + ], + } + monkeypatch.setattr(module, "load_expansion_job", lambda path: None) + monkeypatch.setattr( + module, "preflight_expansion_job", lambda cfg: {"semantic_id": "changed"} + ) + monkeypatch.setattr( + module.subprocess, + "Popen", + lambda *a, **k: pytest.fail("changed source must not execute"), + ) + with pytest.raises(ValueError, match="changed after planning"): + collect_preview(plan) + assert ( + json.loads((tmp_path / "preview-plan.json").read_text())["records"][0]["status"] + == "source_changed" + ) + + +def test_reused_affordance_is_not_full_recipe_coverage(): + from embodichain.lab.expansion._preview_job import _exact_recipe + + sample = {"env_ids": [3, 7], "success": [True, True], "reused": [False, True]} + metadata = { + "expansion": [ + { + "env_id": 7, + "affordance_requested": 1, + "trajectory_requested": 1, + "trajectory_selected": 1, + } + ], + "program_segments": [ + { + "metadata": { + "runtime": { + "calls": [ + { + "plan_attempts": [ + { + "planner_diagnostics": { + "metadata": { + "affordance_sample": {"grasp": sample} + } + } + } + ] + } + ] + } + } + } + ], + } + assert not _exact_recipe(metadata) + sample["reused"][1] = False + assert _exact_recipe(metadata) + metadata["expansion"][0]["trajectory_selected"] = 0 + assert not _exact_recipe(metadata) diff --git a/tests/sim/motion/expansion/test_operators.py b/tests/sim/motion/expansion/test_operators.py index 814593d003..838c4d6e5f 100644 --- a/tests/sim/motion/expansion/test_operators.py +++ b/tests/sim/motion/expansion/test_operators.py @@ -616,3 +616,97 @@ def test_approach_cone_rejects_malformed_inputs() -> None: polar_angles=torch.tensor([0.0]), azimuth_angles=torch.tensor([0.0]), ) + + +@pytest.mark.parametrize("fraction, expected", [(-1.0, 0.05), (0.0, 0.25), (1.0, 0.45)]) +def test_explicit_residual_draws_reach_bounds_and_preserve_protected_rows( + fraction, expected +): + source = template() + result = joint_residual( + source, + joint_limits=torch.tensor([[-1.0, 1.0], [-1.0, 1.0]]), + normalized_scale=0.1, + generator=torch.Generator().manual_seed(17), + joint_samples={"arm": fraction}, + ) + assert float(result.positions[1, 0]) == pytest.approx(expected, abs=1e-6) + torch.testing.assert_close( + result.positions[[0, 2, 3, 4, 5]], source.positions[[0, 2, 3, 4, 5]] + ) + torch.testing.assert_close(result.positions[:, 1], source.positions[:, 1]) + torch.testing.assert_close(source.positions, template().positions) + + +def test_explicit_draws_do_not_bypass_joint_limits_and_require_named_controls(): + limits = torch.tensor([[-1.0, 1.0], [-1.0, 1.0]]) + with pytest.raises(ProposalRejected): + joint_residual( + template(), + joint_limits=limits, + normalized_scale=1.0, + generator=torch.Generator(), + joint_samples={"arm": 1.0}, + ) + for samples in ( + {"missing": 0.0}, + {"arm": float("nan")}, + {"arm": True}, + {"arm": 1.1}, + [], + ): + with pytest.raises(ValueError): + joint_residual( + template(), + joint_limits=limits, + normalized_scale=0.1, + generator=torch.Generator(), + joint_samples=samples, + ) + + +def test_explicit_via_knots_and_configured_payload_are_owned(): + from embodichain.lab.sim.motion.expansion.cfg import TrajectoryAugmentationCfg + + source = template() + source = replace( + source, + allowed_operators=("via_points",), + phases=( + replace(source.phases[0], allowed_operators=("via_points",)), + source.phases[1], + ), + ) + samples = {"arm": -1.0} + cfg = TrajectoryAugmentationCfg.from_mapping( + { + "factors": { + "spatial": { + "enabled": True, + "method": ["via_points"], + "joint_samples": samples, + "knot_samples": [1.0], + } + } + } + ) + samples["arm"] = 1.0 + result = via_points( + source, + joint_limits=torch.tensor([[-1.0, 1.0], [-1.0, 1.0]]), + via_count=1, + normalized_scale=0.1, + generator=torch.Generator(), + joint_samples=cfg.factors.spatial.joint_samples, + knot_samples=cfg.factors.spatial.knot_samples, + ) + assert float(result.positions[1, 0]) == pytest.approx(0.05, abs=1e-6) + + +def test_explicit_sample_mapping_rejects_nonfinite_config_values(): + from embodichain.lab.sim.motion.expansion.cfg import TrajectoryAugmentationCfg + + with pytest.raises(ValueError, match="finite"): + TrajectoryAugmentationCfg.from_mapping( + {"factors": {"spatial": {"joint_samples": {"arm": float("nan")}}}} + ) From 39201fe7901a0f15d310e8dd9fc92574d5dd8faf Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sun, 15 Nov 2026 09:35:35 +0000 Subject: [PATCH 14/45] fix(motion): preserve planning contexts and phase velocities --- embodichain/lab/expansion/_motion_batches.py | 5 +- .../lab/expansion/_planning_batches.py | 63 ++++++-- embodichain/lab/sim/motion/_phase_paths.py | 3 +- tests/lab/expansion/test_planning_batches.py | 149 ++++++++++++++++++ .../motion/test_motion_generator_batched.py | 55 +++++++ 5 files changed, 260 insertions(+), 15 deletions(-) create mode 100644 tests/lab/expansion/test_planning_batches.py diff --git a/embodichain/lab/expansion/_motion_batches.py b/embodichain/lab/expansion/_motion_batches.py index 5bb52f16f6..696756d6c2 100644 --- a/embodichain/lab/expansion/_motion_batches.py +++ b/embodichain/lab/expansion/_motion_batches.py @@ -75,10 +75,7 @@ def generate(generator, target_states, options=None, *, defer_recovery=False): tensors["rows"] = broker.torch.tensor( generator._env_ids, dtype=broker.torch.long ) - mode = ( - broker.torch.is_grad_enabled(), - broker.torch.is_inference_mode_enabled(), - ) + mode = broker._mode() key = ( "motion", id(generator.robot), diff --git a/embodichain/lab/expansion/_planning_batches.py b/embodichain/lab/expansion/_planning_batches.py index 8d7e587b51..552e434f2c 100644 --- a/embodichain/lab/expansion/_planning_batches.py +++ b/embodichain/lab/expansion/_planning_batches.py @@ -89,7 +89,10 @@ def __init__(self, robot, seed): "max_motion_group": 0, } previous = self._rng() - torch.manual_seed(seed) + torch.random.default_generator.manual_seed(seed) + for device in self.devices: + with torch.cuda.device(device): + torch.cuda.manual_seed(seed) np.random.seed(seed % 2**32) random.seed(seed) self.solve_rng = self._rng() @@ -109,6 +112,7 @@ def _rng(self): self.random.getstate(), self.torch.is_grad_enabled(), self.torch.is_inference_mode_enabled(), + self._autocast_state(), ) def _restore_rng(self, state): @@ -117,6 +121,51 @@ def _restore_rng(self, state): self.torch.cuda.set_rng_state(value, device) self.np.random.set_state(state[2]) self.random.setstate(state[3]) + self._restore_autocast(state[6]) + + def _autocast_state(self): + return ( + tuple( + ( + device, + self.torch.is_autocast_enabled(device), + self.torch.get_autocast_dtype(device), + ) + for device in ("cpu", "cuda") + ), + self.torch.is_autocast_cache_enabled(), + ) + + def _restore_autocast(self, state): + if state == self._autocast_state(): + return + # Greenlets share thread-local autocast state and its weight cache. + # A cache populated in another dtype must not cross a row boundary. + self.torch.clear_autocast_cache() + for device, enabled, dtype in state[0]: + self.torch.set_autocast_enabled(device, enabled) + self.torch.set_autocast_dtype(device, dtype) + self.torch.set_autocast_cache_enabled(state[1]) + + def _mode(self): + return ( + self.torch.is_grad_enabled(), + self.torch.is_inference_mode_enabled(), + self._autocast_state(), + ) + + @contextmanager + def _execution_mode(self, mode): + previous = self._autocast_state() + self._restore_autocast(mode[2]) + try: + with ( + self.torch.inference_mode(mode[1]), + self.torch.set_grad_enabled(mode[0]), + ): + yield + finally: + self._restore_autocast(previous) def _bind(self, solver, name): from greenlet import getcurrent @@ -161,10 +210,7 @@ def query(*args, **kwargs): return original(*args, **kwargs) if count is None: return original(*args, **kwargs) - mode = ( - self.torch.is_grad_enabled(), - self.torch.is_inference_mode_enabled(), - ) + mode = self._mode() key = ( id(solver), name, @@ -186,17 +232,14 @@ def query(*args, **kwargs): def _invoke(self, request, tensors): _, original, spec, values, _, _, mode = request if request[0][0] == "motion": - with ( - self.torch.inference_mode(mode[1]), - self.torch.set_grad_enabled(mode[0]), - ): + with self._execution_mode(mode): return original(tensors) arguments = dict(values.arguments) arguments.update(tensors) # BoundArguments retains variadic **kwargs and keyword-only arguments. bound = spec.bind_partial() bound.arguments.update(arguments) - with self.torch.inference_mode(mode[1]), self.torch.set_grad_enabled(mode[0]): + with self._execution_mode(mode): return original(*bound.args, **bound.kwargs) def _part(self, result, start, stop, total=None): diff --git a/embodichain/lab/sim/motion/_phase_paths.py b/embodichain/lab/sim/motion/_phase_paths.py index d675fcb710..0176d93024 100644 --- a/embodichain/lab/sim/motion/_phase_paths.py +++ b/embodichain/lab/sim/motion/_phase_paths.py @@ -99,7 +99,7 @@ def generate_phase_paths( points = p.rejected_candidate.positions candidates.append(points if i == 0 else points[:, 1:]) rejected = RejectedMotionCandidate(torch.cat(candidates, dim=1), dt, mask) - return PlanResult( + combined = PlanResult( success=success, positions=torch.where( success[:, None, None], positions, options.start_qpos[:, None] @@ -108,3 +108,4 @@ def generate_phase_paths( rejected_candidate=rejected, recovery=pieces[-1].recovery, ) + return generator._normalize_plan_result(combined, targets, options) diff --git a/tests/lab/expansion/test_planning_batches.py b/tests/lab/expansion/test_planning_batches.py new file mode 100644 index 0000000000..a1272f2fd0 --- /dev/null +++ b/tests/lab/expansion/test_planning_batches.py @@ -0,0 +1,149 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + + +"""Regression coverage for cooperative planning execution contexts.""" + +from __future__ import annotations + +from types import SimpleNamespace +import pytest +import torch +from embodichain.lab.expansion._planning_batches import batched_planning + + +@pytest.mark.parametrize("fail", [False, True]) +def test_autocast_rows_preserve_precision_and_restore_owner(fail): + class Solver: + def get_fk(self, qpos): + if fail and qpos[0, 0] < 0: + raise ValueError("solver failure") + return qpos @ torch.eye(2) + + solver = Solver() + robot = SimpleNamespace(device=torch.device("cpu"), _solvers={"arm": solver}) + inputs = torch.tensor([[1.1234, 2.3456]]) + + class Cursor: + def __init__(self, enabled, value): + self.enabled, self.value = enabled, value + + def next(self): + with torch.autocast("cpu", dtype=torch.bfloat16, enabled=self.enabled): + first = solver.get_fk(self.value) + # Resuming a row must restore the same context for subsequent calls. + second = solver.get_fk(self.value) + return first + second + + cursors = { + 0: Cursor(True, inputs), + 1: Cursor(False, inputs), + 2: Cursor(True, inputs), + } + expected = {row: cursor.next() for row, cursor in cursors.items()} + cpu_rng = torch.get_rng_state().clone() + with torch.autocast("cpu", enabled=False): + with batched_planning(robot, 7) as planner: + if fail: + cursors[1].value = -inputs + with pytest.raises(ValueError, match="solver failure"): + planner.gather(cursors) + else: + actual = planner.gather(cursors) + for row in actual: + torch.testing.assert_close( + actual[row], expected[row], rtol=0, atol=0 + ) + saved = actual[2].clone() + actual[0].zero_() + torch.testing.assert_close(actual[2], saved) + assert not torch.is_autocast_enabled("cpu") + assert not torch.is_autocast_enabled("cpu") + assert torch.equal(cpu_rng, torch.get_rng_state()) + assert "get_fk" not in solver.__dict__ + + +@pytest.mark.gpu +@pytest.mark.parametrize("robot_device", ["cpu", "cuda:0"]) +def test_planning_scope_leaves_external_rng_generators_unchanged(robot_device): + robot = SimpleNamespace(device=torch.device(robot_device), _solvers={}) + with torch.random.fork_rng(devices=list(range(torch.cuda.device_count()))): + torch.manual_seed(123) + cpu = torch.get_rng_state().clone() + cuda = [state.clone() for state in torch.cuda.get_rng_state_all()] + with batched_planning(robot, 7): + assert torch.equal(cpu, torch.get_rng_state()) + for before, after in zip(cuda, torch.cuda.get_rng_state_all()): + assert torch.equal(before, after) + assert torch.equal(cpu, torch.get_rng_state()) + for before, after in zip(cuda, torch.cuda.get_rng_state_all()): + assert torch.equal(before, after) + + +def test_full_motion_grouping_preserves_autocast(monkeypatch): + from embodichain.lab.sim.motion.motion_generator import ( + MotionGenerator, + MotionGenOptions, + ) + from embodichain.lab.sim.motion.planners.utils import PlanState, PlanResult + + robot = SimpleNamespace(device=torch.device("cpu"), _solvers={}) + + def plan(self, targets, options): + positions = (options.start_qpos @ torch.eye(2))[:, None] + return PlanResult( + success=torch.ones(len(positions), dtype=torch.bool), + positions=positions, + dt=torch.zeros(len(positions), 1), + ) + + monkeypatch.setattr(MotionGenerator, "_generate_once", plan) + + class Cursor: + def __init__(self, row, enabled): + self.enabled = enabled + self.generator = object.__new__(MotionGenerator) + self.generator.robot = robot + self.generator.planner = SimpleNamespace( + cfg=None, collision_world_info=None + ) + self.generator._env_ids = (row,) + + def next(self): + with torch.autocast("cpu", dtype=torch.bfloat16, enabled=self.enabled): + start = torch.tensor([[1.1234, 2.3456]]) + return self.generator.generate( + [PlanState.from_qpos(start)], + MotionGenOptions( + strategy="ik_interp", + interpolation_mode="fixed_grid", + sample_count=2, + interpolation_dt=0.1, + start_qpos=start, + ), + ) + + cursors = {0: Cursor(0, True), 1: Cursor(1, False), 2: Cursor(2, True)} + expected = {row: cursor.next() for row, cursor in cursors.items()} + with batched_planning(robot, 7) as planner: + actual = planner.gather(cursors) + for row in actual: + torch.testing.assert_close( + actual[row].positions, expected[row].positions, rtol=0, atol=0 + ) + saved = actual[2].positions.clone() + actual[0].positions.zero_() + torch.testing.assert_close(actual[2].positions, saved) diff --git a/tests/sim/motion/test_motion_generator_batched.py b/tests/sim/motion/test_motion_generator_batched.py index c4886a743d..e1bd0a66d1 100644 --- a/tests/sim/motion/test_motion_generator_batched.py +++ b/tests/sim/motion/test_motion_generator_batched.py @@ -2371,3 +2371,58 @@ def test_cartesian_timing_budget_exhaustion_returns_failed_safe_hold(monkeypatch ) assert success.tolist() == [False] assert torch.equal(positions, q[:, :1].repeat(1, 2, 1)) + + +@pytest.mark.parametrize("modes", [("joint",), ("joint", "cartesian")]) +def test_phase_paths_materialize_owned_velocity_and_hold_failed_rows( + modes, monkeypatch +): + generator = _mock_generator() + start = torch.zeros(BATCH_SIZE, CONTROLLED_DOF) + states = [PlanState.from_xpos(torch.eye(4).repeat(BATCH_SIZE, 1, 1)) for _ in modes] + pieces = [] + + def segment(_self, targets, options=None, *, defer_recovery=False): + positions = ( + options.start_qpos[:, None] + + torch.arange(options.sample_count, dtype=torch.float32)[ + None, :, None + ].expand(BATCH_SIZE, -1, CONTROLLED_DOF) + * 0.25 + ) + result = _timed_result( + positions, success=torch.tensor([True, False]), step_dt=0.25 + ) + pieces.append(result) + return result + + # Exercise the public composition and real normalization with deterministic + # segment outputs, independent of IK convergence or simulation resources. + original = MotionGenerator.generate + monkeypatch.setattr(MotionGenerator, "generate", segment) + result = original( + generator, + states, + MotionGenOptions( + strategy="ik_interp", + interpolation_mode="fixed_grid", + ik_seed_mode="sequential", + waypoint_modes=modes, + sample_count=5, + interpolation_dt=0.25, + start_qpos=start, + control_part="arm", + ), + ) + expected = torch.ones_like(result.positions) + expected[:, 0] = expected[:, -1] = 0 + expected[1] = 0 + torch.testing.assert_close(result.velocities, expected) + torch.testing.assert_close( + result.positions[1], torch.zeros_like(result.positions[1]) + ) + assert result.positions.shape[1] == 5 + assert result.dt.sum(dim=1).tolist() == [1.0, 1.0] + saved = result.positions.clone() + pieces[0].positions.fill_(99) + torch.testing.assert_close(result.positions, saved) From 85704e16a39402a29470935e22d311628964a198 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sun, 15 Nov 2026 11:53:16 +0000 Subject: [PATCH 15/45] fix(expansion): preview recorded mesh scenes and native runtime metadata --- .../expansion-engine/expansion-engine.md | 6 +- embodichain/lab/expansion/_preview_data.py | 98 ++++++++++++++---- embodichain/lab/expansion/_preview_render.py | 99 +++++++++++++++---- examples/expansion/engine/ghost-preview.md | 16 +-- tests/lab/expansion/test_preview.py | 31 +++++- 5 files changed, 202 insertions(+), 48 deletions(-) diff --git a/agent_context/topics/expansion-engine/expansion-engine.md b/agent_context/topics/expansion-engine/expansion-engine.md index 255bdcaa1e..b27024c8bd 100644 --- a/agent_context/topics/expansion-engine/expansion-engine.md +++ b/agent_context/topics/expansion-engine/expansion-engine.md @@ -96,8 +96,10 @@ process orchestration remain outside the replay core. `embodichain expansion preview --input` reads confirmed job outputs through `_preview_data.py`, then replays full measured states using `_preview_render.py` and a loopback-only `_preview_server.py`. Inputs retain their original evidence; no collection or -physics stepping occurs. The first version accepts one URDF robot and Cube rigid -objects with fixed layouts, and rejects unsupported geometry. Space filters group +physics stepping occurs. It accepts one URDF robot and recorded rigid visual +objects (including file Mesh/USD via native visual loading), with fixed layouts; +unsupported geometry is rejected. Constant static backgrounds remain single +instances. Space filters group input directories; missing motion annotations remain explicit. HTTP publishes a single immutable image/state snapshot; WebRTC is follow-up work. FK and episode budgets bound preload, controls are queued for the render owner, and native diff --git a/embodichain/lab/expansion/_preview_data.py b/embodichain/lab/expansion/_preview_data.py index adf91221d0..06fda24abe 100644 --- a/embodichain/lab/expansion/_preview_data.py +++ b/embodichain/lab/expansion/_preview_data.py @@ -68,7 +68,8 @@ def annotations(metadata: Any, measured: MeasuredTrajectory) -> dict[str, Any]: branches, candidates, targets, phases = [], [], [], [] phase_safe = True for segment in metadata.get("program_segments", ()): - for call in segment.get("metadata", {}).get("runtime", {}).get("calls", ()): + runtime = segment.get("runtime", segment.get("metadata", {}).get("runtime", {})) + for call in runtime.get("calls", ()): for event in call.get("events", ()): if event.get("kind") == "trajectory_segment_entered": mask = event.get("env_mask") @@ -120,6 +121,8 @@ def annotations(metadata: Any, measured: MeasuredTrajectory) -> dict[str, Any]: sampling = sample.get("sampling") or {} overrides = sampling.get("branch_overrides") branch = overrides[row] if overrides else sampling.get("branch_override") + if branch is None and sampling.get("count") == 1: + branch = 0 branches.append("unrecorded" if branch is None else str(branch)) candidates.append(str(sample["candidate_ids"][row])) uid = ( @@ -159,6 +162,14 @@ def annotations(metadata: Any, measured: MeasuredTrajectory) -> dict[str, Any]: for r in records if r.get("trajectory_requested") is not None ] + if not variants: + variants = [ + str(r["operator"]) + for r in records + if r.get("operator") + in {"joint_residual", "via_points", "nullspace_residual"} + ] + variants = list(dict.fromkeys(variants)) return dict( affordance="/".join(branches) or "unrecorded", variant="/".join(variants) or "unrecorded", @@ -169,7 +180,53 @@ def annotations(metadata: Any, measured: MeasuredTrajectory) -> dict[str, Any]: ) -def _scene(job: dict[str, Any]) -> tuple[dict, dict[str, tuple[float, ...]]]: +def _object_specs(cfg: dict[str, Any]) -> dict[str, dict[str, Any]]: + objects = {} + for obj in [*(cfg.get("rigid_object") or []), *(cfg.get("background") or [])]: + uid = obj.get("uid") + if not isinstance(uid, str) or not uid or uid in objects: + raise ValueError("Preview requires unique recorded rigid object IDs") + if obj.get("visuals") is not None or obj.get("visual_shape"): + raise ValueError( + "Preview does not yet support explicit rigid visual replacements" + ) + shape, source = obj.get("shape") or {}, obj.get("source") + if source: + if not isinstance(source, dict) or not source.get("file_path") or shape: + raise ValueError("Preview requires an unambiguous source object") + filename = source["file_path"] + elif shape.get("shape_type") == "Mesh": + filename = shape.get("fpath") + elif shape.get("shape_type") in { + "Cube", + "Sphere", + "Cylinder", + "Capsule", + "Cone", + }: + filename = None + else: + raise ValueError("Unsupported recorded rigid visual geometry") + if (source or shape.get("shape_type") == "Mesh") and ( + not filename or not Path(filename).expanduser().is_file() + ): + raise ValueError(f"Recorded object asset is unavailable: {filename}") + scale = obj.get("body_scale") + if scale is not None and ( + np.asarray(scale).shape != (3,) + or not np.isfinite(scale).all() + or np.any(np.asarray(scale) <= 0) + ): + raise ValueError("Invalid recorded object scale") + objects[uid] = { + key: obj[key] + for key in ("shape", "source", "body_scale", "body_type") + if key in obj + } + return objects + + +def _scene(job: dict[str, Any]) -> tuple[dict, dict[str, tuple[float, ...] | None]]: cfg = job.get("resolved_deployment") if not isinstance(cfg, dict) or not isinstance(cfg.get("robot"), dict): raise ValueError( @@ -178,7 +235,6 @@ def _scene(job: dict[str, Any]) -> tuple[dict, dict[str, tuple[float, ...]]]: unsupported = ( "articulation", "rigid_object_group", - "background", "cable_object", "fluid_object", "soft_object", @@ -187,7 +243,7 @@ def _scene(job: dict[str, Any]) -> tuple[dict, dict[str, tuple[float, ...]]]: ) if any(cfg.get(k) for k in unsupported) or job.get("config", {}).get("scene_atlas"): raise ValueError( - "Preview currently supports a fixed layout, one URDF robot and Cube rigid objects only" + "Preview requires a fixed layout, one URDF robot and recorded rigid objects" ) robot = cfg["robot"] if robot.get("visuals") or robot.get("link_visuals"): @@ -195,23 +251,17 @@ def _scene(job: dict[str, Any]) -> tuple[dict, dict[str, tuple[float, ...]]]: if not np.allclose(robot.get("body_scale", [1, 1, 1]), [1, 1, 1]): raise ValueError("Scaled robot assets are not supported by this preview") boxes = {} - for obj in cfg.get("rigid_object", []): + for uid, obj in _object_specs(cfg).items(): shape = obj.get("shape") or {} - if ( - shape.get("shape_type") != "Cube" - or obj.get("source") - or obj.get("visuals") - or obj.get("visual_shape") - ): - raise ValueError( - "Preview requires Cube rigid geometry; no asset substitution is performed" - ) + if shape.get("shape_type") != "Cube": + boxes[uid] = None + continue size = np.asarray(shape["size"], float) * np.asarray( - obj.get("body_scale", [1, 1, 1]), float + obj.get("body_scale") or [1, 1, 1], float ) if size.shape != (3,) or not np.isfinite(size).all() or np.any(size <= 0): raise ValueError("Invalid recorded Cube dimensions") - boxes[obj["uid"]] = tuple(size) + boxes[uid] = tuple(size) return robot, boxes @@ -221,9 +271,10 @@ class PreviewData: reference: PreviewClip | None asset: Path eef_link: str - boxes: dict[str, tuple[float, ...]] + boxes: dict[str, tuple[float, ...] | None] ground_visible: bool = True coverage: dict[str, Any] | None = None + object_specs: dict[str, dict[str, Any]] | None = None @property def all_clips(self) -> list[PreviewClip]: @@ -272,6 +323,7 @@ def load_preview( asset = None boxes = None robot_spec = None + object_specs = None end = None ground = True seen = set() @@ -282,10 +334,16 @@ def load_preview( raise ValueError(f"Not an expansion job output: {path} (missing job.json)") job = json.loads(job_path.read_text()) robot, shapes = _scene(job) + recorded_specs = _object_specs(job["resolved_deployment"]) + if object_specs is not None and recorded_specs != object_specs: + raise ValueError( + "Result directories disagree about recorded object geometry" + ) + object_specs = recorded_specs spec = {k: robot.get(k) for k in ("uid", "control_parts", "solver_cfg")} if robot_spec is not None and (spec != robot_spec or shapes != boxes): raise ValueError( - "Result directories must share robot/control topology and Cube geometry" + "Result directories must share robot/control topology and object geometry" ) robot_spec = spec boxes = shapes @@ -371,7 +429,9 @@ def load_preview( else: clips.append(clip) clip.label = f"C-{len(clips):03d}" - return PreviewData(clips, reference_clip, asset, end, boxes, ground, coverage) + return PreviewData( + clips, reference_clip, asset, end, boxes, ground, coverage, object_specs + ) class PreviewState: diff --git a/embodichain/lab/expansion/_preview_render.py b/embodichain/lab/expansion/_preview_render.py index 4bf154de4a..4591772dee 100644 --- a/embodichain/lab/expansion/_preview_render.py +++ b/embodichain/lab/expansion/_preview_render.py @@ -80,6 +80,8 @@ def __init__( publisher: Any, names: list[str], center: np.ndarray, + models: dict[str, Any] | None = None, + geometry: dict[str, Any] | None = None, ) -> None: from dexsim import spawn from embodichain.lab.visualization.markers import ( @@ -95,6 +97,28 @@ def __init__( scene = sim.spawn_result self.step = sim.physics_step_index self.previous = None + self.models = models or {} + self.geometry = geometry or {} + self.backgrounds = { + uid + for uid, spec in (data.object_specs or {}).items() + if spec.get("body_type") in {"static", "kinematic"} + and all( + np.allclose( + c.measured.object_poses[uid], + data.clips[0].measured.object_poses[uid][0], + rtol=0, + atol=1e-7, + ) + for c in data.all_clips + ) + } + self.moving_objects = [uid for uid in data.boxes if uid not in self.backgrounds] + for uid in self.backgrounds: + self.models[uid].set_local_pose( + data.clips[0].measured.object_poses[uid][0:1] + ) + self.models[uid].set_visible(True) palette = [ (0.18, 0.58, 1), (0.15, 0.9, 0.5), @@ -141,13 +165,11 @@ def markers(name: str, shapes: dict) -> Any: markers( f"preview_objects_{i}", { - uid: MarkerPrototypeCfg( - shape="box", scale=size, color=(*color, 0.2) - ) - for uid, size in data.boxes.items() + uid: self._object_prototype(uid, color=(*color, 0.2)) + for uid in self.moving_objects }, ) - if data.boxes + if self.moving_objects else None ) self.objects.append(objects) @@ -156,10 +178,10 @@ def markers(name: str, shapes: dict) -> Any: ) robot.set_visibility(False) if objects: - p = np.array([c.measured.object_poses[uid][0] for uid in data.boxes]) - objects.visualize( - p[:, :3], p[:, 3:], marker_indices=np.arange(len(data.boxes)) + p = np.array( + [c.measured.object_poses[uid][0] for uid in self.moving_objects] ) + objects.visualize(p[:, :3], p[:, 3:], marker_indices=np.arange(len(p))) objects.set_visibility(False) self.selected = RobotMarkers( RobotMarkersCfg( @@ -177,17 +199,13 @@ def markers(name: str, shapes: dict) -> Any: markers( "selected_objects", { - uid: MarkerPrototypeCfg( - shape="box", - scale=size, - color=(0.9, 0.58, 0.2, 1), - scene_lighting=True, - unlit=False, + uid: self._object_prototype( + uid, color=(0.9, 0.58, 0.2, 1), scene_lighting=True, unlit=False ) - for uid, size in data.boxes.items() + for uid in self.moving_objects }, ) - if data.boxes + if self.moving_objects and not self.models else None ) self.paths = markers( @@ -244,6 +262,16 @@ def markers(name: str, shapes: dict) -> Any: self.camera.set_visual_overlay_enabled(True) publisher.publish(preload=True) + def _object_prototype(self, uid: str, **appearance: Any) -> Any: + from embodichain.lab.visualization.markers import MarkerPrototypeCfg + + if uid in self.geometry: + vertices, faces = self.geometry[uid] + return MarkerPrototypeCfg( + shape="mesh", vertices=vertices, faces=faces, **appearance + ) + return MarkerPrototypeCfg(shape="box", scale=self.data.boxes[uid], **appearance) + def draw(self, state: PreviewState) -> bytes: v = state.values visible = state.visible() @@ -298,20 +326,29 @@ def draw(self, state: PreviewState) -> bytes: initial = [ p[0, :3] for i in visible - for p in self.data.clips[i].measured.object_poses.values() + for uid, p in self.data.clips[i].measured.object_poses.items() + if uid not in self.backgrounds ] self.positions.visualize(np.asarray(initial).reshape(-1, 3)) self.previous = tuple(visible) self.selected.set_visibility(bool(visible)) if self.selected_objects: self.selected_objects.set_visibility(bool(visible)) + for uid, obj in self.models.items(): + if uid not in self.backgrounds: + obj.set_visible(bool(visible)) if visible: c = self.data.clips[v["selected"]] f = state.frame_at(c) self.selected.visualize_link_poses(c.links[f, self.selected_order][None]) + for uid in self.moving_objects: + if uid in self.models: + self.models[uid].set_local_pose( + c.measured.object_poses[uid][f : f + 1] + ) if self.selected_objects: p = np.array( - [c.measured.object_poses[uid][f] for uid in self.data.boxes] + [c.measured.object_poses[uid][f] for uid in self.moving_objects] ) self.selected_objects.visualize( p[:, :3], p[:, 3:], marker_indices=np.arange(len(p)) @@ -334,7 +371,7 @@ def draw(self, state: PreviewState) -> bytes: robot.visualize_link_poses(c.links[f, order][None]) if objects: p = np.array( - [c.measured.object_poses[uid][f] for uid in self.data.boxes] + [c.measured.object_poses[uid][f] for uid in self.moving_objects] ) objects.visualize( p[:, :3], p[:, 3:], marker_indices=np.arange(len(p)) @@ -366,6 +403,8 @@ def draw(self, state: PreviewState) -> bytes: "ghost_count": ghost_count, "path_segments": self.paths.count if v["paths"] else 0, "grasp_targets": self.targets.count if v["targets"] else 0, + "scene_objects": len(self.data.boxes), + "fixed_backgrounds": len(self.backgrounds), } return buffer.getvalue() @@ -400,10 +439,30 @@ def native_renderer(data: PreviewData, gpu: int = 0) -> Iterator[NativeRenderer] publisher = None try: sim.enable_physics(False) + models = {} + if data.object_specs is not None: + from embodichain.lab.sim.cfg import VisualObjectCfg + + for index, (uid, spec) in enumerate(data.object_specs.items()): + visual = { + key: value + for key, value in spec.items() + if key in {"shape", "source", "body_scale"} and value is not None + } + visual.update(uid=f"expansion_preview_asset_{index}", visible=False) + models[uid] = sim.add_visual_object(VisualObjectCfg.from_dict(visual)) sim.prepare() sim.set_ground_plane_visibility(data.ground_visible) + geometry = {} + for uid, obj in models.items(): + vertices = obj.get_vertices(env_ids=[0], scale=True)[0].cpu().numpy() + faces = obj.get_triangles(env_ids=[0])[0].cpu().numpy() + if not len(vertices) or not len(faces) or not np.isfinite(vertices).all(): + raise ValueError(f"Recorded object has no finite visual mesh: {uid}") + geometry[uid] = vertices, faces + data.boxes[uid] = tuple(np.ptp(vertices, axis=0)) publisher = MarkerFramePublisher(sim) - yield NativeRenderer(data, sim, publisher, names, center) + yield NativeRenderer(data, sim, publisher, names, center, models, geometry) finally: if publisher is not None: publisher.close() diff --git a/examples/expansion/engine/ghost-preview.md b/examples/expansion/engine/ghost-preview.md index e31f38505b..376d3ca9d8 100644 --- a/examples/expansion/engine/ghost-preview.md +++ b/examples/expansion/engine/ghost-preview.md @@ -114,8 +114,10 @@ embodichain expansion preview \ 独立布局子作业,再作为一个参数组加载。 `--reference` 可省略;指定时必须恰好含一个已确认参考 episode,且不能与候选重复。 -模型从 `job.json.resolved_deployment.robot` 的 URDF 路径恢复,场景方块尺寸从 -同一快照恢复,不加载原任务配置,也不重新实例化任务/规划器。原模型及引用的视觉 +模型从 `job.json.resolved_deployment.robot` 的 URDF 路径恢复,场景刚体几何从 +同一快照恢复。方块、球和文件 Mesh/USD 资产通过原生纯视觉对象加载,保留原生 +坐标轴转换与模型缩放;不会用方块替代网格。位置恒定的静态背景只显示一份。 +不加载原任务配置,也不重新实例化任务/规划器。原模型及引用的视觉 文件必须仍可读;若资产移动,使用 `--robot-asset /new/path/robot.urdf` 显式提供 匹配模型。入口检查完整关节名称集合和跨输入模型一致性,但旧结果没有完整视觉 资产包,不能仅凭关节名证明替换模型几何完全一致;调用者负责提供原模型。 @@ -220,10 +222,12 @@ CLI 不触发采集和安装包页面资源由生产模块的聚焦测试覆盖 `_preview_render.py` 负责,HTTP 服务与页面由 `_preview_server.py`、`_preview.html` 负责,CLI 由 `embodichain/cli/expansion.py` 分发。 -当前渲染明确支持单 URDF 机器人、单位机器人缩放和 Cube 刚体场景。其他 articulated -objects、软体、流体、线缆、mesh/source 物体、背景资产和 scene-atlas 输入会被拒绝; -不会用方块代替不支持的几何。预览保持记录中的运动与尺寸,方块颜色和灯光是展示 -外观,不声称精确恢复采集时材质。新任务支持应扩展场景恢复契约并验证实测坐标。 +当前渲染支持单 URDF 机器人、单位机器人缩放,以及有完整实测位姿的刚体场景。 +文件 Mesh 和 USD 通过原生视觉资产加载,选中物体保留源模型外观,虚影复用实测几何。 +其他 articulated +objects、软体、流体、线缆、显式 visual replacement 和直接 scene-atlas 输入会被拒绝; +不会用方块代替不支持的几何。预览保持记录中的运动与尺寸,虚影材质与灯光用于展示, +不声称精确恢复采集传感器的原始图像。新任务支持应扩展场景恢复契约并验证实测坐标。 HTTP JPEG 轮询不提供 WebRTC 的编码、拥塞控制、连接恢复或低延迟保证。 页面是单个服务的共享回放会话,多人操作会改变同一状态;尚未提供独立会话、 diff --git a/tests/lab/expansion/test_preview.py b/tests/lab/expansion/test_preview.py index 48b17ab489..f5d450a0a6 100644 --- a/tests/lab/expansion/test_preview.py +++ b/tests/lab/expansion/test_preview.py @@ -193,10 +193,39 @@ def test_result_loader_rejects_incomplete_scene_and_budget(recorded_job, tmp_pat load_preview([path], reference=path) job["resolved_deployment"]["rigid_object"][0]["shape"]["shape_type"] = "Mesh" (path / "job.json").write_text(json.dumps(job)) - with pytest.raises(ValueError, match="Cube"): + with pytest.raises(ValueError, match="asset is unavailable"): load_preview([path]) +def test_result_loader_keeps_real_mesh_spec_and_native_operator_labels( + recorded_job, tmp_path +): + path, job, episode = recorded_job + mesh = tmp_path / "object.ply" + mesh.write_text( + "ply\nformat ascii 1.0\nelement vertex 3\nproperty float x\nproperty float y\nproperty float z\nelement face 1\nproperty list uchar int vertex_indices\nend_header\n0 0 0\n1 0 0\n0 1 0\n3 0 1 2\n" + ) + job["resolved_deployment"]["rigid_object"][0]["shape"] = { + "shape_type": "Mesh", + "fpath": str(mesh), + } + (path / "job.json").write_text(json.dumps(job)) + data = load_preview([path]) + assert data.boxes["box"] is None + assert data.object_specs["box"]["shape"]["fpath"] == str(mesh) + from embodichain.lab.expansion._preview_data import annotations + + result = annotations( + { + "source_env_id": 0, + "expansion": [{"env_id": 0, "operator": "joint_residual", "proposal": 3}], + }, + data.clips[0].measured, + ) + assert result["variant"] == "joint_residual" + assert result["affordance"] == "unrecorded" + + def test_filter_empty_intersection_and_phase_alignment(recorded_job): path, _, _ = recorded_job data = load_preview([path]) From 239a3d58d48cd8f8750fe9415b0d6261c3874a69 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sun, 15 Nov 2026 12:53:58 +0000 Subject: [PATCH 16/45] feat(expansion): compose native single-world workload collection --- .../expansion-engine/expansion-engine.md | 1 + .../topics/expansion-engine/workloads.md | 11 + docs/source/api_reference/public_api.rst | 22 + embodichain/lab/expansion/__init__.py | 4 + embodichain/lab/expansion/gym_workloads.py | 852 ++++++++++++++++++ embodichain/lab/expansion/host_extensions.py | 6 +- embodichain/lab/expansion/journal.py | 31 + tests/lab/expansion/test_journal.py | 15 + tests/lab/expansion/test_workloads_sim.py | 103 ++- 9 files changed, 1040 insertions(+), 5 deletions(-) create mode 100644 embodichain/lab/expansion/gym_workloads.py diff --git a/agent_context/topics/expansion-engine/expansion-engine.md b/agent_context/topics/expansion-engine/expansion-engine.md index b27024c8bd..71ff869d5b 100644 --- a/agent_context/topics/expansion-engine/expansion-engine.md +++ b/agent_context/topics/expansion-engine/expansion-engine.md @@ -11,6 +11,7 @@ algorithms stay in `sim.motion.expansion`; scene and workspace algorithms stay i | Canonical in-memory session, lifecycle limits and coverage port | `embodichain/lab/expansion/session.py`, `session_cfg.py`, `coverage.py` | | Shared scheduling, admission and commit ports | `embodichain/lab/expansion/engine.py` | | Rolling workload × B execution in one caller-owned World | `embodichain/lab/expansion/workloads.py` | +| Native workload World, source/evidence assembly and bounded export | `embodichain/lab/expansion/gym_workloads.py` | | Job assembly, stage recovery and inspect | `embodichain/lab/expansion/workflow.py`, `stages.py` | | Native World ownership and compatibility facade | `embodichain/lab/expansion/host.py` | | Task-owned render/source/transform/acceptance ports | `embodichain/lab/expansion/host_extensions.py` | diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index 5c47c5d1a4..50e7c58c0d 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -22,6 +22,17 @@ difficult group does not silently borrow another group's B-row allocation. ## Source ports and native accounting +`run_gym_expansion_workloads` is the public native composition for Task Program +scene/plan workloads. `GymExpansionWorkload` supplies a pinned deployment, +independent collection budget, artifacts and row callbacks. The facade opens one +World, captures the original seeded nominal scene in row zero, and composes the +existing session, frame collector, episode assembler and confirmed artifact sink. +Different integration/grasp policies may share that World only when their +physical configuration, program and cadence agree. Physical/visual/prototype +operators remain on the cohort API; unsupported payloads fail explicitly. +Frozen groups export through one bounded writer thread without World access. +Fresh output is required; this facade does not resume interrupted collection. + `WorkloadSource.prepare` runs inside the selected-row `episode_prepare` reset callback. It must not step physics, alter an active peer or mutate World-wide appearance/resources. Incompatible topology or shared resources require a diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 15ba57ac10..31ea4f6bfc 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -1670,6 +1670,8 @@ and durable episode-boundary recovery. See the offline expansion example. WorkloadRowResult WorkloadSource WorkloadProgramExecutor + GymExpansionWorkload + run_gym_expansion_workloads export_expansion_episodes .. autosummary:: @@ -1852,6 +1854,26 @@ Stream confirmed numeric episodes and observation children through the existing export_expansion_lerobot +embodichain.lab.expansion.gym_workloads +-------------------------------------- + +The native workload facade owns one World containing W times B environments. +Task packages supply pinned source declarations and row-local scene/plan ports; +the facade retains native session admission, measured T+1 evidence, physical +acceptance, confirmed receipts and one bounded artifact export thread. +Integration policies may differ while physical topology and cadence must agree. + +.. currentmodule:: embodichain.lab.expansion.gym_workloads + +.. autosummary:: + + GymExpansionWorkload + run_gym_expansion_workloads + +.. autoclass:: GymExpansionWorkload + +.. autofunction:: run_gym_expansion_workloads + embodichain.lab.expansion.host ------------------------------ diff --git a/embodichain/lab/expansion/__init__.py b/embodichain/lab/expansion/__init__.py index fd6dd78f38..a954ea40c1 100644 --- a/embodichain/lab/expansion/__init__.py +++ b/embodichain/lab/expansion/__init__.py @@ -66,6 +66,8 @@ "WorkloadRowResult", "WorkloadSource", "WorkloadProgramExecutor", + "GymExpansionWorkload", + "run_gym_expansion_workloads", "export_expansion_episodes", "GymExpansionHostExtensions", "ExpansionRowContext", @@ -118,6 +120,8 @@ def __getattr__(name: str) -> object: "WorkloadRowResult": "workloads", "WorkloadSource": "workloads", "WorkloadProgramExecutor": "workloads", + "GymExpansionWorkload": "gym_workloads", + "run_gym_expansion_workloads": "gym_workloads", "export_expansion_episodes": "episode_export", "GymExpansionHostExtensions": "host_extensions", "ExpansionRowContext": "host_extensions", diff --git a/embodichain/lab/expansion/gym_workloads.py b/embodichain/lab/expansion/gym_workloads.py new file mode 100644 index 0000000000..4109d677de --- /dev/null +++ b/embodichain/lab/expansion/gym_workloads.py @@ -0,0 +1,852 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Native single-World collection for independent Task Program workloads.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from concurrent.futures import ThreadPoolExecutor +from copy import deepcopy +from dataclasses import asdict, dataclass, replace +from pathlib import Path +import json +import hashlib +import time +from typing import Any + +from embodichain.data_pipeline.recording.media import _atomic_json +from .configured import ExpansionRunCfg +from .host_extensions import ExpansionRowContext, GymExpansionHostExtensions +from .workloads import ( + ExpansionWorkloadCfg, + WorkloadAssignment, + WorkloadExecutionCfg, + WorkloadProgramExecutor, + WorkloadRowResult, +) + +__all__ = ["GymExpansionWorkload", "run_gym_expansion_workloads"] + + +@dataclass(frozen=True) +class GymExpansionWorkload: + """Task-owned declarations for one independently budgeted source group. + + Args: + workload_id: Unique logical source group, independent of its World row. + cfg: Deployment, output, evidence limits and original collection budget. + report: Pinned semantic/execution identities and runtime versions. + source_artifacts: Hashed configuration/source references for each episode. + extensions: Scene, recipe and plan callbacks; rendering and validation + belong to the common extensions passed to the collection function. + + Deployments may select different integration/grasp policies, but must share + exactly the same physical environment, robot, control cadence and program. + This facade supports native Task Programs with row-local scene/plan ports; + configured physical/visual/prototype operators use the cohort job API. + """ + + workload_id: str + cfg: ExpansionRunCfg + report: Mapping[str, object] + source_artifacts: Mapping[str, object] + extensions: GymExpansionHostExtensions = GymExpansionHostExtensions() + + +def _preview_snapshot(env: Any) -> dict[str, object]: + """Freeze actual resolved geometry, including finite JSON parameter encoding.""" + from enum import Enum + import math + + def encode(value: Any) -> Any: + if isinstance(value, Enum): + return encode(value.value) + if isinstance(value, Mapping): + return {str(key): encode(item) for key, item in value.items()} + if isinstance(value, (tuple, list)): + return [encode(item) for item in value] + if isinstance(value, float) and not math.isfinite(value): + if math.isnan(value): + raise ValueError("resolved preview configuration contains NaN") + return "+inf" if value > 0 else "-inf" + if value is None or isinstance(value, (bool, int, float, str)): + return value + if isinstance(value, Path): + return str(value) + if hasattr(value, "tolist"): + return encode(value.tolist()) + return str(value) + + return encode( + { + "robot": env.robot.cfg.to_dict(), + "rigid_object": [ + env.sim.get_rigid_object(uid).cfg.to_dict() + for uid in env.sim.get_rigid_object_uid_list() + ], + "ground_plane_visible": getattr(env.cfg, "ground_plane_visible", True), + } + ) + + +class _GymWorkloadSource: + """Compose existing native admission, frame assembly and persistence owners.""" + + def __init__(self, owner: Any, groups: Mapping[str, Any]) -> None: + from ._gym_frames import GymFrameCollector + + self.owner, self.groups, self.env = owner, groups, owner.env + self.collector = GymFrameCollector(owner._context, {}) + self.active: dict[int, dict[str, Any]] = {} + self.activation_snapshot = None + self.nominal = { + uid: self.env.sim.get_rigid_object(uid) + .get_local_pose(to_matrix=True)[0] + .detach() + .cpu() + .tolist() + for uid in self.env.sim.get_rigid_object_uid_list() + } + + def prepare(self, assignments: tuple[WorkloadAssignment, ...]) -> None: + from embodichain.lab.sim.scene import ( + SceneView, + SceneChangeSet, + apply_changes_batch, + ) + from .scene import apply_joint_scene_candidates + + stamp = time.monotonic() + self.activation_snapshot = None + ids = tuple(a.env_id for a in assignments) + view = SceneView(self.env.sim, env_ids=ids) + # Every candidate starts from the same seeded nominal reset. Original + # reset events may run, but cannot resample an unowned scene axis. + apply_changes_batch( + view, + {row: SceneChangeSet(self.nominal, tuple(self.nominal)) for row in ids}, + ) + candidates = {} + for assignment in assignments: + group = self.groups[assignment.workload_id] + if group["admitted_at"][0] is None: + group["admitted_at"][0] = time.monotonic() + ctx = group["host"]._context + request = ctx.journal.allocate_recipe(ctx.recipe(assignment.ordinal)) + request.recipe.validate_host(ctx.supported_operations, ctx.compatibility) + for operation in request.recipe.operations: + if operation.name not in { + "physical_parameters", + "joint_scene", + "motion", + }: + raise ValueError( + "rolling workloads only support row-local scene and motion operations" + ) + if operation.name == "physical_parameters" and any( + json.loads(operation.payload_json).values() + ): + raise ValueError( + "physical parameter changes require the cohort job API" + ) + ctx.requests[assignment.env_id] = request + if ctx.scene_source is not None: + candidates[assignment.env_id] = ctx.scene_source.candidate( + assignment.ordinal + ) + if candidates: + apply_joint_scene_candidates( + SceneView(self.env.sim, env_ids=tuple(candidates)), candidates + ) + self.owner.timings["prepare"] += time.monotonic() - stamp + + def activate(self, assignment: WorkloadAssignment, observation: object) -> Any: + from embodichain.lab.sim.scene import capture_state + from embodichain.lab.sim.scene.physics import capture_physics_properties + from .contracts import ( + EpisodeCandidate, + SceneCase, + ValidationCheck, + ValidationResult, + ) + from .engine import PreparedEpisodeWorkItem + from ._gym_utils import _hash + + row = assignment.env_id + group = self.groups[assignment.workload_id] + host, engine = group["host"], group["engine"] + ctx = host._context + request = ctx.requests[row] + if self.activation_snapshot is None: + self.activation_snapshot = ( + capture_state(ctx.view), + capture_physics_properties(ctx.view), + ) + initial, physics = self.activation_snapshot + ctx.initial, ctx.initial_obs = initial, observation + ctx.physics_before = physics + collision = ctx.collision + if collision is not None: + collision.prepare(initial, env_ids=(row,)) + values = [asdict(entity) for entity in initial.batches[row]] + initial_id = _hash(values) + physical = json.loads( + next( + op.payload_json + for op in request.recipe.operations + if op.name == "physical_parameters" + ) + ) + parameters = { + "ordinal": assignment.ordinal, + "seed": ctx.cfg.seed, + "physics": physical, + "source": ctx.cfg.source, + "recipe_id": request.recipe.recipe_id, + } + for op in request.recipe.operations: + if op.name == "motion": + parameters["motion_recipe"] = json.loads(op.payload_json)[ + "recipe_index" + ] + elif op.name == "joint_scene": + parameters["scene_candidate_id"] = json.loads(op.payload_json)[ + "candidate_id" + ] + case = SceneCase( + _hash( + ( + ctx.compatibility.key, + physical, + parameters.get("scene_candidate_id"), + request.recipe.recipe_id, + ) + ), + initial_id, + ctx.report["semantic_id"], + ctx.program.program_id, + self.env.robot.uid, + ) + session = group["session"] + session.register_case(case, ctx.limits, joint_names=ctx.names) + identity, _ = session.propose( + case.scene_case_id, + initial_id, + source_id=ctx.program.program_id, + source_revision=ctx.report["recipe_revision"], + template_id=request.recipe.recipe_id, + operator_id="episode", + attempt_id=request.attempt_id, + ) + episode_id, commit_id = session.episode_ids(identity) + record = ctx.journal.update( + request, + status="prepared", + recipe=parameters, + case=asdict(case), + identity=asdict(identity), + episode_id=episode_id, + commit_id=commit_id, + joint_names=ctx.names, + initial_state=values, + ) + ctx.prepared[row] = identity, episode_id, commit_id, request.directory, record + valid = collision is None or collision.initial[row] + ctx.preparation_checks[row] = ValidationResult( + ( + ValidationCheck("preparation.source_configuration", "passed"), + ValidationCheck( + "preparation.initial_state", "passed" if valid else "failed" + ), + ValidationCheck( + "preparation.settling_steps", + "passed", + metrics={"steps": ctx.cfg.settling_steps}, + ), + ) + ) + item = PreparedEpisodeWorkItem( + EpisodeCandidate(identity, json.dumps(request.recipe.to_dict())), + ctx.compatibility.key, + row, + ) + native = engine.admit_row(item) + if native is None: + ctx.journal.update(request, status="initial_rejected") + return None + factory = None + if ctx.extensions.plan_transform_factory is not None: + factory = ctx.extensions.plan_transform_factory( + self.env, + ExpansionRowContext( + row, + identity, + assignment.ordinal, + request.attempt_id, + request.directory, + case, + request.recipe, + ), + ) + if factory is None: + raise ValueError("plan_transform_factory returned no factory") + adapter = group["adapter"].for_env_ids((row,)) + bridge = adapter.create_bridge( + ctx.program, plan_transform_factory=factory, isolate_policy_sessions=True + ) + # The frame owner is shared and observes all active rows once per step. + self.owner._context.initial, self.owner._context.initial_obs = ( + initial, + observation, + ) + self.collector.add({row: ctx.prepared[row]}) + self.active[row] = { + "group": group, + "native": native, + "adapter": adapter, + "factory": factory, + "physics_before": ctx.physics_before, + } + return bridge + + def started(self, assignment: WorkloadAssignment, world_control_step: int) -> None: + row = assignment.env_id + state = self.active[row] + ctx = state["group"]["host"]._context + state["group"]["engine"].mark_row_started(state["native"]) + ctx.prepared[row][4]["world_control_start"] = world_control_step + ctx.journal.update( + ctx.requests[row], status="running", world_control_start=world_control_step + ) + + def observe( + self, + assignments: tuple[WorkloadAssignment, ...], + action: Any, + observation: object, + ) -> Sequence[int]: + ids = tuple(a.env_id for a in assignments) + self.activation_snapshot = None + for name in {a.workload_id for a in assignments}: + self.groups[name]["host"]._context.journal.checkpoint() + self.collector.observe(ids, action, observation) + checks = self.owner.collision + return tuple( + row + for row in ids + if checks is not None + and row in checks.results + and checks.results[row].status != "passed" + ) + + def complete(self, result: WorkloadRowResult) -> Any: + from embodichain.lab.sim.scene.physics import capture_physics_properties + from ._gym_episode import GymEpisodeAssembler + + row = result.assignment.env_id + state = self.active.pop(row, None) + if state is None: + return None + group = state["group"] + ctx = group["host"]._context + outcome = result.program + factory = state["factory"] + if factory is not None: + records = tuple(factory.records) + ctx.prepared[row][4]["expansion"] = [r.to_metadata() for r in records] + portable = [] + ctx.source_artifacts = { + key: value + for key, value in ctx.source_artifacts.items() + if not key.startswith(f"plan_{row}_") + } + for index, record in enumerate(records): + trace = record.to_metadata() + if "plan_artifact" in trace: + key = f"plan_{row}_{index}" + ctx.source_artifacts[key] = trace.pop("plan_artifact") + trace["plan_artifact_key"] = key + portable.append(trace) + outcome = replace(outcome, expansion_records=(tuple(portable),)) + if ( + ctx.extensions.require_plan_transform + and outcome.accepted[0] + and not records + ): + outcome = replace( + outcome, + accepted=(False,), + reasons=("requested_operator_not_applied",), + ) + ctx.physics_before = state["physics_before"] + ctx.prepared[row][4].update( + world_control_start=result.world_control_start, + world_control_end=result.world_control_end, + ) + try: + episode = GymEpisodeAssembler(ctx).assemble( + {row: ctx.prepared[row]}, + self.collector, + {}, + outcome, + capture_physics_properties(ctx.view), + )[row] + if episode is not None: + episode = replace( + episode, + metadata={ + **episode.metadata, + "execution": { + **self.execution, + "workload_id": result.assignment.workload_id, + "world_control_start": result.world_control_start, + "world_control_end": result.world_control_end, + }, + }, + ) + return group["engine"].complete_row(state["native"], episode) + finally: + state["adapter"].close() + self.collector.discard(row) + + def close(self) -> None: + for state in self.active.values(): + state["adapter"].close() + self.active.clear() + + +def run_gym_expansion_workloads( + workloads: Sequence[GymExpansionWorkload], + execution: WorkloadExecutionCfg, + *, + extensions: GymExpansionHostExtensions | None = None, + output_dir: str | Path, +) -> Mapping[str, object]: + """Collect and export independent groups in one process and one owned World. + + The sole collector uses exactly ``workload_count * batch_size`` physical + environments. It captures the seeded nominal scene in this same World; + preparation never creates a reference World. Original per-group quotas, + attempts, measured acceptance and confirmed receipt accounting are retained. + One bounded writer thread exports frozen groups while other groups collect. + Writers receive only artifact paths and never access the live World. + + Args: + workloads: Nonempty unique source groups, each owning a fresh output. + execution: W, B, planning and original step/settling budgets. + extensions: Common collection renderer and measured validator. Per-group + scene/recipe/plan callbacks belong to each workload's extensions. + output_dir: Fresh root for execution/reference/progress diagnostics. + + Returns: + Owned per-workload results and actual process/World/planning evidence. + Partial quotas remain explicit even when qualified artifacts were saved. + """ + from .host import GymExpansionHost + from ._gym_recipe import GymRecipeBuilder + from ._gym_utils import _hash + from .engine import ExpansionEngine, ExpansionJobCfg + from .journal import ExpansionJournal + from .session import ExpansionSession + from .session_cfg import ExpansionSessionCfg + from .persistence import ArtifactEpisodeSink, load_expansion_episode + from .workflow import _RecordedSink + from .episode_export import export_expansion_episodes + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + from embodichain.lab.gym.utils.registration import REGISTERED_ENVS + from embodichain.utils.utility import load_config + from embodichain.lab.gym.envs._json import json_safe_copy + + if not workloads or len({w.workload_id for w in workloads}) != len(workloads): + raise ValueError("workloads must be nonempty with unique IDs") + execution.__post_init__() + if execution.workload_count > len(workloads): + raise ValueError("workload_count exceeds available groups") + common = extensions or GymExpansionHostExtensions() + if ( + common.scene_source_factory + or common.recipe_transform + or common.plan_transform_factory + ): + raise ValueError( + "common extensions may only select rendering and measured validation" + ) + root = Path(output_dir) + root.mkdir(parents=True, exist_ok=True) + configs = [] + for workload in workloads: + cfg = workload.cfg + cfg.__post_init__() + for field, digest in workload.report["input_files"].items(): + value = getattr(cfg, field, None) + if ( + value is None + or hashlib.sha256(Path(value).read_bytes()).hexdigest() != digest + ): + raise ValueError(f"workload input {field} differs from preflight") + if any( + getattr(cfg, field) != getattr(workloads[0].cfg, field) + for field in ( + "capture_media", + "episode_byte_budget", + "media_buffer_bytes", + ) + ): + raise ValueError("workloads must share media and evidence buffer limits") + if not workload.workload_id or not isinstance(workload.workload_id, str): + raise ValueError("workload IDs must be nonempty text") + if ( + any( + getattr(cfg, field) + for field in ( + "source_profile", + "collision_profile", + "prototype_validation_profile", + "motion_profile", + "scene_atlas", + "visual_profile", + "render_profiles", + "mass_scale", + "friction", + "center_of_mass", + "joint_stiffness_scale", + "joint_damping_scale", + ) + ) + or cfg.source != "task_program" + ): + raise ValueError( + "configured physical/visual/prototype operators require the cohort job API" + ) + if (cfg.seed, cfg.max_steps, cfg.settling_steps) != ( + execution.seed, + execution.max_steps, + execution.settling_steps, + ): + raise ValueError( + "workload and execution seeds/step/settling budgets must match" + ) + if ( + workload.extensions.validator_factory + or workload.extensions.collection_renderer + ): + raise ValueError("rendering and validation must use common extensions") + raw = load_config(cfg.deployment) + configs.append( + ( + raw, + config_to_cfg( + raw, + source_path=Path(cfg.deployment), + task_program_path_override=cfg.program, + ), + ) + ) + first = configs[0][1] + + def physical_key(cfg: Any) -> str: + return _hash( + { + name: ( + getattr(cfg, name).to_dict() + if hasattr(getattr(cfg, name), "to_dict") + else getattr(cfg, name) + ) + for name in ( + "robot", + "sim_cfg", + "rigid_object", + "background", + "target_control_frequency", + "sim_steps_per_control", + "events", + "sensor", + ) + if hasattr(cfg, name) + } + ) + + if any(physical_key(cfg) != physical_key(first) for _, cfg in configs[1:]): + raise ValueError( + "workloads must share physical environment, robot, sensors and cadence" + ) + + owner = source = None + groups: dict[str, Any] = {} + adapters: dict[int, Any] = {} + futures: dict[str, Any] = {} + started = time.monotonic() + writer = ThreadPoolExecutor(max_workers=1, thread_name_prefix="expansion-export") + try: + cfg = ExpansionRunCfg( + **{**workloads[0].cfg.to_dict(), "batch_size": execution.num_envs} + ) + owner = GymExpansionHost( + cfg, + workloads[0].report, + root, + ExpansionJournal(root), + workloads[0].source_artifacts, + extensions=common, + ) + env = owner.env + adapters[ + id(REGISTERED_ENVS[configs[0][0]["id"]].task_program_adapter_factory) + ] = env.task_program_adapter + # Sampling only row zero retains the original B=1 seeded reference + # regardless of the collection capacity. All rows use that same scene. + env.reset(seed=execution.seed, options={"save_data": False, "reset_ids": [0]}) + snapshot = _preview_snapshot(env) + nominal = { + uid: env.sim.get_rigid_object(uid) + .get_local_pose()[0] + .detach() + .cpu() + .tolist() + for uid in env.sim.get_rigid_object_uid_list() + } + _atomic_json( + root / "reference-scene.json", + {"seed": execution.seed, "poses_xyzw": nominal}, + ) + for workload, (raw, environment_cfg) in zip(workloads, configs, strict=True): + path = Path(workload.cfg.output_dir) + path.mkdir(parents=True, exist_ok=False) + cfg = ExpansionRunCfg( + **{**workload.cfg.to_dict(), "batch_size": execution.num_envs} + ) + report = { + **deepcopy(dict(workload.report)), + "resolved_deployment": snapshot, + } + journal = ExpansionJournal(path) + journal.open_run(report, cfg.to_dict(), resume=False) + _atomic_json(path / "preflight.json", report) + ctx = replace( + owner._context, + cfg=cfg, + report=report, + root=path, + journal=journal, + source_artifacts=deepcopy(dict(workload.source_artifacts)), + environment_cfg=environment_cfg, + requests={}, + prepared={}, + preparation_checks={}, + services={}, + bridges={}, + extensions=workload.extensions, + scene_source=None, + ) + if workload.extensions.scene_source_factory is not None: + ctx.scene_source = workload.extensions.scene_source_factory(env) + builder = GymRecipeBuilder(ctx) + transform = workload.extensions.recipe_transform + ctx.recipe = lambda ordinal, builder=builder, transform=transform: ( + builder.recipe(ordinal) + if transform is None + else transform(builder.recipe(ordinal)) + ) + # Non-owning facade delegates existing admission/acceptance methods; + # only owner above opens/closes the shared environment. + host = GymExpansionHost.__new__(GymExpansionHost) + host._context = ctx + factory = REGISTERED_ENVS[raw["id"]].task_program_adapter_factory + adapter = adapters.get(id(factory)) + if adapter is None: + adapter = factory.create_adapter(env) + adapters[id(factory)] = adapter + ctx.program = adapter.compile(environment_cfg.task_program) + if ( + workload.extensions.plan_transform_factory is not None + and adapter.configured_nominal_sources + ): + raise ValueError( + "a workload transform cannot replace configured nominal sources" + ) + if ( + groups + and ctx.program.program_id + != next(iter(groups.values()))["host"]._context.program.program_id + ): + raise ValueError("workloads must share the same Task Program") + admitted_at = [None] + + def admission_clock(admitted_at=admitted_at): + return ( + 0.0 if admitted_at[0] is None else time.monotonic() - admitted_at[0] + ) + + session = ExpansionSession( + ExpansionSessionCfg.from_mapping( + { + "collection": { + "target_committed_episodes": cfg.target_episodes, + "max_rollout_attempts": cfg.max_attempts, + "max_proposals": cfg.max_attempts, + "max_wall_time_s": cfg.max_wall_time_s, + }, + "execution": { + "ready_high_watermark": execution.num_envs, + "ready_low_watermark": 0, + }, + "persistence": { + "pending_episode_limit": execution.num_envs, + "pending_max_bytes": cfg.episode_byte_budget + * execution.num_envs, + }, + "validation": {"require_path_collision": True}, + } + ), + episode_namespace=journal.job_instance_id, + clock=admission_clock, + ) + sink = ArtifactEpisodeSink(path) + engine = ExpansionEngine( + ExpansionJobCfg( + batch_size=execution.num_envs, + candidate_budget=execution.num_envs, + episode_byte_budget=cfg.episode_byte_budget, + ), + session=session, + host=host, + sink=_RecordedSink(sink, journal), + ) + groups[workload.workload_id] = { + "host": host, + "session": session, + "sink": sink, + "engine": engine, + "adapter": adapter, + "admitted_at": admitted_at, + } + journal.stage("collection", "running") + source = _GymWorkloadSource(owner, groups) + specs = tuple( + ExpansionWorkloadCfg( + workload_id=w.workload_id, + target_episodes=w.cfg.target_episodes, + max_attempts=w.cfg.max_attempts, + max_wall_time_s=w.cfg.max_wall_time_s, + ) + for w in workloads + ) + + def export(name: str) -> dict[str, Any]: + group = groups[name] + journal = group["host"]._context.journal + stamp = time.monotonic() + journal.stage("export_lerobot", "running") + try: + exported = export_expansion_episodes(journal.root) + journal.stage("export_lerobot", "complete") + return {"lerobot": exported, "export_wall_s": time.monotonic() - stamp} + except Exception as error: + journal.stage("export_lerobot", "failed", error=str(error)) + raise + + def done(name: str, counts: Mapping[str, object]) -> None: + group = groups[name] + journal = group["host"]._context.journal + complete = counts["committed"] == group["host"].cfg.target_episodes + journal.stage("collection", "complete" if complete else "budget_exhausted") + journal.checkpoint(force=True) + futures[name] = writer.submit(export, name) + _atomic_json( + root / "workload-progress.json", + { + key: dict(value["session"].snapshot()["counts"]) + for key, value in groups.items() + }, + ) + + executor = WorkloadProgramExecutor( + env, specs, source, execution, on_workload_done=done + ) + source.execution = { + "mode": "single_process_single_world", + "workload_count": execution.workload_count, + "batch_size": execution.batch_size, + "num_envs": execution.num_envs, + "world": dict(executor.world_info), + } + _atomic_json(root / "execution.json", source.execution) + result = dict(executor.run()) + collection_wall = time.monotonic() - started + source.close() + for adapter in adapters.values(): + adapter.close() + owner.close() + owner = None + results = {} + for name, group in groups.items(): + exported = futures[name].result() + readback = [] + paths = { + item["episode_id"]: str( + Path(exported["lerobot"]["directory"]) / item["dataset_path"] + ) + for item in exported["lerobot"]["datasets"] + } + for receipt in group["sink"].inspect(): + episode = load_expansion_episode(receipt.storage_id) + readback.append( + { + "episode_id": episode.episode_id, + "steps": len(episode.actions), + "expansion": json_safe_copy( + episode.metadata.get("expansion", ()), + field_name="expansion readback", + ), + "lerobot": paths[episode.episode_id], + } + ) + counts = result["counts"][name] + results[name] = { + "complete": counts["committed"] == group["host"].cfg.target_episodes, + "target_episodes": group["host"].cfg.target_episodes, + "committed_episodes": counts["committed"], + "attempts": counts["attempts"], + "readback": readback, + "directory": str(group["host"].root), + **exported, + } + _atomic_json(group["host"].root / "adapter-readback.json", readback) + result.update( + workloads=results, + collection_wall_s=collection_wall, + wall_s=time.monotonic() - started, + ) + _atomic_json(root / "workload-execution.json", result) + return result + except BaseException as error: + for name, group in groups.items(): + if name not in futures: + group["host"]._context.journal.stage( + "collection", "failed", error=str(error) + ) + raise + finally: + writer.shutdown(wait=True, cancel_futures=True) + if source is not None: + source.close() + for adapter in adapters.values(): + adapter.close() + for group in groups.values(): + group["host"]._context.journal.checkpoint(force=True, state="finished") + if owner is not None: + owner.close() diff --git a/embodichain/lab/expansion/host_extensions.py b/embodichain/lab/expansion/host_extensions.py index 5f4e54a0c4..c7acc00d60 100644 --- a/embodichain/lab/expansion/host_extensions.py +++ b/embodichain/lab/expansion/host_extensions.py @@ -39,8 +39,10 @@ class ExpansionValidator(Protocol): terminal: Mapping[int, ValidationResult] results: Mapping[int, Any] - def prepare(self, snapshot: Any = None) -> None: - """Reset measured evidence after the host has settled its cohort.""" + def prepare( + self, snapshot: Any = None, *, env_ids: tuple[int, ...] | None = None + ) -> None: + """Reset evidence after settling; rolling callers select independent rows.""" ... def sample(self, ids: tuple[int, ...], action: Any, snapshot: Any = None) -> None: diff --git a/embodichain/lab/expansion/journal.py b/embodichain/lab/expansion/journal.py index 6573986161..3e717c013f 100644 --- a/embodichain/lab/expansion/journal.py +++ b/embodichain/lab/expansion/journal.py @@ -190,6 +190,37 @@ def update(self, attempt: RecipeAttempt | Path, **values: object) -> dict: _atomic_json(path, record) return record + def allocate_recipe(self, recipe: ExpansionRecipe) -> RecipeAttempt: + """Allocate one fresh rolling attempt while independent peers are active. + + The single writer supplies a unique, monotonically increasing ordinal. + This port does not resume interrupted attempts or change collection + budgets; native workload admission remains responsible for those limits. + """ + if self.run_id is None or self.operation != "all": + raise RuntimeError("rolling allocation requires an open collection run") + records = self.attempts() + if records and recipe.context.ordinal <= max(r["ordinal"] for r in records): + raise ValueError("rolling recipe ordinals must increase") + sequence = len(records) + directory = self.root / "attempts" / f"{sequence:08d}" + if directory.exists(): + raise FileExistsError("attempt sequence already exists") + _atomic_json( + directory / "attempt.json", + { + "sequence": sequence, + "ordinal": recipe.context.ordinal, + "recipe_id": recipe.recipe_id, + "recipe_spec": recipe.to_dict(), + "attempt_id": 0, + "run_id": self.run_id, + "status": "proposed", + "steps": 0, + }, + ) + return RecipeAttempt(recipe, 0, sequence, directory) + def reconcile(self, sink: object) -> None: """Repair frozen writes before admitting any physical retry.""" confirmed = {r.commit_id: r for r in sink.inspect()} diff --git a/tests/lab/expansion/test_journal.py b/tests/lab/expansion/test_journal.py index e50bd796b4..d2d67f3e91 100644 --- a/tests/lab/expansion/test_journal.py +++ b/tests/lab/expansion/test_journal.py @@ -91,6 +91,21 @@ def test_interrupted_recipe_retries_with_new_attempt_after_capacity_change(tmp_p next_run.checkpoint(force=True, state="closed") +def test_rolling_allocation_preserves_live_peers_and_ordinal_identity(tmp_path): + ledger = ExpansionJournal(tmp_path) + ledger.open_run( + {"job_id": "a", "semantic_id": "a", "execution_id": "r"}, {}, resume=False + ) + first = ledger.allocate_recipe(recipe(0)) + ledger.update(first, status="running") + second = ledger.allocate_recipe(recipe(1)) + assert [r["status"] for r in ledger.attempts()] == ["running", "proposed"] + assert (second.sequence, second.recipe.context.ordinal) == (1, 1) + with pytest.raises(ValueError, match="ordinals"): + ledger.allocate_recipe(recipe(1)) + ledger.checkpoint(force=True, state="finished") + + def test_changed_semantics_cannot_reuse_job(tmp_path): ledger = ExpansionJournal(tmp_path) ledger.open_run( diff --git a/tests/lab/expansion/test_workloads_sim.py b/tests/lab/expansion/test_workloads_sim.py index 516f540eb1..d631ae8d8a 100644 --- a/tests/lab/expansion/test_workloads_sim.py +++ b/tests/lab/expansion/test_workloads_sim.py @@ -347,10 +347,105 @@ def complete(self, result): SimulationManager.flush_cleanup_queue() +def _facade_probe(output: str) -> None: + import hashlib + import json + import torch + import yaml + from embodichain.lab.expansion import ( + ArtifactEpisodeSink, + ExpansionRunCfg, + GymExpansionHostExtensions, + GymExpansionWorkload, + WorkloadExecutionCfg, + run_gym_expansion_workloads, + load_expansion_episode, + ) + from embodichain.lab.expansion.collision import _MeasuredCollision + from embodichain.lab.gym.utils.registration import ( + discover_task_packages, + execute_init_hooks, + ) + from embodichain.lab.sim import SimulationManager + + torch.set_num_threads(1) + root = Path(__file__).resolve().parents[3] + task = ( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place" + ) + deployment = task / "task.ur5.yaml" + path = Path(output) + path.mkdir(exist_ok=True) + program = yaml.safe_load((task / "task_program/program.yaml").read_text()) + program["program"]["repeat"]["count"] = 1 + override = path / "program.yaml" + override.write_text(yaml.safe_dump(program)) + digest = hashlib.sha256(deployment.read_bytes()).hexdigest() + discover_task_packages() + execute_init_hooks() + workloads = [] + for name, target in (("pair", 2), ("queued", 1)): + cfg = ExpansionRunCfg( + deployment=str(deployment), + program=str(override), + output_dir=str(path / name), + target_episodes=target, + max_attempts=4, + max_steps=2500, + max_wall_time_s=240, + settling_steps=5, + ) + report = { + "job_id": name, + "semantic_id": name, + "execution_id": name, + "recipe_revision": name, + "input_files": {"deployment": digest}, + "runtime_versions": {}, + } + workloads.append(GymExpansionWorkload(name, cfg, report, {})) + result = run_gym_expansion_workloads( + workloads, + WorkloadExecutionCfg( + workload_count=1, batch_size=2, max_steps=2500, settling_steps=5 + ), + extensions=GymExpansionHostExtensions( + validator_factory=lambda env, output: _MeasuredCollision( + env, + str( + root / "examples/expansion/scene_robot_feasibility/search.ur5.yaml" + ), + output, + ) + ), + output_dir=path / "execution", + ) + assert result["complete"], result + assert result["num_envs"] == 2 and result["world"]["world_count"] == 1 + assert not SimulationManager._instances + for name, expected in (("pair", 2), ("queued", 1)): + group = result["workloads"][name] + assert group["complete"] and group["committed_episodes"] == expected + receipts = ArtifactEpisodeSink(path / name).inspect() + assert len(receipts) == len(group["lerobot"]["datasets"]) == expected + for receipt in receipts: + episode = load_expansion_episode(receipt.storage_id) + assert episode.validation.accepted + assert len(episode.timestamps) == len(episode.actions) + 1 + execution = episode.metadata["execution"] + assert execution["world_control_end"] - execution[ + "world_control_start" + ] + 1 == len(episode.actions) + assert execution["world"] == result["world"] + (path / "facade-result.json").write_text(json.dumps(result, indent=2)) + + @pytest.mark.requires_sim -def test_rolling_workloads_keep_native_receipts_and_world_boundaries(tmp_path): +@pytest.mark.parametrize("mode", ["executor", "facade"]) +def test_rolling_workloads_keep_native_receipts_and_world_boundaries(tmp_path, mode): result = subprocess.run( - [sys.executable, __file__, str(tmp_path)], + [sys.executable, __file__, str(tmp_path), mode], cwd=Path(__file__).resolve().parents[3], env={**os.environ, "SDL_VIDEODRIVER": "offscreen", "OMP_NUM_THREADS": "1"}, capture_output=True, @@ -361,4 +456,6 @@ def test_rolling_workloads_keep_native_receipts_and_world_boundaries(tmp_path): if __name__ == "__main__": - _probe(sys.argv[1]) + (_facade_probe if len(sys.argv) > 2 and sys.argv[2] == "facade" else _probe)( + sys.argv[1] + ) From 78dc36c41fa804d9de17761522499eecd2c8ad0b Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sun, 15 Nov 2026 13:09:23 +0000 Subject: [PATCH 17/45] fix(expansion): isolate workload grasp frames and reset settling --- .../topics/expansion-engine/workloads.md | 7 ++ embodichain/lab/expansion/gym_workloads.py | 52 ++++++++- embodichain/lab/expansion/workloads.py | 5 + .../integrations/_configured_services.py | 26 ++++- .../task_program/integrations/configured.py | 6 +- .../integrations/simulation/environment.py | 32 +++++- tests/lab/expansion/test_workloads_sim.py | 103 ++++++++++++++++-- 7 files changed, 209 insertions(+), 22 deletions(-) diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index 50e7c58c0d..d0ce0faadf 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -30,6 +30,13 @@ existing session, frame collector, episode assembler and confirmed artifact sink Different integration/grasp policies may share that World only when their physical configuration, program and cadence agree. Physical/visual/prototype operators remain on the cohort API; unsupported payloads fail explicitly. +Explicit contact sampling conventions may differ per integration through +`SimulationTaskProgramAdapterFactory.create_adapter(grasp_contact_frames=...)`; +the declared frame must match its grasp service, while TCP, hardware geometry +and drive/command bindings remain strict and the live robot is unchanged. +Only the initial nominal reset may run scene-settling events. The facade then +freezes reset events; rolling preparation is checked against the physical step +counter so another row cannot advance outside the World-owner loop. Frozen groups export through one bounded writer thread without World access. Fresh output is required; this facade does not resume interrupted collection. diff --git a/embodichain/lab/expansion/gym_workloads.py b/embodichain/lab/expansion/gym_workloads.py index 4109d677de..f7e8e56708 100644 --- a/embodichain/lab/expansion/gym_workloads.py +++ b/embodichain/lab/expansion/gym_workloads.py @@ -135,7 +135,7 @@ def prepare(self, assignments: tuple[WorkloadAssignment, ...]) -> None: ids = tuple(a.env_id for a in assignments) view = SceneView(self.env.sim, env_ids=ids) # Every candidate starts from the same seeded nominal reset. Original - # reset events may run, but cannot resample an unowned scene axis. + # scene reset events were frozen before rolling admission began. apply_changes_batch( view, {row: SceneChangeSet(self.nominal, tuple(self.nominal)) for row in ids}, @@ -559,12 +559,21 @@ def run_gym_expansion_workloads( first = configs[0][1] def physical_key(cfg: Any) -> str: + robot = deepcopy(cfg.robot.to_dict()) + for binding in robot.get("gripper_bindings", {}).values(): + binding["declaration"].pop("root_from_contact_frame", None) + binding["defaults"].pop("root_from_contact_frame", None) + binding["defaults"].get("binding", {}).pop("root_from_contact_frame", None) return _hash( { name: ( - getattr(cfg, name).to_dict() - if hasattr(getattr(cfg, name), "to_dict") - else getattr(cfg, name) + robot + if name == "robot" + else ( + getattr(cfg, name).to_dict() + if hasattr(getattr(cfg, name), "to_dict") + else getattr(cfg, name) + ) ) for name in ( "robot", @@ -609,7 +618,30 @@ def physical_key(cfg: Any) -> str: ] = env.task_program_adapter # Sampling only row zero retains the original B=1 seeded reference # regardless of the collection capacity. All rows use that same scene. + import inspect + from embodichain.lab.gym.envs.managers.event_manager import EventManager + + events = getattr(env, "event_manager", None) + if events is not None: + for name in events.active_functors.get("reset", ()): + event = deepcopy(events.get_functor_cfg(name)) + if "allow_partial_envs" in inspect.signature(event.func).parameters: + # No trajectories are live during this one nominal reset. + event.params["allow_partial_envs"] = True + events.set_functor_cfg(name, event) env.reset(seed=execution.seed, options={"save_data": False, "reset_ids": [0]}) + # The reference now owns all reset-time scene randomization/settling. + # Re-running those functors during row refill could advance live peers + # outside the executor and lose measured control boundaries. + if events is not None: + frozen_events = { + name: deepcopy(events.get_functor_cfg(name)) + for mode, names in events.active_functors.items() + if mode != "reset" + for name in names + } + env.cfg.events = frozen_events + env.event_manager = EventManager(frozen_events, env) snapshot = _preview_snapshot(env) nominal = { uid: env.sim.get_rigid_object(uid) @@ -668,7 +700,17 @@ def physical_key(cfg: Any) -> str: factory = REGISTERED_ENVS[raw["id"]].task_program_adapter_factory adapter = adapters.get(id(factory)) if adapter is None: - adapter = factory.create_adapter(env) + contact_frames = {} + for instance, binding in environment_cfg.robot.gripper_bindings.items(): + actual = first.robot.gripper_bindings[instance] + frame = binding["defaults"].get("root_from_contact_frame") + if frame != actual["defaults"].get("root_from_contact_frame"): + contact_frames[binding["declaration"]["control_part"]] = frame + adapter = ( + factory.create_adapter(env, grasp_contact_frames=contact_frames) + if contact_frames + else factory.create_adapter(env) + ) adapters[id(factory)] = adapter ctx.program = adapter.compile(environment_cfg.task_program) if ( diff --git a/embodichain/lab/expansion/workloads.py b/embodichain/lab/expansion/workloads.py index dfbd37b65e..4ecdc9afcb 100644 --- a/embodichain/lab/expansion/workloads.py +++ b/embodichain/lab/expansion/workloads.py @@ -353,6 +353,7 @@ def _fill(self) -> None: return self.refills += bool(self.live) assignments = tuple(added) + before = self.env.sim.physics_step_index result = self.env.reset( seed=self.cfg.seed, options={ @@ -361,6 +362,10 @@ def _fill(self) -> None: "episode_prepare": lambda: self.source.prepare(assignments), }, ) + if self.env.sim.physics_step_index != before: + raise RuntimeError( + "rolling reset/preparation must not advance World physics" + ) self.observation = result[0] rows = [a.env_id for a in added] self.hold[rows] = self.env.robot.get_qpos(target=True)[rows] diff --git a/embodichain/lab/task_program/integrations/_configured_services.py b/embodichain/lab/task_program/integrations/_configured_services.py index f4a7ea3c3b..7b65a1b690 100644 --- a/embodichain/lab/task_program/integrations/_configured_services.py +++ b/embodichain/lab/task_program/integrations/_configured_services.py @@ -358,7 +358,9 @@ class _AntipodalGraspPoseGeneratorFactory: filter_ground_collision: bool | None = None force_refresh: bool | None = None - def validate_binding(self, robot: object, target_id: str) -> None: + def validate_binding( + self, robot: object, target_id: str, *, contact_frame_override: object = None + ) -> None: """Reject a static service snapshot bound to different physical hardware.""" import json @@ -376,7 +378,17 @@ def validate_binding(self, robot: object, target_id: str) -> None: expected_binding = { key: value for key, value in declaration.items() if key != "instance" } - if json.dumps(selected["declaration"], sort_keys=True) != json.dumps( + actual_binding = dict(selected["declaration"]) + if contact_frame_override is not None: + # Explicit workload-local sampling frames change the proposal + # convention, never the installed TCP, mesh or joint commands. + if json.dumps(contact_frame_override, sort_keys=True) != json.dumps( + expected.get("root_from_contact_frame"), sort_keys=True + ): + raise ValueError("Contact frame must match the declared grasp service.") + expected_binding.pop("root_from_contact_frame", None) + actual_binding.pop("root_from_contact_frame", None) + if json.dumps(actual_binding, sort_keys=True) != json.dumps( expected_binding, sort_keys=True ): raise ValueError( @@ -391,11 +403,19 @@ def validate_binding(self, robot: object, target_id: str) -> None: ) def physical_model(values): - return { + result = { key: value for key, value in values.items() if key not in {"min_opening_width", "max_opening_width"} } + if contact_frame_override is not None: + result.pop("root_from_contact_frame", None) + result["binding"] = { + key: value + for key, value in result.get("binding", {}).items() + if key != "root_from_contact_frame" + } + return result if json.dumps(physical_model(actual), sort_keys=True) != json.dumps( physical_model(expected), sort_keys=True diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index a8b50a4376..6cfbae41c1 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -3380,6 +3380,8 @@ def registration(self) -> SimulationTaskProgramRegistration: def create_adapter( self, environment: object, + *, + grasp_contact_frames: Mapping[str, object] | None = None, ) -> TaskProgramEnvironmentAdapter: """Create a live adapter for an initialized environment.""" from .demo_skill import _ConfiguredDemoSkillSource @@ -3390,7 +3392,9 @@ def create_adapter( ): if getattr(environment, "num_envs", None) != 1: raise ValueError("configured Demo Skills require num_envs == 1") - adapter = self.delegate.create_adapter(environment) + adapter = self.delegate.create_adapter( + environment, grasp_contact_frames=grasp_contact_frames + ) adapter._set_configured_nominal_sources(self.nominal_motion_sources) return adapter diff --git a/embodichain/lab/task_program/integrations/simulation/environment.py b/embodichain/lab/task_program/integrations/simulation/environment.py index dfee0f64d9..064b4e2b2d 100644 --- a/embodichain/lab/task_program/integrations/simulation/environment.py +++ b/embodichain/lab/task_program/integrations/simulation/environment.py @@ -1529,16 +1529,44 @@ def registration(self) -> SimulationTaskProgramRegistration: def create_adapter( self, environment: object, + *, + grasp_contact_frames: Mapping[str, object] | None = None, ) -> TaskProgramEnvironmentAdapter: - """Create fresh grasp services and bind the initialized environment.""" + """Create fresh grasp services and bind the initialized environment. + + Args: + environment: Sole owner of the actual robot and physical World. + grasp_contact_frames: Explicit per-control-part sampling conventions + matching this factory's declared grasp services. Only these + contact frames may differ from the installed binding; TCP, + physical meshes, joint commands and all other calibration stay + strictly checked. The live robot configuration is never mutated. + + Returns: + Fresh adapter with independent grasp and execution services. + """ from embodichain.toolkits.graspkit import GraspPoseGenerator self._registration.assert_unchanged() + frames = dict(grasp_contact_frames or {}) + if set(frames) - self._grasp_pose_generator_factories.keys(): + raise ValueError("Contact frame overrides require declared grasp services.") generators: dict[str, GraspPoseGenerator] = {} for target_id, factory in self._grasp_pose_generator_factories.items(): validator = getattr(factory, "validate_binding", None) if validator is not None: - validator(environment.robot, target_id) + if target_id in frames: + validator( + environment.robot, + target_id, + contact_frame_override=frames[target_id], + ) + else: + validator(environment.robot, target_id) + elif target_id in frames: + raise ValueError( + "Grasp service cannot validate a contact frame override." + ) generator = factory() if not isinstance(generator, GraspPoseGenerator): raise TypeError( diff --git a/tests/lab/expansion/test_workloads_sim.py b/tests/lab/expansion/test_workloads_sim.py index d631ae8d8a..fe877ba1fe 100644 --- a/tests/lab/expansion/test_workloads_sim.py +++ b/tests/lab/expansion/test_workloads_sim.py @@ -348,6 +348,7 @@ def complete(self, result): def _facade_probe(output: str) -> None: + from copy import deepcopy import hashlib import json import torch @@ -363,6 +364,7 @@ def _facade_probe(output: str) -> None: ) from embodichain.lab.expansion.collision import _MeasuredCollision from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, discover_task_packages, execute_init_hooks, ) @@ -381,13 +383,44 @@ def _facade_probe(output: str) -> None: program["program"]["repeat"]["count"] = 1 override = path / "program.yaml" override.write_text(yaml.safe_dump(program)) - digest = hashlib.sha256(deployment.read_bytes()).hexdigest() + + def absolute_paths(value, base): + if isinstance(value, dict): + return {key: absolute_paths(item, base) for key, item in value.items()} + if isinstance(value, list): + return [absolute_paths(item, base) for item in value] + if isinstance(value, str) and value.endswith((".yaml", ".yml", ".json")): + return str((base / value).resolve()) + return value + + raw = absolute_paths(yaml.safe_load(deployment.read_text()), deployment.parent) + component = Path(raw["embodiment"]["component"]) + embodiment = absolute_paths(yaml.safe_load(component.read_text()), component.parent) + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + + base_cfg = config_to_cfg( + yaml.safe_load(deployment.read_text()), source_path=deployment + ) + contact = deepcopy( + base_cfg.robot.gripper_bindings["eef"]["defaults"]["root_from_contact_frame"] + ) + contact[0][3] += 0.001 + embodiment["grippers"]["eef"]["root_from_contact_frame"] = contact + component_override = path / "embodiment.yaml" + component_override.write_text(yaml.safe_dump(embodiment)) + raw["id"] = "TaskProgramRepeatedPickPlace-ContactFrame-v1" + raw["embodiment"]["component"] = str(component_override) + policy_deployment = path / "contact-frame.yaml" + policy_deployment.write_text(yaml.safe_dump(raw)) + config_to_cfg(raw, source_path=policy_deployment) discover_task_packages() execute_init_hooks() workloads = [] for name, target in (("pair", 2), ("queued", 1)): + selected_deployment = deployment if name == "pair" else policy_deployment + digest = hashlib.sha256(selected_deployment.read_bytes()).hexdigest() cfg = ExpansionRunCfg( - deployment=str(deployment), + deployment=str(selected_deployment), program=str(override), output_dir=str(path / name), target_episodes=target, @@ -405,25 +438,34 @@ def _facade_probe(output: str) -> None: "runtime_versions": {}, } workloads.append(GymExpansionWorkload(name, cfg, report, {})) + + observed_bindings = [] + + def validator(env, output): + binding = deepcopy(env.robot.cfg.gripper_bindings) + observed_bindings.append((env.robot.cfg, binding)) + factory = REGISTERED_ENVS[raw["id"]].task_program_adapter_factory + with pytest.raises(ValueError, match="actual hand installation"): + factory.create_adapter(env) + assert env.robot.cfg.gripper_bindings == binding + return _MeasuredCollision( + env, + str(root / "examples/expansion/scene_robot_feasibility/search.ur5.yaml"), + output, + ) + result = run_gym_expansion_workloads( workloads, WorkloadExecutionCfg( workload_count=1, batch_size=2, max_steps=2500, settling_steps=5 ), - extensions=GymExpansionHostExtensions( - validator_factory=lambda env, output: _MeasuredCollision( - env, - str( - root / "examples/expansion/scene_robot_feasibility/search.ur5.yaml" - ), - output, - ) - ), + extensions=GymExpansionHostExtensions(validator_factory=validator), output_dir=path / "execution", ) assert result["complete"], result assert result["num_envs"] == 2 and result["world"]["world_count"] == 1 assert not SimulationManager._instances + assert all(cfg.gripper_bindings == binding for cfg, binding in observed_bindings) for name, expected in (("pair", 2), ("queued", 1)): group = result["workloads"][name] assert group["complete"] and group["committed_episodes"] == expected @@ -439,6 +481,45 @@ def _facade_probe(output: str) -> None: ] + 1 == len(episode.actions) assert execution["world"] == result["world"] (path / "facade-result.json").write_text(json.dumps(result, indent=2)) + # A real drive change remains incompatible even though contact conventions + # above are allowed. Reject before constructing another World. + embodiment["simulation"]["joint_drive_props"]["stiffness"][ + "gripper_finger1_joint_1" + ] += 1 + hardware_component = path / "different-hardware.yaml" + hardware_component.write_text(yaml.safe_dump(embodiment)) + raw["id"] += "-Hardware" + raw["embodiment"]["component"] = str(hardware_component) + hardware_deployment = path / "different-hardware-task.yaml" + hardware_deployment.write_text(yaml.safe_dump(raw)) + rejected = GymExpansionWorkload( + "different", + ExpansionRunCfg( + **{ + **workloads[-1].cfg.to_dict(), + "deployment": str(hardware_deployment), + "output_dir": str(path / "different"), + } + ), + { + **workloads[-1].report, + "input_files": { + "deployment": hashlib.sha256( + hardware_deployment.read_bytes() + ).hexdigest() + }, + }, + {}, + ) + with pytest.raises(ValueError, match="physical environment"): + run_gym_expansion_workloads( + [workloads[0], rejected], + WorkloadExecutionCfg( + workload_count=1, batch_size=2, max_steps=2500, settling_steps=5 + ), + output_dir=path / "rejected", + ) + assert not SimulationManager._instances @pytest.mark.requires_sim From 7f9706409c94da2eca6a393eb2f4c65cf7e48dc1 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sun, 15 Nov 2026 16:09:44 +0000 Subject: [PATCH 18/45] feat(expansion): expose declarative collection and preview entrypoints --- .../expansion-engine/expansion-engine.md | 2 + .../topics/expansion-engine/workloads.md | 14 + docs/source/api_reference/public_api.rst | 55 ++ embodichain/lab/expansion/__init__.py | 14 + embodichain/lab/expansion/_preview_data.py | 5 +- .../lab/expansion/_task_program_config.py | 365 ++++++++++++ embodichain/lab/expansion/preview.py | 310 +++++++++++ .../lab/expansion/task_program_jobs.py | 525 ++++++++++++++++++ .../expansion/fixtures/task_program_job.yaml | 48 ++ tests/lab/expansion/test_configured.py | 27 + tests/lab/expansion/test_workloads_sim.py | 38 +- 11 files changed, 1398 insertions(+), 5 deletions(-) create mode 100644 embodichain/lab/expansion/_task_program_config.py create mode 100644 embodichain/lab/expansion/preview.py create mode 100644 embodichain/lab/expansion/task_program_jobs.py create mode 100644 tests/lab/expansion/fixtures/task_program_job.yaml diff --git a/agent_context/topics/expansion-engine/expansion-engine.md b/agent_context/topics/expansion-engine/expansion-engine.md index 71ff869d5b..f1fb5fd569 100644 --- a/agent_context/topics/expansion-engine/expansion-engine.md +++ b/agent_context/topics/expansion-engine/expansion-engine.md @@ -12,6 +12,8 @@ algorithms stay in `sim.motion.expansion`; scene and workspace algorithms stay i | Shared scheduling, admission and commit ports | `embodichain/lab/expansion/engine.py` | | Rolling workload × B execution in one caller-owned World | `embodichain/lab/expansion/workloads.py` | | Native workload World, source/evidence assembly and bounded export | `embodichain/lab/expansion/gym_workloads.py` | +| Declarative scene/motion jobs and pure configuration validation | `embodichain/lab/expansion/task_program_jobs.py`, `_task_program_config.py` | +| Owned result-preview launch, wait and stop | `embodichain/lab/expansion/preview.py` | | Job assembly, stage recovery and inspect | `embodichain/lab/expansion/workflow.py`, `stages.py` | | Native World ownership and compatibility facade | `embodichain/lab/expansion/host.py` | | Task-owned render/source/transform/acceptance ports | `embodichain/lab/expansion/host_extensions.py` | diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index d0ce0faadf..18664b4b9e 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -22,6 +22,20 @@ difficult group does not silently borrow another group's B-row allocation. ## Source ports and native accounting +`run_task_program_expansion` binds a deployment and a closed scene/motion +configuration to `run_gym_expansion_workloads`. It owns bounded pose/follower +proposals, grasp-policy snapshots, original-grid plan transforms, common measured +acceptance and aggregate results. Configuration/viewer loaders do not import the +simulator. Integrators retain task values and provenance inputs rather than +copying these binding components. `derive_grasp_expansion_deployment` exposes the +same immutable-source preparation for configuration preflight. + +`launch_expansion_preview`, `preview_expansion_results` and +`stop_expansion_preview` manage a bounded Linux native service with recorded PID +ownership. The preview reader accepts native aggregate results and derives path +endpoints from recorded motion; downstream workflow-root unwrapping stays with +the integrator. These APIs never recollect or advance physical time. + `run_gym_expansion_workloads` is the public native composition for Task Program scene/plan workloads. `GymExpansionWorkload` supplies a pinned deployment, independent collection budget, artifacts and row callbacks. The facade opens one diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 31ea4f6bfc..54a0d8089f 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -1672,6 +1672,13 @@ and durable episode-boundary recovery. See the offline expansion example. WorkloadProgramExecutor GymExpansionWorkload run_gym_expansion_workloads + load_task_program_expansion + load_expansion_viewer + run_task_program_expansion + derive_grasp_expansion_deployment + launch_expansion_preview + stop_expansion_preview + preview_expansion_results export_expansion_episodes .. autosummary:: @@ -1937,6 +1944,28 @@ Stage immutable numeric evidence and archived source attachments before publishi load_expansion_episode +embodichain.lab.expansion.preview +-------------------------------- + +Launch, wait for or stop a bounded native result-preview service. Status and logs +remain separate from saved episodes; PID ownership is checked before signalling. +Inputs may be native jobs, aggregate scene/motion results or preview plans. +Recorded motion supplies an endpoint when several robot solvers are present. + +.. currentmodule:: embodichain.lab.expansion.preview + +.. autosummary:: + + launch_expansion_preview + preview_expansion_results + stop_expansion_preview + +.. autofunction:: launch_expansion_preview + +.. autofunction:: preview_expansion_results + +.. autofunction:: stop_expansion_preview + embodichain.lab.expansion.program --------------------------------- @@ -2028,6 +2057,32 @@ Source-independent session limits. These settings are separate from deployment-o ExpansionCollectionCfg ExpansionPersistenceCfg +embodichain.lab.expansion.task_program_jobs +------------------------------------------ + +Load closed scene/motion declarations without simulator imports, derive local +grasp policy snapshots and collect through the existing single-World workload +engine. Caller-owned source hashes and runtime versions remain in confirmed +episode evidence. Physical thresholds and protected phase boundaries are retained; +the facade owns configuration binding, preparation, export and optional preview. + +.. currentmodule:: embodichain.lab.expansion.task_program_jobs + +.. autosummary:: + + load_task_program_expansion + load_expansion_viewer + run_task_program_expansion + derive_grasp_expansion_deployment + +.. autofunction:: load_task_program_expansion + +.. autofunction:: load_expansion_viewer + +.. autofunction:: run_task_program_expansion + +.. autofunction:: derive_grasp_expansion_deployment + embodichain.lab.expansion.visual -------------------------------- diff --git a/embodichain/lab/expansion/__init__.py b/embodichain/lab/expansion/__init__.py index a954ea40c1..97a515b7ed 100644 --- a/embodichain/lab/expansion/__init__.py +++ b/embodichain/lab/expansion/__init__.py @@ -68,6 +68,13 @@ "WorkloadProgramExecutor", "GymExpansionWorkload", "run_gym_expansion_workloads", + "load_task_program_expansion", + "load_expansion_viewer", + "run_task_program_expansion", + "derive_grasp_expansion_deployment", + "launch_expansion_preview", + "stop_expansion_preview", + "preview_expansion_results", "export_expansion_episodes", "GymExpansionHostExtensions", "ExpansionRowContext", @@ -122,6 +129,13 @@ def __getattr__(name: str) -> object: "WorkloadProgramExecutor": "workloads", "GymExpansionWorkload": "gym_workloads", "run_gym_expansion_workloads": "gym_workloads", + "load_task_program_expansion": "_task_program_config", + "load_expansion_viewer": "_task_program_config", + "run_task_program_expansion": "task_program_jobs", + "derive_grasp_expansion_deployment": "task_program_jobs", + "launch_expansion_preview": "preview", + "stop_expansion_preview": "preview", + "preview_expansion_results": "preview", "export_expansion_episodes": "episode_export", "GymExpansionHostExtensions": "host_extensions", "ExpansionRowContext": "host_extensions", diff --git a/embodichain/lab/expansion/_preview_data.py b/embodichain/lab/expansion/_preview_data.py index 06fda24abe..1d65e19715 100644 --- a/embodichain/lab/expansion/_preview_data.py +++ b/embodichain/lab/expansion/_preview_data.py @@ -313,7 +313,10 @@ def load_preview( raise ValueError( "Preview plan has no qualified trajectories; inspect its rejection/unrun records" ) - paths = [Path(p).expanduser().resolve() for p in outputs] + from .preview import _inputs, _recorded_eef + + paths = _inputs([Path(p) for p in outputs]) + eef_link = eef_link or _recorded_eef(paths) if len(set(paths)) != len(paths): raise ValueError("Duplicate output directory") if reference is not None: diff --git a/embodichain/lab/expansion/_task_program_config.py b/embodichain/lab/expansion/_task_program_config.py new file mode 100644 index 0000000000..b092888ffc --- /dev/null +++ b/embodichain/lab/expansion/_task_program_config.py @@ -0,0 +1,365 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Closed scene/motion declarations for native Task Program workloads.""" + +from __future__ import annotations +from pathlib import Path +from typing import Any +import math +import json +import yaml + +__all__: list[str] = [] + + +def load_mapping(path: str | Path) -> dict[str, Any]: + """Read declarations without importing the simulator or its logging hooks.""" + path = Path(path) + text = path.read_text(encoding="utf-8") + value = json.loads(text) if path.suffix == ".json" else yaml.safe_load(text) + if not isinstance(value, dict): + raise ValueError(f"Expected a mapping in {path}") + return value + + +OPERATORS = ( + "object_pose", + "affordance", + "joint_residual", + "via_points", + "nullspace_residual", +) + + +def load_expansion_viewer(value: dict | None = None) -> dict: + """Resolve presentation options without importing the native simulator. + + Args: + value: Optional headless and renderer declarations. + Returns: + Owned normalized options; unsupported values reject. + """ + if value is None: + value = {} + if not isinstance(value, dict) or set(value) - {"headless", "renderer"}: + raise ValueError("Expansion viewer accepts only headless and renderer") + headless, renderer = value.get("headless", True), value.get("renderer", "rt") + if type(headless) is not bool: + raise ValueError("Expansion viewer.headless must be a boolean") + if renderer not in ("rt", "fast-rt", "hybrid"): + raise ValueError("Expansion viewer.renderer must be rt, fast-rt or hybrid") + return {"headless": headless, "renderer": renderer} + + +def load_task_program_expansion(path: str | Path) -> dict[str, Any]: + """Load bounded scene/motion declarations while retaining physical criteria. + + Args: + path: YAML/JSON engine, scene, trajectory and validation declarations. + Returns: + Owned normalized declaration, including workload capacity. + Raises: + ValueError: For invalid scopes, protected phases, budgets or criteria. + """ + path = Path(path) + value = load_mapping(path) + if ( + set(value) != {"schema_version", "engine", "scene", "trajectory", "validation"} + or value["schema_version"] != 1 + ): + raise ValueError("Invalid demo expansion configuration") + engine, motion = value["engine"], value["trajectory"] + required_engine = { + "episodes_per_operator", + "max_attempts", + "max_steps", + "max_wall_time_s", + "settling_steps", + "capture_media", + } + if required_engine - set(engine) or set(engine) - required_engine - { + "cpu_threads", + "operator_workers", + "workload_count", + "batch_size", + "collision_device", + "grasp_nms_chunk_size", + "collection_renderer", + "collision_backend", + }: + raise ValueError("Unknown or missing expansion engine settings") + engine.setdefault("cpu_threads", 1) + if "operator_workers" in engine: + if "workload_count" in engine: + raise ValueError( + "Use engine.workload_count without legacy operator_workers" + ) + # Compatibility is a capacity migration, never a process fallback. + engine["workload_count"] = engine.pop("operator_workers") + engine.setdefault("workload_count", 1) + engine.setdefault("batch_size", 1) + engine.setdefault("collision_device", "cpu") + engine.setdefault("grasp_nms_chunk_size", 2048) + engine.setdefault("collection_renderer", "auto") + engine.setdefault("collision_backend", "newton") + if engine["collection_renderer"] not in ("auto", "rt", "fast-rt", "hybrid"): + raise ValueError( + "engine.collection_renderer must be auto, rt, fast-rt or hybrid" + ) + if engine["collision_backend"] not in ("newton", "native_contacts"): + raise ValueError("engine.collision_backend must be newton or native_contacts") + for name in ( + "episodes_per_operator", + "max_attempts", + "max_steps", + "settling_steps", + "cpu_threads", + "workload_count", + "batch_size", + "grasp_nms_chunk_size", + ): + if type(engine[name]) is not int or engine[name] < 1: + raise ValueError(f"engine.{name} must be a positive integer") + device = engine["collision_device"] + if not isinstance(device, str) or not ( + device == "cpu" + or device == "cuda" + or (device.startswith("cuda:") and device[5:].isdigit()) + ): + raise ValueError("engine.collision_device must be cpu, cuda or cuda:") + if ( + engine["max_attempts"] < engine["episodes_per_operator"] + or type(engine["capture_media"]) is not bool + ): + raise ValueError("Invalid expansion attempt or media settings") + required = { + "operators", + "joint_offset_scale", + "via_count", + "max_proposals", + "minimum_joint_delta", + "velocity_limit", + "acceleration_limit", + "nullspace_task_rows", + "nullspace_translation_tolerance_m", + "nullspace_rotation_tolerance_rad", + "free_phases", + } + if required - set(motion) or set(motion) - required - { + "affordance", + "call_indices", + }: + raise ValueError("Unknown or missing trajectory settings") + calls = motion.get("call_indices") + if calls is not None and ( + not isinstance(calls, list) + or not calls + or any(type(index) is not int or index < 0 for index in calls) + or len(set(calls)) != len(calls) + ): + raise ValueError( + "Trajectory call_indices must be distinct non-negative integers" + ) + operators = motion["operators"] + if ( + not isinstance(operators, list) + or not operators + or len(set(operators)) != len(operators) + or set(operators) - set(OPERATORS) + ): + raise ValueError( + "trajectory.operators must contain distinct supported operators" + ) + affordance = motion.get("affordance") + if "affordance" in operators and affordance is None: + raise ValueError("trajectory.affordance is required for affordance expansion") + if affordance is not None: + required_affordance = {"branches", "call_indices", "action_options"} + if ( + not isinstance(affordance, dict) + or required_affordance - set(affordance) + or set(affordance) - required_affordance - {"grasp_generators"} + ): + raise ValueError("Invalid trajectory.affordance settings") + if type(affordance["branches"]) is not int or affordance["branches"] < 2: + raise ValueError("trajectory.affordance.branches must be at least two") + calls = affordance["call_indices"] + if ( + not isinstance(calls, list) + or not calls + or any(type(index) is not int or index < 0 for index in calls) + or len(set(calls)) != len(calls) + ): + raise ValueError( + "Affordance call_indices must be distinct non-negative integers" + ) + if not isinstance(affordance["action_options"], dict) or any( + not isinstance(name, str) or not isinstance(options, dict) + for name, options in affordance["action_options"].items() + ): + raise ValueError( + "Affordance action_options must map preset names to settings" + ) + generators = affordance.get("grasp_generators", {}) + if not isinstance(generators, dict): + raise ValueError("Affordance grasp_generators must be a mapping") + for name, settings in generators.items(): + if ( + not isinstance(name, str) + or not isinstance(settings, dict) + or not settings + or set(settings) + - { + "max_candidates", + "approach_deviation_angle", + "kind", + "gripper", + "root_from_contact_frame", + } + ): + raise ValueError("Invalid affordance grasp generator overrides") + if {"kind", "gripper"} & settings.keys() and ( + not {"kind", "gripper"} <= settings.keys() + or settings["kind"] != "antipodal_parallel_jaw" + or not isinstance(settings["gripper"], dict) + ): + raise ValueError( + "A new grasp generator requires its kind and physical gripper description" + ) + count = settings.get("max_candidates", 2) + angle = settings.get("approach_deviation_angle", 0.0) + if type(count) is not int or count < 1: + raise ValueError("Affordance max_candidates must be positive") + if type(angle) not in (int, float) or not math.isfinite(angle) or angle < 0: + raise ValueError( + "Affordance approach_deviation_angle must be finite and non-negative" + ) + for name in ("via_count", "max_proposals"): + if type(motion[name]) is not int or motion[name] < 1: + raise ValueError(f"trajectory.{name} must be a positive integer") + for name in required - { + "operators", + "via_count", + "max_proposals", + "nullspace_task_rows", + "free_phases", + }: + if ( + type(motion[name]) not in (float, int) + or not math.isfinite(motion[name]) + or motion[name] <= 0 + ): + raise ValueError(f"trajectory.{name} must be finite and positive") + if motion["nullspace_task_rows"] not in ([0, 1, 2], [0, 1, 2, 3, 4, 5]): + raise ValueError( + "Nullspace tasks must explicitly constrain position or full pose" + ) + # These are the upstream skills' conservative free phases, narrowed for these demos. + allowed = { + "hand_over": {"pickup_approach"}, + "pick_up": {"approach"}, + "place": {"retract"}, + } + if not isinstance(motion["free_phases"], dict) or not motion["free_phases"]: + raise ValueError("Explicit free_phases are required") + for skill, phases in motion["free_phases"].items(): + if ( + skill not in allowed + or not isinstance(phases, list) + or not phases + or set(phases) - allowed[skill] + ): + raise ValueError(f"Unsupported or protected free phase for {skill}") + scene = value["scene"] + if not isinstance(scene, dict) or not scene: + raise ValueError("Explicit scene pose ranges are required") + for uid, options in scene.items(): + if set(options) == {"follow"}: + if options["follow"] not in scene or "follow" in scene[options["follow"]]: + raise ValueError( + "Scene followers must reference an independent pose range" + ) + continue + if ( + set(options) != {"translation_m", "yaw_rad"} + or len(options["translation_m"]) != 3 + ): + raise ValueError(f"Invalid scene pose range for {uid}") + for limits in (*options["translation_m"], options["yaw_rad"]): + if ( + len(limits) != 2 + or any( + type(x) not in (float, int) or not math.isfinite(x) for x in limits + ) + or limits[0] > limits[1] + ): + raise ValueError(f"Invalid finite ordered pose range for {uid}") + if not math.isfinite(engine["max_wall_time_s"]) or engine["max_wall_time_s"] <= 0: + raise ValueError("engine.max_wall_time_s must be finite and positive") + required_validation = { + "initial_checks", + "final_checks", + "events", + "allowed_contacts", + "mounting_contacts", + } + if required_validation - set(value["validation"]) or set( + value["validation"] + ) - required_validation - {"contact_hold"}: + raise ValueError( + "Explicit measured validation and contact permissions are required" + ) + hold = value["validation"].get("contact_hold") + if hold is not None: + if ( + engine["collision_backend"] != "native_contacts" + or not isinstance(hold, dict) + or set(hold) + != { + "entity", + "links", + "duration_s", + "max_translation_drift_m", + "max_rotation_drift_rad", + } + ): + raise ValueError( + "contact_hold requires native_contacts and explicit hold settings" + ) + if ( + not isinstance(hold["entity"], str) + or not hold["entity"] + or not isinstance(hold["links"], list) + or len(hold["links"]) != 2 + or len(set(hold["links"])) != 2 + or any(not isinstance(link, str) or not link for link in hold["links"]) + ): + raise ValueError( + "contact_hold requires an entity and two distinct robot links" + ) + for field in ( + "duration_s", + "max_translation_drift_m", + "max_rotation_drift_rad", + ): + if ( + type(hold[field]) not in (float, int) + or not math.isfinite(hold[field]) + or hold[field] <= 0 + ): + raise ValueError(f"contact_hold.{field} must be finite and positive") + return value diff --git a/embodichain/lab/expansion/preview.py b/embodichain/lab/expansion/preview.py new file mode 100644 index 0000000000..b136d85869 --- /dev/null +++ b/embodichain/lab/expansion/preview.py @@ -0,0 +1,310 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Owned launch, wait and stop operations for native result-preview services.""" + +from __future__ import annotations + +import json +import math +import os +from pathlib import Path +import signal +import subprocess +import sys +import time +from uuid import uuid4 + +from ._task_program_config import load_mapping +from embodichain.data_pipeline.recording.media import _atomic_json as atomic_json +from datetime import datetime, timezone + + +def utc_now() -> str: + return datetime.now(timezone.utc).isoformat() + + +__all__ = [ + "launch_expansion_preview", + "preview_expansion_results", + "stop_expansion_preview", +] + + +def _process_command(pid: int) -> bytes: + try: + return Path(f"/proc/{pid}/cmdline").read_bytes() + except FileNotFoundError: + return b"" + + +def _inputs(paths: list[Path]) -> list[Path]: + result = [] + for supplied in paths: + path = supplied.expanduser().resolve() + if (path / "job.json").is_file() or (path / "preview-plan.json").is_file(): + result.append(path) + elif (path / "expansion.json").is_file(): + data = load_mapping(path / "expansion.json") + result.extend( + Path(row["directory"]).resolve() + for row in data.get("operators", {}).values() + if row.get("committed_episodes", 0) > 0 + ) + else: + raise ValueError( + f"Preview input must be an aggregate result or native job: {path}" + ) + if not result or len(set(result)) != len(result): + raise ValueError( + "Preview requires distinct job outputs with confirmed episodes" + ) + return result + + +def _recorded_eef(inputs: list[Path]) -> str | None: + """Resolve the first measured motion endpoint, without robot-name routing.""" + from embodichain.lab.expansion import ArtifactEpisodeSink, load_expansion_episode + + for path in inputs: + if not (path / "job.json").is_file(): + continue + job = load_mapping(path / "job.json") + robot = job.get("resolved_deployment", {}).get("robot", {}) + solvers = robot.get("solver_cfg", {}) + ends = { + item.get("end_link_name") + for item in solvers.values() + if item.get("end_link_name") + } + if len(ends) == 1: + return next(iter(ends)) + receipts = ArtifactEpisodeSink(path).inspect() + if not receipts: + continue + episode = load_expansion_episode(receipts[0].storage_id) + for record in episode.metadata.get("expansion", ()): + joints = set(record.get("controlled_joint_names", ())) + matches = [ + cfg.get("end_link_name") + for cfg in solvers.values() + if joints + and joints == set(cfg.get("joint_names") or ()) + and cfg.get("end_link_name") + ] + if len(set(matches)) == 1: + return matches[0] + for segment in episode.metadata.get("program_segments", ()): + runtime = segment.get( + "runtime", segment.get("metadata", {}).get("runtime", {}) + ) + for call in runtime.get("calls", ()): + for endpoint in call.get("resolved_core_policy", {}).get( + "endpoints", () + ): + if endpoint.get("endpoint_id") == "motion": + end = solvers.get(endpoint.get("target_id"), {}).get( + "end_link_name" + ) + if end: + return end + return None + + +def launch_expansion_preview( + paths: list[Path], + *, + output: Path | None = None, + port: int = 0, + gpu: int = 0, + duration: float = 900, + open_browser: bool = False, + eef_link: str | None = None, + startup_timeout: float = 240, + max_episodes: int = 32, +) -> dict: + """Start a bounded native preview service after collection/export completes. + + Logs/status are separate from immutable run artifacts. Inputs are read-only; + no trajectory is recollected, substituted or credited as another success. + + Args: + paths: Native job, aggregate result or preview-plan directories. + output: New directory for owned service status and logs. + port: Loopback port; zero allocates an available port. + gpu: Visible GPU index used by the read-only renderer. + duration: Service seconds after readiness; zero waits for interruption. + open_browser: Open a browser after native rendering is ready. + eef_link: Optional endpoint; recorded motion supplies it otherwise. + startup_timeout: Bounded wait for the first complete frame. + max_episodes: Maximum recorded episodes to preload, between one and 64. + Returns: + Owned service status, command identity and ready URL. Linux PID ownership + is checked before the stop/wait APIs signal the process. + """ + paths = [Path(path) for path in paths] + if ( + type(port) is not int + or not 0 <= port <= 65535 + or type(gpu) is not int + or gpu < 0 + ): + raise ValueError("Invalid preview port or GPU") + if ( + not math.isfinite(duration) + or duration < 0 + or not math.isfinite(startup_timeout) + or startup_timeout <= 0 + ): + raise ValueError("Invalid preview duration/startup timeout") + if type(max_episodes) is not int or not 1 <= max_episodes <= 64: + raise ValueError("Preview max_episodes must be in [1,64]") + inputs = _inputs(paths) + output = ( + output or (paths[0].resolve().parent / ".previews" / uuid4().hex) + ).resolve() + output.mkdir(parents=True, exist_ok=True) + status_path = output / "status.json" + if status_path.exists(): + raise FileExistsError(f"Preview status already exists: {status_path}") + command = [sys.executable, "-m", "embodichain", "expansion", "preview"] + for path in inputs: + command.extend(["--input", str(path)]) + command.extend( + [ + "--port", + str(port), + "--gpu", + str(gpu), + "--duration", + str(duration), + "--max-episodes", + str(max_episodes), + ] + ) + end = eef_link or _recorded_eef(inputs) + if end: + command.extend(["--eef-link", end]) + if open_browser: + command.append("--open") + log_path = output / "server.log" + with log_path.open("w") as log: + process = subprocess.Popen( + command, stdout=log, stderr=subprocess.STDOUT, start_new_session=True + ) + status = { + "state": "starting", + "pid": process.pid, + "command": command, + "inputs": [str(p) for p in inputs], + "started_at": utc_now(), + "status_file": str(status_path), + "log": str(log_path), + } + atomic_json(status_path, status) + deadline = time.monotonic() + startup_timeout + try: + while time.monotonic() < deadline: + for line in log_path.read_text(errors="replace").splitlines(): + try: + ready = json.loads(line) + except ValueError: + continue + if isinstance(ready, dict) and ready.get("state") == "ready": + status.update(ready, state="ready", eef_link=end) + atomic_json(status_path, status) + return status + if process.poll() is not None: + raise RuntimeError( + f"Native preview exited {process.returncode}: {log_path.read_text(errors='replace')[-3000:]}" + ) + time.sleep(0.1) + raise TimeoutError(f"Native preview startup timed out; see {log_path}") + except BaseException: + if process.poll() is None: + process.send_signal(signal.SIGINT) + try: + process.wait(timeout=30) + except subprocess.TimeoutExpired: + process.kill() + process.wait() + atomic_json( + status_path, {**status, "state": "failed", "returncode": process.returncode} + ) + raise + + +def stop_expansion_preview(path: str | Path) -> dict[str, object]: + """Stop only the native service whose recorded command still owns its PID. + + Args: + path: Service status JSON or its directory. + Returns: + Closed status after the owned process stops; changed ownership rejects. + """ + path = Path(path) + status_path = path / "status.json" if path.is_dir() else path + status = load_mapping(status_path) + pid = status["pid"] + command = _process_command(pid) + if command: + actual = command.rstrip(b"\0").split(b"\0") + if actual != [os.fsencode(arg) for arg in status["command"]]: + raise ValueError("Preview PID is no longer owned by the recorded command") + try: + os.kill(pid, signal.SIGINT) + except ProcessLookupError: + pass + deadline = time.monotonic() + 30 + while _process_command(pid) and time.monotonic() < deadline: + time.sleep(0.1) + if _process_command(pid): + raise TimeoutError("Preview has not stopped; inspect its log") + status.update(state="closed", closed_at=utc_now()) + atomic_json(status_path, status) + return status + + +def preview_expansion_results( + paths: list[Path], **options: object +) -> dict[str, object]: + """Run the native browser viewer until its duration ends or Ctrl+C stops it. + + Args: + paths: Confirmed native job or aggregate result directories. + **options: Launch options accepted by :func:`launch_expansion_preview`. + Returns: + Final service status after bounded playback or interruption. + """ + status = launch_expansion_preview(paths, **options) + sys.stdout.write(json.dumps(status, ensure_ascii=False) + "\n") + sys.stdout.flush() + try: + expected = [os.fsencode(arg) for arg in status["command"]] + while True: + command = _process_command(status["pid"]) + if not command or command.rstrip(b"\0").split(b"\0") != expected: + break + time.sleep(0.2) + except KeyboardInterrupt: + return stop_expansion_preview(Path(status["status_file"])) + try: + os.waitpid(status["pid"], os.WNOHANG) + except ChildProcessError: + pass + status.update(state="closed", closed_at=utc_now()) + atomic_json(Path(status["status_file"]), status) + return status diff --git a/embodichain/lab/expansion/task_program_jobs.py b/embodichain/lab/expansion/task_program_jobs.py new file mode 100644 index 0000000000..d32682ad68 --- /dev/null +++ b/embodichain/lab/expansion/task_program_jobs.py @@ -0,0 +1,525 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Declarative scene/motion jobs composed through the native workload engine.""" + +from __future__ import annotations +from collections.abc import Mapping, Sequence +from copy import deepcopy +from dataclasses import replace +from pathlib import Path +from typing import Any, TYPE_CHECKING +import hashlib +import json +import os +from embodichain.utils.utility import load_config as load_mapping +from embodichain.data_pipeline.recording.media import ( + _digest as file_digest, + _atomic_json as atomic_json, +) +from ._task_program_config import load_task_program_expansion, load_expansion_viewer + +if TYPE_CHECKING: + from embodichain.lab.sim.scene.feasibility import JointSceneCandidate + +__all__ = [ + "load_task_program_expansion", + "load_expansion_viewer", + "run_task_program_expansion", + "derive_grasp_expansion_deployment", +] + + +def _load_deployment(path: Path) -> None: + from embodichain.lab.gym.utils.registration import ( + discover_task_packages, + execute_init_hooks, + ) + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + + discover_task_packages() + execute_init_hooks() + config_to_cfg(load_mapping(path), source_path=path) + + +class _BoundedPoseSource: + """Sample bounded arena-frame changes with task-declared support followers.""" + + def __init__( + self, env: Any, configuration: Mapping[str, Any], *, seed: int, revision: str + ) -> None: + self.configuration, self.seed, self.request_id = configuration, seed, revision + self.poses = { + uid: env.sim.get_rigid_object(uid) + .get_local_pose(to_matrix=True)[0] + .detach() + .cpu() + .numpy() + .copy() + for uid in configuration + } + self.roots = { + env.robot.uid: env.robot.get_local_pose(to_matrix=True)[0] + .detach() + .cpu() + .tolist() + } + + def candidate(self, ordinal: int) -> JointSceneCandidate: + import numpy as np + from embodichain.lab.sim.scene.expansion import ScenePoseChange, SceneVariant + from embodichain.lab.sim.scene.feasibility import ( + JointSceneCandidate, + RobotDeploymentCandidate, + ) + + offsets = {} + for uid, options in self.configuration.items(): + if "follow" in options: + continue + seed = int.from_bytes( + hashlib.sha256(f"{self.seed}/{ordinal}/{uid}".encode()).digest()[:8], + "little", + ) + generator = np.random.default_rng(seed) + offsets[uid] = [ + generator.uniform(*bounds) + for bounds in (*options["translation_m"], options["yaw_rad"]) + ] + changes, parameters = [], [] + for uid, options in self.configuration.items(): + dx, dy, dz, yaw = offsets[options.get("follow", uid)] + pose = self.poses[uid].copy() + c, s = np.cos(yaw), np.sin(yaw) + pose[:3, :3] = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) @ pose[:3, :3] + pose[:3, 3] += (dx, dy, dz) + changes.append(ScenePoseChange(uid, tuple(map(tuple, pose)))) + parameters.extend((dx, dy, dz, yaw)) + scene = SceneVariant(self.request_id, self.seed, ordinal, tuple(changes)) + return JointSceneCandidate( + self.request_id, + scene, + RobotDeploymentCandidate(self.roots), + "object_pose", + tuple(parameters), + ) + + +def _task_expansion_extensions(*, options, operator, seed, report): + """Declare task proposals; upstream owns collection and persistence.""" + from .host_extensions import GymExpansionHostExtensions + + pose_source = None + + def source(env): + nonlocal pose_source + pose_source = _BoundedPoseSource( + env, options["scene"], seed=seed, revision=report["semantic_id"] + ) + return pose_source + + def recipe_transform(recipe): + operations = [] + for operation in recipe.operations: + payload = json.loads(operation.payload_json) + if operation.name == "motion": + payload.update(operator=operator, options=options["trajectory"]) + elif operation.name == "joint_scene": + candidate = pose_source.candidate(recipe.context.ordinal) + payload["pose_changes"] = { + change.entity_uid: change.arena_pose + for change in candidate.scene.pose_changes + } + operations.append(replace(operation, payload_json=json.dumps(payload))) + return replace(recipe, operations=tuple(operations)) + + def plans(env, row): + from embodichain.lab.task_program.integrations.expansion import ( + TaskProgramSameGridExpansionFactory, + ) + + return TaskProgramSameGridExpansionFactory.from_native_plan( + operator, + options["trajectory"], + seed=seed + row.ordinal, + scene_case=row.scene_case, + revision=report["recipe_revision"], + output=row.directory / "plans", + episode_id=row.ordinal, + ) + + return GymExpansionHostExtensions( + scene_source_factory=source, + recipe_transform=recipe_transform, + plan_transform_factory=plans, + require_plan_transform=operator != "object_pose", + ) + + +def _prepare_deployment(deployment: Path, output: Path) -> Path: + """Snapshot configuration without opening a reference simulation World.""" + from embodichain.utils.config_paths import resolve_config_path + import yaml + + original = load_mapping(deployment) + + def paths(value): + if isinstance(value, dict): + return {key: paths(item) for key, item in value.items()} + if isinstance(value, list): + return [paths(item) for item in value] + if isinstance(value, str) and value.endswith((".yaml", ".yml", ".json")): + return str(resolve_config_path(value, base_dir=deployment.parent).resolve()) + return value + + snapshot = paths(deepcopy(original)) + owner = "environment" if "environment" in snapshot else "scene" + if "component" in snapshot[owner]: + physical = load_mapping(Path(snapshot[owner]["component"])) + snapshot[owner] = {"component": str((output / "environment.yaml").resolve())} + else: + from embodichain.lab.gym.utils.composition import resolve_gym_components + + physical = resolve_gym_components( + {owner: snapshot[owner]}, base_dir=deployment.parent + ).config + (output / "environment.yaml").write_text(yaml.safe_dump(physical, sort_keys=False)) + snapshot["id"] += "-Expansion-v1" + path = output / "deployment.json" + atomic_json(path, snapshot) + atomic_json( + output / "source.json", + { + "deployment": str(deployment), + "sha256": file_digest(deployment), + "config": original, + }, + ) + return path + + +def derive_grasp_expansion_deployment( + deployment: str | Path, output: str | Path, settings: Mapping[str, Any] +) -> Path: + """Snapshot a native grasp policy without mutating the source deployment. + + Args: + deployment: Native deployment with integration and embodiment references. + output: Fresh directory for derived component files. + settings: Validated affordance action_options and grasp_generators. + + Returns: + Derived deployment path, usable by native configuration preflight or + ordinary task execution. Physical task criteria remain unchanged. + """ + deployment, output = Path(deployment), Path(output) + from embodichain.utils.config_paths import resolve_config_path + import yaml + + snapshot = load_mapping(deployment) + integration_path = resolve_config_path( + snapshot["task_program"]["integration"], base_dir=deployment.parent + ) + integration = load_mapping(integration_path) + profile = integration["profile"] + presets = profile.get("skill_options", profile.get("action_options", {})) + for name, changes in settings["action_options"].items(): + if name not in presets or "kind" in changes: + raise ValueError( + "Affordance options must retain existing skill preset kinds" + ) + for key, value in changes.items(): + if value is None: + presets[name].pop(key, None) + else: + presets[name][key] = value + output.mkdir(parents=True) + integration_path = output / "integration.yaml" + integration_path.write_text(yaml.safe_dump(integration, sort_keys=False)) + snapshot["task_program"]["integration"] = str(integration_path.resolve()) + generator_overrides = settings.get("grasp_generators", {}) + if generator_overrides: + embodiment_path = resolve_config_path( + snapshot["embodiment"]["component"], base_dir=deployment.parent + ) + embodiment = load_mapping(embodiment_path) + generators = ( + embodiment["skill_profile"] + .setdefault("runtime_services", {}) + .setdefault("grasp_pose_generators", {}) + ) + for name, changes in generator_overrides.items(): + changes = dict(changes) + if "root_from_contact_frame" in changes: + # Bound physical data belongs to the actual gripper declaration, + # not its generator policy. Keep both native views consistent. + bound = [ + item + for item in embodiment.get("grippers", {}).values() + if item.get("control_part") == name + ] + if len(bound) != 1: + raise ValueError( + "A contact frame override requires one bound gripper" + ) + bound[0]["root_from_contact_frame"] = changes.pop( + "root_from_contact_frame" + ) + if name not in generators: + if not {"kind", "gripper"} <= changes.keys(): + raise ValueError(f"Unknown embodiment grasp generator: {name}") + generators[name] = {} + generators[name].update(changes) + embodiment_path = output / "embodiment.yaml" + embodiment_path.write_text(yaml.safe_dump(embodiment, sort_keys=False)) + snapshot["embodiment"]["component"] = str(embodiment_path.resolve()) + snapshot["id"] += "-Affordance-v1" + path = output / "deployment.json" + atomic_json(path, snapshot) + return path + + +def _run_operators( + deployment, output, options, selected, *, seed, sources, viewer, runtime_versions +): + """Call the public upstream collector without operator processes.""" + from embodichain.lab.expansion import ( + ExpansionRunCfg, + GymExpansionHostExtensions, + GymExpansionWorkload, + MeasuredExpansionValidator, + WorkloadExecutionCfg, + run_gym_expansion_workloads, + ) + from embodichain.utils.nms import pose_nms_scope + import torch + + settings = options["engine"] + versions = dict(runtime_versions) + workloads = [] + for name in selected: + operator_deployment = deployment + artifacts = dict(sources) + affordance = options["trajectory"].get("affordance", {}) + if name == "affordance" and ( + affordance["action_options"] or affordance.get("grasp_generators") + ): + operator_deployment = derive_grasp_expansion_deployment( + deployment, output / "inputs" / "affordance", affordance + ) + for path in operator_deployment.parent.iterdir(): + if path.is_file(): + artifacts[f"affordance_{path.name}"] = { + "path": str(path), + "sha256": file_digest(path), + } + _load_deployment(operator_deployment) + identity = hashlib.sha256( + json.dumps( + { + "options": options, + "operator": name, + "seed": seed, + "source_hashes": sorted( + value["sha256"] for value in artifacts.values() + ), + }, + sort_keys=True, + ).encode() + ).hexdigest() + report = { + "job_id": identity, + "semantic_id": identity, + "execution_id": identity, + "recipe_revision": identity, + "input_files": {"deployment": file_digest(operator_deployment)}, + "operator": name, + "adapter": "embodichain.task-program-workloads-v1", + "runtime_versions": versions, + "viewer": viewer, + } + cfg = ExpansionRunCfg( + deployment=str(operator_deployment), + output_dir=str((output / "dataset" / name).resolve()), + batch_size=settings["batch_size"], + target_episodes=settings["episodes_per_operator"], + max_attempts=settings["max_attempts"], + max_steps=settings["max_steps"], + max_wall_time_s=settings["max_wall_time_s"], + settling_steps=settings["settling_steps"], + capture_media=settings["capture_media"], + seed=seed, + ) + workloads.append( + GymExpansionWorkload( + name, + cfg, + report, + artifacts, + _task_expansion_extensions( + options=options, operator=name, seed=seed, report=report + ), + ) + ) + renderer = settings["collection_renderer"] + if renderer == "auto": + renderer = viewer["renderer"] if settings["capture_media"] else None + common = GymExpansionHostExtensions( + collection_renderer=renderer, + validator_factory=lambda env, path: MeasuredExpansionValidator( + env, + options["validation"], + path, + backend=( + "native_contacts" + if settings["collision_backend"] == "native_contacts" + else "geometry" + ), + device=settings["collision_device"], + ), + ) + execution = WorkloadExecutionCfg( + workload_count=min(settings["workload_count"], len(selected)), + batch_size=settings["batch_size"], + max_steps=settings["max_steps"], + settling_steps=settings["settling_steps"], + seed=seed, + ) + chunks = settings["grasp_nms_chunk_size"] if torch.cuda.is_available() else 2048 + with pose_nms_scope(chunk_size=chunks): + result = run_gym_expansion_workloads( + workloads, execution, extensions=common, output_dir=output + ) + atomic_json(output / "workload-execution.json", result) + return result["workloads"] + + +def run_task_program_expansion( + deployment: str | Path, + configuration: str | Path, + output_dir: str | Path, + *, + seed: int = 0, + operators: Sequence[str] | None = None, + source_artifacts: Mapping[str, object] | None = None, + runtime_versions: Mapping[str, object] | None = None, + viewer: Mapping[str, object] | None = None, + preview_output: str | Path | None = None, +) -> Mapping[str, object]: + """Collect declared scene/motion operators through the single-World engine. + + Args: + deployment: Native Gym deployment; task discovery uses installed providers. + configuration: Closed YAML/JSON scene, trajectory and measured criteria. + output_dir: Fresh collection directory; aggregate results are saved here. + seed: Candidate seed, independent of physical row allocation. + operators: Optional enabled subset in its desired group order. + source_artifacts: Caller-owned hashed inputs retained in native evidence. + runtime_versions: Caller provenance; package versions are read when absent. + viewer: Headless/renderer declarations. A non-headless job opens the native + read-only preview after collection, qualification and export. + preview_output: Optional service status/log directory outside saved data. + + Returns: + Owned operator quotas, confirmed readback, native dataset paths and preview + status. Incomplete groups remain explicit and never earn success credit. + """ + from importlib import import_module, metadata + import torch + + deployment, configuration, output = ( + Path(deployment), + Path(configuration), + Path(output_dir), + ) + options = load_task_program_expansion(configuration) + view = load_expansion_viewer(None if viewer is None else dict(viewer)) + selected = list( + options["trajectory"]["operators"] if operators is None else operators + ) + if ( + not selected + or len(set(selected)) != len(selected) + or set(selected) - set(options["trajectory"]["operators"]) + ): + raise ValueError("Requested operators must be a distinct enabled subset") + import_module("lerobot.datasets.lerobot_dataset") + threads = options["engine"]["cpu_threads"] + for name in ("OMP_NUM_THREADS", "MKL_NUM_THREADS", "OPENBLAS_NUM_THREADS"): + os.environ[name] = str(threads) + torch.set_num_threads(threads) + if runtime_versions is None: + runtime_versions = {} + for name in ( + "embodichain", + "dexsim-engine", + "torch", + "torchvision", + "torchcodec", + "lerobot", + "numpy", + ): + try: + runtime_versions[name] = metadata.version(name) + except metadata.PackageNotFoundError: + runtime_versions[name] = None + output.mkdir(parents=True, exist_ok=True) + inputs = output / "inputs" + inputs.mkdir(exist_ok=True) + sources = dict(source_artifacts or {}) + derived = _prepare_deployment(deployment, inputs) + atomic_json(inputs / "expansion.json", options) + for path in (derived, inputs / "environment.yaml", inputs / "expansion.json"): + sources[f"derived_{path.name}"] = { + "path": str(path), + "sha256": file_digest(path), + } + groups = _run_operators( + derived, + output, + options, + selected, + seed=seed, + sources=sources, + viewer=view, + runtime_versions=runtime_versions, + ) + result = { + "complete": all(value.get("complete", False) for value in groups.values()), + "operators": groups, + "configuration_sha256": file_digest(configuration), + "viewer": view, + } + atomic_json(output / "expansion.json", result) + if not view["headless"]: + from .preview import launch_expansion_preview + + result["preview"] = launch_expansion_preview( + [ + Path(group["directory"]) + for group in groups.values() + if group.get("committed_episodes", 0) + ], + output=( + Path(preview_output) + if preview_output is not None + else output.parent / ".previews" / output.name + ), + open_browser=True, + max_episodes=64, + ) + atomic_json(output / "expansion.json", result) + return result diff --git a/tests/lab/expansion/fixtures/task_program_job.yaml b/tests/lab/expansion/fixtures/task_program_job.yaml new file mode 100644 index 0000000000..bd11a2db29 --- /dev/null +++ b/tests/lab/expansion/fixtures/task_program_job.yaml @@ -0,0 +1,48 @@ +schema_version: 1 +engine: + workload_count: 2 + batch_size: 1 + episodes_per_operator: 1 + max_attempts: 4 + max_steps: 3000 + max_wall_time_s: 240 + settling_steps: 5 + capture_media: false +scene: + cube: + translation_m: [[0.0, 0.0], [0.0, 0.0], [0.0, 0.0]] + yaw_rad: [0.0, 0.0] +trajectory: + operators: [object_pose, joint_residual] + joint_offset_scale: 0.0001 + via_count: 2 + max_proposals: 8 + minimum_joint_delta: 0.0000001 + velocity_limit: 10.0 + acceleration_limit: 100.0 + nullspace_task_rows: [0, 1, 2] + nullspace_translation_tolerance_m: 0.001 + nullspace_rotation_tolerance_rad: 0.01 + free_phases: + pick_up: [approach] + place: [retract] +validation: + initial_checks: + - {kind: pose_near, entity: cube, position: [-0.42, -0.08, 0.025], tolerance_m: 0.08} + final_checks: &final + - {kind: pose_near, entity: cube, position: [-0.4, 0.48, 0.025], tolerance_m: 0.04} + - {kind: joint_range, joint: gripper_finger1_joint_1, lower: -0.001, upper: 0.005} + - {kind: speed_below, entity: cube, linear_m_s: 0.03, angular_rad_s: 0.2} + events: + - event_id: lift + conditions: + - {kind: lifted, entity: cube, height_m: 0.05} + - event_id: placed_and_released + conditions: *final + consecutive_steps: 3 + allowed_contacts: + - [cube, UR5/gripper_finger1_link_1] + - [cube, UR5/gripper_finger2_link_1] + - [cube, default_plane] + mounting_contacts: + - ['@root', default_plane] diff --git a/tests/lab/expansion/test_configured.py b/tests/lab/expansion/test_configured.py index fa77556963..332883b635 100644 --- a/tests/lab/expansion/test_configured.py +++ b/tests/lab/expansion/test_configured.py @@ -22,6 +22,33 @@ from embodichain.lab.expansion import ExpansionEngine, load_expansion_job +def test_task_program_declarations_are_pure_and_protect_contact_phases(tmp_path): + import json + import os + import subprocess + import sys + import yaml + from embodichain.lab.expansion import load_task_program_expansion + + path = Path(__file__).parent / "fixtures/task_program_job.yaml" + code = "from embodichain.lab.expansion import load_task_program_expansion; import json,sys; value=load_task_program_expansion(sys.argv[1]); assert not any(k=='dexsim' or k.startswith('dexsim.') for k in sys.modules); print(json.dumps(value))" + process = subprocess.run( + [sys.executable, "-c", code, str(path)], + capture_output=True, + text=True, + check=True, + env={**os.environ}, + ) + value = json.loads(process.stdout) + assert (value["engine"]["workload_count"], value["engine"]["batch_size"]) == (2, 1) + assert value["trajectory"]["operators"] == ["object_pose", "joint_residual"] + value["trajectory"]["free_phases"]["pick_up"] = ["approach", "lift"] + invalid = tmp_path / "protected.yaml" + invalid.write_text(yaml.safe_dump(value)) + with pytest.raises(ValueError, match="protected free phase"): + load_task_program_expansion(invalid) + + @pytest.mark.parametrize("name", ["default", "newton", "rgbd", "motion"]) def test_example_preflight_resolves_owned_backend_and_budget(name, tmp_path): path = ( diff --git a/tests/lab/expansion/test_workloads_sim.py b/tests/lab/expansion/test_workloads_sim.py index fe877ba1fe..b10b70d0ac 100644 --- a/tests/lab/expansion/test_workloads_sim.py +++ b/tests/lab/expansion/test_workloads_sim.py @@ -522,8 +522,38 @@ def validator(env, output): assert not SimulationManager._instances +def _declarative_probe(output: str) -> None: + import json + from embodichain.lab.expansion import ( + run_task_program_expansion, + ArtifactEpisodeSink, + load_expansion_episode, + ) + from embodichain.lab.sim import SimulationManager + + root = Path(__file__).resolve().parents[3] + result = run_task_program_expansion( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task.ur5.yaml", + Path(__file__).parent / "fixtures/task_program_job.yaml", + output, + ) + assert result["complete"], result + execution = json.loads((Path(output) / "workload-execution.json").read_text()) + assert execution["world"]["world_count"] == 1 and execution["num_envs"] == 2 + assert not SimulationManager._instances + for operator, group in result["operators"].items(): + receipts = ArtifactEpisodeSink(group["directory"]).inspect() + assert len(receipts) == len(group["lerobot"]["datasets"]) == 1 + episode = load_expansion_episode(receipts[0].storage_id) + assert episode.validation.accepted + assert len(episode.timestamps) == len(episode.actions) + 1 + if operator == "joint_residual": + assert episode.metadata["expansion"] + + @pytest.mark.requires_sim -@pytest.mark.parametrize("mode", ["executor", "facade"]) +@pytest.mark.parametrize("mode", ["executor", "facade", "declarative"]) def test_rolling_workloads_keep_native_receipts_and_world_boundaries(tmp_path, mode): result = subprocess.run( [sys.executable, __file__, str(tmp_path), mode], @@ -537,6 +567,6 @@ def test_rolling_workloads_keep_native_receipts_and_world_boundaries(tmp_path, m if __name__ == "__main__": - (_facade_probe if len(sys.argv) > 2 and sys.argv[2] == "facade" else _probe)( - sys.argv[1] - ) + {"executor": _probe, "facade": _facade_probe, "declarative": _declarative_probe}[ + sys.argv[2] if len(sys.argv) > 2 else "executor" + ](sys.argv[1]) From 722e385a0dfa7813cac92b6b61960939d887fa0c Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 16 Nov 2026 05:18:15 +0000 Subject: [PATCH 19/45] fix(sim): collect released native views before closing their scene Move the existing pre-Scene collection after clearing facade-owned data and entity references. Cyclic wrappers newly orphaned by those clears must finalize while their Scene and World parents are still alive, instead of being collected during a later teardown. Extend the existing deferred-destruction test with a cyclic view finalizer; both registry-ownership cases fail before the change. Keep backend synchronization, ownership checks, physical validation and GC count unchanged. Validation: 288 manager CPU tests passed; the full 149-node failed CI GPU shard passed 143 with 6 pre-existing capability skips on pinned DexSim 0.6.0rc2. Black 26.10.1, API docs 4963/4963, context and diff checks passed. Lifecycle context already requires child-before-parent cleanup and remains accurate. The reported native abort itself was not deterministic locally; this fixes the demonstrated lifetime gap at its cleanup boundary. --- embodichain/lab/sim/sim_manager.py | 11 +++++++---- tests/sim/test_sim_manager.py | 28 ++++++++++++++++++++++++++-- 2 files changed, 33 insertions(+), 6 deletions(-) diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index bc6511173a..ad9948aafa 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -6558,10 +6558,6 @@ def _deferred_destroy(self) -> None: # Release backend-owned views before Scene closes the native # resources that back them. Newton also synchronizes its device here. self.physics.prepare_for_teardown() - # Run wrapper destructors while their World is still alive. The later - # collections continue to break cycles left by the native teardown. - gc.collect() - # Render-only cameras may be attached to Spawn articulation link # nodes. Remove their Arena views before closing Scene, which # releases those parent nodes, and before World.quit releases their @@ -6608,6 +6604,13 @@ def _deferred_destroy(self) -> None: asset._spawn_result = None if hasattr(asset, "_entities"): asset._entities = [] + + # Dropping facade references can make cyclic native views collectible. + # Run their finalizers now, while Scene/World parents are still alive; + # collecting before the clears would leave them for a later World. + gc.collect() + + if self._spawn_scene is not None: try: self._spawn_scene.close() finally: diff --git a/tests/sim/test_sim_manager.py b/tests/sim/test_sim_manager.py index bc0c8a8af3..6150778c92 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -20,6 +20,7 @@ import gc import queue import threading +import weakref from pathlib import Path @@ -606,7 +607,19 @@ def test_deferred_destroy_prepares_backend_before_releasing_world( ) sim._gizmos = {} sim._markers = {} - sim._rigid_objects = {} + + class CyclicView: + def __init__(self): + self.retained_view = self + + view = CyclicView() + released = weakref.finalize(view, events.append, "view_released") + sim._rigid_objects = { + "asset": SimpleNamespace( + _data=view, _entities=[view], _spawn_result=spawn_scene + ) + } + del view sim._constraints = {} sim._rigid_object_groups = {} sim._fluid_objects = {} @@ -638,10 +651,21 @@ def reset(instance_id): instances.pop(instance_id) monkeypatch.setattr(SimulationManager, "reset", reset) - monkeypatch.setattr(gc, "collect", lambda: events.append("gc_collect")) + collect = gc.collect + + def collect_views(): + events.append("gc_collect") + return collect() + + monkeypatch.setattr(gc, "collect", collect_views) sim._deferred_destroy() + # Clearing a facade can make its cyclic native views collectible. Their + # finalizers must run before the owning native Scene/World are closed. + assert not released.alive + assert events.index("backend_prepare") < events.index("view_released") + assert events.index("view_released") < events.index("spawn_close") assert events.index("backend_prepare") < events.index("gc_collect") assert events.index("backend_prepare") < events.index("spawn_close") assert events.index("backend_prepare") < events.index("world_quit") From f3e08ae6b6ea25e4f956ed15cbf0ae9148d7a684 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 16 Nov 2026 07:56:55 +0000 Subject: [PATCH 20/45] fix(expansion): address admission, acceptance and preview review findings Identify explicit references after aggregate expansion; round contact holds up to the control grid and verify measured elapsed time; resolve the default-plane actor only when enabled. Retire expired warming attempts without proposal/credit and allow active peers to drain and export, including expiry during initial validation. Reject incompatible expert command modes, ActionManager declarations and controlled-joint selections before constructing a World. Preserve component-owned resource paths when relocating deployment snapshots by reusing the component resolver's resource normalization. Add focused regressions and extend real workload integration with an expired group alongside a peer that commits and exports a receipt. Validation: 106 expansion unit tests, 4 native integration tests on pinned DexSim 0.6.0rc2, and 42 context tests passed. Regressions reproduce all six review findings against 722e385a0. Black 26.10.1, API docs 4963/4963 and diff checks passed. Update workload guidance for action-schema compatibility and admission expiry. --- .../topics/expansion-engine/workloads.md | 10 +- embodichain/lab/expansion/_preview_data.py | 3 +- embodichain/lab/expansion/acceptance.py | 22 +- embodichain/lab/expansion/gym_workloads.py | 56 +++- .../lab/expansion/task_program_jobs.py | 8 +- embodichain/lab/expansion/workloads.py | 23 +- tests/lab/expansion/test_acceptance.py | 142 ++++++++++ tests/lab/expansion/test_configured.py | 40 +++ tests/lab/expansion/test_gym_workloads.py | 245 ++++++++++++++++++ tests/lab/expansion/test_preview.py | 42 +++ tests/lab/expansion/test_workloads_sim.py | 53 ++++ 11 files changed, 620 insertions(+), 24 deletions(-) create mode 100644 tests/lab/expansion/test_acceptance.py create mode 100644 tests/lab/expansion/test_gym_workloads.py diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index 18664b4b9e..dd2c4d1440 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -42,7 +42,9 @@ independent collection budget, artifacts and row callbacks. The facade opens one World, captures the original seeded nominal scene in row zero, and composes the existing session, frame collector, episode assembler and confirmed artifact sink. Different integration/grasp policies may share that World only when their -physical configuration, program and cadence agree. Physical/visual/prototype +physical configuration, program, cadence and expert action schema agree. +Command mode, ActionManager declarations and controlled-joint selections cannot +vary between workloads in one World. Physical/visual/prototype operators remain on the cohort API; unsupported payloads fail explicitly. Explicit contact sampling conventions may differ per integration through `SimulationTaskProgramAdapterFactory.create_adapter(grasp_contact_frames=...)`; @@ -63,7 +65,11 @@ After at least the declared physical settling substeps, `activate` captures the initial measured boundary, performs the source's required preparation checks, registers a native candidate and uses `ExpansionEngine.admit_row`. It returns a fresh bridge bound exactly to that row and the environment's control cadence. -Rejected preparation returns None and never earns quota. +Rejected preparation returns None and never earns quota. A warming row whose +admission time budget expires is retired without proposal or receipt; its group +can finish incomplete while already-admitted peers continue and export normally. +The native source also handles expiry during initial-state validation before +atomic session proposal. `started` calls `mark_row_started` after the first actual command. `observe` captures every active T+1 boundary and performs the original physical checks; diff --git a/embodichain/lab/expansion/_preview_data.py b/embodichain/lab/expansion/_preview_data.py index 1d65e19715..2fa9ce66d0 100644 --- a/embodichain/lab/expansion/_preview_data.py +++ b/embodichain/lab/expansion/_preview_data.py @@ -331,6 +331,7 @@ def load_preview( ground = True seen = set() frames = 0 + reference_index = len(paths) - 1 if reference is not None else None for group, path in enumerate(paths): job_path = path / "job.json" if not job_path.is_file(): @@ -382,7 +383,7 @@ def load_preview( ): raise ValueError("Outputs disagree about ground visibility") receipts = [r for r in ArtifactEpisodeSink(path).inspect() if r.confirmed] - is_reference = reference is not None and group == len(outputs) + is_reference = group == reference_index if not receipts or (is_reference and len(receipts) != 1): raise ValueError( "Each output needs confirmed episodes; reference must contain exactly one" diff --git a/embodichain/lab/expansion/acceptance.py b/embodichain/lab/expansion/acceptance.py index 71e8220df4..805d05fd79 100644 --- a/embodichain/lab/expansion/acceptance.py +++ b/embodichain/lab/expansion/acceptance.py @@ -24,6 +24,7 @@ from collections.abc import Mapping, Sequence from types import SimpleNamespace import xml.etree.ElementTree as ET +import math import numpy as np import torch from embodichain.data_pipeline.recording.media import _atomic_json as atomic_json @@ -268,7 +269,7 @@ def __init__(self, env, configuration, output, *, sensor, env_id=0): ) self.hold = configuration.get("contact_hold") count = ( - round(self.hold["duration_s"] * env.control_frequency) + 1 + math.ceil(self.hold["duration_s"] * env.control_frequency) + 1 if self.hold else 1 ) @@ -282,7 +283,10 @@ def __init__(self, env, configuration, output, *, sensor, env_id=0): ].tolist(), ): self.frames[actor] = f"{env.robot.uid}/{link}" - for uid in (*env.sim.get_rigid_object_uid_list(), "default_plane"): + objects = list(env.sim.get_rigid_object_uid_list()) + if env.sim.sim_config.enable_ground_plane: + objects.append("default_plane") + for uid in objects: self.frames[int(self.sensor.get_actor_ids(uid)[self.env_id, 0])] = uid ignored, allowed = self.contact_permissions( env, configuration, set(self.frames.values()) @@ -334,6 +338,10 @@ def _hold_report(self): } if len(self.samples) != self.samples.maxlen: return {**report, "passed": False} + elapsed = self.samples[-1][2] - self.samples[0][2] + report.update( + elapsed_duration_s=elapsed, required_duration_s=self.hold["duration_s"] + ) poses = np.asarray([sample[0] for sample in self.samples]) from scipy.spatial.transform import Rotation @@ -348,7 +356,13 @@ def _hold_report(self): ), ) report["passed"] = ( - report["both_fingers_samples"] == self.samples.maxlen + ( + elapsed >= self.hold["duration_s"] + or math.isclose( + elapsed, self.hold["duration_s"], rel_tol=1e-12, abs_tol=1e-12 + ) + ) + and report["both_fingers_samples"] == self.samples.maxlen and report["max_translation_drift_m"] <= self.hold["max_translation_drift_m"] and report["max_rotation_drift_rad"] <= self.hold["max_rotation_drift_rad"] @@ -392,7 +406,7 @@ def sample(self, ids, action, snapshot=None): .get_local_pose()[self.env_id] .tolist() ) - self.samples.append((pose, touching)) + self.samples.append((pose, touching, self.env.sim.simulation_time)) report = ( self._hold_report() if terminal.accepted diff --git a/embodichain/lab/expansion/gym_workloads.py b/embodichain/lab/expansion/gym_workloads.py index f7e8e56708..f6690cbddd 100644 --- a/embodichain/lab/expansion/gym_workloads.py +++ b/embodichain/lab/expansion/gym_workloads.py @@ -191,6 +191,11 @@ def activate(self, assignment: WorkloadAssignment, observation: object) -> Any: host, engine = group["host"], group["engine"] ctx = host._context request = ctx.requests[row] + if group["session"].stop_reason == "wall_time_exhausted": + ctx.journal.update( + request, status="initial_rejected", reason="wall_time_exhausted" + ) + return None if self.activation_snapshot is None: self.activation_snapshot = ( capture_state(ctx.view), @@ -243,15 +248,25 @@ def activate(self, assignment: WorkloadAssignment, observation: object) -> Any: ) session = group["session"] session.register_case(case, ctx.limits, joint_names=ctx.names) - identity, _ = session.propose( - case.scene_case_id, - initial_id, - source_id=ctx.program.program_id, - source_revision=ctx.report["recipe_revision"], - template_id=request.recipe.recipe_id, - operator_id="episode", - attempt_id=request.attempt_id, - ) + try: + identity, _ = session.propose( + case.scene_case_id, + initial_id, + source_id=ctx.program.program_id, + source_revision=ctx.report["recipe_revision"], + template_id=request.recipe.recipe_id, + operator_id="episode", + attempt_id=request.attempt_id, + ) + except RuntimeError: + # The budget can expire during initial-state validation, including + # between the early check and the session's atomic proposal guard. + if session.stop_reason != "wall_time_exhausted": + raise + ctx.journal.update( + request, status="initial_rejected", reason="wall_time_exhausted" + ) + return None episode_id, commit_id = session.episode_ids(identity) record = ctx.journal.update( request, @@ -360,6 +375,13 @@ def complete(self, result: WorkloadRowResult) -> Any: row = result.assignment.env_id state = self.active.pop(row, None) if state is None: + ctx = self.groups[result.assignment.workload_id]["host"]._context + if result.program.reasons == ("wall_time_exhausted",): + ctx.journal.update( + ctx.requests[row], + status="initial_rejected", + reason="wall_time_exhausted", + ) return None group = state["group"] ctx = group["host"]._context @@ -584,6 +606,8 @@ def physical_key(cfg: Any) -> str: "sim_steps_per_control", "events", "sensor", + "expert_trajectory", + "actions", ) if hasattr(cfg, name) } @@ -591,9 +615,21 @@ def physical_key(cfg: Any) -> str: if any(physical_key(cfg) != physical_key(first) for _, cfg in configs[1:]): raise ValueError( - "workloads must share physical environment, robot, sensors and cadence" + "workloads must share physical environment, robot, sensors, cadence and expert action schema" ) + def control_schema(raw: Mapping[str, Any]) -> tuple: + cls = REGISTERED_ENVS[raw["id"]].cls + return ( + tuple(getattr(cls, "control_parts", None) or ()), + tuple(getattr(cls, "active_joint_ids", ())), + ) + + if any( + control_schema(raw) != control_schema(configs[0][0]) for raw, _ in configs[1:] + ): + raise ValueError("workloads must share the expert controlled-joint schema") + owner = source = None groups: dict[str, Any] = {} adapters: dict[int, Any] = {} diff --git a/embodichain/lab/expansion/task_program_jobs.py b/embodichain/lab/expansion/task_program_jobs.py index d32682ad68..4576457c4f 100644 --- a/embodichain/lab/expansion/task_program_jobs.py +++ b/embodichain/lab/expansion/task_program_jobs.py @@ -188,7 +188,13 @@ def paths(value): snapshot = paths(deepcopy(original)) owner = "environment" if "environment" in snapshot else "scene" if "component" in snapshot[owner]: - physical = load_mapping(Path(snapshot[owner]["component"])) + from embodichain.lab.gym.utils._component_composition import ( + _resolve_local_resource_paths, + ) + + component_path = Path(snapshot[owner]["component"]) + physical = load_mapping(component_path) + _resolve_local_resource_paths(physical, base_dir=component_path.parent) snapshot[owner] = {"component": str((output / "environment.yaml").resolve())} else: from embodichain.lab.gym.utils.composition import resolve_gym_components diff --git a/embodichain/lab/expansion/workloads.py b/embodichain/lab/expansion/workloads.py index 4ecdc9afcb..89fca5bd4c 100644 --- a/embodichain/lab/expansion/workloads.py +++ b/embodichain/lab/expansion/workloads.py @@ -376,16 +376,27 @@ def _activate(self) -> None: for row in tuple(self.warming): state = self.warming[row] - if state["settled"] < self.cfg.settling_steps: - continue assignment = state["assignment"] - self.env._elapsed_steps[row] = 0 - self.env._reset_physical_objective((row,)) - bridge = self.source.activate(assignment, self.observation) + expired = ( + time.monotonic() - self.started_at[assignment.workload_id] + >= self.workloads[assignment.workload_id].max_wall_time_s + ) + if state["settled"] < self.cfg.settling_steps and not expired: + continue + if expired: + bridge = None + else: + self.env._elapsed_steps[row] = 0 + self.env._reset_physical_objective((row,)) + bridge = self.source.activate(assignment, self.observation) del self.warming[row] if bridge is None: outcome = ProgramBatchResult( - (row,), (0,), (False,), ("preparation_rejected",), ((),) + (row,), + (0,), + (False,), + ("wall_time_exhausted" if expired else "preparation_rejected",), + ((),), ) if ( self.source.complete( diff --git a/tests/lab/expansion/test_acceptance.py b/tests/lab/expansion/test_acceptance.py new file mode 100644 index 0000000000..2653b21f21 --- /dev/null +++ b/tests/lab/expansion/test_acceptance.py @@ -0,0 +1,142 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Measured acceptance boundaries without native simulation resources.""" + +from __future__ import annotations + +from types import SimpleNamespace +import pytest +import torch +from embodichain.lab.expansion.acceptance import _ContactRow + +pytestmark = pytest.mark.no_sim + + +@pytest.fixture +def contact_row(tmp_path): + def build(duration=0.25, ground=False): + asset = tmp_path / "robot.urdf" + asset.write_text( + '' + ) + actors = {"table": 10, "book": 11} + if ground: + actors["default_plane"] = 12 + + def actor_ids(uid, links=None): + if uid == "robot": + ids = {"root": 1, "left": 2, "right": 3} + return torch.tensor([[ids[link] for link in links]]) + return torch.tensor([[actors[uid]]]) + + sensor = SimpleNamespace(get_actor_ids=actor_ids) + env = SimpleNamespace( + control_frequency=10, + robot=SimpleNamespace( + uid="robot", + link_names=["root", "left", "right"], + cfg=SimpleNamespace( + fpath=str(asset), + root_props=SimpleNamespace(self_collision_enabled=True), + ), + ), + sim=SimpleNamespace( + sim_config=SimpleNamespace(enable_ground_plane=ground), + get_rigid_object_uid_list=lambda: ["table", "book"], + get_rigid_object=lambda uid: SimpleNamespace( + body_type="dynamic" if uid == "book" else "static" + ), + ), + ) + cfg = { + "initial_checks": [], + "mounting_contacts": [], + "allowed_contacts": [], + "events": [], + "final_checks": [ + {"kind": "joint_range", "joint": "j", "lower": -1, "upper": 1} + ], + "contact_hold": { + "entity": "book", + "links": ["left", "right"], + "duration_s": duration, + "max_translation_drift_m": 0.001, + "max_rotation_drift_rad": 0.001, + }, + } + return _ContactRow(env, cfg, tmp_path, sensor=sensor) + + return build + + +@pytest.mark.parametrize("duration,required", [(0.25, 4), (0.025, 2), (0.3, 4)]) +def test_contact_hold_requires_full_duration_and_distinct_measured_times( + contact_row, duration, required +): + row = contact_row(duration, ground=True) + pose = [0, 0, 0, 0, 0, 0, 1] + for i in range(required - 1): + row.samples.append((pose.copy(), [True, True], i / 10)) + assert not row._hold_report()["passed"] + row.samples.append((pose.copy(), [True, True], (required - 1) / 10)) + report = row._hold_report() + assert report["passed"] + assert report["elapsed_duration_s"] >= duration + assert row.samples.maxlen == required + # Enough samples at one physical instant cannot establish a hold duration. + row.samples.clear() + row.samples.extend((pose.copy(), [True, True], 0.0) for _ in range(required)) + assert not row._hold_report()["passed"] + row.samples.clear() + row.samples.extend((pose.copy(), [True, True], i / 10) for i in range(required)) + row.samples[-1][1][0] = False + assert not row._hold_report()["passed"] + row.samples[-1][1][0] = True + row.samples[-1][0][0] = 0.1 + assert not row._hold_report()["passed"] + + +@pytest.mark.parametrize("ground", [False, True]) +def test_native_contacts_only_resolve_present_default_plane(contact_row, ground): + row = contact_row(ground=ground) + assert {"table", "book", "robot/root", "robot/left", "robot/right"} <= set( + row.frames.values() + ) + assert ("default_plane" in row.frames.values()) is ground + + +def test_contact_sampling_uses_measured_world_time(contact_row): + from unittest.mock import Mock + from embodichain.lab.expansion.contracts import ValidationCheck, ValidationResult + + row = contact_row(ground=True) + passed = ValidationResult((ValidationCheck("task.success", "passed"),)) + row.monitor = SimpleNamespace(observe=Mock(), validation=lambda env: passed) + row.env.sim.get_rigid_object = lambda uid: SimpleNamespace( + get_local_pose=lambda: torch.tensor([[0.0, 0, 0, 0, 0, 0, 1]]) + ) + row.sensor.get_data = lambda: { + "user_ids": torch.tensor([[[11, 2], [11, 3]]]), + "is_valid": torch.ones((1, 2), dtype=torch.bool), + "impulse_valid": torch.ones((1, 2), dtype=torch.bool), + } + for timestamp, accepted in ((0.0, False), (0.1, False), (0.2, False), (0.3, True)): + row.env.sim.simulation_time = timestamp + row._interval_samples = 1 + row.sample((0,), None) + assert row.terminal[0].accepted is accepted + assert row._hold_report()["elapsed_duration_s"] == 0.3 diff --git a/tests/lab/expansion/test_configured.py b/tests/lab/expansion/test_configured.py index 332883b635..94bb78c69f 100644 --- a/tests/lab/expansion/test_configured.py +++ b/tests/lab/expansion/test_configured.py @@ -96,3 +96,43 @@ def test_resource_changes_preserve_semantics_but_record_new_execution(tmp_path): assert a["execution_id"] != b["execution_id"] cfg.mass_scale = {"cube": (2.0, 3.0)} assert preflight_expansion_job(cfg)["semantic_id"] != a["semantic_id"] + + +@pytest.mark.parametrize("owner", ["environment", "scene"]) +def test_deployment_snapshot_preserves_component_relative_assets(tmp_path, owner): + import json + import yaml + from embodichain.lab.expansion.task_program_jobs import _prepare_deployment + from embodichain.lab.gym.utils.composition import resolve_gym_components + + source = tmp_path / "source" + source.mkdir() + mesh = source / "mesh.obj" + mesh.write_text("v 0 0 0\nv 1 0 0\nv 0 1 0\nf 1 2 3\n") + simulation = { + "rigid_object": [ + {"uid": "mesh", "shape": {"shape_type": "Mesh", "fpath": "./mesh.obj"}} + ] + } + component = ( + { + "environment_id": "mesh-test", + "physics": "default", + "simulation": simulation, + "env": {}, + } + if owner == "environment" + else {"scene_id": "mesh-test", "simulation": simulation} + ) + (source / "component.yaml").write_text(yaml.safe_dump(component)) + declaration = {"id": "SnapshotTest-v1", owner: {"component": "component.yaml"}} + deployment = source / "deployment.yaml" + deployment.write_text(yaml.safe_dump(declaration)) + before = {p.name: p.read_bytes() for p in source.iterdir()} + output = tmp_path / "output" + output.mkdir() + derived = _prepare_deployment(deployment, output) + snapshot = json.loads(derived.read_text()) + resolved = resolve_gym_components(snapshot, base_dir=derived.parent).config + assert resolved["rigid_object"][0]["shape"]["fpath"] == str(mesh) + assert {p.name: p.read_bytes() for p in source.iterdir()} == before diff --git a/tests/lab/expansion/test_gym_workloads.py b/tests/lab/expansion/test_gym_workloads.py new file mode 100644 index 0000000000..44abf5ab6f --- /dev/null +++ b/tests/lab/expansion/test_gym_workloads.py @@ -0,0 +1,245 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Workload admission and action-schema compatibility boundaries.""" + +from __future__ import annotations + +from types import SimpleNamespace +from unittest.mock import Mock +import hashlib +import json +import pytest +import torch +from embodichain.lab.expansion.gym_workloads import ( + _GymWorkloadSource, + GymExpansionWorkload, + run_gym_expansion_workloads, +) +from embodichain.lab.expansion.workloads import ( + ExpansionWorkloadCfg, + WorkloadExecutionCfg, + WorkloadAssignment, + WorkloadProgramExecutor, +) + +pytestmark = pytest.mark.no_sim + + +@pytest.mark.parametrize("settled", [0, 5]) +def test_expired_warming_row_finishes_group_without_disturbing_live_peer( + monkeypatch, settled +): + monkeypatch.setattr( + "embodichain.lab.expansion.workloads.time.monotonic", lambda: 1.0 + ) + executor = object.__new__(WorkloadProgramExecutor) + executor.cfg = WorkloadExecutionCfg( + workload_count=2, batch_size=1, settling_steps=5 + ) + executor.workloads = { + name: ExpansionWorkloadCfg(name, 1, 1, budget) + for name, budget in (("expired", 0.01), ("peer", 10)) + } + executor.started_at = {"expired": 0.0, "peer": 0.0} + executor.counts = { + name: {"attempts": 1, "rollouts": 0, "committed": 0} + for name in executor.workloads + } + executor.warming = { + 0: {"assignment": WorkloadAssignment("expired", 0, 0, 0), "settled": settled} + } + peer = {"assignment": WorkloadAssignment("peer", 0, 1, 0)} + executor.live = {1: peer} + executor.lanes, executor.completed = ["expired", "peer"], set() + executor.on_workload_done = Mock() + executor.source = SimpleNamespace(activate=Mock(), complete=Mock(return_value=None)) + executor._activate() + assert executor.warming == {} + assert executor.live[1] is peer + executor.source.activate.assert_not_called() + result = executor.source.complete.call_args.args[0] + assert result.program.reasons == ("wall_time_exhausted",) + assert result.world_control_start is result.world_control_end is None + executor._fill() + executor.on_workload_done.assert_called_once_with( + "expired", executor.counts["expired"] + ) + assert executor.live[1] is peer + + +def test_native_source_retires_expired_attempt_before_reading_world(): + from embodichain.lab.expansion.session import ExpansionSession + from embodichain.lab.expansion.session_cfg import ExpansionSessionCfg + + clock = [0.0] + session = ExpansionSession( + ExpansionSessionCfg.from_mapping({"collection": {"max_wall_time_s": 0.01}}), + clock=lambda: clock[0], + ) + clock[0] = 1.0 + request = object() + journal = Mock() + ctx = SimpleNamespace(requests={0: request}, journal=journal) + source = object.__new__(_GymWorkloadSource) + source.groups = { + "expired": { + "host": SimpleNamespace(_context=ctx), + "engine": object(), + "session": session, + } + } + assert source.activate(WorkloadAssignment("expired", 0, 0, 0), None) is None + journal.update.assert_called_once_with( + request, status="initial_rejected", reason="wall_time_exhausted" + ) + assert session.snapshot()["counts"]["proposed"] == 0 + + +@pytest.mark.parametrize("stop", ["wall_time_exhausted", "unexpected"]) +def test_admission_expiry_race_only_suppresses_budget_exhaustion(stop): + session = SimpleNamespace(stop_reason=None, register_case=Mock()) + + def propose(*args, **kwargs): + session.stop_reason = stop + raise RuntimeError("proposal failed") + + session.propose = propose + request = SimpleNamespace( + recipe=SimpleNamespace( + recipe_id="recipe", + operations=[SimpleNamespace(name="physical_parameters", payload_json="{}")], + ), + attempt_id=0, + ) + ctx = SimpleNamespace( + requests={0: request}, + journal=Mock(), + collision=None, + report={"semantic_id": "semantic", "recipe_revision": "revision"}, + cfg=SimpleNamespace(seed=0, source="task_program"), + compatibility=SimpleNamespace(key="compatible"), + program=SimpleNamespace(program_id="program"), + limits=object(), + names=("joint",), + ) + source = object.__new__(_GymWorkloadSource) + source.groups = { + "group": { + "host": SimpleNamespace(_context=ctx), + "engine": object(), + "session": session, + } + } + source.activation_snapshot = (SimpleNamespace(batches={0: ()}), {}) + source.env = SimpleNamespace(robot=SimpleNamespace(uid="robot")) + assignment = WorkloadAssignment("group", 0, 0, 0) + if stop == "wall_time_exhausted": + assert source.activate(assignment, None) is None + assert ctx.journal.update.call_args.kwargs["reason"] == stop + else: + with pytest.raises(RuntimeError, match="proposal failed"): + source.activate(assignment, None) + ctx.journal.update.assert_not_called() + + +@pytest.mark.parametrize( + "difference", [None, "position_velocity", "use_action_manager", "joint_selection"] +) +def test_workload_action_schema_checked_before_constructing_world( + tmp_path, monkeypatch, difference +): + from embodichain.lab.expansion import ExpansionRunCfg + from embodichain.lab.gym.envs.expert_trajectory import ExpertTrajectoryCfg + from embodichain.lab.gym.utils.registration import REGISTERED_ENVS + import embodichain.lab.gym.utils.gym_utils as utils + import embodichain.lab.expansion.host as host + + class ReachedWorld(Exception): + pass + + factory = Mock(side_effect=ReachedWorld) + monkeypatch.setattr(host, "GymExpansionHost", factory) + workloads, configs = [], {} + for i, name in enumerate(("first", "second")): + deployment = tmp_path / f"{name}.json" + deployment.write_text(json.dumps({"id": name})) + expert = ExpertTrajectoryCfg( + joint_command_mode=( + "position_velocity" + if i and difference == "position_velocity" + else "position" + ), + use_action_manager=bool(i and difference == "use_action_manager"), + ) + configs[name] = SimpleNamespace( + robot=SimpleNamespace( + to_dict=lambda: { + "joint_names": ["a", "b"], + "control_parts": {"arm": [0, 1]}, + } + ), + expert_trajectory=expert, + ) + cls = type( + f"Env{i}", + (), + { + "control_parts": ["arm"], + "active_joint_ids": ( + [1] if i and difference == "joint_selection" else [0] + ), + }, + ) + monkeypatch.setitem(REGISTERED_ENVS, name, SimpleNamespace(cls=cls)) + cfg = ExpansionRunCfg( + deployment=str(deployment), + output_dir=str(tmp_path / name), + max_steps=20, + settling_steps=1, + ) + workloads.append( + GymExpansionWorkload( + name, + cfg, + { + "input_files": { + "deployment": hashlib.sha256( + deployment.read_bytes() + ).hexdigest() + } + }, + {}, + ) + ) + monkeypatch.setattr( + utils, "config_to_cfg", lambda raw, **kwargs: configs[raw["id"]] + ) + execution = WorkloadExecutionCfg( + workload_count=2, batch_size=1, max_steps=20, settling_steps=1 + ) + if difference is None: + with pytest.raises(ReachedWorld): + run_gym_expansion_workloads( + workloads, execution, output_dir=tmp_path / "run" + ) + factory.assert_called_once() + else: + with pytest.raises(ValueError, match="schema"): + run_gym_expansion_workloads( + workloads, execution, output_dir=tmp_path / "run" + ) + factory.assert_not_called() diff --git a/tests/lab/expansion/test_preview.py b/tests/lab/expansion/test_preview.py index 32d58e9214..1c9054d10a 100644 --- a/tests/lab/expansion/test_preview.py +++ b/tests/lab/expansion/test_preview.py @@ -414,3 +414,45 @@ def test_row_identity_without_motion_and_legacy_ambiguity(recorded_job): result = annotations(metadata, measured) np.testing.assert_allclose(result["targets"][0, :3, 3], [12, 20, 0]) assert result["candidate_ids"] == ("31",) + + +@pytest.mark.parametrize("second_count", [1, 2]) +def test_aggregate_input_keeps_explicit_reference_separate(recorded_job, second_count): + path, job, episode = recorded_job + root = path.parent + directories = [] + for name, count in (("first", 1), ("second", second_count), ("reference", 1)): + directory = root / name + directory.mkdir() + (directory / "job.json").write_text(json.dumps(job)) + for index in range(count): + ArtifactEpisodeSink(directory).submit( + replace( + episode, + episode_id=f"{name}-{index}", + commit_id=f"{name}-commit-{index}", + ), + submission_id=index, + ) + directories.append(directory) + aggregate = root / "aggregate" + aggregate.mkdir() + (aggregate / "expansion.json").write_text( + json.dumps( + { + "operators": { + str(i): { + "directory": str(p), + "committed_episodes": 1 if i == 0 else second_count, + } + for i, p in enumerate(directories[:2]) + } + } + ) + ) + data = load_preview( + [aggregate], reference=directories[2], max_episodes=1 + second_count + ) + assert len(data.clips) == 1 + second_count + assert data.reference.space.endswith("reference") + assert all(not clip.space.endswith("reference") for clip in data.clips) diff --git a/tests/lab/expansion/test_workloads_sim.py b/tests/lab/expansion/test_workloads_sim.py index b10b70d0ac..eb457d8e5b 100644 --- a/tests/lab/expansion/test_workloads_sim.py +++ b/tests/lab/expansion/test_workloads_sim.py @@ -521,6 +521,59 @@ def validator(env, output): ) assert not SimulationManager._instances + # Expiry during settling retires only the unstarted group. A peer must + # still collect/export a real receipt, and the aggregate stays incomplete. + from dataclasses import replace + + budget_groups = [ + replace( + workloads[0], + workload_id=name, + cfg=ExpansionRunCfg( + **{ + **workloads[0].cfg.to_dict(), + "output_dir": str(path / name), + "target_episodes": 1, + "max_attempts": 1, + "max_wall_time_s": budget, + } + ), + ) + for name, budget in (("expired", 1e-9), ("peer", 240.0)) + ] + budget_result = run_gym_expansion_workloads( + budget_groups, + WorkloadExecutionCfg( + workload_count=2, batch_size=1, max_steps=2500, settling_steps=5 + ), + extensions=GymExpansionHostExtensions( + validator_factory=lambda env, output: _MeasuredCollision( + env, + str( + root / "examples/expansion/scene_robot_feasibility/search.ur5.yaml" + ), + output, + ) + ), + output_dir=path / "budget-execution", + ) + assert not budget_result["complete"] + assert budget_result["counts"]["expired"] == { + "attempts": 1, + "rollouts": 0, + "committed": 0, + } + assert budget_result["workloads"]["peer"]["complete"] + assert len(ArtifactEpisodeSink(path / "peer").inspect()) == 1 + assert len(budget_result["workloads"]["peer"]["lerobot"]["datasets"]) == 1 + assert not ArtifactEpisodeSink(path / "expired").inspect() + records = list((path / "expired" / "attempts").glob("*/attempt.json")) + assert len(records) == 1 + expired_record = json.loads(records[0].read_text()) + assert expired_record["status"] == "initial_rejected" + assert expired_record["reason"] == "wall_time_exhausted" + assert not SimulationManager._instances + def _declarative_probe(output: str) -> None: import json From 2dfcbed03f614ec74bba7f84ef8d2e84e05c4cb7 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 16 Nov 2026 10:55:52 +0000 Subject: [PATCH 21/45] fix(expansion): stabilize hold timing and joint selection checks Normalize ULP-scale error in exact-grid hold durations before ceiling. Measure hold elapsed time with bounded fsum reductions of actual physics callback intervals instead of subtracting accumulated World clocks. Preserve distinct samples, upward rounding of real off-grid durations, both-finger contact and drift requirements. Compare control_parts and active_joint_ids from resolved environment configurations rather than their shared implementation class. Replace artificial-class coverage with real decoded configurations and a full deployment-file regression sharing ordinary EmbodiedEnv. Validation: 117 expansion unit tests, four native integration tests on pinned DexSim 0.6.0rc2 and 42 context tests passed. Regressions reproduce all three follow-up findings on 8c244cdc9, including 50 Hz/100 Hz boundary holds, 500k/5M-step clock accumulation, truly short windows and pre-World schema rejection. Black 26.10.1, API docs 5010/5010, context map and diff checks passed. Update context for measured hold timing and configuration-owned joint selections. --- .../expansion-engine/expansion-engine.md | 5 +- .../topics/expansion-engine/workloads.md | 3 +- embodichain/lab/expansion/acceptance.py | 41 ++++- embodichain/lab/expansion/gym_workloads.py | 14 +- tests/lab/expansion/test_acceptance.py | 171 +++++++++++++----- .../lab/expansion/test_extension_ports_sim.py | 18 +- tests/lab/expansion/test_gym_workloads.py | 120 +++++++++--- 7 files changed, 283 insertions(+), 89 deletions(-) diff --git a/agent_context/topics/expansion-engine/expansion-engine.md b/agent_context/topics/expansion-engine/expansion-engine.md index f1fb5fd569..79a12efc97 100644 --- a/agent_context/topics/expansion-engine/expansion-engine.md +++ b/agent_context/topics/expansion-engine/expansion-engine.md @@ -46,7 +46,10 @@ configured jobs. `sampled_rigid_v1` collision profiles independently check all physical box/plane scene bodies and task predicates from measured states. Their robot-base transforms are rebound after each cohort's scene preparation. State-only jobs retain independent and robot-mounted ContactSensors while -omitting cameras; contact acquisition does not require media capture. +omitting cameras; contact acquisition does not require media capture. Sustained +contact windows round off-grid durations upward, allowing only ULP-scale grid +roundoff. Their elapsed time sums measured physics intervals within the bounded +window; it does not subtract accumulated World clocks. Immutable recipes declare source/backend/phase/scope and shared-resource compatibility; the host checks them before mutating the World. diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index dd2c4d1440..c22d2af281 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -44,7 +44,8 @@ existing session, frame collector, episode assembler and confirmed artifact sink Different integration/grasp policies may share that World only when their physical configuration, program, cadence and expert action schema agree. Command mode, ActionManager declarations and controlled-joint selections cannot -vary between workloads in one World. Physical/visual/prototype +vary between workloads in one World. Selections belong to the resolved environment +configurations, not the shared registered environment implementation class. Physical/visual/prototype operators remain on the cohort API; unsupported payloads fail explicitly. Explicit contact sampling conventions may differ per integration through `SimulationTaskProgramAdapterFactory.create_adapter(grasp_contact_frames=...)`; diff --git a/embodichain/lab/expansion/acceptance.py b/embodichain/lab/expansion/acceptance.py index 805d05fd79..03ec618bbe 100644 --- a/embodichain/lab/expansion/acceptance.py +++ b/embodichain/lab/expansion/acceptance.py @@ -19,6 +19,7 @@ from __future__ import annotations from collections import deque +from itertools import islice from pathlib import Path from typing import Any, TYPE_CHECKING from collections.abc import Mapping, Sequence @@ -268,12 +269,18 @@ def __init__(self, env, configuration, output, *, sensor, env_id=0): 0, ) self.hold = configuration.get("contact_hold") - count = ( - math.ceil(self.hold["duration_s"] * env.control_frequency) + 1 - if self.hold - else 1 - ) + count = 1 + if self.hold: + intervals = self.hold["duration_s"] * env.control_frequency + nearest = round(intervals) + # Multiplication can place an exact grid duration just above an + # integer (e.g. 0.14 * 50). Only normalize floating-point roundoff; + # genuinely off-grid holds still round upward. + if abs(intervals - nearest) <= 2 * math.ulp(intervals): + intervals = nearest + count = max(1, math.ceil(intervals)) + 1 self.samples = deque(maxlen=count) + self._interval_dts: list[float] = [] self.sensor = sensor self.frames = {} for link, actor in zip( @@ -317,6 +324,7 @@ def reset(self, env_ids=None): 0, ) self.samples.clear() + self._interval_dts.clear() def prepare(self, snapshot=None): super().prepare(snapshot) @@ -326,6 +334,7 @@ def prepare(self, snapshot=None): def begin_control_step(self): self._bad = None self._interval_samples = 0 + self._interval_dts.clear() def before_sample(self): return None @@ -338,7 +347,10 @@ def _hold_report(self): } if len(self.samples) != self.samples.maxlen: return {**report, "passed": False} - elapsed = self.samples[-1][2] - self.samples[0][2] + # The first pose starts the hold; count only subsequent measured + # physics intervals. Bounded-window fsum avoids subtracting accumulated + # World clocks, whose rounding error grows throughout a collection. + elapsed = math.fsum(sample[2] for sample in islice(self.samples, 1, None)) report.update( elapsed_duration_s=elapsed, required_duration_s=self.hold["duration_s"] ) @@ -357,9 +369,15 @@ def _hold_report(self): ) report["passed"] = ( ( - elapsed >= self.hold["duration_s"] - or math.isclose( - elapsed, self.hold["duration_s"], rel_tol=1e-12, abs_tol=1e-12 + elapsed > 0 + and ( + elapsed >= self.hold["duration_s"] + or math.isclose( + elapsed, + self.hold["duration_s"], + rel_tol=0, + abs_tol=2 * math.ulp(self.hold["duration_s"]), + ) ) ) and report["both_fingers_samples"] == self.samples.maxlen @@ -406,7 +424,8 @@ def sample(self, ids, action, snapshot=None): .get_local_pose()[self.env_id] .tolist() ) - self.samples.append((pose, touching, self.env.sim.simulation_time)) + self.samples.append((pose, touching, math.fsum(self._interval_dts))) + self._interval_dts.clear() report = ( self._hold_report() if terminal.accepted @@ -586,6 +605,8 @@ def after_sample(self, dt: float, token: object) -> None: for check in active: check.substeps += 1 check._interval_samples += 1 + if check.hold: + check._interval_dts.append(dt) code, actor_a, actor_b = summaries[check.env_id] if code: check._bad = f"native contact evidence rejected: code={code}; actors={actor_a},{actor_b}" diff --git a/embodichain/lab/expansion/gym_workloads.py b/embodichain/lab/expansion/gym_workloads.py index f6690cbddd..98616ebe46 100644 --- a/embodichain/lab/expansion/gym_workloads.py +++ b/embodichain/lab/expansion/gym_workloads.py @@ -608,6 +608,8 @@ def physical_key(cfg: Any) -> str: "sensor", "expert_trajectory", "actions", + "control_parts", + "active_joint_ids", ) if hasattr(cfg, name) } @@ -618,18 +620,6 @@ def physical_key(cfg: Any) -> str: "workloads must share physical environment, robot, sensors, cadence and expert action schema" ) - def control_schema(raw: Mapping[str, Any]) -> tuple: - cls = REGISTERED_ENVS[raw["id"]].cls - return ( - tuple(getattr(cls, "control_parts", None) or ()), - tuple(getattr(cls, "active_joint_ids", ())), - ) - - if any( - control_schema(raw) != control_schema(configs[0][0]) for raw, _ in configs[1:] - ): - raise ValueError("workloads must share the expert controlled-joint schema") - owner = source = None groups: dict[str, Any] = {} adapters: dict[int, Any] = {} diff --git a/tests/lab/expansion/test_acceptance.py b/tests/lab/expansion/test_acceptance.py index 2653b21f21..bfb1e65a73 100644 --- a/tests/lab/expansion/test_acceptance.py +++ b/tests/lab/expansion/test_acceptance.py @@ -28,7 +28,7 @@ @pytest.fixture def contact_row(tmp_path): - def build(duration=0.25, ground=False): + def build(duration=0.25, ground=False, frequency=10): asset = tmp_path / "robot.urdf" asset.write_text( '' @@ -45,7 +45,7 @@ def actor_ids(uid, links=None): sensor = SimpleNamespace(get_actor_ids=actor_ids) env = SimpleNamespace( - control_frequency=10, + control_frequency=frequency, robot=SimpleNamespace( uid="robot", link_names=["root", "left", "right"], @@ -83,31 +83,96 @@ def actor_ids(uid, links=None): return build -@pytest.mark.parametrize("duration,required", [(0.25, 4), (0.025, 2), (0.3, 4)]) +@pytest.fixture +def sample_contacts(): + """Feed the production observer/sample boundary using explicit physics intervals.""" + from unittest.mock import Mock + from embodichain.lab.expansion.acceptance import MeasuredExpansionValidator + from embodichain.lab.expansion.contracts import ValidationCheck, ValidationResult + + def build(row): + passed = ValidationResult((ValidationCheck("task.success", "passed"),)) + row.monitor = SimpleNamespace(observe=Mock(), validation=lambda env: passed) + row.active = True + state = SimpleNamespace( + pose=torch.tensor([[0.0, 0, 0, 0, 0, 0, 1]]), + contacts=torch.ones((1, 2), dtype=torch.bool), + ) + row.env.sim.get_rigid_object = lambda uid: SimpleNamespace( + get_local_pose=lambda: state.pose + ) + row.env.sim.simulation_time = 0.0 + row.env.sim.physics_step_index = 0 + row.sensor.get_data = lambda: { + "user_ids": torch.tensor([[[11, 2], [11, 3]]]), + "is_valid": state.contacts, + "impulse_valid": torch.ones((1, 2), dtype=torch.bool), + } + validator = object.__new__(MeasuredExpansionValidator) + validator.rows = {0: row} + validator.sensor = row.sensor + validator.checker = SimpleNamespace( + check=lambda: torch.zeros((1, 3), dtype=torch.int64) + ) + validator.terminal, validator.results = {}, {} + + def sample(dts=None): + validator.begin_control_step() + for dt in ((1 / row.env.control_frequency,) if dts is None else dts): + row.env.sim.simulation_time += dt + row.env.sim.physics_step_index += 1 + validator.after_sample(dt, None) + validator.sample((0,), None) + return row.terminal[0] + + return sample, state + + return build + + +@pytest.mark.parametrize( + "duration,frequency,required", + [ + (0.25, 10, 4), + (0.025, 10, 2), + (0.3, 10, 4), + (0.14, 50, 8), + (0.07, 100, 8), + (0.1400000001, 50, 9), + (0.0700000001, 100, 9), + ], +) def test_contact_hold_requires_full_duration_and_distinct_measured_times( - contact_row, duration, required + contact_row, sample_contacts, duration, frequency, required ): - row = contact_row(duration, ground=True) - pose = [0, 0, 0, 0, 0, 0, 1] - for i in range(required - 1): - row.samples.append((pose.copy(), [True, True], i / 10)) - assert not row._hold_report()["passed"] - row.samples.append((pose.copy(), [True, True], (required - 1) / 10)) + row = contact_row(duration, ground=True, frequency=frequency) + sample, state = sample_contacts(row) + for _ in range(required - 1): + assert not sample().accepted + assert sample().accepted report = row._hold_report() - assert report["passed"] - assert report["elapsed_duration_s"] >= duration - assert row.samples.maxlen == required - # Enough samples at one physical instant cannot establish a hold duration. - row.samples.clear() - row.samples.extend((pose.copy(), [True, True], 0.0) for _ in range(required)) - assert not row._hold_report()["passed"] - row.samples.clear() - row.samples.extend((pose.copy(), [True, True], i / 10) for i in range(required)) - row.samples[-1][1][0] = False - assert not row._hold_report()["passed"] - row.samples[-1][1][0] = True - row.samples[-1][0][0] = 0.1 + assert report["elapsed_duration_s"] >= duration - 1e-15 + assert report["required_samples"] == required + # Repeated observations without physics cannot establish a hold. + row.reset() + row.active = True + for _ in range(required): + assert not sample(()).accepted assert not row._hold_report()["passed"] + row.reset() + row.active = True + for _ in range(required - 1): + sample() + state.contacts[0, 0] = False + assert not sample().accepted + row.reset() + row.active = True + state.contacts.fill_(True) + for _ in range(required - 1): + sample() + state.pose[0, 0] = 0.1 + assert not sample().accepted + assert row._hold_report()["max_translation_drift_m"] == pytest.approx(0.1) @pytest.mark.parametrize("ground", [False, True]) @@ -119,24 +184,44 @@ def test_native_contacts_only_resolve_present_default_plane(contact_row, ground) assert ("default_plane" in row.frames.values()) is ground -def test_contact_sampling_uses_measured_world_time(contact_row): - from unittest.mock import Mock - from embodichain.lab.expansion.contracts import ValidationCheck, ValidationResult - +def test_contact_sampling_uses_measured_physics_intervals(contact_row, sample_contacts): row = contact_row(ground=True) - passed = ValidationResult((ValidationCheck("task.success", "passed"),)) - row.monitor = SimpleNamespace(observe=Mock(), validation=lambda env: passed) - row.env.sim.get_rigid_object = lambda uid: SimpleNamespace( - get_local_pose=lambda: torch.tensor([[0.0, 0, 0, 0, 0, 0, 1]]) - ) - row.sensor.get_data = lambda: { - "user_ids": torch.tensor([[[11, 2], [11, 3]]]), - "is_valid": torch.ones((1, 2), dtype=torch.bool), - "impulse_valid": torch.ones((1, 2), dtype=torch.bool), - } - for timestamp, accepted in ((0.0, False), (0.1, False), (0.2, False), (0.3, True)): - row.env.sim.simulation_time = timestamp - row._interval_samples = 1 - row.sample((0,), None) - assert row.terminal[0].accepted is accepted - assert row._hold_report()["elapsed_duration_s"] == 0.3 + sample, _ = sample_contacts(row) + for accepted in (False, False, False, True): + assert sample().accepted is accepted + assert row._hold_report()["elapsed_duration_s"] == pytest.approx(0.3) + + +@pytest.mark.parametrize("prior_steps", [500_000, 5_000_000]) +@pytest.mark.parametrize("physics_dt,passed", [(0.001, True), (0.0009999, False)]) +def test_hold_clock_is_independent_of_long_running_world_roundoff( + contact_row, sample_contacts, prior_steps, physics_dt, passed +): + row = contact_row(0.7, ground=True, frequency=100) + sample, _ = sample_contacts(row) + clock = 0.0 + for _ in range(prior_steps): + clock += 0.001 + row.env.sim.simulation_time = clock + row.env.sim.physics_step_index = prior_steps + # 71 samples span 700 steps; verify additional rolling windows as well. + for index in range(80): + result = sample((physics_dt,) * 10) + assert result.accepted is (passed and index >= 70) + report = row._hold_report() + assert report["elapsed_duration_s"] == pytest.approx(700 * physics_dt, abs=1e-15) + assert row.env.sim.simulation_time > clock + assert report["passed"] is passed + + +def test_hold_clock_follows_variable_physics_intervals(contact_row, sample_contacts): + row = contact_row(0.25, ground=True, frequency=10) + sample, _ = sample_contacts(row) + for intervals, passed in ( + ((0.03, 0.07), False), + ((0.02, 0.08), False), + ((0.01, 0.04, 0.05), False), + ((0.04, 0.06), True), + ): + assert sample(intervals).accepted is passed + assert row._hold_report()["elapsed_duration_s"] == pytest.approx(0.3) diff --git a/tests/lab/expansion/test_extension_ports_sim.py b/tests/lab/expansion/test_extension_ports_sim.py index ab6a1ecfbf..abec9dad6c 100644 --- a/tests/lab/expansion/test_extension_ports_sim.py +++ b/tests/lab/expansion/test_extension_ports_sim.py @@ -276,8 +276,17 @@ def _probe(output): ], } for backend in ("geometry", "native_contacts"): + selected = dict(configuration) + if backend == "native_contacts": + selected["contact_hold"] = { + "entity": "cube", + "links": ["gripper_finger1_link_1", "gripper_finger2_link_1"], + "duration_s": env.step_dt * 1.4, + "max_translation_drift_m": 0.001, + "max_rotation_drift_rad": 0.001, + } check = MeasuredExpansionValidator( - env, configuration, Path(output) / backend, backend=backend + env, selected, Path(output) / backend, backend=backend ) try: check.prepare() @@ -293,6 +302,13 @@ def _probe(output): assert check.terminal[1] is peer and 0 not in check.terminal if backend == "native_contacts": assert check.rows[1].substeps > 0 and check.rows[0].substeps == 0 + # The clock uses the same native physics intervals that + # supplied contact evidence, independently of World time. + report = check.rows[1]._hold_report() + assert report["samples"] == report["required_samples"] == 3 + assert report["elapsed_duration_s"] == pytest.approx( + 2 * env.step_dt + ) finally: check.close() finally: diff --git a/tests/lab/expansion/test_gym_workloads.py b/tests/lab/expansion/test_gym_workloads.py index 44abf5ab6f..3df961fe9a 100644 --- a/tests/lab/expansion/test_gym_workloads.py +++ b/tests/lab/expansion/test_gym_workloads.py @@ -157,7 +157,14 @@ def propose(*args, **kwargs): @pytest.mark.parametrize( - "difference", [None, "position_velocity", "use_action_manager", "joint_selection"] + "difference", + [ + None, + "position_velocity", + "use_action_manager", + "active_joint_ids", + "joint_order", + ], ) def test_workload_action_schema_checked_before_constructing_world( tmp_path, monkeypatch, difference @@ -173,6 +180,16 @@ class ReachedWorld(Exception): factory = Mock(side_effect=ReachedWorld) monkeypatch.setattr(host, "GymExpansionHost", factory) + from copy import deepcopy + from pathlib import Path + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv + from embodichain.utils.utility import load_config + + deployment = ( + Path(__file__).resolve().parents[3] + / "extensions/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task.ur5.yaml" + ) + base = utils.config_to_cfg(load_config(deployment), source_path=deployment) workloads, configs = [], {} for i, name in enumerate(("first", "second")): deployment = tmp_path / f"{name}.json" @@ -185,26 +202,19 @@ class ReachedWorld(Exception): ), use_action_manager=bool(i and difference == "use_action_manager"), ) - configs[name] = SimpleNamespace( - robot=SimpleNamespace( - to_dict=lambda: { - "joint_names": ["a", "b"], - "control_parts": {"arm": [0, 1]}, - } - ), - expert_trajectory=expert, - ) - cls = type( - f"Env{i}", - (), - { - "control_parts": ["arm"], - "active_joint_ids": ( - [1] if i and difference == "joint_selection" else [0] - ), - }, - ) - monkeypatch.setitem(REGISTERED_ENVS, name, SimpleNamespace(cls=cls)) + config = deepcopy(base) + config.expert_trajectory = expert + config.control_parts = ["arm"] + config.active_joint_ids = [] + if difference in ("active_joint_ids", "joint_order"): + config.control_parts = None + config.active_joint_ids = ( + [i] if difference == "active_joint_ids" else ([1, 0] if i else [0, 1]) + ) + configs[name] = config + # Configured tasks share this implementation class. Selection belongs + # to the resolved configs above, never to artificial per-task classes. + monkeypatch.setitem(REGISTERED_ENVS, name, SimpleNamespace(cls=EmbodiedEnv)) cfg = ExpansionRunCfg( deployment=str(deployment), output_dir=str(tmp_path / name), @@ -243,3 +253,71 @@ class ReachedWorld(Exception): workloads, execution, output_dir=tmp_path / "run" ) factory.assert_not_called() + + +def test_real_deployments_with_shared_class_reject_different_control_parts( + tmp_path, monkeypatch +): + from pathlib import Path + from embodichain.lab.expansion import ExpansionRunCfg + from embodichain.lab.expansion.task_program_jobs import _prepare_deployment + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + from embodichain.lab.gym.utils.registration import REGISTERED_ENVS + from embodichain.utils.utility import load_config + import embodichain.lab.expansion.host as host + + import yaml + + task = ( + Path(__file__).resolve().parents[3] + / "extensions/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task.ur5.yaml" + ) + inputs = tmp_path / "inputs" + inputs.mkdir() + base = load_config(_prepare_deployment(task, inputs)) + workloads = [] + for name, parts in (("Arm", ["arm"]), ("ArmEef", ["arm", "eef"])): + component = load_config(task.parent / "envs/default.yaml") + component["env"]["control_parts"] = parts + physical = inputs / f"{name}.environment.yaml" + physical.write_text(yaml.safe_dump(component)) + declaration = { + **base, + "id": f"WorkloadControlSchema-{name}-v1", + "environment": {"component": str(physical)}, + } + path = inputs / f"{name}.json" + path.write_text(json.dumps(declaration)) + config = config_to_cfg(declaration, source_path=path) + assert config.control_parts == parts + assert REGISTERED_ENVS[declaration["id"]].cls is EmbodiedEnv + cfg = ExpansionRunCfg( + deployment=str(path), + output_dir=str(tmp_path / name), + max_steps=20, + settling_steps=1, + ) + workloads.append( + GymExpansionWorkload( + name, + cfg, + { + "input_files": { + "deployment": hashlib.sha256(path.read_bytes()).hexdigest() + } + }, + {}, + ) + ) + constructor = Mock(side_effect=AssertionError("World must not be constructed")) + monkeypatch.setattr(host, "GymExpansionHost", constructor) + with pytest.raises(ValueError, match="schema"): + run_gym_expansion_workloads( + workloads, + WorkloadExecutionCfg( + workload_count=2, batch_size=1, max_steps=20, settling_steps=1 + ), + output_dir=tmp_path / "run", + ) + constructor.assert_not_called() From 565d9812925a305ef692b041f2f3f562eb054985 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Mon, 16 Nov 2026 13:53:19 +0000 Subject: [PATCH 22/45] fix(gensim): clear supports and settle opening before release --- agent_context/topics/gen-sim/gen-sim.md | 6 ++++++ embodichain/gen_sim/README.md | 14 ++++++++++--- embodichain/gen_sim/_run_binding.py | 11 ++++++---- embodichain/gen_sim/_run_config.py | 3 ++- .../lab/sim/atomic_skills/primitives/place.py | 21 ++++++++++++++++++- .../lab/task_program/integrations/catalog.py | 2 ++ .../task_program/integrations/configured.py | 8 +++++++ examples/gensim/scripted_pick_place/README.md | 10 ++++++--- tests/gen_sim/test_workflow.py | 15 ++++++++++++- .../lab/expansion/test_extension_ports_sim.py | 21 +++++++++++++++++++ 10 files changed, 98 insertions(+), 13 deletions(-) diff --git a/agent_context/topics/gen-sim/gen-sim.md b/agent_context/topics/gen-sim/gen-sim.md index 95f7877943..3615432177 100644 --- a/agent_context/topics/gen-sim/gen-sim.md +++ b/agent_context/topics/gen-sim/gen-sim.md @@ -64,6 +64,12 @@ normative TaskSpec certification remain distinct. See the [command guide](../../../embodichain/gen_sim/README.md) and provider-free [scripted reproduction](../../../examples/gensim/scripted_pick_place/README.md). +Automatic placements default to 0.02 m release clearance above their measured +resting target and a 0.4 s open-command hold on the native control grid. Explicit +clearance/Place timing declarations keep their ownership. Opening, terminal +separation, resting-position and stability evidence are unchanged; clearance +and actuation time are planning behavior, not weaker acceptance thresholds. + Articulated USDC admission allows absent `gen_sim:closedPosition` metadata; present declarations must identify a finite joint-limit endpoint. Missing endpoint semantics remain unknown. Operation-specific target qualification diff --git a/embodichain/gen_sim/README.md b/embodichain/gen_sim/README.md index 955eb4360a..0da3041172 100644 --- a/embodichain/gen_sim/README.md +++ b/embodichain/gen_sim/README.md @@ -128,9 +128,17 @@ through the existing closed decoders. `binding.resource_anchors` maps declared resources to planar `[x, y]` anchors: a single-arm call without a resource then uses the arm already holding its object, or the arm anchored nearest to it, and a placement on a support prefers its robot-side region. `binding.release_clearance` -(metres, default 0) releases placements above their resting pose while validators -keep the resting target; the place gate otherwise expects a released object to -separate from the open gripper before retracting. +(metres, default 0.02) releases placements above their resting pose so fingers +can open without wedging against the support. An explicit zero keeps contact +placement. Ordinary generated Place calls also hold the final open command for +0.4 physical seconds before the unchanged release gate; the native planner +rounds this duration upward to the actual control grid. Explicit +`release_settle_duration` values, including zero, are retained; declaring +`release_settle_steps` suppresses that generated duration default. When both +options are explicitly supplied, the longer hold applies. External nominal +sources retain their authored timing. The gate measures opening before retract, +terminal verification measures separation, and the original resting-position +and stability validators remain unchanged. Further optional run fields: `task` reuses a published task-plan directory instead of planning, after verifying its artifacts and that it planned the same instruction diff --git a/embodichain/gen_sim/_run_binding.py b/embodichain/gen_sim/_run_binding.py index 72adfd6614..6cc99c54f7 100644 --- a/embodichain/gen_sim/_run_binding.py +++ b/embodichain/gen_sim/_run_binding.py @@ -979,10 +979,9 @@ def _bind_deployment( if set(anchors) - declared_resources: raise ValueError("resource_anchors must name declared profile resources") holders: dict[str, str] = {} - # Placements may release above their resting pose. The place gate requires a - # released object to separate from the still-open gripper before retracting, - # which an object already resting on its support cannot do. - release_clearance = float(cfg.binding.get("release_clearance", 0.0)) + # Open above the support to avoid wedging a finger against it. The landing + # validators below retain the original measured resting pose. + release_clearance = cfg.binding["release_clearance"] configured_services = runtime_services # Measured legacy geometry binds only the registered calls whose lowerer the # run configuration does not supply. A configured lowerer owns its targets, @@ -1063,6 +1062,10 @@ def subject_of(node: dict[str, Any]) -> str | None: _register_call_options( options, monitors, call, cid, "pick" if skill == "pick_up" else skill, skill ) + if cid == "place" and "release_settle_steps" not in options[key]: + # Allow actual opening before the unchanged release monitor starts. + # This physical duration is converted on the native control grid. + options[key].setdefault("release_settle_duration", 0.4) if cid == "hand_over": options[key].setdefault("release_at_target", False) options[key].setdefault("arm_selection", "bound") diff --git a/embodichain/gen_sim/_run_config.py b/embodichain/gen_sim/_run_config.py index f37af1d132..7398cf4853 100644 --- a/embodichain/gen_sim/_run_config.py +++ b/embodichain/gen_sim/_run_config.py @@ -82,13 +82,14 @@ def __post_init__(self) -> None: "binding supports profile, release_clearance, resource_anchors, " "runtime_services, scene_binding and segments" ) - clearance = self.binding.get("release_clearance", 0.0) + clearance = self.binding.get("release_clearance", 0.02) if ( isinstance(clearance, bool) or not isinstance(clearance, (int, float)) or not 0.0 <= clearance < 0.2 ): raise ValueError("release_clearance must be a height in [0, 0.2) m") + self.binding = {**self.binding, "release_clearance": float(clearance)} anchors = self.binding.get("resource_anchors", {}) if type(anchors) is not dict or any( type(value) is not list diff --git a/embodichain/lab/sim/atomic_skills/primitives/place.py b/embodichain/lab/sim/atomic_skills/primitives/place.py index 5ca67d4308..26f1e0836b 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/place.py +++ b/embodichain/lab/sim/atomic_skills/primitives/place.py @@ -19,6 +19,7 @@ from __future__ import annotations from dataclasses import dataclass, field +import math from typing import ClassVar, Literal import torch @@ -138,6 +139,14 @@ class PlaceOptions(SkillOptions): release_settle_steps: int = 0 """Fully open hold frames before retracting the end-effector.""" + release_settle_duration: float = field(default=0.0, kw_only=True) + """Minimum physical seconds to hold open in a newly planned native Place. + + Rounded upward to control intervals and combined with release_settle_steps + using the larger hold. This is actuation time, not an acceptance tolerance. + External nominal sources retain their authored release timing unchanged. + """ + lift_height: float = 0.1 """Height (m) to retract the end-effector after opening the gripper.""" @@ -157,6 +166,13 @@ class PlaceOptions(SkillOptions): """Keep the held object's observed world orientation at the place target.""" def __post_init__(self) -> None: + if ( + isinstance(self.release_settle_duration, bool) + or not isinstance(self.release_settle_duration, (int, float)) + or not math.isfinite(self.release_settle_duration) + or self.release_settle_duration < 0 + ): + raise ValueError("release_settle_duration must be finite and nonnegative.") if type(self.cartesian_approach) is not bool: raise TypeError("cartesian_approach must be boolean.") if self.hand_interp_steps < 1: @@ -459,7 +475,10 @@ def _plan( hold_hand_qpos, hand_open_qpos, n_waypoints=n_open ) - n_settle = options.release_settle_steps + n_settle = max( + options.release_settle_steps, + math.ceil(options.release_settle_duration / context.require_control_dt()), + ) open_start = n_down settle_start = open_start + n_open back_start = settle_start + n_settle diff --git a/embodichain/lab/task_program/integrations/catalog.py b/embodichain/lab/task_program/integrations/catalog.py index 2d82e87787..64afa95dff 100644 --- a/embodichain/lab/task_program/integrations/catalog.py +++ b/embodichain/lab/task_program/integrations/catalog.py @@ -251,6 +251,8 @@ def _canonical_value(value: object) -> object: metadata.pop("grasp_retract_distance") if isinstance(value, PlaceOptions) and value.cartesian_approach is False: metadata.pop("cartesian_approach") + if isinstance(value, PlaceOptions) and value.release_settle_duration == 0.0: + metadata.pop("release_settle_duration") if isinstance(value, SlideOptions) and value.push_approach_offset is None: metadata.pop("push_approach_offset") return {"type": _qualified_name(value), "fields": metadata} diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index c8af7a9fdc..847420d1dd 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -1087,6 +1087,7 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: "object_motion_keyframes", "grasp_settle_steps", "release_settle_steps", + "release_settle_duration", "pick_object_part", "lift_height", "pre_grasp_distance", @@ -1287,6 +1288,7 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: { "hand_interp_steps", "release_settle_steps", + "release_settle_duration", "lift_height", "max_approach_retract_z", "cartesian_waypoint_count", @@ -1312,6 +1314,12 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: path=f"{path}.release_settle_steps", minimum=0, ) + if "release_settle_duration" in config: + kwargs["release_settle_duration"] = _real( + config["release_settle_duration"], + path=f"{path}.release_settle_duration", + minimum=0.0, + ) if "lift_height" in config: kwargs["lift_height"] = _real( config["lift_height"], diff --git a/examples/gensim/scripted_pick_place/README.md b/examples/gensim/scripted_pick_place/README.md index 4c766aa754..17e6985147 100644 --- a/examples/gensim/scripted_pick_place/README.md +++ b/examples/gensim/scripted_pick_place/README.md @@ -18,9 +18,13 @@ This example prepares fixed inputs, runs the shared GenSim workflow, and measure wall-clock time through native execution and cleanup. It demonstrates the previous dual-UR5 table/cube case without a live model or reconstruction service. -The recorded outcome is **pickup succeeds, placement release verification fails**. -It is a reproducible failure investigation, not a successful expert dataset -collection or qualification of real photograph-to-scene generation. +The current defaults release 0.02 m above the resting target and hold open for +0.4 physical seconds before release verification. This avoids the previously +recorded finger/support interference while retaining measured opening, +separation, stability and landing requirements. Explicit zero clearance can +still reproduce a physically rejected contact placement. The historical failure +measurements below remain tied to their original revision; this scripted case +does not qualify real photograph-to-scene generation or model quality. ## Requirements and inputs diff --git a/tests/gen_sim/test_workflow.py b/tests/gen_sim/test_workflow.py index 094eb72c59..dba3cf2242 100644 --- a/tests/gen_sim/test_workflow.py +++ b/tests/gen_sim/test_workflow.py @@ -259,6 +259,19 @@ def run_cfg(tmp_path: Path) -> _RunCfg: ) +@pytest.mark.parametrize( + "binding,expected", + [({}, 0.02), ({"profile": {}}, 0.02), ({"release_clearance": 0.0}, 0.0)], +) +def test_release_clearance_default_preserves_explicit_contact_placement( + run_cfg, binding, expected +): + original = json.loads(json.dumps(binding)) + cfg = _RunCfg(**{**run_cfg.to_dict(), "binding": binding}) + assert cfg.binding["release_clearance"] == expected + assert binding == original + + @pytest.mark.parametrize("success", [True, False]) def test_workflow_generates_binds_and_preserves_actual_execution_verdict( run_cfg: _RunCfg, monkeypatch: pytest.MonkeyPatch, success: bool @@ -274,7 +287,7 @@ def test_workflow_generates_binds_and_preserves_actual_execution_verdict( run_cfg.instruction += " \n" run_cfg.task_id = " gensim_task " - # Releasing above the resting pose is opt-in; cover both settings. + # Exercise raised release and an explicit zero-clearance contact placement. clearance = 0.03 if success else 0.0 run_cfg = _RunCfg( **{ diff --git a/tests/lab/expansion/test_extension_ports_sim.py b/tests/lab/expansion/test_extension_ports_sim.py index abec9dad6c..30d8422bba 100644 --- a/tests/lab/expansion/test_extension_ports_sim.py +++ b/tests/lab/expansion/test_extension_ports_sim.py @@ -143,6 +143,27 @@ def _check_place_keyframes(env, generator): ), context, ) + for period, steps, expected in ((0.04, 0, 3), (0.02, 0, 6), (0.04, 5, 5)): + timed = engine.plan( + replace( + invocation, + skill_options=replace( + options, release_settle_duration=0.11, release_settle_steps=steps + ), + ), + engine.initial_context(control_dt=period), + ) + release = timed.segment("release") + assert release.stop - release.start == options.hand_interp_steps + expected + hand_ids = env.robot.get_joint_ids("eef") + torch.testing.assert_close( + timed.joint_trajectory.positions[ + :, release.stop - expected : release.stop, hand_ids + ], + hand_limits[None, :, 0].expand(1, expected, -1), + rtol=0, + atol=0, + ) def _probe(output): From 33fe553aec636a5d9f1398d09defb68a73ac3270 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 17 Nov 2026 02:09:05 +0000 Subject: [PATCH 23/45] test(gensim): align binding coverage with release clearance defaults Keep landing-pose acceptance independent of the raised release target, covering zero and default clearance as well as configured lowerer ownership. Reconstruct the coordinated-transport configuration through its normal validator instead of replacing its normalized binding dictionary. Validation: reproduced both failed CI cases; all 62 GenSim workflow tests and the native placement/release integration probe passed on pinned DexSim 0.6.0rc2. Black 26.10.1, API docs 5010/5010, context map and diff checks passed. No production behavior changed; existing GenSim context already describes clearance and landing validation correctly. --- tests/gen_sim/test_workflow.py | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/tests/gen_sim/test_workflow.py b/tests/gen_sim/test_workflow.py index dba3cf2242..3ff9de567d 100644 --- a/tests/gen_sim/test_workflow.py +++ b/tests/gen_sim/test_workflow.py @@ -1120,8 +1120,12 @@ def _node(node_id: str, call: dict, depends_on: list[str], **semantic) -> dict: @pytest.mark.parametrize("configured", [False, True]) +@pytest.mark.parametrize("clearance", [0.0, 0.02]) def test_relative_placement_validators_follow_the_selected_lowerer( - run_cfg: _RunCfg, monkeypatch: pytest.MonkeyPatch, configured: bool + run_cfg: _RunCfg, + monkeypatch: pytest.MonkeyPatch, + configured: bool, + clearance: float, ) -> None: instruction = "Put the cube right of the table" semantic = dict( @@ -1166,7 +1170,7 @@ def test_relative_placement_validators_follow_the_selected_lowerer( **{ **run_cfg.to_dict(), "instruction": instruction, - "binding": {"runtime_services": services}, + "binding": {"runtime_services": services, "release_clearance": clearance}, } ) @@ -1184,7 +1188,13 @@ def test_relative_placement_validators_follow_the_selected_lowerer( assert validators == [] else: (validator,) = validators - assert lowerer["routes"][0]["world_displacement"] == validator["displacement"] + # The route is the release pose; acceptance still measures the resting + # landing pose. Explicit zero clearance retains contact placement. + landing = validator["displacement"] + release = lowerer["routes"][0]["world_displacement"] + assert release[:2] == landing[:2] + assert landing[2] == pytest.approx(0.005) + assert release[2] == pytest.approx(0.005 + clearance) def test_held_upright_without_reference_validates_its_terminal_pose( @@ -1923,7 +1933,7 @@ def test_legacy_configured_coordinated_target_survives_binding(run_cfg, monkeypa route = dict( object_id="source", target_id="tray_target", world_displacement=[0.1, 0.0, 0.0] ) - run_cfg.binding = { + binding = { "runtime_services": { "registered_semantic_lowerers": [ dict(kind="coordinated_transport", routes=[route]) @@ -1937,6 +1947,7 @@ def test_legacy_configured_coordinated_target_survives_binding(run_cfg, monkeypa } }, } + run_cfg = _RunCfg(**{**run_cfg.to_dict(), "binding": binding}) run_cfg.output.mkdir() with _services(step=step, plan=proposed) as services: task = plan_image_task( From bf4a25422890b2524814119439a255da57f37840 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 17 Nov 2026 02:49:51 +0000 Subject: [PATCH 24/45] fix(expansion): preserve selected rows and batch measured validation --- .../topics/atomic-actions/atomic-actions.md | 6 + .../topics/expansion-engine/workloads.md | 17 ++ embodichain/data_pipeline/recording/media.py | 5 +- embodichain/lab/expansion/_gym_episode.py | 9 +- embodichain/lab/expansion/acceptance.py | 174 +++++++++++++++--- .../lab/gym/envs/task_program/bridge.py | 58 ++++-- .../sim/atomic_skills/primitives/hand_over.py | 5 + tests/gym/envs/task_program/test_bridge.py | 33 ++++ .../lab/expansion/test_extension_ports_sim.py | 121 +++++++++++- 9 files changed, 380 insertions(+), 48 deletions(-) diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index f8927e9e9b..2dcc7d3636 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -24,6 +24,12 @@ Hosts acquire versioned measurements; verification is the boundary that commits or invalidates observed effects. Preserve required stop/hold boundaries on the executed control grid instead of relying only on native planner timing. +Row-local planning within a larger World must carry the same explicit +`context.env_ids` through FK/IK, root poses and diagnostics. HandOver applies this +to both source/destination arms, including its continuation and failed-waypoint +diagnostic routes. A one-row planning context does not implicitly select arena +zero or all arenas of the robot. + ## Package map | Concern | Source of truth | diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index c22d2af281..b1be19998c 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -118,6 +118,23 @@ each native event state machine and receipts. `ContactPermissionChecker` checks all valid native contact slots with the original applied-impulse and loss gates; hosts still own sampling at every required physics substep. +`MeasuredExpansionValidator` uses these same batch interfaces. Compatible rows +share one immutable geometry declaration/checker; selected rows are compacted +into bounded Newton queries and mapped back in their original order. Different +geometry retains separate checker groups. Query models never step physics. +Predicate inputs share the current complete measured snapshot, while events, +consecutive counts, failures and contact holds remain row-owned. Shape snapshots +are needed only by declared support-bound predicates; native-contact rows do not +gain a homogeneous-topology requirement. + +The built-in joint transport writes all of a frame's destinations into one owned +output. Single commands, subclasses, replaced encoder methods and custom +transports retain normal dispatch and registration order. Numerical/frame checks, +inactive holds and position-velocity evidence remain unchanged. Episode assembly +shares validated immutable segment metadata internally; mutable readback still +owns its containers. Durable JSON uses the same finite sorted representation and +fsync/atomic publication, with one encoded text allocation. + Default GPU dynamics use Direct GPU reporting, whose static contact identity limitations remain explicit admission constraints. Physics devices and task qualification must be reported separately. Existing CPU deployments do not diff --git a/embodichain/data_pipeline/recording/media.py b/embodichain/data_pipeline/recording/media.py index e36105a060..8be9ddbdae 100644 --- a/embodichain/data_pipeline/recording/media.py +++ b/embodichain/data_pipeline/recording/media.py @@ -65,7 +65,10 @@ def _atomic_json(path: Path, value: object) -> None: handle, temporary = tempfile.mkstemp(dir=path.parent, prefix=".pending-") try: with os.fdopen(handle, "w") as stream: - json.dump(value, stream, allow_nan=False, sort_keys=True) + # The C encoder returns one owned string instead of issuing a + # Python write for every metadata leaf. Keep the durable publish + # boundary and finite-value rejection unchanged. + stream.write(json.dumps(value, allow_nan=False, sort_keys=True)) stream.flush() os.fsync(stream.fileno()) os.replace(temporary, path) diff --git a/embodichain/lab/expansion/_gym_episode.py b/embodichain/lab/expansion/_gym_episode.py index fd686fc458..6eb1f02a83 100644 --- a/embodichain/lab/expansion/_gym_episode.py +++ b/embodichain/lab/expansion/_gym_episode.py @@ -120,6 +120,11 @@ def assemble(self, assignments, collector, services, result, physics_after): key = f"scene_state_{index}" observations[key] = torch.stack(values) scene_keys.append({"key": key, "entity_uid": uid, "field": field}) + from embodichain.utils._owned_json import freeze_metadata + + # Both compatibility fields describe the same immutable audit tree. + # Validate/own it once; mutable public projections still detach. + segments = freeze_metadata(result.segments[result_row]) metadata = { "source_env_id": row, "attempt_sequence": ctx.requests[row].sequence, @@ -146,13 +151,13 @@ def assemble(self, assignments, collector, services, result, physics_after): "initial_state": record["initial_state"], "scene_state_fields": scene_keys, "physics_backend": env.sim.physics_backend, - "program_segments": result.segments[result_row], + "program_segments": segments, "expansion": ( result.expansion_records[result_row] if result.expansion_records else () ), - "segments": result.segments[result_row], + "segments": segments, "instruction": program.instruction, "instruction_source": "task_program", } diff --git a/embodichain/lab/expansion/acceptance.py b/embodichain/lab/expansion/acceptance.py index 03ec618bbe..2b7937e28e 100644 --- a/embodichain/lab/expansion/acceptance.py +++ b/embodichain/lab/expansion/acceptance.py @@ -137,7 +137,14 @@ def pairs(values): return ignored, allowed def __init__( - self, env, configuration: dict, output: Path, *, device="cpu", env_id=0 + self, + env, + configuration: dict, + output: Path, + *, + device="cpu", + env_id=0, + checker_pool=None, ): self.env_id = env_id from embodichain.lab.task_program.integrations.simulation.feasibility_geometry import ( @@ -151,9 +158,14 @@ def __init__( ignored, self.allowed_contacts = self.contact_permissions( env, configuration, known ) - checker = NewtonCollisionChecker( - bodies, ignored_pairs=ignored, device=device, max_shapes=512 - ) + key = (tuple(bodies), tuple(ignored), device) + checker = None if checker_pool is None else checker_pool.get(key) + if checker is None: + checker = NewtonCollisionChecker( + bodies, ignored_pairs=ignored, device=device, max_shapes=512 + ) + if checker_pool is not None: + checker_pool[key] = checker self.checkers = [(checker, self.allowed_contacts, known)] atomic_json( output / "collision-geometry.json", @@ -222,28 +234,32 @@ def sample(self, ids, action, snapshot=None): allowed_contacts=contacts, moving_objects=tuple(uid for uid in self.moving if uid in aliases), ) - if self.env_id not in self.results or report.status != "passed": - self.results[self.env_id] = SimpleNamespace( - status=report.status, - reason=f"{report.reason}; pair={checker.last_pair}", - ) + self.record_geometry(report, checker.last_pair, action) if report.status != "passed": - metadata = action.metadata.get("row_actions", {}).get( - str(self.env_id), action.metadata - ) - atomic_json( - self.output / "collision-failure.json", - { - "reason": self.results[self.env_id].reason, - "action": { - key: str(value) - for key, value in metadata.items() - if isinstance(value, (str, int, float, bool)) - }, - }, - ) break + def record_geometry(self, report, pair, action): + """Retain measured failure and its original row-owned diagnostic.""" + if self.env_id not in self.results or report.status != "passed": + self.results[self.env_id] = SimpleNamespace( + status=report.status, reason=f"{report.reason}; pair={pair}" + ) + if report.status != "passed": + metadata = action.metadata.get("row_actions", {}).get( + str(self.env_id), action.metadata + ) + atomic_json( + self.output / "collision-failure.json", + { + "reason": self.results[self.env_id].reason, + "action": { + key: str(value) + for key, value in metadata.items() + if isinstance(value, (str, int, float, bool)) + }, + }, + ) + def close(self): for checker, _, _ in self.checkers: checker.close() @@ -533,18 +549,48 @@ def __init__( ) ) environment.sensors[self.sensor_uid] = self.sensor + checker_pool = {} for row in range(environment.num_envs): path = output if environment.num_envs == 1 else output / f"row-{row}" path.mkdir(parents=True, exist_ok=True) self.rows[row] = ( _GeometryRow( - environment, configuration, path, device=device, env_id=row + environment, + configuration, + path, + device=device, + env_id=row, + checker_pool=checker_pool, ) if self.sensor is None else _ContactRow( environment, configuration, path, sensor=self.sensor, env_id=row ) ) + conditions = [ + *configuration["initial_checks"], + *configuration["final_checks"], + ] + for event in configuration["events"]: + conditions.extend(event["conditions"]) + shape_entities = { + condition["entity"] + for condition in conditions + if condition["kind"] == "support_bounds" + } + self._shapes = { + uid: ( + tuple( + environment.sim.get_rigid_object(uid).get_collision_shapes( + env_id=row, allow_heterogeneous=True + ) + for row in range(environment.num_envs) + ) + if uid in shape_entities + else () + ) + for uid in environment.sim.get_rigid_object_uid_list() + } if self.sensor is not None: permissions = self.rows[0].allowed self.checker = ContactPermissionChecker( @@ -624,13 +670,83 @@ def sample( snapshot: SceneState | None = None, ) -> None: """Check measured task state and retain each row's first physical failure.""" + if not ids: + return + from .collision import _measured_frame + from embodichain.lab.task_program.integrations.feasibility import ( + MeasuredTaskMonitor, + ) + + env = self.env + measured = ( + _measured_frame(env, snapshot, self._shapes) + if getattr(snapshot, "env_ids", None) == tuple(range(env.num_envs)) + else env + ) + monitors = tuple(self.rows[row].monitor for row in ids) + with MeasuredTaskMonitor.batch_scope(measured, monitors): + for row in ids: + check = self.rows[row] + if self.sensor is None: + check.monitor.observe(measured, action) + check.terminal[row] = check.monitor.validation(measured) + else: + check.sample((row,), action, snapshot) + check._interval_samples = 0 + self.terminal[row] = check.terminal[row] + if self.sensor is None: + self._sample_geometry(ids, action) + for row in ids: + self.results[row] = self.rows[row].results[row] + + def _sample_geometry(self, ids, action): + """Batch compatible physical geometry, retaining heterogeneous-row checks.""" + from embodichain.utils.math import matrix_from_quat + + env = self.env + poses = env.robot.body_data.body_link_pose + matrices = torch.eye(4, dtype=poses.dtype, device=poses.device).repeat( + env.num_envs, len(env.robot.link_names), 1, 1 + ) + matrices[..., :3, :3] = matrix_from_quat(poses[..., 3:]) + matrices[..., :3, 3] = poses[..., :3] + matrices = matrices.detach().cpu().numpy() + frames = { + f"{env.robot.uid}/{link}": matrices[:, index] + for index, link in enumerate(env.robot.link_names) + } + frames.update( + { + uid: env.sim.get_rigid_object(uid) + .get_local_pose(to_matrix=True) + .detach() + .cpu() + .numpy() + for uid in env.sim.get_rigid_object_uid_list() + } + ) + frames["default_plane"] = np.broadcast_to(np.eye(4), (env.num_envs, 4, 4)) + groups = {} for row in ids: check = self.rows[row] - check.sample((row,), action, snapshot) - self.terminal[row] = check.terminal[row] - self.results[row] = check.results[row] - if self.sensor is not None: - check._interval_samples = 0 + for checker, allowed, aliases in check.checkers: + moving = tuple(uid for uid in check.moving if uid in aliases) + groups.setdefault((checker, tuple(allowed), moving), []).append(row) + for (checker, allowed, moving), rows in groups.items(): + # Respect the existing aggregate contact-buffer ceiling. Compact + # selected rows also prevent inactive/heterogeneous rows consuming it. + width = max(1, min(env.num_envs, 1048576 // checker._capacity)) + for start in range(0, len(rows), width): + selected = rows[start : start + width] + reports = checker.check_batch( + {uid: values[selected] for uid, values in frames.items()}, + allowed_contacts=allowed, + moving_objects=moving, + ) + for row, report, pair in zip( + selected, reports, checker.last_batch_pairs + ): + self.rows[row].record_geometry(report, pair, action) def close(self) -> None: """Detach owned observers before releasing resources; safe on failure.""" diff --git a/embodichain/lab/gym/envs/task_program/bridge.py b/embodichain/lab/gym/envs/task_program/bridge.py index 9de1c60d4d..5ee67cc941 100644 --- a/embodichain/lab/gym/envs/task_program/bridge.py +++ b/embodichain/lab/gym/envs/task_program/bridge.py @@ -452,14 +452,33 @@ def encode( active_mask: torch.Tensor, ) -> EnvAction: """Write addressed joints while holding every other qpos column.""" + return self._encode_commands( + (command,), base_action=base_action, active_mask=active_mask + ) + + def _encode_commands(self, commands, *, base_action, active_mask): + """Own one output for a validated frame's built-in joint destinations.""" + if not isinstance(base_action, (torch.Tensor, TensorDict)): + raise TypeError( + "The built-in joint-position encoder requires a tensor action or " + "a TensorDict containing qpos." + ) + action = base_action.clone() + for command in commands: + self._write_command(command, action=action, active_mask=active_mask) + return action + + @staticmethod + def _write_command(command, *, action, active_mask): + """Apply one command to an already-owned frame output.""" if not isinstance(command.target, JointPositionTarget): raise TypeError("Joint-position transport requires JointPositionTarget.") if not isinstance(command.payload, JointPositionPayload): raise TypeError("Joint-position transport requires JointPositionPayload.") - if isinstance(base_action, torch.Tensor): - qpos = base_action - elif isinstance(base_action, TensorDict) and "qpos" in base_action: - qpos = base_action["qpos"] + if isinstance(action, torch.Tensor): + qpos = action + elif isinstance(action, TensorDict) and "qpos" in action: + qpos = action["qpos"] else: raise TypeError( "The built-in joint-position encoder requires a tensor action or " @@ -487,38 +506,34 @@ def encode( if not qpos.is_floating_point(): raise TypeError("The full-qpos base action must be floating point.") - action_qpos = qpos.clone() + action_qpos = qpos columns = torch.tensor(joint_ids, dtype=torch.long, device=qpos.device) selected = action_qpos.index_select(1, columns) selected[active_mask] = positions[active_mask].to(dtype=action_qpos.dtype) action_qpos[:, columns] = selected - if isinstance(base_action, torch.Tensor): - return action_qpos + if isinstance(action, torch.Tensor): + return velocities = command.payload.velocities if velocities is None: raise ValueError( "position_velocity joint commands require payload velocities." ) - if "qvel" not in base_action: + if "qvel" not in action: raise TypeError( "Position-velocity joint actions require qpos and qvel base targets." ) - qvel = base_action["qvel"] + qvel = action["qvel"] if qvel.shape != qpos.shape or qvel.device != qpos.device: raise ValueError( "Full qpos and qvel base targets must share shape and device." ) - action_qvel = qvel.clone() + action_qvel = qvel selected_velocity = action_qvel.index_select(1, columns) selected_velocity[active_mask] = velocities[active_mask].to( dtype=action_qvel.dtype ) action_qvel[:, columns] = selected_velocity - action = base_action.clone() - action["qpos"] = action_qpos - action["qvel"] = action_qvel - return action def hold( self, @@ -534,6 +549,9 @@ def hold( return base_action.clone() +_BUILTIN_JOINT_ENCODE = JointPositionGymTransportEncoder.encode + + class RuntimeCommandFrameEncoder: """Encode transport-neutral command frames to controller-ready Gym actions. @@ -742,7 +760,17 @@ def encode(self, frame: RuntimeCommandFrame) -> EnvAction: self._validate_command_types(transport, command) by_transport.setdefault(command.transport_id, []).append(command) for transport_id, transport in self._transports.items(): - for command in by_transport.get(transport_id, ()): + commands = by_transport.get(transport_id, ()) + if ( + len(commands) > 1 + and type(transport) is JointPositionGymTransportEncoder + and getattr(transport.encode, "__func__", None) is _BUILTIN_JOINT_ENCODE + ): + action = transport._encode_commands( + commands, base_action=action, active_mask=frame.active_mask + ) + continue + for command in commands: action = transport.encode( command, base_action=action, diff --git a/embodichain/lab/sim/atomic_skills/primitives/hand_over.py b/embodichain/lab/sim/atomic_skills/primitives/hand_over.py index c868348965..3ba75931b5 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_skills/primitives/hand_over.py @@ -740,6 +740,7 @@ def _plan_existing_hold( qpos=source_start_qpos, name=resources.first.arm.control_part, to_matrix=True, + env_ids=context.env_ids, ) current_object_pose = torch.bmm(source_eef, pose_inv(source_object_to_eef)) @@ -781,6 +782,7 @@ def _plan_existing_hold( qpos=destination_start_qpos, name=resources.second.arm.control_part, to_matrix=True, + env_ids=context.env_ids, ) # Approach diagonally from the receiver's side of the embodiment. A # top-down receiver places two bulky parallel grippers in the same @@ -1268,12 +1270,14 @@ def _plan_direction( qpos=handover_start_qpos, name=handover.arm.control_part, to_matrix=True, + env_ids=context.env_ids, ) receive_start_qpos = start_qpos[:, list(receive.arm.joint_ids)] receive_start_eef = self.robot.compute_fk( qpos=receive_start_qpos, name=receive.arm.control_part, to_matrix=True, + env_ids=context.env_ids, ) vertical_mode = ( @@ -1967,6 +1971,7 @@ def _report_phase_failure( pose=target_poses[:, waypoint_index], name=arm.control_part, joint_seed=joint_seed, + env_ids=context.env_ids, ) ik_success = normalize_success_mask( ik_success, diff --git a/tests/gym/envs/task_program/test_bridge.py b/tests/gym/envs/task_program/test_bridge.py index 1746c21e81..a461f3e8d9 100644 --- a/tests/gym/envs/task_program/test_bridge.py +++ b/tests/gym/envs/task_program/test_bridge.py @@ -1148,6 +1148,39 @@ def test_joint_encoder_emits_full_qpos_and_holds_inactive_rows() -> None: assert torch.equal(action[1], qpos[1]) +@pytest.mark.parametrize("mode", ["position", "position_velocity"]) +def test_joint_encoder_groups_destinations_without_aliasing_source(mode) -> None: + """A complete frame preserves independent destinations and inactive rows.""" + qpos = torch.arange(BATCH_SIZE * ROBOT_DOF, dtype=torch.float32).reshape( + BATCH_SIZE, ROBOT_DOF + ) + source = qpos.clone() + encoder = RuntimeCommandFrameEncoder(_QposProvider(qpos), joint_command_mode=mode) + first = _joint_frame( + duration=STEP_DT, + active_mask=torch.tensor([True, False]), + velocities=torch.tensor([[1.0, 3.0], [11.0, 13.0]]), + ) + extra = EndpointCommand( + target=JointPositionTarget(control_part="second", joint_ids=(0,)), + payload=JointPositionPayload( + positions=torch.tensor([[90.0], [91.0]]), + velocities=torch.tensor([[9.0], [9.1]]), + ), + ) + frame = replace(first, commands=(*first.commands, extra)) + action = encoder.encode(frame) + positions = action if mode == "position" else action["qpos"] + assert torch.equal(positions[0], torch.tensor([90.0, 10.0, 2.0, 30.0, 4.0])) + assert torch.equal(positions[1], source[1]) + if mode == "position_velocity": + assert torch.equal(action["qvel"][0], torch.tensor([9.0, 1.0, 0.0, 3.0, 0.0])) + assert torch.equal(action["qvel"][1], torch.zeros(ROBOT_DOF)) + positions.zero_() + assert torch.equal(qpos, source) + assert torch.equal(extra.payload.positions, torch.tensor([[90.0], [91.0]])) + + def test_joint_encoder_holds_unaddressed_joints_at_controller_targets() -> None: """A scoped command must not chase measured drift on another resource.""" current = torch.zeros(BATCH_SIZE, ROBOT_DOF) diff --git a/tests/lab/expansion/test_extension_ports_sim.py b/tests/lab/expansion/test_extension_ports_sim.py index 30d8422bba..8230db610e 100644 --- a/tests/lab/expansion/test_extension_ports_sim.py +++ b/tests/lab/expansion/test_extension_ports_sim.py @@ -321,6 +321,52 @@ def _probe(output): peer = check.terminal[1] check.prepare(env_ids=(0,)) assert check.terminal[1] is peer and 0 not in check.terminal + if backend == "geometry": + from embodichain.lab.gym.envs.types import ControllerAction + + # A real collider overlap in only one selected arena must + # not reject the safe peer, including reordered selection. + forbidden = { + **configuration, + "allowed_contacts": [ + pair + for pair in configuration["allowed_contacts"] + if not ( + "cube" in pair + and any( + name.startswith(env.robot.uid + "/") + for name in pair + ) + ) + ], + } + measured = MeasuredExpansionValidator( + env, + forbidden, + Path(output) / "geometry-selected", + backend="geometry", + ) + cube = env.sim.get_rigid_object("cube") + original = cube.get_local_pose(to_matrix=True)[1:2].clone() + try: + measured.prepare() + body = next( + b + for b in collision_bodies_from_simulation(env, env_id=1) + if b.robot and b.kind != "halfspace" + ) + link = body.frame_uid.split("/", 1)[1] + pose = env.robot.get_link_pose(link, to_matrix=True)[1:2] + pose = pose @ torch.tensor(body.local_pose, dtype=pose.dtype) + before_query = env.sim.physics_step_index + cube.set_local_pose(pose, env_ids=[1]) + measured.sample((1, 0), ControllerAction(env.robot.get_qpos())) + assert measured.results[0].status == "passed" + assert measured.results[1].status == "failed" + assert env.sim.physics_step_index == before_query + finally: + cube.set_local_pose(original, env_ids=[1]) + measured.close() if backend == "native_contacts": assert check.rows[1].substeps > 0 and check.rows[0].substeps == 0 # The clock uses the same native physics intervals that @@ -350,5 +396,78 @@ def test_explicit_phase_paths_and_selected_row_acceptance(tmp_path): assert result.returncode == 0, result.stdout[-3000:] + result.stderr[-5000:] +def _probe_handover_rows(): + """Plan and consume selected rows in a larger real handover World.""" + import torch + from embodichain.lab.expansion.program import collect_program_batch + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + discover_task_packages, + execute_init_hooks, + ) + from embodichain.lab.sim import SimulationManager + from embodichain.utils.utility import load_config + + torch.set_num_threads(1) + root = Path(__file__).resolve().parents[3] + path = ( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/hand_over/task.tianji_marvin.yaml" + ) + discover_task_packages() + execute_init_hooks() + cfg = config_to_cfg(load_config(path), source_path=path) + cfg.num_envs = cfg.sim_cfg.num_envs = 4 + cfg.sim_cfg.device, cfg.sim_cfg.headless = "cpu", True + cfg.sim_cfg.render_cfg.renderer = "no-render" + cfg.sensor, cfg.enable_sensor, cfg.dataset = [], False, None + cfg.init_rollout_buffer = cfg.record_trajectory = False + env = REGISTERED_ENVS[load_config(path)["id"]].make(cfg=cfg) + adapters = [] + try: + env.reset(seed=2) + program = env.task_program_adapter.compile(cfg.task_program) + bridges = {} + for row in (3, 1): + adapter = env.task_program_adapter.for_env_ids((row,)) + adapters.append(adapter) + bridges[row] = adapter.create_bridge(program, isolate_policy_sessions=True) + observed = [] + result = collect_program_batch( + env, + bridges, + max_steps=2, + observe_step=lambda ids, action, observation: observed.append(ids), + ) + # The original FK omitted selected IDs and rejected both plans at zero + # commands. Budget cancellation must instead follow real commands. + assert result.env_ids == (3, 1) + assert all(length >= 1 for length in result.lengths), result.lengths + assert observed and set(observed[0]) == {3, 1} + assert not any(result.accepted) + finally: + for adapter in adapters: + adapter.close() + env.close(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +@pytest.mark.subprocess_sim +def test_selected_handover_rows_preserve_world_batch(tmp_path): + result = subprocess.run( + [sys.executable, __file__, "handover"], + cwd=Path(__file__).resolve().parents[3], + env={**os.environ, "SDL_VIDEODRIVER": "offscreen", "OMP_NUM_THREADS": "1"}, + capture_output=True, + text=True, + timeout=300, + ) + assert result.returncode == 0, result.stdout[-3000:] + result.stderr[-5000:] + + if __name__ == "__main__": - _probe(sys.argv[1]) + if sys.argv[1] == "handover": + _probe_handover_rows() + else: + _probe(sys.argv[1]) From c3e6cd1bac6b273f1c5c46d017d0e074d7a36a30 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 17 Nov 2026 03:04:45 +0000 Subject: [PATCH 25/45] test(expansion): use native monitors in hold sampling coverage Bind the test validator to its measured environment and retain the real MeasuredTaskMonitor with deterministic joint evidence. This keeps hold-duration and contact assertions intact while exercising the new batched predicate boundary, instead of supplying an incomplete synthetic monitor. Validation: 471 related expansion/bridge/configuration/GenSim tests passed, plus both native validation/selected-handover probes on pinned DexSim 0.6.0rc2. Black 26.10.1, API docs 5013/5013, context map and diff checks passed. Dataset cache was isolated to task-owned tmpfs because the host root filesystem had no non-reserved free space. Production behavior and documentation contracts are unchanged. --- tests/lab/expansion/test_acceptance.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/tests/lab/expansion/test_acceptance.py b/tests/lab/expansion/test_acceptance.py index bfb1e65a73..cfd5507e56 100644 --- a/tests/lab/expansion/test_acceptance.py +++ b/tests/lab/expansion/test_acceptance.py @@ -86,13 +86,12 @@ def actor_ids(uid, links=None): @pytest.fixture def sample_contacts(): """Feed the production observer/sample boundary using explicit physics intervals.""" - from unittest.mock import Mock from embodichain.lab.expansion.acceptance import MeasuredExpansionValidator - from embodichain.lab.expansion.contracts import ValidationCheck, ValidationResult def build(row): - passed = ValidationResult((ValidationCheck("task.success", "passed"),)) - row.monitor = SimpleNamespace(observe=Mock(), validation=lambda env: passed) + row.env.num_envs = 1 + row.env.robot.joint_names = ["j"] + row.env.robot.get_qpos = lambda: torch.zeros((1, 1)) row.active = True state = SimpleNamespace( pose=torch.tensor([[0.0, 0, 0, 0, 0, 0, 1]]), @@ -108,7 +107,9 @@ def build(row): "is_valid": state.contacts, "impulse_valid": torch.ones((1, 2), dtype=torch.bool), } + row.monitor.reset(row.env) validator = object.__new__(MeasuredExpansionValidator) + validator.env = row.env validator.rows = {0: row} validator.sensor = row.sensor validator.checker = SimpleNamespace( From d887e99361bc5e20d12c7e1c9f00448fe957ffa7 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 17 Nov 2026 04:57:25 +0000 Subject: [PATCH 26/45] fix(expansion): validate prepared scenes and PickUp path conflicts Include scene preparation declarations and parameters in shared-World compatibility before creating the host. Reject cartesian_lift combined with phase_paths, consistent with Place's explicit conflict handling. Cover real deployment decoding, matching and mismatched preparation hooks, and public fixed-grid PickUp planning. Keep the dual-arm test robot aligned with selected-row FK queries. Update the affected project context. --- .../topics/expansion-engine/workloads.md | 4 +- .../topics/motion-planning/motion-planning.md | 3 + embodichain/lab/expansion/gym_workloads.py | 3 +- .../sim/atomic_skills/primitives/pick_up.py | 7 +- tests/lab/expansion/test_gym_workloads.py | 55 ++++++++++++++-- tests/sim/atomic_skills/test_actions.py | 66 ++++++++++++++++++- 6 files changed, 126 insertions(+), 12 deletions(-) diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index b1be19998c..3b4a93bfb1 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -42,7 +42,9 @@ independent collection budget, artifacts and row callbacks. The facade opens one World, captures the original seeded nominal scene in row zero, and composes the existing session, frame collector, episode assembler and confirmed artifact sink. Different integration/grasp policies may share that World only when their -physical configuration, program, cadence and expert action schema agree. +physical configuration, program, cadence and expert action schema agree. The +scene-preparation declaration and its parameters must also match, because the +common nominal scene is prepared once before constructing the World. Command mode, ActionManager declarations and controlled-joint selections cannot vary between workloads in one World. Selections belong to the resolved environment configurations, not the shared registered environment implementation class. Physical/visual/prototype diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index eda26894ab..8163844115 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -60,6 +60,9 @@ failure masks. No robot UID or waypoint count enables the behavior. Atomic `MotionPolicy.phase_paths` names the PickUp/Place phases that opt into this path; ordinary calls and other primitives retain their own planning semantics. All inserted Place translation keyframes inherit their original phase's mode. +PickUp's explicit `cartesian_lift` and Place's `cartesian_approach` are mutually +exclusive with `phase_paths`; conflicting declarations reject before trajectory +generation. `motion_gen` delegates timing to its backend. `ik_interp` needs explicit start/count/dt, rejects backend-specific options and uses minimum-jerk segments diff --git a/embodichain/lab/expansion/gym_workloads.py b/embodichain/lab/expansion/gym_workloads.py index 98616ebe46..7b8f638419 100644 --- a/embodichain/lab/expansion/gym_workloads.py +++ b/embodichain/lab/expansion/gym_workloads.py @@ -610,6 +610,7 @@ def physical_key(cfg: Any) -> str: "actions", "control_parts", "active_joint_ids", + "scene_preparation", ) if hasattr(cfg, name) } @@ -617,7 +618,7 @@ def physical_key(cfg: Any) -> str: if any(physical_key(cfg) != physical_key(first) for _, cfg in configs[1:]): raise ValueError( - "workloads must share physical environment, robot, sensors, cadence and expert action schema" + "workloads must share physical environment, robot, sensors, cadence, expert action schema and scene preparation" ) owner = source = None diff --git a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py index 566c6488e5..ce65f7fb95 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py @@ -223,7 +223,10 @@ class PickUpOptions(SkillOptions): """ cartesian_lift: bool = False - """Preserve a straight world-Z lift with IK interpolation and native limits.""" + """Preserve a straight world-Z lift with IK interpolation and native limits. + + Mutually exclusive with explicit ``MotionPolicy.phase_paths``. + """ initial_lift_height: float = field(default=0.0, kw_only=True) """Initial world-Z clearance (m) from the current TCP pose before approach.""" @@ -382,6 +385,8 @@ def _get_full_pickup_trajectory( hand_grasp_qpos: torch.Tensor, interpolation_dt: float, ) -> tuple[torch.Tensor, torch.Tensor, dict[str, int]]: + if options.cartesian_lift and motion_policy.phase_paths: + raise ValueError("cartesian_lift and phase_paths are mutually exclusive.") if options.approach_along_grasp_axis: approach_direction = grasp_xpos[:, :3, 2] pre_grasp_xpos = translate_pose_world( diff --git a/tests/lab/expansion/test_gym_workloads.py b/tests/lab/expansion/test_gym_workloads.py index 3df961fe9a..02905565b1 100644 --- a/tests/lab/expansion/test_gym_workloads.py +++ b/tests/lab/expansion/test_gym_workloads.py @@ -39,6 +39,21 @@ pytestmark = pytest.mark.no_sim +def _prepare_cube_scene(cfg, *, x): + """A valid pre-World hook whose parameters alter the prepared scene.""" + from copy import deepcopy + + prepared = deepcopy(cfg) + cube = next(obj for obj in prepared.rigid_object if obj.uid == "cube") + cube.init_pos = (x, *cube.init_pos[1:]) + return prepared + + +def _prepare_cube_scene_offset(cfg, *, x): + """A distinct callable declaration with different scene preparation.""" + return _prepare_cube_scene(cfg, x=x + 0.1) + + @pytest.mark.parametrize("settled", [0, 5]) def test_expired_warming_row_finishes_group_without_disturbing_live_peer( monkeypatch, settled @@ -164,6 +179,9 @@ def propose(*args, **kwargs): "use_action_manager", "active_joint_ids", "joint_order", + "preparation_params", + "preparation_func", + "preparation_same", ], ) def test_workload_action_schema_checked_before_constructing_world( @@ -211,6 +229,18 @@ class ReachedWorld(Exception): config.active_joint_ids = ( [i] if difference == "active_joint_ids" else ([1, 0] if i else [0, 1]) ) + if difference in {"preparation_params", "preparation_func", "preparation_same"}: + from embodichain.lab.gym.envs.managers.cfg import FunctorCfg + + function = ( + "_prepare_cube_scene_offset" + if i and difference == "preparation_func" + else "_prepare_cube_scene" + ) + config.scene_preparation = FunctorCfg( + func=f"{__name__}:{function}", + params={"x": 0.8 if i and difference == "preparation_params" else 0.2}, + ) configs[name] = config # Configured tasks share this implementation class. Selection belongs # to the resolved configs above, never to artificial per-task classes. @@ -241,7 +271,7 @@ class ReachedWorld(Exception): execution = WorkloadExecutionCfg( workload_count=2, batch_size=1, max_steps=20, settling_steps=1 ) - if difference is None: + if difference in (None, "preparation_same"): with pytest.raises(ReachedWorld): run_gym_expansion_workloads( workloads, execution, output_dir=tmp_path / "run" @@ -255,8 +285,9 @@ class ReachedWorld(Exception): factory.assert_not_called() -def test_real_deployments_with_shared_class_reject_different_control_parts( - tmp_path, monkeypatch +@pytest.mark.parametrize("field", ["control_parts", "scene_preparation"]) +def test_real_deployments_reject_incompatible_shared_world_setup( + tmp_path, monkeypatch, field ): from pathlib import Path from embodichain.lab.expansion import ExpansionRunCfg @@ -277,9 +308,17 @@ def test_real_deployments_with_shared_class_reject_different_control_parts( inputs.mkdir() base = load_config(_prepare_deployment(task, inputs)) workloads = [] - for name, parts in (("Arm", ["arm"]), ("ArmEef", ["arm", "eef"])): + settings = ( + (("Arm", ["arm"]), ("ArmEef", ["arm", "eef"])) + if field == "control_parts" + else tuple( + (name, {"func": f"{__name__}:_prepare_cube_scene", "params": {"x": x}}) + for name, x in (("Near", 0.2), ("Far", 0.8)) + ) + ) + for name, value in settings: component = load_config(task.parent / "envs/default.yaml") - component["env"]["control_parts"] = parts + component["env"][field] = value physical = inputs / f"{name}.environment.yaml" physical.write_text(yaml.safe_dump(component)) declaration = { @@ -290,7 +329,11 @@ def test_real_deployments_with_shared_class_reject_different_control_parts( path = inputs / f"{name}.json" path.write_text(json.dumps(declaration)) config = config_to_cfg(declaration, source_path=path) - assert config.control_parts == parts + if field == "control_parts": + assert config.control_parts == value + else: + assert config.scene_preparation.func == value["func"] + assert config.scene_preparation.params == value["params"] assert REGISTERED_ENVS[declaration["id"]].cls is EmbodiedEnv cfg = ExpansionRunCfg( deployment=str(path), diff --git a/tests/sim/atomic_skills/test_actions.py b/tests/sim/atomic_skills/test_actions.py index c2d3391ad9..fc3defc09c 100644 --- a/tests/sim/atomic_skills/test_actions.py +++ b/tests/sim/atomic_skills/test_actions.py @@ -677,8 +677,9 @@ def get_link_pose( env_ids: list[int] | None = None, to_matrix: bool = True, ) -> torch.Tensor: - del env_ids, to_matrix - pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + del to_matrix + count = NUM_ENVS if env_ids is None else len(env_ids) + pose = torch.eye(4).repeat(count, 1, 1) pose[:, 0, 3] = -1.0 if link_name == "left_root" else 1.0 return pose @@ -745,8 +746,15 @@ def compute_fk( qpos: torch.Tensor | None = None, name: str | None = None, to_matrix: bool = True, + env_ids: list[int] | torch.Tensor | None = None, ) -> torch.Tensor: - count = NUM_ENVS if qpos is None else qpos.shape[0] + count = ( + (NUM_ENVS if env_ids is None else len(env_ids)) + if qpos is None + else qpos.shape[0] + ) + if env_ids is not None: + assert len(env_ids) == count return torch.eye(4).repeat(count, 1, 1) robot.get_qpos.side_effect = get_qpos @@ -2339,6 +2347,58 @@ def generate(states, options=None): ) +@pytest.mark.parametrize("cartesian_lift", [False, True]) +def test_pick_phase_paths_and_cartesian_lift_require_explicit_choice( + cartesian_lift, tmp_path +): + generator = _motion_generator() + # Fixed-grid planning retains the real authored-velocity-limit check. + urdf = tmp_path / "arm.urdf" + urdf.write_text( + '' + + "".join(f'' for i in range(ROBOT_DOF + 1)) + + "".join( + f'' + f'' + '' + for i in range(ROBOT_DOF) + ) + + "" + ) + generator.robot.cfg.fpath = str(urdf) + generator.generate = Mock(wraps=generator.generate) + action = _bind_action(generator, PickUp()) + pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + invocation = SkillInvocation( + skill_id="pick_up", + goal=GraspGoal(semantics=_semantics(entity_id="target"), grasp_xpos=pose), + binding=_binding(action), + motion_policy=MotionPolicy( + strategy="ik_interp", + sample_count=20, + interpolation_mode="fixed_grid", + ik_seed_mode="sequential", + acceleration_limit=None, + phase_paths={"pre_grasp": "cartesian"}, + ), + skill_options=PickUpOptions(cartesian_lift=cartesian_lift, lift_height=0.1), + ) + context = _context(scene=_target_scene(pose, timestamp=0.0, version=0)) + if cartesian_lift: + with pytest.raises(ValueError, match="cartesian_lift and phase_paths"): + _plan_action(action, invocation, context) + generator.generate.assert_not_called() + else: + _plan_action(action, invocation, context) + assert generator.generate.call_args_list[0].kwargs[ + "options" + ].waypoint_modes == ( + "cartesian", + "joint", + "joint", + ) + + def test_pick_holds_fully_closed_before_lifting_when_configured() -> None: generator = _motion_generator() generator.generate = Mock(wraps=generator.generate) From cba37a4aeab5786c53415ac8ed6644772d459f30 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 17 Nov 2026 05:26:01 +0000 Subject: [PATCH 27/45] fix(expansion): retain reset holds and batch seeded solver paths Keep settling rows controller-active without trajectory or receipt credit. Retain configured grasp preload through coordinated placement. Use explicitly supported preceding-seed path kernels and cooperatively merge compatible requests, preserving active planning budgets and legacy solver fallbacks. Validation: actual reset/grasp negative regressions reproduce on the published SDK; selected-row real path batching passes, motion checks 123 passed, workload integration 3 passed, SSIK checks 2 passed. Five-task repeated physical qualification follows separately. --- .../topics/atomic-actions/atomic-actions.md | 4 + .../topics/expansion-engine/workloads.md | 11 +- .../topics/motion-planning/motion-planning.md | 5 + .../lab/expansion/_planning_batches.py | 5 +- embodichain/lab/expansion/workloads.py | 5 + .../primitives/coordinated_placement.py | 7 +- .../lab/sim/motion/motion_generator.py | 11 ++ .../lab/sim/motion/solvers/base_solver.py | 10 ++ .../lab/sim/motion/solvers/ssik_solver.py | 9 + .../lab/expansion/test_extension_ports_sim.py | 154 ++++++++++++++++++ tests/lab/expansion/test_workloads_sim.py | 13 ++ 11 files changed, 230 insertions(+), 4 deletions(-) diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 2dcc7d3636..c538354cfd 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -105,6 +105,10 @@ are calibrated scene inputs. Preserve their selected owners and fingerprints; never infer capabilities or general physical success from a sample name. The host's sampled footprint/support checks remain finite measured acceptance, separate from geometric proposal qualification. +`CoordinatedPlacement` retains each binding's configured grasp command during +approach/hold and on the support hand through release/retreat. Contact-blocked +measured jaw positions cannot replace these closing targets. Measured robot +observations and held-object transforms still supply geometry and validation. ## Recommended change sites diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index 3b4a93bfb1..58f6ff49c1 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -74,6 +74,12 @@ can finish incomplete while already-admitted peers continue and export normally. The native source also handles expiry during initial-state validation before atomic session proposal. +Settling rows remain controller-active so Gym consumes their explicit reset +hold targets. They are not live trajectories: only cursor rows receive source +observations, command acknowledgements, trajectory lengths or receipt credit. +Using Gym's inactive measured-qpos substitution during settling would remove +restoring drive error on every tick and change the admitted initial posture. + `started` calls `mark_row_started` after the first actual command. `observe` captures every active T+1 boundary and performs the original physical checks; it returns only active rows needing native safe rejection. The executor retains @@ -95,7 +101,10 @@ Partial quotas remain explicit failures even when some episodes were exported. The optional cooperative planner merges compatible fixed-grid motion requests and pure FK/IK/Jacobian calls. Budget-bearing whole-motion requests keep their -original execution so charge semantics are preserved. Context variables and +original execution so charge semantics are preserved. Solvers declaring the +strong sequential-path capability also merge compatible native path requests, +retaining each row's preceding-valid seed and independently owned results. +Context variables and candidate random streams remain row-local; results are independently owned. Default CPU command gathering shares native numeric reads through a bounded World-owner scope in `sim/objects/backends/_numeric_observations.py`. It ends diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index 8163844115..900db5cad7 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -70,6 +70,11 @@ when limits apply. Adaptive timing and batched IK remain defaults. Explicit `fixed_grid` preserves count/intervals and rejects overspeed rows against runtime and authored joint caps; `sequential` retains previous-valid-posture seeds and callback/RNG order. Neither mode is a generic option for other strategies. +Solvers may opt into `supports_sequential_batch_ik` only when their continuous +path hook enumerates the ordinary candidate pool against each preceding valid +seed. SSIK Warp SRS `seed_local` supplies this stronger contract, allowing +bounded path batches instead of Python point dispatch. Other solvers and active +planning budgets retain the original point-query sequence and charging rules. Rejected rows hold their start; `rejected_candidate` retains original samples and rejected-row identity for audit/recovery. A zero-speed hold does not prove diff --git a/embodichain/lab/expansion/_planning_batches.py b/embodichain/lab/expansion/_planning_batches.py index 552e434f2c..87df329d23 100644 --- a/embodichain/lab/expansion/_planning_batches.py +++ b/embodichain/lab/expansion/_planning_batches.py @@ -98,7 +98,10 @@ def __init__(self, robot, seed): self.solve_rng = self._rng() self._restore_rng(previous) for solver in dict.fromkeys(robot._solvers.values()): - for name in ("get_fk", "get_ik", "get_jacobian"): + names = ("get_fk", "get_ik", "get_jacobian") + if getattr(solver, "supports_sequential_batch_ik", False): + names += ("_get_continuous_ik_path",) + for name in names: self._bind(solver, name) from ._motion_batches import MotionBatches diff --git a/embodichain/lab/expansion/workloads.py b/embodichain/lab/expansion/workloads.py index 89fca5bd4c..af147937f7 100644 --- a/embodichain/lab/expansion/workloads.py +++ b/embodichain/lab/expansion/workloads.py @@ -585,6 +585,11 @@ def run(self) -> Mapping[str, object]: env.num_envs, dtype=torch.bool, device=env.device ) mask[list(commands)] = True + # Settling rows execute their explicit reset hold. Marking + # them inactive lets Gym replace that target with measured + # qpos at every tick, removing the drive's restoring error. + # They still have no cursor, receipt or acceptance samples. + mask[list(self.warming)] = True env._demo_active_mask = mask before = env.sim.physics_step_index self.observation, _, terminated, truncated, _ = env.step(action) diff --git a/embodichain/lab/sim/atomic_skills/primitives/coordinated_placement.py b/embodichain/lab/sim/atomic_skills/primitives/coordinated_placement.py index 34a7c42dd4..9ad8e0d16b 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/coordinated_placement.py +++ b/embodichain/lab/sim/atomic_skills/primitives/coordinated_placement.py @@ -400,8 +400,11 @@ def failed_with_batching(message: str) -> SkillPlan: placing_place_qpos = placing_approach_traj[:, -1] support_place_qpos = support_approach_traj[:, -1] - placing_hand_qpos = state.last_qpos[:, list(resources.placing_hand.joint_ids)] - support_hand_qpos = state.last_qpos[:, list(resources.support_hand.joint_ids)] + # A contact-blocked measured position does not retain closing effort. + # Both bindings already require a grasp command; use it until release, + # just as MoveHeldObject does for a single held object. + placing_hand_qpos = resources.placing_hand_close_qpos + support_hand_qpos = resources.support_hand_close_qpos approach_trajectory = self._assemble_segment( state.last_qpos, placing_approach_traj, diff --git a/embodichain/lab/sim/motion/motion_generator.py b/embodichain/lab/sim/motion/motion_generator.py index 0da837274a..23fdb63547 100644 --- a/embodichain/lab/sim/motion/motion_generator.py +++ b/embodichain/lab/sim/motion/motion_generator.py @@ -1270,6 +1270,17 @@ def _solve_seeded_cartesian_waypoints( or poses.shape[1] == 0 ): raise ValueError("Cartesian waypoints must have shape (B, N, 4, 4), N > 0.") + solver = self.robot.get_solver(control_part) + from ._budget import _ACTIVE_BUDGET + + if ( + solver is not None + and getattr(solver, "supports_sequential_batch_ik", False) + and _ACTIVE_BUDGET.get() is None + ): + # The solver owns the sequential dependency inside its bounded + # path hook. Do not re-enter Python/Robot once per Cartesian node. + return self._solve_cartesian_waypoints(poses, seed, control_part) success = torch.ones(batch, dtype=torch.bool, device=seed.device) points = [] for index in range(poses.shape[1]): diff --git a/embodichain/lab/sim/motion/solvers/base_solver.py b/embodichain/lab/sim/motion/solvers/base_solver.py index 54e5cbe6e0..4b5cd78402 100644 --- a/embodichain/lab/sim/motion/solvers/base_solver.py +++ b/embodichain/lab/sim/motion/solvers/base_solver.py @@ -531,6 +531,16 @@ def get_tcp(self) -> np.ndarray: """ return self.tcp_xpos + @property + def supports_sequential_batch_ik(self) -> bool: + """Whether continuous batching uses each preceding point-query seed. + + This stronger contract also preserves seed-dependent candidate search. + Merely supporting continuous selection from a fixed candidate pool is + insufficient. Solvers opt in only when their path hook implements it. + """ + return False + @property def supports_continuous_batch_ik(self) -> bool: """Whether all-candidate IK and temporally continuous selection are supported. diff --git a/embodichain/lab/sim/motion/solvers/ssik_solver.py b/embodichain/lab/sim/motion/solvers/ssik_solver.py index c8befce863..8eeb36445e 100644 --- a/embodichain/lab/sim/motion/solvers/ssik_solver.py +++ b/embodichain/lab/sim/motion/solvers/ssik_solver.py @@ -352,6 +352,15 @@ def prepare_buffers(self, max_batch: int) -> None: if provider is not None: provider.prepare_buffers(capacity) + @property + def supports_sequential_batch_ik(self) -> bool: + """Whether the path hook enumerates the ordinary preceding-seed pool.""" + return ( + self.cfg.backend == "warp" + and self.family == "srs" + and self.cfg.srs_search == "seed_local" + ) + @property def supports_continuous_batch_ik(self) -> bool: """Whether candidate generation and previous-frame selection are supported.""" diff --git a/tests/lab/expansion/test_extension_ports_sim.py b/tests/lab/expansion/test_extension_ports_sim.py index 8230db610e..0b4304d017 100644 --- a/tests/lab/expansion/test_extension_ports_sim.py +++ b/tests/lab/expansion/test_extension_ports_sim.py @@ -396,6 +396,159 @@ def test_explicit_phase_paths_and_selected_row_acceptance(tmp_path): assert result.returncode == 0, result.stdout[-3000:] + result.stderr[-5000:] +def _check_coordinated_grasp_targets(env): + """Real selected-row IK keeps commanded preload on contact-blocked hands.""" + import torch + from embodichain.lab.sim.atomic_skills import ( + AntipodalAffordance, + AtomicSkillEngine, + ControlPartCommandProfile, + CoordinatedPlacementGoal, + CoordinatedPlacementOptions, + HeldObjectState, + MotionPolicy, + ObjectSemantics, + SkillInvocation, + TaskState, + ) + from embodichain.lab.sim.motion.motion_generator import ( + MotionGenerator, + MotionGenCfg, + ) + from embodichain.lab.sim.motion.planners.trapezoidal_planner import ( + TrapezoidalPlannerCfg, + ) + + rows = (3, 1) + generator_cfg = MotionGenCfg( + planner_cfg=TrapezoidalPlannerCfg( + robot_uid=env.robot.uid, sim_instance_id=env.sim.instance_id + ) + ) + generator = MotionGenerator(generator_cfg, env_ids=rows) + profiles = {} + for part in ("left_eef", "right_eef"): + limits = env.robot.get_qpos_limits(name=part)[0] + profiles[part] = ControlPartCommandProfile.joint_positions( + open=limits[:, 0], grasp=limits[:, 1] + ) + engine = AtomicSkillEngine(generator, env_ids=rows, control_profiles=profiles) + binding = engine.bind_control_parts( + "coordinated_placement", + { + "placing": {"motion": "left_arm", "grasp": "left_eef"}, + "support": {"motion": "right_arm", "grasp": "right_eef"}, + }, + ) + poses, held = {}, {} + for slot, arm in (("placing", "left_arm"), ("support", "right_arm")): + poses[slot] = env.robot.compute_fk( + qpos=env.robot.get_qpos(name=arm)[list(rows)], + name=arm, + to_matrix=True, + env_ids=rows, + ) + held[binding.endpoint(slot, "motion").task_state_key] = HeldObjectState( + semantics=ObjectSemantics( + entity_id=slot, + label=slot, + geometry={}, + affordance=AntipodalAffordance(), + ), + object_to_eef=torch.eye(4).repeat(2, 1, 1), + grasp_xpos=poses[slot], + ) + context = engine.initial_context( + task=TaskState(batch_size=2, device="cpu", held_objects=held), control_dt=0.04 + ) + before = env.sim.physics_step_index + # The stronger solver capability must preserve actual preceding-seed IK, + # including a failed peer, without losing non-contiguous physical rows. + start = context.robot.qpos[:, env.robot.get_joint_ids("right_arm")] + nodes = start[:, None].repeat(1, 4, 1) + nodes[:, :, 0] += torch.tensor([0.0, 0.01, 0.02, 0.03]) + targets = torch.stack( + [ + env.robot.compute_fk( + qpos=node, name="right_arm", to_matrix=True, env_ids=rows + ) + for node in nodes.unbind(1) + ], + dim=1, + ) + for failed_peer in (False, True): + requested = targets.clone() + if failed_peer: + requested[1, 2, 0, 3] += 100 + seed, points = start, [] + accepted = torch.ones(2, dtype=torch.bool) + for pose in requested.unbind(1): + valid, point = env.robot.compute_ik( + pose=pose, name="right_arm", joint_seed=seed, env_ids=rows + ) + seed = torch.where(valid[:, None], point, seed) + points.append(seed) + accepted &= valid + expected = torch.where( + accepted[:, None, None], torch.stack(points, 1), start[:, None] + ) + valid, actual = generator._solve_seeded_cartesian_waypoints( + requested, start, "right_arm" + ) + assert torch.equal(valid, accepted) + torch.testing.assert_close(actual, expected, rtol=0, atol=1e-6) + from embodichain.lab.expansion._planning_batches import batched_planning + + class PathQuery: + # A numerical request producer, with the real Robot/SSIK implementation. + def __init__(self, index): + self.index = index + + def next(self): + index = self.index + single = MotionGenerator(generator_cfg, env_ids=(rows[index],)) + return single._solve_seeded_cartesian_waypoints( + requested[index : index + 1], start[index : index + 1], "right_arm" + ) + + with batched_planning(env.robot, seed=2) as broker: + answers = broker.gather({row: PathQuery(i) for i, row in enumerate(rows)}) + assert broker.stats["max_group"] == 2 + for index, row in enumerate(rows): + assert torch.equal(answers[row][0], accepted[index : index + 1]) + torch.testing.assert_close( + answers[row][1], expected[index : index + 1], rtol=0, atol=1e-6 + ) + for release in (False, True): + plan = engine.plan( + SkillInvocation( + skill_id="coordinated_placement", + binding=binding, + goal=CoordinatedPlacementGoal( + placing_object_target_pose=poses["placing"], + support_object_target_pose=poses["support"], + ), + motion_policy=MotionPolicy(sample_count=30, strategy="ik_interp"), + skill_options=CoordinatedPlacementOptions( + release=release, lift_height=0, hand_interp_steps=4, retreat_steps=4 + ), + ), + context, + ) + assert plan.success_all, plan.diagnostics + for part in ("left_eef", "right_eef"): + ids = env.robot.get_joint_ids(part) + expected = env.robot.get_qpos_limits(name=part)[0, :, 1] + actual = plan.joint_trajectory.positions[:, :, ids] + if part == "left_eef" and release: + actual = actual[:, : plan.segments[0].stop] + assert not torch.equal(context.robot.qpos[:, ids], expected.expand(2, -1)) + torch.testing.assert_close( + actual, expected.expand_as(actual), rtol=0, atol=0 + ) + assert env.sim.physics_step_index == before + + def _probe_handover_rows(): """Plan and consume selected rows in a larger real handover World.""" import torch @@ -427,6 +580,7 @@ def _probe_handover_rows(): adapters = [] try: env.reset(seed=2) + _check_coordinated_grasp_targets(env) program = env.task_program_adapter.compile(cfg.task_program) bridges = {} for row in (3, 1): diff --git a/tests/lab/expansion/test_workloads_sim.py b/tests/lab/expansion/test_workloads_sim.py index eb457d8e5b..419e01dfab 100644 --- a/tests/lab/expansion/test_workloads_sim.py +++ b/tests/lab/expansion/test_workloads_sim.py @@ -193,9 +193,22 @@ def __init__(self): def prepare(self, assignments): assert len(SimulationManager._instances) == 1 + self.reset_holds = getattr(self, "reset_holds", {}) + for assignment in assignments: + self.reset_holds[assignment.env_id] = robot.get_qpos(target=True)[ + assignment.env_id + ].clone() def activate(self, assignment, observation): row = assignment.env_id + # The physical settling ticks must keep the authored drive + # target, rather than repeatedly holding gravity-displaced qpos. + torch.testing.assert_close( + robot.get_qpos(target=True)[row], + self.reset_holds[row], + rtol=0, + atol=0, + ) checks.prepare(env_ids=(row,)) from copy import deepcopy From 4dad7078dd2e568d17fb2520e923debc3258446f Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 17 Nov 2026 08:41:01 +0000 Subject: [PATCH 28/45] feat(expansion): reuse idle rows for bounded retry candidates Keep W times B physical capacity and original attempt/time/byte/step limits. Competing retries reserve only running slots; pending writes and confirmed commits retain the hard quota. Cancel peers through the original command handshake after confirmed persistence, retaining canonical outcomes and measured partial evidence. Preserve phase metadata on safe stops and do not advance an already pending rejection hold twice. Native Task Program jobs enable idle-row reuse; custom sources opt in with an explicit cancellation port. Validation: 41 focused lifecycle, configuration and real simulation checks pass on DexSim rc2. The actual known-bad scheduler copy fails the new unexecuted-hold regression. Independent engine/robotics/verification passes completed; API/context/Black gates pass. Full five-task after measurements follow on an installed VCS package. --- .../expansion-engine/expansion-engine.md | 6 + .../topics/expansion-engine/workloads.md | 23 ++ embodichain/lab/expansion/_gym_episode.py | 45 ++-- .../lab/expansion/_task_program_config.py | 3 + embodichain/lab/expansion/_workload_cursor.py | 10 + embodichain/lab/expansion/engine.py | 34 ++- embodichain/lab/expansion/gym_workloads.py | 61 ++++- embodichain/lab/expansion/session.py | 18 +- .../lab/expansion/task_program_jobs.py | 1 + embodichain/lab/expansion/workloads.py | 100 +++++++- tests/lab/expansion/test_session.py | 89 +++++++ tests/lab/expansion/test_workloads_sim.py | 220 +++++++++++++++++- 12 files changed, 578 insertions(+), 32 deletions(-) diff --git a/agent_context/topics/expansion-engine/expansion-engine.md b/agent_context/topics/expansion-engine/expansion-engine.md index 79a12efc97..e900ef03d3 100644 --- a/agent_context/topics/expansion-engine/expansion-engine.md +++ b/agent_context/topics/expansion-engine/expansion-engine.md @@ -156,6 +156,12 @@ Compatible fixed-grid planning and pure solver requests can be cooperatively merged on the owner thread. Scoped planning budgets retain per-row charges; unsupported requests keep native execution. Export callbacks receive frozen group evidence and never access the World. See [rolling execution](workloads.md). +Optional idle-row reuse keeps the same W×B capacity while competing retry +candidates use otherwise idle slots. Native write reservations still enforce +the original quota; peers are cancelled only after a confirmed receipt, using +measured partial evidence and the bridge's ordinary stop handshake. The low-level +source must supply an explicit cancellation port. Native Task Program jobs enable +this policy by default without adding processes or Worlds. Native numerical evidence has T submitted actions and T+1 measured states. It includes all represented dynamic scene entities, source/action semantics, physical parameters diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index 58f6ff49c1..28f45c742d 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -59,6 +59,22 @@ counter so another row cannot advance outside the World-owner loop. Frozen groups export through one bounded writer thread without World access. Fresh output is required; this facade does not resume interrupted collection. +`WorkloadExecutionCfg.reuse_idle_rows` optionally lends unused physical rows to +unfinished groups; capacity remains exactly W times B. Queued groups keep their +normal lane priority. Healthy candidates fill only their remaining quota. Each +observed unsuccessful attempt permits one additional competing candidate, bounded +by the existing attempt, admission-time, settling, step and pending-byte limits. +The low-level flag defaults false; the native Task Program job enables it by +default and accepts `engine.reuse_idle_rows: false` to retain fixed lanes. + +This mode requires a source `cancel(result, *, reason)` port. Only a confirmed +receipt fills quota. Remaining live candidates use the ordinary safe-stop +handshake; unstarted warming rows are cancelled without fabricated commands. +Already completed peers retain their canonical program acceptance in cancelled +diagnostics; they do not become physical failures or successful persisted rows. +The native source retains measured T+1 partial buffers and plan traces, then +releases the native assignment. Cancelled rows earn no receipt or quota credit. + `WorkloadSource.prepare` runs inside the selected-row `episode_prepare` reset callback. It must not step physics, alter an active peer or mutate World-wide appearance/resources. Incompatible topology or shared resources require a @@ -92,6 +108,13 @@ control indices. It assembles the original source evidence and calls count. None is a rejection, never a placeholder success. Unavailable or ambiguous writes retain the ordinary native stage/reconciliation contract and stop the rolling owner from issuing unsupported quota credit. +`ExpansionSession.take_ready(speculative=True)` may reserve competing running +rows while quota is unfilled. Required physical checks are unchanged, and +pending writes plus confirmed commits can never exceed the original target. +`ExpansionEngine.cancel_row` cannot release pending persistence; its existing +reconciliation path still owns uncertain or failed writes. Safe-stop controller +values retain the interrupted invocation/phase metadata for ordinary physical +validators, while their cancellation kind and reason remain explicit. Frozen groups can call `export_expansion_episodes` from bounded writer threads. The callback must not retain mutable live state or operate on the World. diff --git a/embodichain/lab/expansion/_gym_episode.py b/embodichain/lab/expansion/_gym_episode.py index 6eb1f02a83..274be4bbdd 100644 --- a/embodichain/lab/expansion/_gym_episode.py +++ b/embodichain/lab/expansion/_gym_episode.py @@ -37,6 +37,29 @@ __all__: list[str] = [] +def _save_partial(numeric, path: Path, step_dt: float) -> dict: + """Publish actual T+1 buffers for rejected or administratively cancelled work.""" + arrays = { + "qpos": torch.stack(numeric["qpos"]).numpy(), + "timestamps": np.arange(len(numeric["qpos"])) * step_dt, + } + if numeric["actions"]: + arrays["actions"] = torch.stack(numeric["actions"]).numpy() + arrays.update( + { + "object_" + str(index): torch.stack(values).numpy() + for index, values in enumerate(numeric["objects"].values()) + } + ) + with path.open("wb") as stream: + np.savez_compressed(stream, **arrays) + return { + "path": path.name, + "sha256": hashlib.sha256(path.read_bytes()).hexdigest(), + "object_uids": list(numeric["objects"]), + } + + class GymEpisodeAssembler: """Convert frozen cohort buffers into canonical accepted or rejected evidence.""" @@ -73,27 +96,9 @@ def assemble(self, assignments, collector, services, result, physics_after): record["source_execution_reports"] = dict( services[row].tracer.execution_reports ) - partial = records[row] - path = directory / "rejected.npz" - arrays = { - "qpos": torch.stack(partial["qpos"]).numpy(), - "timestamps": np.arange(len(partial["qpos"])) * env.step_dt, - } - if partial["actions"]: - arrays["actions"] = torch.stack(partial["actions"]).numpy() - arrays.update( - { - "object_" + str(index): torch.stack(values).numpy() - for index, values in enumerate(partial["objects"].values()) - } + record["partial_rollout"] = _save_partial( + records[row], directory / "rejected.npz", env.step_dt ) - with path.open("wb") as stream: - np.savez_compressed(stream, **arrays) - record["partial_rollout"] = { - "path": path.name, - "sha256": hashlib.sha256(path.read_bytes()).hexdigest(), - "object_uids": list(partial["objects"]), - } _atomic_json(directory / "attempt.json", record) episodes[row] = None continue diff --git a/embodichain/lab/expansion/_task_program_config.py b/embodichain/lab/expansion/_task_program_config.py index b092888ffc..a4b1518890 100644 --- a/embodichain/lab/expansion/_task_program_config.py +++ b/embodichain/lab/expansion/_task_program_config.py @@ -100,6 +100,7 @@ def load_task_program_expansion(path: str | Path) -> dict[str, Any]: "grasp_nms_chunk_size", "collection_renderer", "collision_backend", + "reuse_idle_rows", }: raise ValueError("Unknown or missing expansion engine settings") engine.setdefault("cpu_threads", 1) @@ -116,6 +117,8 @@ def load_task_program_expansion(path: str | Path) -> dict[str, Any]: engine.setdefault("grasp_nms_chunk_size", 2048) engine.setdefault("collection_renderer", "auto") engine.setdefault("collision_backend", "newton") + if "reuse_idle_rows" in engine and type(engine["reuse_idle_rows"]) is not bool: + raise ValueError("engine.reuse_idle_rows must be a boolean") if engine["collection_renderer"] not in ("auto", "rt", "fast-rt", "hybrid"): raise ValueError( "engine.collection_renderer must be auto, rt, fast-rt or hybrid" diff --git a/embodichain/lab/expansion/_workload_cursor.py b/embodichain/lab/expansion/_workload_cursor.py index b61fe04fea..3a0c04449f 100644 --- a/embodichain/lab/expansion/_workload_cursor.py +++ b/embodichain/lab/expansion/_workload_cursor.py @@ -68,6 +68,16 @@ def next(self) -> ControllerAction | None: try: action = super().next() if action is not None: + if ( + self.rejection is not None or self.emergency is not None + ) and self.last_command is not None: + # The safe-stop value is new; its active invocation and + # phase still belong to the command being interrupted. + # Phase-aware physical validators must see that context. + action = ControllerAction( + action.value, + metadata={**self.last_command.metadata, **action.metadata}, + ) self.last_command = action if self.finished and self.rejection is not None: self.accepted, self.reason = False, self.rejection diff --git a/embodichain/lab/expansion/engine.py b/embodichain/lab/expansion/engine.py index ef38064d6e..1c5db9e250 100644 --- a/embodichain/lab/expansion/engine.py +++ b/embodichain/lab/expansion/engine.py @@ -480,12 +480,16 @@ def on_started(identity: CandidateIdentity) -> None: break return self._snapshot() - def admit_row(self, item: PreparedEpisodeWorkItem) -> ExpansionAssignment | None: + def admit_row( + self, item: PreparedEpisodeWorkItem, *, speculative: bool = False + ) -> ExpansionAssignment | None: """Admit one independently prepared physical row through the native session. Args: item: Host-owned candidate proposed in this session, bound to its actual World row. Cohort collection remains unchanged. + speculative: Permit a competing rollout while quota is unfilled; + persistence still reserves at most the original target count. Returns: Assigned episode/commit identity, or None for rejected preparation @@ -494,6 +498,8 @@ def admit_row(self, item: PreparedEpisodeWorkItem) -> ExpansionAssignment | None """ if self._configured_cfg is not None or self._failed: raise RuntimeError("row admission requires a healthy custom engine") + if type(speculative) is not bool: + raise TypeError("speculative must be a bool") if not isinstance(item, PreparedEpisodeWorkItem): raise TypeError("rolling admission requires PreparedEpisodeWorkItem") if ( @@ -517,6 +523,7 @@ def admit_row(self, item: PreparedEpisodeWorkItem) -> ExpansionAssignment | None identity.initial_state_id, episode_byte_budget=self.cfg.episode_byte_budget, candidate_id=identity.candidate_id, + speculative=speculative, ) if ready is None: self.session.release(identity, reason="row_admission_backpressure") @@ -596,6 +603,31 @@ def complete_row( self._started_rows.discard(row) return receipt + def cancel_row(self, assignment: ExpansionAssignment, *, reason: str) -> None: + """Retire an uncommitted row without a receipt or fabricated rejection. + + Args: + assignment: Exact active identity returned by :meth:`admit_row`. + reason: Explicit cancellation reason retained in native diagnostics. + + Raises: + ValueError: For an inactive row or work with pending persistence. + """ + row = assignment.env_id + if self._row_assignments.get(row) != assignment: + raise ValueError("cancellation does not identify an active rolling row") + if ( + assignment.commit_id in self._pending + or assignment.commit_id in self._submitted + ): + raise ValueError( + "pending persistence must be reconciled before cancellation" + ) + self.session.release(assignment.item.spec.identity, reason=reason) + self._queue.remove(assignment.item) + del self._row_assignments[row] + self._started_rows.discard(row) + def run_source(self, source: ExpansionRecipeSource) -> Mapping[str, object]: """Drive bounded source batches through the same admission/execution/writer core. diff --git a/embodichain/lab/expansion/gym_workloads.py b/embodichain/lab/expansion/gym_workloads.py index 7b8f638419..2c1d871072 100644 --- a/embodichain/lab/expansion/gym_workloads.py +++ b/embodichain/lab/expansion/gym_workloads.py @@ -106,10 +106,13 @@ def encode(value: Any) -> Any: class _GymWorkloadSource: """Compose existing native admission, frame assembly and persistence owners.""" - def __init__(self, owner: Any, groups: Mapping[str, Any]) -> None: + def __init__( + self, owner: Any, groups: Mapping[str, Any], *, reuse_idle_rows: bool = False + ) -> None: from ._gym_frames import GymFrameCollector self.owner, self.groups, self.env = owner, groups, owner.env + self.reuse_idle_rows = reuse_idle_rows self.collector = GymFrameCollector(owner._context, {}) self.active: dict[int, dict[str, Any]] = {} self.activation_snapshot = None @@ -299,7 +302,7 @@ def activate(self, assignment: WorkloadAssignment, observation: object) -> Any: ctx.compatibility.key, row, ) - native = engine.admit_row(item) + native = engine.admit_row(item, speculative=self.reuse_idle_rows) if native is None: ctx.journal.update(request, status="initial_rejected") return None @@ -445,6 +448,56 @@ def complete(self, result: WorkloadRowResult) -> Any: state["adapter"].close() self.collector.discard(row) + def cancel(self, result: WorkloadRowResult, *, reason: str) -> None: + """Retire competing work without changing its canonical program outcome.""" + from ._gym_episode import _save_partial + + row = result.assignment.env_id + state = self.active.pop(row, None) + group = self.groups[result.assignment.workload_id] + ctx = group["host"]._context + request = ctx.requests[row] + record = { + "status": "cancelled", + "reason": reason, + "program_accepted": result.program.accepted[0], + "program_reason": result.program.reasons[0], + "program_segments": result.program.segments[0], + "steps": result.program.lengths[0], + "world_control_start": result.world_control_start, + "world_control_end": result.world_control_end, + } + if state is None: + ctx.journal.update(request, **record) + return + try: + group["engine"].cancel_row(state["native"], reason=reason) + measured = ( + None + if self.owner.collision is None + else self.owner.collision.results.get(row) + ) + if measured is not None: + record["measured_collision"] = { + "status": measured.status, + "reason": measured.reason, + } + if state["factory"] is not None: + record["expansion"] = [ + r.to_metadata() for r in state["factory"].records + ] + record["partial_rollout"] = _save_partial( + self.collector.records[row], + request.directory / "cancelled.npz", + self.env.step_dt, + ) + if row in self.collector.spools: + record["media_manifest"] = str(self.collector.spools[row].finish()) + ctx.journal.update(request, **record) + finally: + state["adapter"].close() + self.collector.discard(row) + def close(self) -> None: for state in self.active.values(): state["adapter"].close() @@ -804,7 +857,9 @@ def admission_clock(admitted_at=admitted_at): "admitted_at": admitted_at, } journal.stage("collection", "running") - source = _GymWorkloadSource(owner, groups) + source = _GymWorkloadSource( + owner, groups, reuse_idle_rows=execution.reuse_idle_rows + ) specs = tuple( ExpansionWorkloadCfg( workload_id=w.workload_id, diff --git a/embodichain/lab/expansion/session.py b/embodichain/lab/expansion/session.py index 8087f9d7b9..53d5248e2a 100644 --- a/embodichain/lab/expansion/session.py +++ b/embodichain/lab/expansion/session.py @@ -564,6 +564,12 @@ def _reserved(self) -> int: for attempt in self._attempts.values() ) + def _write_reserved(self) -> int: + return sum( + attempt.state in {"pending_write", "committed"} + for attempt in self._attempts.values() + ) + def take_ready( self, case_id: str, @@ -571,6 +577,7 @@ def take_ready( *, episode_byte_budget: int | None = None, candidate_id: str | None = None, + speculative: bool = False, ) -> CandidateTrajectoryBatch | EpisodeCandidate | None: """Reserve a row, rollout attempt, and pending payload capacity before execution. @@ -586,12 +593,17 @@ def take_ready( episode_byte_budget: Maximum episode payload bytes to reserve for this row. candidate_id: Optional exact ready candidate to reserve. Omission preserves deterministic FIFO selection within the case. + speculative: Permit competing rollouts for unfilled quota. Payload, + attempt and write reservations still bound execution; only one + passed episode can reserve each remaining persistence slot. Returns: One assigned candidate row, or ``None`` when no matching row or capacity is available or the session has stopped. """ self._cases[(case_id, initial_state_id)] + if type(speculative) is not bool: + raise TypeError("speculative must be a bool") if candidate_id is not None and ( type(candidate_id) is not str or not candidate_id.strip() ): @@ -608,7 +620,8 @@ def take_ready( ) if ( self.stop_reason - or self._reserved() >= self._cfg.collection.target_committed_episodes + or (self._write_reserved() if speculative else self._reserved()) + >= self._cfg.collection.target_committed_episodes or self._counts["rollout_attempted"] + assigned >= self._cfg.collection.max_rollout_attempts or pending_count >= self._cfg.persistence.pending_episode_limit @@ -761,6 +774,9 @@ def accept_episode(self, episode: ExpertEpisode) -> bool: if self._episode_bytes(episode) > attempt.episode_byte_budget: self.release(episode.identity, reason="episode_byte_budget_exceeded") raise BufferError("Payload exceeds its reserved episode byte budget.") + if self._write_reserved() >= self._cfg.collection.target_committed_episodes: + self.release(episode.identity, reason="quota_reserved_by_peer") + return False owned = replace(episode) if not self._reserve_coverage(owned): self.release(episode.identity, reason="coverage_rejected") diff --git a/embodichain/lab/expansion/task_program_jobs.py b/embodichain/lab/expansion/task_program_jobs.py index 4576457c4f..23d8a1a755 100644 --- a/embodichain/lab/expansion/task_program_jobs.py +++ b/embodichain/lab/expansion/task_program_jobs.py @@ -403,6 +403,7 @@ def _run_operators( max_steps=settings["max_steps"], settling_steps=settings["settling_steps"], seed=seed, + reuse_idle_rows=settings.get("reuse_idle_rows", True), ) chunks = settings["grasp_nms_chunk_size"] if torch.cuda.is_available() else 2048 with pose_nms_scope(chunk_size=chunks): diff --git a/embodichain/lab/expansion/workloads.py b/embodichain/lab/expansion/workloads.py index af147937f7..b655d02175 100644 --- a/embodichain/lab/expansion/workloads.py +++ b/embodichain/lab/expansion/workloads.py @@ -93,6 +93,10 @@ class WorkloadExecutionCfg: settling_steps: Minimum physical substeps before a row becomes active. seed: Base seed; each candidate owns a stream keyed by local ordinal. batch_planning: Cooperatively merge compatible native motion/solver calls. + reuse_idle_rows: Let unfinished groups borrow otherwise idle rows. A + rejected attempt permits one additional competing candidate, bounded + by physical capacity and the original admission budgets. Sources must + implement ``cancel(result, *, reason)`` without returning a receipt. """ workload_count: int = 1 @@ -101,6 +105,7 @@ class WorkloadExecutionCfg: settling_steps: int = 5 seed: int = 0 batch_planning: bool = True + reuse_idle_rows: bool = False def __post_init__(self) -> None: for name in ("workload_count", "batch_size", "max_steps"): @@ -111,6 +116,8 @@ def __post_init__(self) -> None: raise ValueError(f"{name} must be a nonnegative integer") if type(self.batch_planning) is not bool: raise TypeError("batch_planning must be a bool") + if type(self.reuse_idle_rows) is not bool: + raise TypeError("reuse_idle_rows must be a bool") @property def num_envs(self) -> int: @@ -160,6 +167,9 @@ class WorkloadSource(Protocol): after all required checks pass and native persistence confirms the write. Callbacks must not advance physics or create another World. A completed group's frozen evidence can be exported independently of the live World. + Idle-row reuse additionally requires ``cancel(result, *, reason) -> None``. + That port retires uncommitted native work and preserves the canonical + program result; administrative cancellation never earns quota. """ def prepare(self, assignments: tuple[WorkloadAssignment, ...]) -> None: @@ -239,6 +249,8 @@ def __init__( if len(self.workloads) != len(workloads) or cfg.workload_count > len(workloads): raise ValueError("workload IDs must be unique and cover every active lane") self.cfg, self.source = WorkloadExecutionCfg(**cfg.to_dict()), source + if cfg.reuse_idle_rows and not callable(getattr(source, "cancel", None)): + raise TypeError("idle-row reuse requires a native source cancellation port") self.checkpoint, self.on_workload_done = checkpoint, on_workload_done self.owner = get_ident() from embodichain.lab.sim import SimulationManager @@ -279,6 +291,8 @@ def __init__( self.hold = self.env.robot.get_qpos(target=True).clone() self.observation = None self.planning_stats: dict[str, int] = {} + self.reused_rows = 0 + self.cancelled = {name: 0 for name in self.workloads} self.torch = torch @property @@ -349,6 +363,36 @@ def _fill(self) -> None: self.started_at.setdefault(name, time.monotonic()) self.warming[row] = {"assignment": assignment, "settled": 0} added.append(assignment) + if self.cfg.reuse_idle_rows: + for row in range(self.cfg.num_envs): + if row in self.live or row in self.warming: + continue + candidates = [] + for name in dict.fromkeys(self.lanes): + if name is None or not self._eligible(name): + continue + counts = self.counts[name] + inflight = self._inflight(name) + failed = counts["attempts"] - counts["committed"] - inflight + demand = ( + self.workloads[name].target_episodes + - counts["committed"] + + failed + ) + if inflight < demand: + candidates.append((inflight, counts["attempts"], name)) + if not candidates: + break + name = min(candidates)[2] + counts = self.counts[name] + assignment = WorkloadAssignment( + name, counts["attempts"], row, self.cfg.seed + counts["attempts"] + ) + counts["attempts"] += 1 + self.started_at.setdefault(name, time.monotonic()) + self.warming[row] = {"assignment": assignment, "settled": 0} + added.append(assignment) + self.reused_rows += 1 if not added: return self.refills += bool(self.live) @@ -439,9 +483,15 @@ def _finish(self, row: int) -> None: (cursor.reason,), (tuple(cursor.results),), ) - receipt = self.source.complete( - WorkloadRowResult(assignment, result, state["start"], end) - ) + row_result = WorkloadRowResult(assignment, result, state["start"], end) + cancelled = state.get("cancel_reason") + if cancelled: + if getattr(self.source, "cancel")(row_result, reason=cancelled) is not None: + raise ValueError("cancelled work cannot earn a receipt") + self.cancelled[assignment.workload_id] += 1 + receipt = None + else: + receipt = self.source.complete(row_result) if receipt is not None: if ( not isinstance(receipt, CommitReceipt) @@ -463,6 +513,41 @@ def _finish(self, row: int) -> None: self.counts[assignment.workload_id]["committed"] += 1 cursor.close() del self.live[row] + name = assignment.workload_id + if ( + receipt is not None + and self.cfg.reuse_idle_rows + and self.counts[name]["committed"] == self.workloads[name].target_episodes + ): + self._cancel_peers(name) + + def _cancel_peers(self, name: str) -> None: + from .program import ProgramBatchResult + + reason = "quota_fulfilled_by_peer" + for state in self.live.values(): + if state["assignment"].workload_id == name and not state.get( + "cancel_reason" + ): + state["cancel_reason"] = reason + cursor = state["cursor"] + # An existing rejection/stop already yielded its next real + # hold during gather. Do not resume and acknowledge it twice. + state["replace_command"] = ( + not cursor.finished + and cursor.rejection is None + and cursor.emergency is None + ) + state["cursor"].abort(reason) + for row, state in tuple(self.warming.items()): + if state["assignment"].workload_id != name: + continue + outcome = ProgramBatchResult((row,), (0,), (False,), (reason,), ((),)) + result = WorkloadRowResult(state["assignment"], outcome, None, None) + if getattr(self.source, "cancel")(result, reason=reason) is not None: + raise ValueError("cancelled preparation cannot earn a receipt") + self.cancelled[name] += 1 + del self.warming[row] def run(self) -> Mapping[str, object]: """Collect until every group meets quota or exhausts its original budget. @@ -525,6 +610,13 @@ def run(self) -> Mapping[str, object]: ) for row in tuple(commands): cursor = self.live[row]["cursor"] + if self.live[row].pop("replace_command", False): + replacement = cursor.next() + if replacement is None: + del commands[row] + self._finish(row) + continue + commands[row] = replacement if ( cursor.length >= self.cfg.max_steps and cursor.emergency is None @@ -644,6 +736,8 @@ def run(self) -> Mapping[str, object]: self.counts[name]["committed"] == spec.target_episodes for name, spec in self.workloads.items() ), + "reused_rows": self.reused_rows, + "cancelled": dict(self.cancelled), "num_envs": self.cfg.num_envs, "workload_count": self.cfg.workload_count, "batch_size": self.cfg.batch_size, diff --git a/tests/lab/expansion/test_session.py b/tests/lab/expansion/test_session.py index d8c802989d..c3301850dc 100644 --- a/tests/lab/expansion/test_session.py +++ b/tests/lab/expansion/test_session.py @@ -59,6 +59,95 @@ def test_old_and_new_session_paths_export_the_identical_class(): assert ExpansionSession.__module__ == "embodichain.lab.expansion.session" +def test_competing_rollouts_keep_one_pending_write_and_confirmed_quota(): + session = ExpansionSession( + ExpansionSessionCfg.from_mapping( + { + "collection": { + "target_committed_episodes": 1, + "max_proposals": 3, + "max_rollout_attempts": 3, + }, + "execution": {"ready_high_watermark": 3, "ready_low_watermark": 0}, + "persistence": {"pending_episode_limit": 2, "pending_max_bytes": 4096}, + } + ) + ) + session.register_case( + SceneCase("case", "initial", "scene", "task", "robot"), + torch.tensor([[-1.0, 1.0]]), + joint_names=("joint",), + ) + passed = ValidationResult( + ( + ValidationCheck("path_collision", "passed"), + ValidationCheck("task_success", "passed"), + ) + ) + episodes = [] + for index in range(2): + identity, _ = session.propose( + "case", + "initial", + source_id="source", + source_revision="v1", + template_id=f"recipe{index}", + operator_id="episode", + ) + session.add_prepared_episode(EpisodeCandidate(identity, "{}"), passed) + if index: + assert ( + session.take_ready( + "case", + "initial", + episode_byte_budget=2048, + candidate_id=identity.candidate_id, + ) + is None + ) + assert ( + session.take_ready( + "case", + "initial", + episode_byte_budget=2048, + candidate_id=identity.candidate_id, + speculative=True, + ) + is not None + ) + session.mark_rollout_started(identity) + episode_id, commit_id = session.episode_ids(identity) + episodes.append( + ExpertEpisode( + identity, + {"joint_positions": torch.tensor([[0.0], [0.1]])}, + torch.tensor([[0.1]]), + torch.tensor([0.0, 0.1]), + "qpos", + passed, + episode_id, + commit_id, + ) + ) + assert session.accept_episode(episodes[0]) + # Even before the writer confirms its receipt, the second qualified row + # cannot reserve or write the only quota slot. + assert not session.accept_episode(episodes[1]) + assert session.snapshot()["counts"]["validated_accepted"] == 1 + episode = episodes[0] + receipt = CommitReceipt( + episode.episode_id, + episode.identity.candidate_id, + episode.identity.attempt_id, + "stored", + episode.commit_id, + "case", + ) + assert session.apply_receipt(receipt) + assert not session.apply_receipt(receipt) + assert session.snapshot()["counts"]["committed"] == 1 + + @pytest.mark.parametrize("legacy", [False, True]) def test_explicit_policy_keeps_legacy_trajectory_dedup_and_generic_episode_credit( legacy, diff --git a/tests/lab/expansion/test_workloads_sim.py b/tests/lab/expansion/test_workloads_sim.py index 419e01dfab..ed2ddaf50b 100644 --- a/tests/lab/expansion/test_workloads_sim.py +++ b/tests/lab/expansion/test_workloads_sim.py @@ -588,6 +588,215 @@ def validator(env, output): assert not SimulationManager._instances +def _reuse_probe(output: str) -> None: + """A real rejected installation opens reuse, then only one receipt wins.""" + import json + import numpy as np + import torch + from embodichain.lab.expansion import ( + ArtifactEpisodeSink, + ExpansionRunCfg, + GymExpansionHostExtensions, + GymExpansionWorkload, + WorkloadExecutionCfg, + run_gym_expansion_workloads, + load_expansion_episode, + ) + from embodichain.lab.expansion.collision import _MeasuredCollision + from embodichain.lab.sim.scene.expansion import SceneVariant + from embodichain.lab.sim.scene.feasibility import ( + JointSceneCandidate, + RobotDeploymentCandidate, + ) + from embodichain.lab.sim import SimulationManager + from embodichain.lab.expansion._planning_batches import PlanningBatches + from embodichain.utils.utility import load_config + import yaml + + torch.set_num_threads(1) + root = Path(__file__).resolve().parents[3] + task = ( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place" + ) + path = Path(output) + path.mkdir(exist_ok=True) + raw = load_config(task / "task.ur5.yaml") + + def absolute(value, base): + if isinstance(value, dict): + return {k: absolute(v, base) for k, v in value.items()} + if isinstance(value, list): + return [absolute(v, base) for v in value] + if isinstance(value, str) and value.endswith((".yaml", ".yml", ".json")): + return str((base / value).resolve()) + return value + + raw = absolute(raw, task) + program = yaml.safe_load((task / "task_program/program.yaml").read_text()) + program["program"]["repeat"]["count"] = 1 + program_path = path / "program.yaml" + program_path.write_text(yaml.safe_dump(program)) + raw["task_program"]["program"] = str(program_path) + deployment = path / "deployment.yaml" + deployment.write_text(yaml.safe_dump(raw)) + + def scenes(env): + nominal = env.robot.get_local_pose(to_matrix=True)[0].cpu().tolist() + + class Source: + def candidate(self, ordinal): + pose = np.array(nominal) + if ordinal == 0: + pose[2, 3] -= 0.6 + elif ordinal == 2: + pose[0, 3] += 0.02 + return JointSceneCandidate( + "retry", + SceneVariant("retry", 2, ordinal, ()), + RobotDeploymentCandidate({env.robot.uid: pose.tolist()}), + "installation", + (), + ) + + return Source() + + cfg = ExpansionRunCfg( + deployment=str(deployment), + output_dir=str(path / "group"), + batch_size=1, + target_episodes=1, + max_attempts=6, + max_steps=2500, + max_wall_time_s=180, + settling_steps=5, + seed=2, + ) + workload = GymExpansionWorkload( + "retry", + cfg, + { + "job_id": "retry", + "semantic_id": "retry", + "execution_id": "retry", + "recipe_revision": "v1", + "input_files": {}, + "runtime_versions": {}, + }, + {}, + GymExpansionHostExtensions(scene_source_factory=scenes), + ) + original_gather = PlanningBatches.gather + boundary = {"injected": False, "executed": False} + + def gather(broker, cursors): + actions = original_gather(broker, cursors) + if not boundary["injected"] and any( + c.finished and c.accepted for c in cursors.values() + ): + for row, cursor in cursors.items(): + if row not in actions or cursor.finished: + continue + # Exercise the real host-stop path before quota completion; + # the controlled administrative stop is not fake physics. + cursor.abort("test_stop_before_peer_quota") + hold = cursor.next() + assert hold is not None + actions[row] = hold + boundary.update( + injected=True, + row=row, + cursor=cursor, + clock=cursor.bridge.clock.step_index, + hold=hold, + ) + break + return actions + + def validator(env, output): + original_step = env.step + + def step(action, *args, **kwargs): + pending = boundary["injected"] and not boundary["executed"] + if pending: + assert boundary["cursor"].bridge.clock.step_index == boundary["clock"] + actual = ( + action.value + if isinstance(action.value, torch.Tensor) + else action.value["qpos"] + ) + expected = boundary["hold"].value + expected = ( + expected if isinstance(expected, torch.Tensor) else expected["qpos"] + ) + torch.testing.assert_close( + actual[boundary["row"]], expected[0], rtol=0, atol=0 + ) + result = original_step(action, *args, **kwargs) + if pending: + boundary["executed"] = True + return result + + env.step = step + return _MeasuredCollision( + env, + str(root / "examples/expansion/scene_robot_feasibility/search.ur5.yaml"), + output, + ) + + PlanningBatches.gather = gather + try: + result = run_gym_expansion_workloads( + (workload,), + WorkloadExecutionCfg( + workload_count=1, + batch_size=2, + max_steps=2500, + settling_steps=5, + seed=2, + reuse_idle_rows=True, + ), + extensions=GymExpansionHostExtensions(validator_factory=validator), + output_dir=path / "execution", + ) + finally: + PlanningBatches.gather = original_gather + assert boundary["injected"] and boundary["executed"] + assert result["complete"] and result["reused_rows"] > 0, result + assert result["counts"]["retry"]["committed"] == 1 + assert result["cancelled"]["retry"] > 0 + assert result["num_envs"] == 2 and result["world"]["world_count"] == 1 + receipts = ArtifactEpisodeSink(path / "group").inspect() + assert len(receipts) == 1 + episode = load_expansion_episode(receipts[0].storage_id) + assert ( + episode.validation.accepted + and len(episode.timestamps) == len(episode.actions) + 1 + ) + records = [ + json.loads(p.read_text()) + for p in (path / "group/attempts").glob("*/attempt.json") + ] + assert next(r for r in records if r["ordinal"] == 0)["status"] in { + "initial_rejected", + "rejected", + } + cancelled = [ + p + for p in (path / "group/attempts").glob("*/attempt.json") + if json.loads(p.read_text())["status"] == "cancelled" + ] + assert cancelled + for entry in cancelled: + record = json.loads(entry.read_text()) + assert record["reason"] == "quota_fulfilled_by_peer" + if "partial_rollout" in record: + with np.load(entry.parent / record["partial_rollout"]["path"]) as data: + assert len(data["qpos"]) == record["steps"] + 1 + assert len(data.get("actions", [])) == record["steps"] + assert not SimulationManager._instances + + def _declarative_probe(output: str) -> None: import json from embodichain.lab.expansion import ( @@ -619,7 +828,7 @@ def _declarative_probe(output: str) -> None: @pytest.mark.requires_sim -@pytest.mark.parametrize("mode", ["executor", "facade", "declarative"]) +@pytest.mark.parametrize("mode", ["executor", "facade", "declarative", "reuse"]) def test_rolling_workloads_keep_native_receipts_and_world_boundaries(tmp_path, mode): result = subprocess.run( [sys.executable, __file__, str(tmp_path), mode], @@ -633,6 +842,9 @@ def test_rolling_workloads_keep_native_receipts_and_world_boundaries(tmp_path, m if __name__ == "__main__": - {"executor": _probe, "facade": _facade_probe, "declarative": _declarative_probe}[ - sys.argv[2] if len(sys.argv) > 2 else "executor" - ](sys.argv[1]) + { + "executor": _probe, + "facade": _facade_probe, + "declarative": _declarative_probe, + "reuse": _reuse_probe, + }[sys.argv[2] if len(sys.argv) > 2 else "executor"](sys.argv[1]) From 4759be0eed03ceb781ad954b57c06fc6276fd8f7 Mon Sep 17 00:00:00 2001 From: matafela Date: Tue, 17 Nov 2026 17:06:25 +0800 Subject: [PATCH 29/45] update --- agent_context/MAP.yaml | 6 +- .../topics/atomic-actions/atomic-actions.md | 104 +++- .../topics/data-pipeline/data-pipeline.md | 2 +- .../topics/env-framework/execution.md | 23 + .../topics/motion-planning/motion-planning.md | 12 +- .../sim-visualization/sim-visualization.md | 5 + .../physics-configuration.md | 9 + embodichain/compute/trajectory/splicing.py | 149 +++++ embodichain/lab/gym/envs/wrapper/replay.py | 95 +++ .../lab/gym/utils/_component_composition.py | 2 +- embodichain/lab/gym/utils/gym_utils.py | 6 + embodichain/lab/scripts/run_task.py | 52 +- embodichain/lab/sim/atomic_skills/fitting.py | 391 ++++++++++-- .../lab/sim/motion/expansion/combined.py | 6 +- embodichain/lab/sim/shapes.py | 22 + embodichain/lab/sim/spawn/_convex_parts.py | 52 ++ embodichain/lab/sim/spawn/descriptors.py | 22 +- embodichain/toolkits/skill_fitting/_bundle.py | 368 +++++++++++ .../toolkits/skill_fitting/_collision.py | 238 +++++++ .../skill_fitting/_collision_worker.py | 48 ++ .../toolkits/skill_fitting/_episode.py | 247 ++++++++ .../toolkits/skill_fitting/_execution.py | 442 +++++++++++++ embodichain/toolkits/skill_fitting/_export.py | 347 +++++++++++ .../toolkits/skill_fitting/_native_export.py | 151 +++++ .../toolkits/skill_fitting/_participants.py | 115 ++++ .../toolkits/skill_fitting/_pipeline.py | 96 ++- embodichain/toolkits/skill_fitting/_robot.py | 18 + embodichain/toolkits/skill_fitting/_scene.py | 589 ++++++++++++++++++ .../skill_fitting/_sequence_expansion.py | 384 ++++++++++++ .../toolkits/skill_fitting/_sequence_trace.py | 122 ++++ embodichain/toolkits/skill_fitting/cli.py | 241 ++++++- .../toolkits/trajectory_annotator/README.md | 139 ++++- .../toolkits/trajectory_annotator/_cli.py | 59 +- .../toolkits/trajectory_annotator/_fitting.py | 212 +++++-- .../_scene_preparation.py | 113 ++++ .../toolkits/trajectory_annotator/_server.py | 66 +- .../toolkits/trajectory_annotator/_viewer.py | 272 ++++++++ .../toolkits/trajectory_annotator/web/app.js | 14 +- .../trajectory_annotator/web/fitting.js | 246 +++++--- .../trajectory_annotator/web/index.html | 24 +- .../trajectory_annotator/web/styles.css | 8 +- examples/toolkits/skill_fitting/README.md | 271 +++++++- .../toolkits/trajectory_annotation/README.md | 23 +- tests/compute/test_trajectory_splicing.py | 21 + tests/gym/envs/test_replay_actions.py | 51 ++ tests/lab/scripts/test_run_task_expansion.py | 76 +++ tests/sim/atomic_skills/test_fitting.py | 190 +++++- tests/sim/spawn/test_descriptors.py | 69 ++ tests/sim/test_cfg.py | 6 + tests/toolkits/test_scene_collision.py | 149 +++++ tests/toolkits/test_scene_skill_fitting.py | 578 +++++++++++++++++ .../toolkits/trajectory_annotator/conftest.py | 70 ++- .../trajectory_annotator/test_annotation.py | 476 ++++++++++++++ .../trajectory_annotator/test_browser.py | 68 +- .../trajectory_annotator/test_viewer.py | 161 +++++ 55 files changed, 7393 insertions(+), 333 deletions(-) create mode 100644 embodichain/lab/sim/spawn/_convex_parts.py create mode 100644 embodichain/toolkits/skill_fitting/_bundle.py create mode 100644 embodichain/toolkits/skill_fitting/_collision.py create mode 100644 embodichain/toolkits/skill_fitting/_collision_worker.py create mode 100644 embodichain/toolkits/skill_fitting/_episode.py create mode 100644 embodichain/toolkits/skill_fitting/_execution.py create mode 100644 embodichain/toolkits/skill_fitting/_export.py create mode 100644 embodichain/toolkits/skill_fitting/_native_export.py create mode 100644 embodichain/toolkits/skill_fitting/_participants.py create mode 100644 embodichain/toolkits/skill_fitting/_scene.py create mode 100644 embodichain/toolkits/skill_fitting/_sequence_expansion.py create mode 100644 embodichain/toolkits/skill_fitting/_sequence_trace.py create mode 100644 embodichain/toolkits/trajectory_annotator/_scene_preparation.py create mode 100644 embodichain/toolkits/trajectory_annotator/_viewer.py create mode 100644 tests/toolkits/test_scene_collision.py create mode 100644 tests/toolkits/test_scene_skill_fitting.py create mode 100644 tests/toolkits/trajectory_annotator/test_viewer.py diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 9af51b0752..90d1a2e29d 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -77,9 +77,10 @@ topics: - embodichain/lab/gym/utils/trajectory_state.py - embodichain/lab/gym/envs/managers/observation_types.py - embodichain/lab/gym/envs/demo.py + - embodichain/lab/gym/envs/wrapper/replay.py - embodichain/lab/gym/utils/profiler.py watch_paths: [embodichain/lab/gym/envs/managers/observation_types.py, embodichain/lab/gym/envs/episode_context.py, embodichain/lab/gym/envs/base_env.py, embodichain/lab/gym/envs/embodied_env.py, embodichain/lab/gym/envs/expert_trajectory.py, - embodichain/lab/gym/envs/demo.py, + embodichain/lab/gym/envs/demo.py, embodichain/lab/gym/envs/wrapper/replay.py, embodichain/lab/gym/utils/, embodichain/lab/gym/envs/task_program/, embodichain/cli/, embodichain/lab/scripts/run_task.py, extensions/embodichain_tasks/, tests/gym/envs/, embodichain/lab/gym/envs/objectives/, embodichain/lab/scripts/evaluate_task_objective.py, tests/lab/scripts/test_evaluate_task_objective.py] @@ -443,7 +444,7 @@ topics: - id: atomic-actions title: Atomic Skills aliases: [atomic skill, atomic skills, atomic action, 原子技能, 原子动作] - keywords: [fit-trajectory, SkillFitCfg, PickUpMotionGoal, SlideMotionGoal, TwistMotionGoal, HandOverMotionGoal, 视频关节轨迹拟合, 轨迹回填, PlanningBudget, PlanningRecovery, ExecutionPlanLifecycle, AtomicSkillEngine, run-atomic-skill, PlanningContext, ExecutionSession, ExecutionRunner, SkillInvocation, + keywords: [fit-trajectory, --skill-sequence, scene-skill-fitting-inputs, SkillFitCfg, PickUpMotionGoal, SlideMotionGoal, TwistMotionGoal, HandOverMotionGoal, 视频关节轨迹拟合, 轨迹回填, PlanningBudget, PlanningRecovery, ExecutionPlanLifecycle, AtomicSkillEngine, run-atomic-skill, PlanningContext, ExecutionSession, ExecutionRunner, SkillInvocation, hand_geometry_cfgs, GraspOpeningTarget, prepare_grasp_query, scoped grasp preparation, RecoveryPolicy, HeldObjectGuardRequest, PhaseEffectGateRequest, articulation_geometry, control-grid retiming, AffordanceSamplingContext, AffordancePoseCandidates, AffordanceSample, affordance sampling] @@ -453,6 +454,7 @@ topics: - embodichain/lab/sim/atomic_skills/fitting.py - embodichain/toolkits/skill_fitting/ - embodichain/toolkits/trajectory_annotator/_fitting.py + - embodichain/toolkits/trajectory_annotator/_viewer.py - embodichain/toolkits/trajectory_annotator/web/fitting.js - embodichain/data_pipeline/skill_fitting.py - embodichain/lab/sim/atomic_skills/core.py diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 27a241daf1..d1f77094b7 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -154,29 +154,101 @@ contracts through the same nominal-source and runner interfaces. Its internal events do not create independent recovery boundaries. Video-selected reconstruction enters through `embodichain fit-trajectory` -(`toolkits/skill_fitting/cli.py`) or the annotation website's selected-interval +(`toolkits/skill_fitting/cli.py`) or the annotation website's whole-trajectory fitting panel (`toolkits/trajectory_annotator/_fitting.py`). Both hosts reuse `toolkits/skill_fitting/_pipeline.py`; the web host retains one background job and result, keeps decoding on its existing owner, and invalidates results after annotation revisions change. Annotation choices cover all built-in skills without importing simulation; only supported reconstruction types enter fitting. A saved optional core interval separates loose annotation boundaries from reconstruction, -preserving surrounding motion; DemoSkill labels do not create executable assets. -Its bounded joint/FK preview follows video time; -the downloadable NPZ retains full samples. See the +preserving surrounding motion. `_episode.py` fits each annotation independently +from the immutable recording, combines only its core, and partitions every +remaining source sample into DemoSkill spans. Unsupported authored skills fail +explicitly. The CLI selects this path with `--all-segments --annotations ...`; +its existing single-segment mode remains available. +Its bounded joint charts follow video time. `_viewer.py` owns a local Viser worker +with independent original-material and translucent-blue URDF models; playback +samples all recorded joints on the full clock, including explicit mimic columns. +Model bases use the profile calibration relative to each solver's URDF root, +without TCP offsets. One latest-time slot bounds playback work; replacing the fit +or closing the host stops its viewer. The downloadable NPZ retains full samples. +`_export.py` packages accepted episodes for `run-task --skill-sequence skills.yaml`. +`_native_export.py` writes object-bound Pickup goals, actual participant bindings, +native options/policy and a sealed local resource inventory. `_execution.py` is a +direct AtomicSkillEngine caller: each native invocation plans from measured robot +state and current object pose, then submits ControllerAction commands through Gym. +Only explicitly bound motion/grasp parts take new native-plan commands. Unbound +joints retain the previous commanded targets, not measured positions: a loaded +opposite gripper must keep its closing preload. Demo spans retain full source +command ownership, including deliberate releases. Grasp command/TCP calibration +belongs to the input robot profile; export copies solver TCPs into the Gym robot. +It is not a Task Program lowerer. Only DemoSkill gaps carry original DOF-by-frame +PT matrices and source timestamps; there is no atomic/full-episode trajectory PT. +The host resamples demo clocks and inserts derivative-bounded quintic connections +between adjacent postures. Its execution report separates command completion from +measured object heights and leaves physical success unqualified. +`_sequence_expansion.py` accepts a matching Combined Expansion Profile with +`source.kind=skill_sequence`. This B=1 fixed-scene host applies shared +`atomic_skills/_plan_expansion.py` transforms only to native spans; DemoSkill +source tensors and resampled command paths are excluded. The timeline indices +include every span. Native free-phase permissions, bound motion joints and +protected boundary stencils constrain perturbations. Each recipe resets physics, +and rejected proposals retry without nominal substitution. `_sequence_trace.py` +streams T submitted commands and T+1 measured robot/object states into bounded +HDF5 chunks. These artifacts record command completion with unknown physical +success, not verified Expansion Engine dataset receipts. Editable `expansion.yaml` +is separate from the sealed physical inventory. The +[fitting example](../../../examples/toolkits/skill_fitting/README.md) owns commands +and profile settings. +`_bundle.py` owns `scene-skill-fitting-inputs/v1`: local scene meshes/colliders and +explicit poses, calibrated robot model/profile, original joints, annotations and +frame presentation clock. `fit-trajectory --bundle` requires no image pixels or +original exporter directory; `--package-output` creates portable input resources. +Optional `frame_source` adds a content-bound local video for +`annotate-trajectory --bundle`. This entry resolves all fitting defaults from the +same inventory and persists mutable `annotations.edited.json` separately from +the sealed annotation seed. `--package-video` creates video-enabled resources; +the web fit and exported native runtime need no original recording directory. +`_collision.py` prepares content-addressed scene convex parts; web import and +packaging supply precomputed files for decomposition-free runtime startup. +Scene-owned Z compensation precedes fitting; the existing pickup interpolation +factor becomes native duration scaling at an unchanged control cadence. +`RobotProfile.simulation` carries explicit physics overlays without changing +kinematic calibration or custom control names. See the [web example](../../../examples/toolkits/trajectory_annotation/README.md). -`atomic_skills/fitting.py` identifies explicit -world-TCP motion goals for PickUp, Slide, Twist and HandOver, then calls their -ordinary registered planners. Motion goals carry no object identity and emit -empty effects; they cannot establish an attachment or physical success. Native -generation remains independent of the source joint samples. A source-derived -monotone phase clock and bounded boundary correction are qualified separately; -rejected bundles retain candidates while leaving the patched reference unchanged. -The host supplies a named robot profile and an offline FK/IK port, without -constructing a simulator. Annotation source identity, frame endpoint conversion, -joint semantics and clock offsets remain explicit. Automatic video contact -recognition is not provided. The [runnable guide](../../../examples/toolkits/skill_fitting/README.md) -owns supported phase partitions, calibration and standalone result arrays. +`toolkits/skill_fitting/_scene.py` grounds reconstructed local meshes in explicit +world poses and obtains calibrated, finite-surface-qualified antipodal grasps. +Single-arm scene fitting ranks valid TCP poses against the recorded grasp, then +`atomic_skills/fitting.py` calls the ordinary native planner. PickUp uses an +object-bound `GraspGoal`; Slide/Twist retain their motion/axis contracts with the +selected grasp anchor. Parallel-hand alternatives apply an exact half turn in +local TCP Z; contact-frame calibration is conjugated into the physical surface +query so tilted/offset virtual TCPs do not change the rotation axis. Both variants +must pass target-surface checks before ranking. Fitting uses a wider configurable +approach angle and disables the inferred support-plane filter by default; +these host policies do not change native affordance defaults or certify table +clearance. PickUp approaches along the selected grasp's Z. Source motion supplies approach, phase clocks and motion +parameters rather than pointwise acceptance targets. Quintic free-phase bridges +preserve the outer derivative stencils and the contact core. Pickup may rebalance +its free-phase clock inside the unchanged outer interval; explicit phase hints +remain fixed and observed phases remain in provenance. Qualification covers +every changed joint/derivative stencil, while pre-existing source-limit violations +remain separately visible. Rejected results retain candidates without replacing +the original reference. The native fitter does not execute physics; the host +exports the native invocation deployment described above. + +`_participants.py` pairs configured motion/hand groups through URDF ancestry and +selects a unique closing hand; ambiguous evidence requires an explicit choice. +Names do not establish capabilities. A profile must supply explicit runtime TCP +calibration and actual finger collision assets; automatic scene geometry supports +opposing prismatic fingers. Without a scene, the effect-free world-TCP motion +fitter remains available for PickUp, Slide, Twist and explicitly bound HandOver. +Scene HandOver needs time-resolved object/attachment bindings and is not inferred +from an initial object pose. Both paths retain explicit source identity, inclusive +frame conversion, joint semantics and clock offsets. Automatic RGB contact +recognition and physical-success certification are absent. The +[runnable guide](../../../examples/toolkits/skill_fitting/README.md) owns scene +formats, calibration conventions, phase partitions and result arrays. PickUp, MoveHeldObject and Place can also assemble a pre-planning nominal motion source. The source does not own semantic effects or environment lifecycle; see diff --git a/agent_context/topics/data-pipeline/data-pipeline.md b/agent_context/topics/data-pipeline/data-pipeline.md index 87bafce17a..7d562726b1 100644 --- a/agent_context/topics/data-pipeline/data-pipeline.md +++ b/agent_context/topics/data-pipeline/data-pipeline.md @@ -39,7 +39,7 @@ planning do not establish collision freedom or physical task success. See video-to-joint clock conversion and standalone numeric/JSON reconstruction bundles. It does not relax Real2Sim's object-reference requirements. The [Atomic Skills overview](../atomic-actions/atomic-actions.md) owns the fitting -entry point and the effect-free motion-goal boundary. +entry point and the scene-grounded versus effect-free motion-only boundaries. Read [online sampling](online-sampling.md) for worker states, refill, shared errors, action schemas, sample provenance, causal histories and example-runner diff --git a/agent_context/topics/env-framework/execution.md b/agent_context/topics/env-framework/execution.md index 831d980574..88a54e3ecf 100644 --- a/agent_context/topics/env-framework/execution.md +++ b/agent_context/topics/env-framework/execution.md @@ -304,6 +304,23 @@ not orchestrate multiple experiments or qualify kinematic playback. accepted. Objective-enabled dynamic replay requires one row and a full reset. `control` exposes `go_to_step(step)` for O(1) scrubbing. The replay env must use the same robot/objects/`actions` config as the recording env. + External expert recordings may declare `meta.state_joint_names` for the full + state columns. Replay verifies the exact robot and active joint sets, then + maps state qpos/qvel and explicit expert commands into native order. Missing, + duplicate or mismatched joints reject before stepping. Legacy files without + this field retain numeric-layout checks; raw policy actions cannot be remapped + as joint commands. Legacy fitted replay packages remain readable through this path. +- **Native fitted sequences**: `run-task --skill-sequence skills.yaml` is a separate + direct AtomicSkillEngine host in `toolkits/skill_fitting/_execution.py`. It uses + the plain physical Gym deployment, replans native goals from live state and + submits ControllerAction through `env.step`; only source DemoSkill gaps use PT. + It cannot combine with replay or Task Program ownership. A matching + `--expansion-profile` delegates fixed-scene B=1 spatial variants to + `toolkits/skill_fitting/_sequence_expansion.py`; DemoSkill paths stay unchanged. + Execution + completion/observed heights are recorded separately from physical success. + The portable input/output contract is owned by + [Atomic Skills](../atomic-actions/atomic-actions.md). - **Policy projection**: `rollout_buffer` retains actual policy observations, including sampled noise, and its existing obs/action/reward alignment for LeRobot/RL. It shares the acquisition boundary with raw history, rather than @@ -315,6 +332,12 @@ not orchestrate multiple experiments or qualify kinematic playback. policy action space; a controller/expert trajectory uses the private `ExpertActionSpec` layout. One trajectory cannot mix these schemas. - **CLI**: `run-task --replay --replay_trajectory --replay_mode {kinematic,dynamic,control}`. + Automatic `kinematic` / `dynamic` replay runs without wall-clock pacing and + closes the viewer on completion. For persistent inspection use `control`: + `--headless --viser` exposes a frame slider, and `--no-headless` without + `--viser` opens the native window even when the file sets `headless: true`. + Control replay starts paused; terminal `a` plays at the control interval, + `n` / `p` step, a number plus Enter seeks, `r` resets, and `q` exits. --- diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index eada23644e..72cb2f01d4 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -134,11 +134,14 @@ solver-dependent pose/IK adaptation and compatibility re-exports. Trapezoidal/ Double-S Warp profiles live in `compute/kinematics/_warp/trapezoidal.py`; compute must not import simulation objects. -Offline atomic reconstruction uses `compute.trajectory.phase_alignment` for +Motion-only atomic reconstruction uses `compute.trajectory.phase_alignment` for bounded monotone native-path clock fitting and `splicing` for fixed-clock joint replacement. Callers retain required anchors and qualify TCP constraints and joint derivatives after mapping and correction. The discrete boundary-stencil -contract is not continuous-time smoothness or collision evidence. An explicitly +contract is not continuous-time smoothness or collision evidence. Scene-grounded +fitting retains the outer clock and native contact core through free-phase +connections; its ownership and qualification scope live in the +[Atomic Skills overview](../atomic-actions/atomic-actions.md). An explicitly injected planner/robot port bypasses simulator lookup; ordinary construction continues to resolve the simulator-owned robot. @@ -206,7 +209,10 @@ expansion: they decode the combined profile, enumerate reference-family, affordance and trajectory recipes, assign one visual profile per episode, and reserve generic compatible slots. `schedule_digest` provides deterministic rerun identity; restoration, measured acceptance, and artifact writing remain -host responsibilities. A profile that declares a conservative +host responsibilities. `source.kind` selects `task_program` or `skill_sequence`; +the native sequence host's supported factors and DemoSkill exclusion are owned by +[Atomic Skills](../atomic-actions/atomic-actions.md). +A profile that declares a conservative `trajectory.spatial.joint_offset_scale` also names a smaller `trajectory.spatial.dedup_tolerance`, which the Task Program integration forwards as the augmentation coverage tolerance; omitting it keeps the contract diff --git a/agent_context/topics/sim-visualization/sim-visualization.md b/agent_context/topics/sim-visualization/sim-visualization.md index 181d834a91..5b51665a2d 100644 --- a/agent_context/topics/sim-visualization/sim-visualization.md +++ b/agent_context/topics/sim-visualization/sim-visualization.md @@ -27,6 +27,11 @@ Human-facing behavior and usage are documented in `docs/source/overview/sim/visualization/index.rst`. +Offline annotation's dual-robot Viser playback lives in +`toolkits/trajectory_annotator/_viewer.py`, outside `SimulationManager`. +See [video-selected skill fitting](../atomic-actions/atomic-actions.md) for its +calibration, full-recording clock, and session-owned lifecycle. + ## Ownership and Data Flow ``` diff --git a/agent_context/topics/simulation-system/physics-configuration.md b/agent_context/topics/simulation-system/physics-configuration.md index f3408e77a7..034eaf18e4 100644 --- a/agent_context/topics/simulation-system/physics-configuration.md +++ b/agent_context/topics/simulation-system/physics-configuration.md @@ -80,6 +80,15 @@ imported articulation geometry retains its source approximation. Parser compatibility and current algorithm defaults stay in `shapes.py`, `cfg/rigid_object.py` and `spawn/descriptors.py`, not duplicated here. +For `convex_decomposition`, optional `decomposition_fpath` supplies an OBJ with +one object/group per precomputed convex piece, in the unscaled source-mesh local +frame. `spawn/_convex_parts.py` validates the parts; the rigid compiler emits +separate convex-hull colliders with the declared physics and body scale while +retaining the original visual mesh. Missing/invalid files fail without recooking. +The deployment resource resolver treats this file as an owner-relative asset. +The fitting toolkit prepares and bundles these files before run-task starts; +its cache does not replace ordinary native convex-shape initialization. + Portable `contact_offset/rest_offset` becomes Newton margin/gap: `rest_offset → margin`, `contact_offset - effective_margin → gap`. Native margin/gap take precedence; an ambiguous standalone contact offset is rejected diff --git a/embodichain/compute/trajectory/splicing.py b/embodichain/compute/trajectory/splicing.py index 8f89c57a48..b0ab7bdd78 100644 --- a/embodichain/compute/trajectory/splicing.py +++ b/embodichain/compute/trajectory/splicing.py @@ -132,6 +132,155 @@ def splice_joint_segment( return result +def _connect_joint_segment( + original: np.ndarray, + replacement: np.ndarray, + timestamps: np.ndarray, + *, + start: int, + stop: int, + joint_indices: tuple[int, ...], + protected: tuple[int, int], + blend_samples: int, + velocity_limits: np.ndarray, + acceleration_limits: np.ndarray, +) -> NDArray[np.floating]: + """Join a native path with quintic bridges, leaving its contact core exact. + + Three original rows at each join retain the original discrete derivative + stencils. Unlike a pointwise blend, the bridges do not inherit interior + measurement noise. ``protected`` uses segment-relative half-open bounds. + """ + q, t = _path(original, timestamps) + candidate = np.asarray(replacement) + count = stop - start + first, last = protected + if ( + not 0 <= start < stop <= len(q) + or candidate.shape != (count, q.shape[1]) + or not np.isfinite(candidate).all() + or not 6 <= first < last <= count - 6 + or blend_samples < 4 + or not joint_indices + or len(set(joint_indices)) != len(joint_indices) + or any(not 0 <= j < q.shape[1] for j in joint_indices) + ): + raise ValueError( + "Native contact core needs room for both boundary connections." + ) + ids = list(joint_indices) + result = q.copy() + result[start:stop, ids] = candidate[:, ids] + segment = result[start:stop, ids].astype(np.float64) + source = q[start:stop, ids].astype(np.float64) + clock = t[start:stop] + + def derivatives( + values: np.ndarray, indices: np.ndarray, at: int + ) -> tuple[np.ndarray, np.ndarray]: + offset = clock[indices] - clock[at] + polynomial = np.linalg.solve( + np.column_stack((np.ones(3), offset, offset**2)), values[indices] + ) + return polynomial[1], 2 * polynomial[2] + + def bridge(a: int, b: int, left_source: bool) -> None: + duration = clock[b] - clock[a] + if left_source: + p0, p1 = source[a], segment[b] + v0, a0 = derivatives(source, np.arange(a - 2, a + 1), a) + v1, a1 = derivatives(segment, np.arange(b - 1, b + 2), b) + else: + p0, p1 = segment[a], source[b] + v0, a0 = derivatives(segment, np.arange(a - 1, a + 2), a) + v1, a1 = derivatives(source, np.arange(b, b + 3), b) + c0, c1, c2 = p0, v0 * duration, a0 * duration**2 / 2 + rhs = np.stack( + (p1 - c0 - c1 - c2, v1 * duration - c1 - 2 * c2, a1 * duration**2 - 2 * c2) + ) + c3, c4, c5 = np.linalg.solve( + np.array([[1, 1, 1], [3, 4, 5], [6, 12, 20]], dtype=float), rhs + ) + u = ((clock[a : b + 1] - clock[a]) / duration)[:, None] + segment[a : b + 1] = c0 + u * (c1 + u * (c2 + u * (c3 + u * (c4 + u * c5)))) + + def boundary_index(indices: range, left: bool) -> int: + # Preserve noisy measured boundary samples and start the connection at + # a quieter stencil inside the free phase. Do not propagate boundary + # noise into a long quintic polynomial or alter an unrelated interval. + best, best_score = indices[0], float("inf") + for index in indices: + stencil = ( + np.arange(index - 2, index + 1) if left else np.arange(index, index + 3) + ) + velocity, acceleration = derivatives(source, stencil, index) + score = max( + float(np.max(np.abs(velocity) / velocity_limits[ids])), + float(np.max(np.abs(acceleration) / acceleration_limits[ids])), + ) + if score < 0.25: + return index + if score < best_score: + best, best_score = index, score + return best + + a = boundary_index(range(2, max(3, first // 3)), True) + b = boundary_index( + range(count - 3, min(count - 4, last + 2 * (count - last) // 3), -1), False + ) + left_end = min(first - 1, max(a + blend_samples, (a + first) // 2)) + right_start = max(last, min(b - blend_samples, last + (b - last) // 4)) + bridge(a, left_end, True) + bridge(right_start, b, False) + segment[: a + 1] = source[: a + 1] + segment[b:] = source[b:] + result[start:stop, ids] = segment.astype(q.dtype) + result[start : start + 3, ids] = q[start : start + 3, ids] + result[stop - 3 : stop, ids] = q[stop - 3 : stop, ids] + return result + + +def _replacement_diagnostics( + original: np.ndarray, + proposed: np.ndarray, + timestamps: np.ndarray, + *, + lower: np.ndarray, + upper: np.ndarray, + velocity_limits: np.ndarray, + acceleration_limits: np.ndarray, +) -> dict[str, object]: + """Qualify every changed position/edge/acceleration stencil, including joins. + + Unchanged source data may already violate limits. It remains reported by + ``joint_path_diagnostics`` and cannot be repaired by editing another arm or + another interval. This check never excuses a newly generated violation. + """ + q, t = _path(proposed, timestamps) + changed = q != original + velocity_mask = changed[1:] | changed[:-1] + acceleration_mask = velocity_mask[1:] | velocity_mask[:-1] + dt = np.diff(t) + velocity = np.diff(q.astype(float), axis=0) / dt[:, None] + acceleration = np.diff(velocity, axis=0) / ((dt[1:] + dt[:-1]) * 0.5)[:, None] + peak_v = np.max(np.where(velocity_mask, np.abs(velocity), 0), axis=0) + peak_a = np.max(np.where(acceleration_mask, np.abs(acceleration), 0), axis=0) + positions = changed & ((q < lower - 1e-6) | (q > upper + 1e-6)) + return { + "accepted": bool( + not positions.any() + and np.all(peak_v <= velocity_limits * (1 + 1e-5)) + and np.all(peak_a <= acceleration_limits * (1 + 1e-5)) + ), + "position_violations": int(positions.sum()), + "peak_velocity": peak_v.tolist(), + "peak_acceleration": peak_a.tolist(), + "velocity_ratio": float(np.max(peak_v / velocity_limits)), + "acceleration_ratio": float(np.max(peak_a / acceleration_limits)), + "scope": "modified positions and their velocity/acceleration stencils", + } + + def joint_path_diagnostics( qpos: np.ndarray, timestamps: np.ndarray, diff --git a/embodichain/lab/gym/envs/wrapper/replay.py b/embodichain/lab/gym/envs/wrapper/replay.py index f9766e26b4..140aebf964 100644 --- a/embodichain/lab/gym/envs/wrapper/replay.py +++ b/embodichain/lab/gym/envs/wrapper/replay.py @@ -55,6 +55,9 @@ class ReplayWrapper(gym.Wrapper): Expert recordings carry the canonical schema so qpos/qvel targets can be reconstructed before stepping. Schema metadata alone cannot identify controller actions: older position-only raw policy recordings carry it too. + External recordings may declare ``meta.state_joint_names`` to align their + complete state and explicit expert commands with the native robot's order. + Joint sets must match exactly; raw policy commands are not reinterpreted. Physical-objective dynamic replay currently supports one environment and full resets only. Stepping past exhaustion raises instead of extending @@ -81,6 +84,7 @@ def __init__( ) self._mode = mode self._trajectory = load_trajectory(trajectory) + self._align_named_joint_layout() meta = self._trajectory["meta"] self._controller_spec = None self._task_outcome_replay = mode == "dynamic" and ( @@ -137,6 +141,97 @@ def __init__( self.env.num_envs, dtype=torch.long, device=self.env.device ) + def _align_named_joint_layout(self) -> None: + """Align explicitly named external recordings to native state/action order. + + Legacy recordings without ``state_joint_names`` retain their strict + numeric layout. Named recordings must describe the exact robot and + active joint sets; names cannot authorize a missing or extra actuator. + """ + data = self._trajectory + meta = data["meta"] + if "state_joint_names" not in meta: + return + names = meta["state_joint_names"] + native = list(self.env.robot.joint_names) + if ( + not isinstance(names, (list, tuple)) + or any(not isinstance(n, str) for n in names) + or len(set(names)) != len(names) + or set(names) != set(native) + or len(names) != self.env.robot.dof + or meta.get("robot_dof") != len(names) + ): + raise ValueError( + "Trajectory state_joint_names must match the complete robot joint set." + ) + ids = meta.get("active_joint_ids", []) + if ( + not isinstance(ids, (list, tuple)) + or any(type(i) is not int or not 0 <= i < len(names) for i in ids) + or len(set(ids)) != len(ids) + ): + raise ValueError( + "Trajectory active_joint_ids must be unique state-column indices." + ) + source_active = [names[i] for i in ids] + target_active = [native[i] for i in self.env.active_joint_ids] + if set(source_active) != set(target_active) or len(source_active) != len( + target_active + ): + raise ValueError( + "Trajectory active joint names differ from the replay environment." + ) + state_order = [names.index(n) for n in native] + states = data["states"].clone() + if "robot" not in states.keys() or "qpos" not in states["robot"].keys(): + raise ValueError("Named trajectory requires complete robot qpos states.") + for key in ("qpos", "qvel", "qf"): + if key in states["robot"].keys(): + value = states["robot"][key] + if value.shape != (*states.batch_size, len(names)): + raise ValueError( + f"Named trajectory robot {key} does not match its state layout." + ) + states["robot"][key] = value[..., state_order] + actions = data["actions"] + metadata = dict(meta) + if meta.get("action_kind") in {"expert", "expert_controller"}: + spec = build_expert_action_spec( + joint_names=source_active, + joint_command_mode=meta.get("joint_command_mode"), + ) + expected = spec.metadata(step_dt=meta.get("step_dt", 0)) + if ( + any( + meta.get(k) != expected[k] + for k in ("joint_names", "qpos_slice", "qvel_slice") + ) + or actions.ndim != 3 + or actions.shape[-1] != spec.width + ): + raise ValueError( + "Named expert trajectory action schema does not match its source joint order." + ) + order = [source_active.index(n) for n in target_active] + if spec.qvel_slice is not None: + order += [i + len(source_active) for i in order] + actions = actions[..., order] + metadata["joint_names"] = target_active + elif self._mode == "dynamic" and source_active != target_active: + raise ValueError( + "Raw policy actions cannot be reordered by joint names; use explicit expert targets." + ) + metadata.update( + state_joint_names=native, active_joint_ids=list(self.env.active_joint_ids) + ) + self._trajectory = { + **data, + "states": states, + "actions": actions, + "meta": metadata, + } + def _validate_action_schema(self, meta: dict) -> None: """Validate explicit controller recordings before any simulation step.""" kind = meta.get("action_kind", "policy") diff --git a/embodichain/lab/gym/utils/_component_composition.py b/embodichain/lab/gym/utils/_component_composition.py index e4150a0986..641c51acdc 100644 --- a/embodichain/lab/gym/utils/_component_composition.py +++ b/embodichain/lab/gym/utils/_component_composition.py @@ -153,7 +153,7 @@ def _resolve_local_resource_paths(value: object, *, base_dir: Path) -> None: if isinstance(value, dict): for key, child in value.items(): if ( - key in {"fpath", "urdf_path", "xml_path"} + key in {"fpath", "urdf_path", "xml_path", "decomposition_fpath"} and isinstance(child, str) and child.startswith(("./", "../")) ): diff --git a/embodichain/lab/gym/utils/gym_utils.py b/embodichain/lab/gym/utils/gym_utils.py index e8965a70cc..14ecc68f32 100644 --- a/embodichain/lab/gym/utils/gym_utils.py +++ b/embodichain/lab/gym/utils/gym_utils.py @@ -683,6 +683,12 @@ def resolve_asset_paths( asset.shape.fpath = get_data_path(path) elif isinstance(asset, ArticulationCfg) and path is not None: asset.fpath = get_data_path(path) + if isinstance(getattr(asset, "shape", None), MeshCfg): + collision = asset.shape.collision + if collision is not None and collision.decomposition_fpath is not None: + collision.decomposition_fpath = get_data_path( + collision.decomposition_fpath + ) @configclass class ComponentCfg: diff --git a/embodichain/lab/scripts/run_task.py b/embodichain/lab/scripts/run_task.py index a939b1be56..436031ec86 100644 --- a/embodichain/lab/scripts/run_task.py +++ b/embodichain/lab/scripts/run_task.py @@ -1447,6 +1447,21 @@ def replay_control(replay_env: ReplayWrapper) -> None: def main(args: Any, env: Any, gym_config: dict[str, Any]) -> None: """Run the selected workflow without taking ownership of ``env``.""" + if getattr(args, "skill_sequence", None): + from embodichain.toolkits.skill_fitting._execution import execute_sequence + + if ( + getattr(args, "expansion_profile", None) is not None + or "expansion" in gym_config + ): + from embodichain.toolkits.skill_fitting._sequence_expansion import ( + run_sequence_expansion, + ) + + run_sequence_expansion(env, args, gym_config) + else: + execute_sequence(env, Path(args.skill_sequence)) + return if getattr(args, "replay", False): log_info("Replay mode.", color="green") replay( @@ -1653,6 +1668,11 @@ def _create_parser() -> argparse.ArgumentParser: add_env_launcher_args_to_parser(parser, require_gym_config=True) parser.set_defaults(viser_image_fps=None) + parser.add_argument( + "--skill-sequence", + type=str, + help="Execute a fitted native-skill sequence with recorded qpos gaps (no replay trajectory).", + ) parser.add_argument( "--task-program", @@ -1800,6 +1820,12 @@ def cli(argv: Sequence[str] | None = None) -> None: torch.set_printoptions(precision=5, sci_mode=False) args = _create_parser().parse_args(argv) + if getattr(args, "skill_sequence", None) and ( + args.replay or args.preview or args.task_program or args.real2sim_profile + ): + raise ValueError( + "--skill-sequence cannot be combined with replay, preview or Task Program." + ) if getattr(args, "replay", False): if not args.replay_trajectory: @@ -1816,6 +1842,12 @@ def cli(argv: Sequence[str] | None = None) -> None: execute_init_hooks() env_cfg, gym_config, action_config = build_env_cfg_from_args(args) + if getattr(args, "skill_sequence", None) and any( + k in gym_config for k in ("task_program", "real2sim") + ): + raise ValueError( + "Native fitted sequences require a plain physical Gym configuration." + ) real2sim_profile = None real2sim_profile_path = getattr(args, "real2sim_profile", None) @@ -1860,7 +1892,25 @@ def cli(argv: Sequence[str] | None = None) -> None: or "expansion" in gym_config ): expansion_request = _resolve_expansion_request(args, gym_config) - if expansion_request is not None: + if expansion_request is not None and getattr(args, "skill_sequence", None): + from embodichain.toolkits.skill_fitting._sequence_expansion import ( + validate_sequence_expansion, + validate_sequence_collection, + ) + + validate_sequence_expansion( + expansion_request[0], + Path(args.skill_sequence).resolve(strict=True), + env_cfg.num_envs, + ) + validate_sequence_collection( + args, + gym_config, + expansion_request[0], + expansion_request[1], + expansion_request[3], + ) + elif expansion_request is not None: from embodichain.lab.sim.motion.expansion import CombinedExpansionProfile profile = expansion_request[0] diff --git a/embodichain/lab/sim/atomic_skills/fitting.py b/embodichain/lab/sim/atomic_skills/fitting.py index 53c51100f8..281fac14ca 100644 --- a/embodichain/lab/sim/atomic_skills/fitting.py +++ b/embodichain/lab/sim/atomic_skills/fitting.py @@ -28,6 +28,8 @@ from embodichain.compute.trajectory.phase_alignment import align_joint_phase from embodichain.compute.trajectory.splicing import ( + _connect_joint_segment, + _replacement_diagnostics, joint_path_diagnostics, splice_joint_segment, ) @@ -36,6 +38,7 @@ from .control import OPEN_COMMAND, GRASP_COMMAND from .policies import MotionPolicy, RecoveryPolicy from .primitives import ( + GraspGoal, PickUpMotionGoal, PickUpOptions, SlideMotionGoal, @@ -68,20 +71,41 @@ class SkillFitCfg: max_contact_error_m: float = 0.005 max_contact_rotation_rad: float = 0.03 min_twist_angle_rad: float = 0.05 + grasp_sample_count: int = 4000 + grasp_max_candidates: int = 64 + grasp_rotation_weight_m: float = 0.05 + grasp_approach_deviation_rad: float = np.pi / 3 + grasp_filter_ground_collision: bool = False + max_grasp_distance_m: float = 0.2 + grasp_contact_inset_m: float = 0.004 def __post_init__(self) -> None: for key, value in asdict(self).items(): + if key == "grasp_filter_ground_collision": + if type(value) is not bool: + raise ValueError("grasp_filter_ground_collision must be a boolean.") + continue if isinstance(value, bool) or not np.isfinite(value) or value <= 0: raise ValueError(f"SkillFitCfg.{key} must be finite and positive.") - for key in ("blend_samples", "max_samples", "max_candidates"): + for key in ( + "blend_samples", + "max_samples", + "max_candidates", + "grasp_sample_count", + "grasp_max_candidates", + ): if type(getattr(self, key)) is not int: raise ValueError(f"{key} must be an integer.") if ( self.max_candidates > 9 or self.max_samples > 200_000 or self.blend_samples < 4 + or self.grasp_sample_count > 100_000 + or self.grasp_max_candidates > 256 ): raise ValueError("Fitting budgets are outside supported bounds.") + if self.grasp_approach_deviation_rad > np.pi / 2: + raise ValueError("grasp_approach_deviation_rad must not exceed pi/2.") def _transitions( @@ -111,8 +135,36 @@ def _rot_error(left: np.ndarray, right: np.ndarray) -> np.ndarray: ).magnitude() -def _approach(poses: np.ndarray, close_start: int) -> tuple[np.ndarray, float, int]: +def _approach( + poses: np.ndarray, close_start: int, *, relaxed: bool = False +) -> tuple[np.ndarray, float, int]: grasp = poses[close_start] + if relaxed: + # Real approach motion may rotate and contain measurement noise. Estimate + # its final direction over a finite window, rather than demanding that + # multiple observed rotations coincide to 3e-5 radians. + if close_start < 6: + raise ValueError("Select an approach before the closing event.") + index = max(1, close_start // 2) + offset = grasp[:3, 3] - np.median( + poses[max(0, index - 2) : index + 3, :3, 3], axis=0 + ) + distance = float(np.linalg.norm(offset)) + if distance < 1e-4: + raise ValueError("Approach displacement is not observable.") + direction = offset / distance + # A curved reach is not its final contact approach. Prefer the robust + # calibrated TCP +Z near closure when it agrees with the observed + # displacement within 45 degrees; retain displacement for other TCP + # conventions or a genuinely side-on reach. + axis = np.median( + poses[max(0, close_start - 4) : close_start + 1, :3, 2], axis=0 + ) + axis /= np.linalg.norm(axis) + if np.dot(axis, direction) >= np.sqrt(0.5): + direction = axis + distance = float(np.dot(offset, direction)) + return direction, distance, index candidates = np.flatnonzero(_rot_error(poses[:close_start], grasp) < 0.00003) if len(candidates) < 2: raise ValueError("No stable TCP orientation before grasp; provide phase hints.") @@ -159,6 +211,7 @@ def _phase_map( phases: list[dict[str, Any]], observations: np.ndarray, approach_anchor: tuple[int, int] | None = None, + source_times: np.ndarray | None = None, ) -> tuple[np.ndarray, np.ndarray]: if list(native_lengths) != [p["name"] for p in phases]: raise ValueError( @@ -174,7 +227,19 @@ def _phase_map( start, stop = phase["start"], phase["stop"] if stop <= start: raise ValueError("Every source phase must contain a sample.") - if phase["name"] == "approach" and approach_anchor is not None: + if source_times is not None: + clock = source_times[start:stop] + progress = ( + (clock - clock[0]) / (clock[-1] - clock[0]) + if len(clock) > 1 + else np.zeros(1) + ) + native_clock = np.linspace(0, 1, len(part)) + output[start:stop] = np.column_stack( + [np.interp(progress, native_clock, column) for column in part.T] + ) + timing[start:stop] = progress + elif phase["name"] == "approach" and approach_anchor is not None: source_anchor, native_anchor = approach_anchor if not start < source_anchor < stop or not 0 < native_anchor < count - 1: raise ValueError("Pre-grasp anchor must be inside its approach phase.") @@ -214,8 +279,9 @@ def fit_skill_segment( acceleration_limits: np.ndarray, cfg: SkillFitCfg | None = None, phase_hints: list[dict[str, Any]] | None = None, + scene_grasps: tuple[GraspGoal, ...] | None = None, ) -> tuple[dict[str, np.ndarray], dict[str, Any]]: - """Identify and regenerate one segment without object pose or geometry. + """Regenerate one segment using observed motion or supplied scene grasps. Only the native AtomicSkillEngine creates candidate joint motion. Source joints supply observations, initial seeds, phase clocks and outer boundary @@ -232,6 +298,10 @@ def fit_skill_segment( cfg: Numerical budgets and independent native/patched acceptance gates. phase_hints: Optional complete contiguous phase partition. Indices are relative to this segment, with half-open bounds. + scene_grasps: Ranked geometry-qualified object grasps for a single-arm + skill. Scene fitting regenerates the skill from these anchors and + qualifies the modified trajectory, without requiring pointwise + agreement with the noisy source. Omission retains motion-only fitting. Returns: Full-length original/native/patched arrays and fit metadata. Rejected @@ -240,6 +310,22 @@ def fit_skill_segment( cfg = SkillFitCfg() if cfg is None else cfg cfg.validate() cfg.__post_init__() + if scene_grasps is not None and (not scene_grasps or skill_id == "hand_over"): + raise ValueError( + "Scene grasp fitting requires a single-arm skill and at least one valid object grasp." + ) + if scene_grasps is not None: + if len(scene_grasps) > cfg.max_candidates or any( + not isinstance(g, GraspGoal) + or not isinstance(g.grasp_xpos, torch.Tensor) + or g.grasp_xpos.shape != (4, 4) + or not isinstance(g.object_pose, torch.Tensor) + or g.object_pose.shape != (4, 4) + for g in scene_grasps + ): + raise ValueError( + "Scene candidates must be explicit single-row object/TCP poses within max_candidates." + ) began = time.monotonic() if skill_id not in {"pick_up", "slide", "twist", "hand_over"}: raise ValueError("Unsupported reconstruction skill.") @@ -302,7 +388,9 @@ def fit_skill_segment( opens = [(hints[open_name]["start"], hints[open_name]["stop"])] participant_data[role] = (poses, closes, opens, arm_ids, hand_ids) - params: dict[str, Any] = {"pose_origin": "fk_derived"} + params: dict[str, Any] = { + "pose_origin": "scene_affordance" if scene_grasps is not None else "fk_derived" + } contact_ranges: list[tuple[str, int, int]] = [] def tensor(value: np.ndarray) -> torch.Tensor: @@ -323,7 +411,7 @@ def partition(boundaries: list[tuple[str, int]]) -> list[dict[str, Any]]: "Expected one observable closure; select a complete skill or provide matching command data." ) close_start, lift_start = closes[0] - if ( + if scene_grasps is None and ( np.max( np.linalg.norm( poses[close_start:lift_start, :3, 3] - poses[close_start, :3, 3], @@ -337,7 +425,9 @@ def partition(boundaries: list[tuple[str, int]]) -> list[dict[str, Any]]: raise ValueError( "The native skill requires a stationary arm during closure." ) - direction, pre_distance, pre_index = _approach(poses, close_start) + direction, pre_distance, pre_index = _approach( + poses, close_start, relaxed=scene_grasps is not None + ) if "reach" in hints: pre_index = hints["reach"]["start"] offset = poses[close_start, :3, 3] - poses[pre_index, :3, 3] @@ -345,15 +435,30 @@ def partition(boundaries: list[tuple[str, int]]) -> list[dict[str, Any]]: if pre_distance <= 1e-4: raise ValueError("Hinted pre-grasp displacement is not observable.") direction = offset / pre_distance - grasp = poses[close_start] + observed_grasp = poses[close_start] + grasp = ( + observed_grasp + if scene_grasps is None + else scene_grasps[0].grasp_xpos.detach().cpu().numpy() + ) close_count = lift_start - close_start if skill_id == "pick_up": - lift = poses[-1, :3, 3] - grasp[:3, 3] - if lift[2] <= 0 or np.linalg.norm(lift[:2]) > cfg.max_contact_error_m: + lift = poses[-1, :3, 3] - observed_grasp[:3, 3] + if lift[2] <= 0 or ( + scene_grasps is None + and np.linalg.norm(lift[:2]) > cfg.max_contact_error_m + ): raise ValueError("Pickup requires an observable world-Z lift.") - if float(_rot_error(poses[-1], grasp)) > cfg.max_contact_rotation_rad: + if ( + scene_grasps is None + and float(_rot_error(poses[-1], grasp)) > cfg.max_contact_rotation_rad + ): raise ValueError("Pickup lift changes TCP orientation.") - goal = PickUpMotionGoal(tensor(grasp)) + goal = ( + PickUpMotionGoal(tensor(grasp)) + if scene_grasps is None + else scene_grasps[0] + ) options = PickUpOptions( lift_height=float(lift[2]), pre_grasp_distance=pre_distance, @@ -381,7 +486,7 @@ def partition(boundaries: list[tuple[str, int]]) -> list[dict[str, Any]]: raise ValueError("Slide/Twist requires one observable release.") open_start, retract_start = releases[0] if skill_id == "slide": - move = poses[open_start, :3, 3] - grasp[:3, 3] + move = poses[open_start, :3, 3] - observed_grasp[:3, 3] distance = float(np.linalg.norm(move)) if distance < 1e-4: raise ValueError("Slide displacement is not observable.") @@ -391,10 +496,12 @@ def partition(boundaries: list[tuple[str, int]]) -> list[dict[str, Any]]: ) mode = "push" if retract_distance > cfg.max_contact_error_m else "pull" axis = rail if mode == "push" else -rail - projected = (poses[lift_start:open_start, :3, 3] - grasp[:3, 3]) @ rail + projected = ( + poses[lift_start:open_start, :3, 3] - observed_grasp[:3, 3] + ) @ rail residual = ( poses[lift_start:open_start, :3, 3] - - grasp[:3, 3] + - observed_grasp[:3, 3] - projected[:, None] * rail ) if np.max(np.linalg.norm(residual, axis=1)) > cfg.max_contact_error_m: @@ -594,14 +701,81 @@ def partition(boundaries: list[tuple[str, int]]) -> list[dict[str, Any]]: sample_count=max(len(q), 80), acceleration_limit=0.5, ) + owned = sorted( + {j for data in participant_data.values() for ids in data[3:] for j in ids} + ) + editable = tuple(owned) + untouched = [i for i in range(q.shape[1]) if i not in editable] + limits = robot.get_qpos_limits()[0].cpu().numpy() + velocity = robot.get_qvel_limits()[0].cpu().numpy() + acceleration = np.asarray(acceleration_limits, dtype=np.float64) + + source_phases = [dict(p) for p in phases] + phase_options = [phases] + if scene_grasps is not None and skill_id == "pick_up" and phase_hints is None: + # Only the outer clock/boundaries are fixed for scene reconstruction. + # A changed grasp may need more time to connect to the recorded end. + # Search a bounded set of free-phase allocations while preserving the + # observed closing duration; explicit phase hints remain authoritative. + closing = lift_start - close_start + free = len(q) - closing + for fraction in (0.5, 0.45, 0.4, 0.55, 0.6): + before = int(round(free * fraction)) + alternative = [ + dict(name="approach", start=0, stop=before), + dict(name="close", start=before, stop=before + closing), + dict(name="lift", start=before + closing, stop=len(q)), + ] + if alternative not in phase_options: + phase_options.append(alternative) + + def connect_scene( + values: np.ndarray, fit_phases: list[dict[str, Any]] + ) -> np.ndarray: + first_contact = next(p["start"] for p in fit_phases if p["name"] == "close") + last_contact = ( + next(p["stop"] for p in fit_phases if p["name"] == "close") + if skill_id == "pick_up" + else next(p["start"] for p in fit_phases if p["name"] == "open") + ) + return _connect_joint_segment( + recording.qpos, + values, + recording.timestamps, + start=start, + stop=stop, + joint_indices=editable, + blend_samples=cfg.blend_samples, + protected=(first_contact, last_contact), + velocity_limits=velocity, + acceleration_limits=acceleration, + ) + + def qualify_scene(values: np.ndarray) -> dict[str, object]: + return _replacement_diagnostics( + recording.qpos, + values, + recording.timestamps, + lower=limits[:, 0], + upper=limits[:, 1], + velocity_limits=velocity, + acceleration_limits=acceleration, + ) + candidates = [] - for candidate in range(cfg.max_candidates if skill_id == "pick_up" else 1): + candidate_checks = [] + attempts = ( + len(scene_grasps) + if scene_grasps is not None + else (cfg.max_candidates if skill_id == "pick_up" else 1) + ) + for candidate in range(attempts): if time.monotonic() - began > cfg.max_seconds: break distance = ( pre_distance + (0.0, -0.005, 0.005, -0.01, 0.01, -0.02, 0.02, -0.03, 0.03)[candidate] - if skill_id == "pick_up" + if skill_id == "pick_up" and scene_grasps is None else None ) trial = ( @@ -609,9 +783,17 @@ def partition(boundaries: list[tuple[str, int]]) -> list[dict[str, Any]]: if distance is not None else options ) + trial_goal = goal + if scene_grasps is not None: + selected = scene_grasps[candidate] + if skill_id == "pick_up": + trial_goal = selected + trial = replace(trial, approach_direction=selected.grasp_xpos[:3, 2]) + else: + trial_goal = replace(goal, grasp_xpos=selected.grasp_xpos) invocation = engine.make_invocation( skill_id, - goal, + trial_goal, control_parts=endpoints, motion_policy=policy, skill_options=trial, @@ -619,11 +801,12 @@ def partition(boundaries: list[tuple[str, int]]) -> list[dict[str, Any]]: ) plan = engine.plan(invocation, context) if not bool(plan.plan_success.all()): + candidate_checks.append({"rank": candidate, "plan_success": False}) continue native = plan.joint_trajectory.positions[0].detach().cpu().numpy() lengths = {p.name: p.stop - p.start for p in plan.segments} anchor = None - if skill_id == "pick_up": + if skill_id == "pick_up" and scene_grasps is None: with torch.no_grad(): approach_fk = ( robot.compute_fk( @@ -645,36 +828,120 @@ def partition(boundaries: list[tuple[str, int]]) -> list[dict[str, Any]]: anchor = ( (pre_index, native_pre) if pre_index > 0 and native_pre > 0 else None ) - mapped, timing = _phase_map(native, lengths, phases, q, anchor) - owned = sorted( - {j for data in participant_data.values() for ids in data[3:] for j in ids} - ) - score = float(np.sqrt(np.mean((mapped[:, owned] - q[:, owned]) ** 2))) - candidates.append((score, mapped, native, lengths, trial, timing)) + selected_trial = None + best_penalty = float("inf") + for fit_phases in phase_options: + mapped, timing = _phase_map( + native, + lengths, + fit_phases, + q, + anchor, + source_times=times if scene_grasps is not None else None, + ) + mapped[:, untouched] = q[:, untouched] + score = float(np.sqrt(np.mean((mapped[:, owned] - q[:, owned]) ** 2))) + check = ( + None + if scene_grasps is None + else qualify_scene(connect_scene(mapped, fit_phases)) + ) + qualifies = check is None or bool(check["accepted"]) + penalty = ( + 0.0 + if check is None + else max( + check["velocity_ratio"], + check["acceleration_ratio"], + 2.0 if check["position_violations"] else 0.0, + ) + ) + if qualifies or selected_trial is None or penalty < best_penalty: + # Keep one schedule's arrays rather than a path per trial. + selected_trial = ( + score, + mapped, + native, + lengths, + trial, + timing, + trial_goal, + candidate, + qualifies, + fit_phases, + ) + best_penalty = penalty + if check is not None: + candidate_checks.append( + { + "rank": candidate, + "plan_success": True, + "phases": fit_phases, + "limits": check, + } + ) + if qualifies or time.monotonic() - began > cfg.max_seconds: + break + assert selected_trial is not None + qualifies = selected_trial[8] + candidates.append(selected_trial) + if scene_grasps is not None and qualifies: + # Candidate order is proximity to the real grasp, independent of + # source joint RMSE. Keep the closest fully qualified replacement. + break if not candidates: raise ValueError("Native skill generation failed within the fitting budget.") - score, mapped, raw_native, lengths, selected_options, timing = min( - candidates, key=lambda item: item[0] + ( + score, + mapped, + raw_native, + lengths, + selected_options, + timing, + goal, + selected_index, + _, + phases, + ) = min( + candidates, + key=( + (lambda item: item[0]) + if scene_grasps is None + else (lambda item: (not item[8], item[7])) + ), ) if skill_id == "pick_up": params["pre_grasp_distance"] = selected_options.pre_grasp_distance # Every non-participant column stays observed, including passive/mimic state. - editable = tuple(owned) - mapped[:, [i for i in range(q.shape[1]) if i not in editable]] = q[ - :, [i for i in range(q.shape[1]) if i not in editable] - ] candidate_full = recording.qpos.copy() candidate_full[start:stop] = mapped - patched = splice_joint_segment( - recording.qpos, - mapped, - recording.timestamps, - start=start, - stop=stop, - joint_indices=editable, - blend_samples=cfg.blend_samples, - anchors=tuple(start + p["start"] for p in phases[1:]), - ) + if scene_grasps is None: + patched = splice_joint_segment( + recording.qpos, + mapped, + recording.timestamps, + start=start, + stop=stop, + joint_indices=editable, + blend_samples=cfg.blend_samples, + anchors=tuple(start + p["start"] for p in phases[1:]), + ) + else: + patched = connect_scene(mapped, phases) + params.update( + grasp_xpos=scene_grasps[selected_index].grasp_xpos.detach().cpu().tolist(), + object_pose=scene_grasps[selected_index] + .object_pose.detach() + .cpu() + .tolist(), + object_id=scene_grasps[selected_index].semantics.entity_id, + grasp_rotation_error_rad=float( + _rot_error( + scene_grasps[selected_index].grasp_xpos.detach().cpu().numpy(), + observed_grasp, + ) + ), + ) errors = {} failures = [] for role, (observed, _, _, arm_ids, _) in participant_data.items(): @@ -698,12 +965,14 @@ def fk(values: np.ndarray) -> np.ndarray: "native_tcp_rmse_m": float(np.sqrt(np.mean(position**2))), "native_rotation_rmse_rad": float(np.sqrt(np.mean(rotation**2))), } - if ( + if scene_grasps is None and ( errors[role]["native_tcp_rmse_m"] > cfg.max_tcp_rmse_m or errors[role]["native_rotation_rmse_rad"] > cfg.max_rotation_rmse_rad ): failures.append(f"{role}: native TCP reconstruction exceeds tolerance") for contact_role, a, b in contact_ranges: + if scene_grasps is not None: + continue if contact_role == role: if ( position[a:b].max() > cfg.max_contact_error_m @@ -723,11 +992,8 @@ def fk(values: np.ndarray) -> np.ndarray: failures.append( f"{role}: boundary correction changes protected contact motion" ) - if score > cfg.max_joint_rmse: + if scene_grasps is None and score > cfg.max_joint_rmse: failures.append("native joint RMSE exceeds tolerance") - limits = robot.get_qpos_limits()[0].cpu().numpy() - velocity = robot.get_qvel_limits()[0].cpu().numpy() - acceleration = np.asarray(acceleration_limits, dtype=np.float64) diagnostics = joint_path_diagnostics( patched, recording.timestamps, @@ -736,6 +1002,17 @@ def fk(values: np.ndarray) -> np.ndarray: velocity_limits=velocity, acceleration_limits=acceleration, ) + source_diagnostics = None + if scene_grasps is not None: + source_diagnostics = joint_path_diagnostics( + recording.qpos, + recording.timestamps, + lower=limits[:, 0], + upper=limits[:, 1], + velocity_limits=velocity, + acceleration_limits=acceleration, + ) + diagnostics = qualify_scene(patched) if not diagnostics["accepted"]: failures.append("patched path exceeds joint limits") accepted = not failures @@ -756,14 +1033,27 @@ def serializable(value: Any) -> Any: "failures": failures, "parameters": params, "phases": phases, + "source_phases": source_phases, "native_phase_lengths": lengths, "native_joint_rmse": score, "native_options": serializable(asdict(selected_options)), - "native_goal": serializable(asdict(goal)), + "native_goal": ( + serializable(asdict(goal)) + if scene_grasps is None + else { + "object_id": params["object_id"], + "object_pose": params["object_pose"], + "grasp_xpos": params["grasp_xpos"], + } + ), "initial_qpos": q[0].tolist(), - "algorithm_revision": 1, + "algorithm_revision": 1 if scene_grasps is None else 3, "per_joint_native_rmse": np.sqrt(np.mean((mapped - q) ** 2, axis=0)).tolist(), - "time_mapping": "phasewise_monotone_native_path_projection", + "time_mapping": ( + "phasewise_monotone_native_path_projection" + if scene_grasps is None + else "phasewise_physical_clock" + ), "native_motion_policy": { "strategy": policy.strategy, "sample_count": policy.sample_count, @@ -775,6 +1065,8 @@ def serializable(value: Any) -> Any: ), "tcp_errors": errors, "limits": diagnostics, + "source_limits": source_diagnostics, + "selected_grasp_rank": selected_index if scene_grasps is not None else None, "start": start, "stop": stop, "editable_joints": owned, @@ -784,6 +1076,7 @@ def serializable(value: Any) -> Any: "collision_evidence": "unverified", "fit_cfg": asdict(cfg), "candidate_count": len(candidates), + "candidate_checks": candidate_checks, "elapsed_seconds": time.monotonic() - began, "joint_names": list(recording.joint_names), } diff --git a/embodichain/lab/sim/motion/expansion/combined.py b/embodichain/lab/sim/motion/expansion/combined.py index 0acae6e3b6..45c1bce79b 100644 --- a/embodichain/lab/sim/motion/expansion/combined.py +++ b/embodichain/lab/sim/motion/expansion/combined.py @@ -138,8 +138,10 @@ class _CombinedSourceCfg: trajectory_call_indices: tuple[int, ...] = (1, 3, 5) def __post_init__(self) -> None: - if self.kind != "task_program": - raise ValueError("combined source.kind must be task_program") + if self.kind not in {"task_program", "skill_sequence"}: + raise ValueError( + "combined source.kind must be task_program or skill_sequence" + ) for name in ("source_id", "source_revision", "template_id"): _text(getattr(self, name), f"source.{name}") if self.unit_scope != "episode": diff --git a/embodichain/lab/sim/shapes.py b/embodichain/lab/sim/shapes.py index 2c2e4612b4..9130e88b6e 100755 --- a/embodichain/lab/sim/shapes.py +++ b/embodichain/lab/sim/shapes.py @@ -71,6 +71,14 @@ class MeshCollisionCfg: VisACD requires a DexSim build with CUDA/OptiX support. """ + decomposition_fpath: str | None = None + """Precomputed convex parts in an OBJ, one object/group per convex piece. + + Valid only for ``convex_decomposition``. Parts use the source mesh's local + frame and units, before body scaling. Spawn loads each part as a separate + convex collider without running decomposition; the visual mesh is retained. + """ + sdf_resolution: int | None = None """Maximum SDF grid resolution; valid only for the ``sdf`` strategy.""" @@ -120,10 +128,22 @@ def __post_init__(self) -> None: ) if self.acd_method not in (None, "visacd", "coacd", "vhacd"): raise ValueError("acd_method must be 'visacd', 'coacd', or 'vhacd'.") + if self.decomposition_fpath is not None and ( + not isinstance(self.decomposition_fpath, str) + or not self.decomposition_fpath.strip() + ): + raise ValueError("decomposition_fpath must be a non-empty path.") elif self.max_hulls is not None or self.acd_method is not None: raise ValueError( "max_hulls and acd_method are valid only for convex_decomposition." ) + if ( + self.decomposition_fpath is not None + and self.approximation != "convex_decomposition" + ): + raise ValueError( + "decomposition_fpath is valid only for convex_decomposition." + ) sdf_values = { "sdf_resolution": self.sdf_resolution, @@ -249,6 +269,7 @@ def _mesh_collision_cfg_init_with_legacy_max_hulls( sdf_padding: float | None = None, *, max_convex_hull_num: int | None = None, + decomposition_fpath: str | None = None, ) -> None: """Initialize with the deprecated hull-count spelling at the API boundary.""" if max_convex_hull_num is not None: @@ -283,6 +304,7 @@ def _mesh_collision_cfg_init_with_legacy_max_hulls( approximation="convex_hull" if approximation is None else approximation, max_hulls=max_hulls, acd_method=acd_method, + decomposition_fpath=decomposition_fpath, sdf_resolution=sdf_resolution, is_hydroelastic=is_hydroelastic, sdf_narrow_band_range=sdf_narrow_band_range, diff --git a/embodichain/lab/sim/spawn/_convex_parts.py b/embodichain/lab/sim/spawn/_convex_parts.py new file mode 100644 index 0000000000..2e7e6a0701 --- /dev/null +++ b/embodichain/lab/sim/spawn/_convex_parts.py @@ -0,0 +1,52 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Read precomputed object-local convex pieces without decomposition.""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np + +__all__ = [] + + +def load_convex_parts(path: str | Path) -> list[tuple[np.ndarray, np.ndarray]]: + """Require non-empty, finite, three-dimensional indexed convex-piece inputs.""" + from dexsim.kit.meshproc.convex_cache import load_obj_as_convex_parts + + source = Path(path).expanduser().resolve(strict=True) + parts = load_obj_as_convex_parts(str(source)) + if not parts: + raise ValueError( + f"Precomputed decomposition contains no convex parts: {source}" + ) + for vertices, triangles in parts: + if ( + vertices.ndim != 2 + or vertices.shape[1:] != (3,) + or len(vertices) < 4 + or not np.isfinite(vertices).all() + or triangles.ndim != 2 + or triangles.shape[1:] != (3,) + or len(triangles) < 4 + or triangles.min() < 0 + or triangles.max() >= len(vertices) + or np.linalg.matrix_rank(vertices.astype(np.float64) - vertices[0]) != 3 + ): + raise ValueError(f"Invalid precomputed convex part in {source}") + return parts diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py index 7956b9e774..85169c6fcf 100644 --- a/embodichain/lab/sim/spawn/descriptors.py +++ b/embodichain/lab/sim/spawn/descriptors.py @@ -382,6 +382,26 @@ def rigid_desc_from_cfg( ) collision.render_source_index = 0 + collisions = [collision] + mesh_collision = cfg.shape.collision if isinstance(cfg.shape, MeshCfg) else None + if mesh_collision is not None and mesh_collision.decomposition_fpath is not None: + from ._convex_parts import load_convex_parts + + parts = load_convex_parts(mesh_collision.decomposition_fpath) + if len(parts) > max_hulls: + raise ValueError("Precomputed decomposition exceeds max_hulls.") + collisions = [ + replace( + collision, + file_path=None, + vertices=vertices, + triangles=triangles, + approximation=CollisionApproximation.CONVEX_HULL, + render_source_index=None, + ) + for vertices, triangles in parts + ] + descriptor = ObjectDesc( name=uid, pose=_pose_from_cfg(cfg), @@ -392,7 +412,7 @@ def rigid_desc_from_cfg( material_ref=material_ref, ) ], - collisions=[collision], + collisions=collisions, physics=_compile_rigid_physics(physics, cfg.body_type), per_env=per_env, body_scale=_vector3(cfg.body_scale, field_name="body_scale"), diff --git a/embodichain/toolkits/skill_fitting/_bundle.py b/embodichain/toolkits/skill_fitting/_bundle.py new file mode 100644 index 0000000000..3f443dc5bb --- /dev/null +++ b/embodichain/toolkits/skill_fitting/_bundle.py @@ -0,0 +1,368 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Minimal portable fitting inputs, with optional video for web annotation.""" + +from __future__ import annotations + +from copy import deepcopy +from dataclasses import asdict +import json +from pathlib import Path +import shutil +import tempfile +from typing import Any + +import numpy as np +import yaml + +from embodichain.data_pipeline.skill_fitting import file_digest, load_joint_recording +from ._pipeline import FitInputs +from ._robot import RobotProfile + +__all__: list[str] = [] + + +def _contained(root: Path, value: str) -> Path: + path = (root / value).resolve(strict=True) + if Path(value).is_absolute() or not path.is_relative_to(root): + raise ValueError("Bundle resources must use contained relative paths.") + return path + + +def pack_inputs( + directory: Path, + inputs: FitInputs, + document: dict[str, Any], + clock: np.ndarray, + durations: np.ndarray, + *, + video: Path | None = None, + video_stream: int | None = None, +) -> Path: + """Publish self-contained robot, scene, joint and annotation inputs.""" + from ._collision import prepare_scene_collision, resolve_scene_path + from ._export import _copy_urdf + from ._scene import load_scene + + directory = directory.resolve() + if directory.exists(): + raise FileExistsError(directory) + profile = RobotProfile.load(inputs.robot_profile) + models = {Path(s["urdf_path"]) for s in profile.solvers.values()} + if len(models) != 1: + raise ValueError("A portable fitting bundle requires one complete robot URDF.") + scene = resolve_scene_path(inputs.scene, inputs.joints) + if scene is None: + raise ValueError("A reconstructed scene is required for an input bundle.") + objects, scene_files = load_scene(scene, include_ungraspable=True) + collisions = prepare_scene_collision(scene) + scene_files.update(collisions["source_files_sha256"]) + source_yaml = scene / "scene_assets.yaml" if scene.is_dir() else scene + raw = yaml.safe_load(source_yaml.read_text()) + recording = load_joint_recording( + inputs.joints, + joint_names=profile.joint_names, + semantics=inputs.qpos_semantics, + qpos_key=inputs.qpos_key, + timestamps_key=inputs.timestamps_key, + sample_dt=inputs.joint_dt, + env_index=inputs.env_index, + ) + original_files = { + str(Path(inputs.robot_profile).resolve()): file_digest(inputs.robot_profile), + str(Path(inputs.joints).resolve()): file_digest(inputs.joints), + **scene_files, + } + if video_stream is not None and ( + video is None or type(video_stream) is not int or video_stream < 0 + ): + raise ValueError("video_stream requires a video and a non-negative integer.") + if video is not None: + video = video.expanduser().resolve(strict=True) + original_files[str(video)] = file_digest(video) + directory.parent.mkdir(parents=True, exist_ok=True) + with tempfile.TemporaryDirectory( + prefix=".fitting-inputs-", dir=directory.parent + ) as tmp: + root = Path(tmp) / "bundle" + root.mkdir() + _copy_urdf(models.pop(), root) + portable = deepcopy(profile.metadata()) + for solver in portable["solvers"].values(): + solver["urdf_path"] = "robot/robot.urdf" + (root / "robot_profile.json").write_text(json.dumps(portable, indent=2)) + (root / "scene").mkdir() + for index, obj in enumerate(objects): + declaration = raw["objects"][obj.identifier] + declaration["pose"] = obj.pose.tolist() + target = root / "scene" / (f"{index:03d}" + obj.mesh_path.suffix) + shutil.copyfile(obj.mesh_path, target) + declaration["shape"]["fpath"] = target.name + if obj.identifier in collisions["objects"]: + cached = collisions["objects"][obj.identifier] + if file_digest(target) != cached["sha256"]: + target = target.with_name(f"{index:03d}.convex.obj") + shutil.copyfile(cached["fpath"], target) + declaration["shape"]["collision"]["decomposition_fpath"] = target.name + # The contract deliberately excludes exporter reports and Blender metadata. + scene_data = {"objects": raw["objects"]} + if "skill_fitting" in raw: + scene_data["skill_fitting"] = raw["skill_fitting"] + (root / "scene" / "scene_assets.yaml").write_text( + yaml.safe_dump(scene_data, sort_keys=False) + ) + np.savez_compressed( + root / "joints.npz", + qpos=recording.qpos, + timestamps=recording.timestamps, + joint_names=np.asarray(profile.joint_names), + ) + np.savez_compressed( + root / "frame_clock.npz", timestamps=clock, durations=durations + ) + (root / "annotations.json").write_text( + json.dumps(document, ensure_ascii=False, indent=2) + ) + frame_source = None + if video is not None: + from embodichain.toolkits.trajectory_annotator._cfg import AnnotationCfg + from embodichain.toolkits.trajectory_annotator._storage import ( + AnnotationStore, + ) + from embodichain.toolkits.trajectory_annotator.video import VideoFrameSource + import threading + + # Validate content, selected stream and image timing against the + # annotation seed before publishing a portable web project. + source = VideoFrameSource(video, stream_index=video_stream) + store = AnnotationStore( + root / "annotations.json", AnnotationCfg().document_bytes + ) + try: + info = source.prepare(threading.Event(), lambda **_: None) + store.initialize(info) + source.check_unchanged() + finally: + source.close() + (root / "video").mkdir() + target = root / "video" / video.name + shutil.copyfile(video, target) + # Refresh descriptive provenance; no original source directory is + # needed to reopen the seed. The content identity is unchanged. + document = store.snapshot()["document"] + document["source"]["path"] = target.relative_to(root).as_posix() + (root / "annotations.json").write_text( + json.dumps(document, ensure_ascii=False, indent=2) + ) + frame_source = dict( + kind="video", + path=target.relative_to(root).as_posix(), + stream_index=video_stream, + ) + settings = asdict(inputs) + settings.update( + joints="joints.npz", + robot_profile="robot_profile.json", + scene="scene/scene_assets.yaml", + qpos_key="qpos", + timestamps_key="timestamps", + env_index=0, + joint_dt=None, + device="cpu", + ) + manifest = dict( + schema="scene-skill-fitting-inputs/v1", + inputs=settings, + annotations="annotations.json", + frame_clock="frame_clock.npz", + files={ + p.relative_to(root).as_posix(): file_digest(p) + for p in sorted(root.rglob("*")) + if p.is_file() + }, + ) + if frame_source is not None: + manifest["frame_source"] = frame_source + (root / "fitting.json").write_text( + json.dumps(manifest, indent=2, allow_nan=False) + ) + (root / "README.md").write_text( + "# Portable scene fitting inputs\n\n" + "All lengths are metres, angles radians; poses are world-from-object 4x4 matrices. " + "Mesh vertices are object-local. Frame bounds are inclusive; joint clocks are seconds.\n\n" + "```bash\nconda activate py312_bk\n" + "embodichain fit-trajectory --bundle . --output fit.npz --run-task-output run-task\n```\n\n" + "`fitting.json` pins input resources by SHA-256. `frame_clock.npz` supports offline fitting. " + "`joints.npz` is the original full-joint recording; `annotations.json` defines skill intervals. " + "`robot_profile.json` owns control parts, solver/TCP calibration and explicit simulation settings. " + "`scene/scene_assets.yaml` owns object meshes, poses, dynamics and fitting defaults; " + "convex-part OBJ files are prepared colliders, so run-task does not decompose again. " + "The installed EmbodiChain/py312_bk runtime is required. No original source directory, " + "Blender project, export report, USDA or prior fit is required.\n" + + ( + "\nThe original encoded video is included with its recorded frame timing. Start the web workflow with:\n\n" + "```bash\nembodichain annotate-trajectory --bundle . --port 8898 --no-browser\n```\n\n" + "The web editor saves `annotations.edited.json`, independently of the sealed annotation seed. " + "Fit the full sequence and download its run-task ZIP in the webpage. The runtime bundle includes " + "`expansion.yaml`; execute it with `--skill-sequence skills.yaml --expansion-profile expansion.yaml`. " + "No original recording directory is required.\n" + if frame_source is not None + else "Video pixels are unnecessary for this offline-only bundle.\n" + ) + ) + for path, digest in original_files.items(): + if file_digest(path) != digest: + raise ValueError("Source resources changed during packaging.") + load_inputs(root) + root.rename(directory) + return directory + + +def load_inputs( + directory: Path, +) -> tuple[FitInputs, dict[str, Any], np.ndarray, np.ndarray]: + """Validate file identity and resolve a moved folder without source lookups.""" + root = directory.resolve(strict=True) + manifest = json.loads((root / "fitting.json").read_text()) + required = {"schema", "inputs", "annotations", "frame_clock", "files"} + if ( + not required <= manifest.keys() + or manifest.keys() - (required | {"frame_source"}) + or manifest["schema"] != "scene-skill-fitting-inputs/v1" + ): + raise ValueError("Unsupported fitting input bundle.") + for name, digest in manifest["files"].items(): + if file_digest(_contained(root, name)) != digest: + raise ValueError(f"Bundle resource changed: {name}") + settings = deepcopy(manifest["inputs"]) + if settings.keys() - FitInputs.__dataclass_fields__.keys(): + raise ValueError("Unknown fitting bundle input field.") + for key in ("joints", "robot_profile", "scene"): + settings[key] = str(_contained(root, settings[key])) + inputs = FitInputs(**settings) + inputs.check() + # Robot/scene dependencies are also confined to the sealed file inventory. + profile = RobotProfile.load(inputs.robot_profile) + from ._scene import load_scene + + _, sources = load_scene(Path(inputs.scene), include_ungraspable=True) + dependencies = set(sources) | {s["urdf_path"] for s in profile.solvers.values()} + import xml.etree.ElementTree as ET + import yourdfpy + + for solver in profile.solvers.values(): + model_path = Path(solver["urdf_path"]) + tree = ET.parse(model_path) + for element in tree.findall(".//mesh") + tree.findall(".//texture"): + uri = element.get("filename") + if uri: + dependencies.add( + str( + Path( + yourdfpy.filename_handler_magic( + uri, dir=str(model_path.parent) + ) + ).resolve(strict=True) + ) + ) + scene_data = yaml.safe_load(Path(inputs.scene).read_text()) + for obj in scene_data["objects"].values(): + decomposition = ( + obj.get("shape", {}).get("collision", {}).get("decomposition_fpath") + ) + if decomposition: + dependencies.add( + str((Path(inputs.scene).parent / decomposition).resolve(strict=True)) + ) + dependencies.update( + { + inputs.joints, + inputs.robot_profile, + str(_contained(root, manifest["annotations"])), + str(_contained(root, manifest["frame_clock"])), + } + ) + visual = _video_source(root, manifest) + if visual is not None: + dependencies.add(str(visual[0])) + inventory = {str(_contained(root, p)) for p in manifest["files"]} + if dependencies - inventory: + raise ValueError("Bundle has unlisted or external robot/scene dependencies.") + document = json.loads(_contained(root, manifest["annotations"]).read_text()) + with np.load(_contained(root, manifest["frame_clock"]), allow_pickle=False) as data: + clock, durations = data["timestamps"].copy(), data["durations"].copy() + if ( + clock.ndim != 1 + or len(clock) != document["source"]["frame_count"] + or durations.shape != clock.shape + or not np.isfinite(clock).all() + or not np.isfinite(durations).all() + or np.any(np.diff(clock) <= 0) + or np.any(durations <= 0) + ): + raise ValueError("Invalid frame clock in fitting bundle.") + return inputs, document, clock, durations + + +def _video_source( + root: Path, manifest: dict[str, Any] +) -> tuple[Path, int | None] | None: + """Decode the optional closed, contained web frame-source declaration.""" + if "frame_source" not in manifest: + return None + source = manifest["frame_source"] + if ( + not isinstance(source, dict) + or source.keys() != {"kind", "path", "stream_index"} + or source["kind"] != "video" + or not isinstance(source["path"], str) + or ( + source["stream_index"] is not None + and (type(source["stream_index"]) is not int or source["stream_index"] < 0) + ) + ): + raise ValueError("Bundle frame_source requires a video path and stream_index.") + return _contained(root, source["path"]), source["stream_index"] + + +def load_annotation_inputs( + directory: Path, annotations: Path | None = None +) -> tuple[Path, int | None, Path, dict[str, Any]]: + """Resolve a web project and isolate mutable edits from sealed input files.""" + root = directory.expanduser().resolve(strict=True) + inputs, document, _, _ = load_inputs(root) + manifest = json.loads((root / "fitting.json").read_text()) + visual = _video_source(root, manifest) + if visual is None: + raise ValueError( + "This offline-only bundle has no video. Repackage with --package-video
-

原子技能拟合与轨迹回填

选中一段已提交的标注,使用关节数据辨识参数并重建。

未开始
-
请在右侧选择一段已标注区间
+

原子技能拟合与轨迹回填

从实录估计接近方向,在重建场景的有效抓取中选择相近姿态,生成原生技能并衔接边界。

未开始
+
整条轨迹 · 已标注区间按原子技能拟合

关节记录与机器人配置 本机文件路径 · 可从启动参数预填
- + + + +
+
加载场景后准备碰撞网格。
数据字段、阶段提示与拟合容差
-

视频只用于选区与时间同步;参数辨识使用关节角与机器人 FK/IK 标定。每次回填一个选区,保留原始关节时钟与选区外的采样。

+

一次处理完整轨迹。已标注区间按对应技能拟合,未标注区间保留原始 qpos 并作为 DemoSkill 导出。保留原始关节时钟、段首段尾及未参与部件;目标物体留空可逐段自动选择。

可读取 NPZ、HDF5 或 Atomic PT;需要包含配置中的夹爪关节。

diff --git a/embodichain/toolkits/trajectory_annotator/web/styles.css b/embodichain/toolkits/trajectory_annotator/web/styles.css index e00e559e90..660909cc98 100644 --- a/embodichain/toolkits/trajectory_annotator/web/styles.css +++ b/embodichain/toolkits/trajectory_annotator/web/styles.css @@ -102,6 +102,8 @@ main { max-width:1680px; padding:22px 28px 12px; margin:auto; } .fit-wide { grid-column:1/-1; } .fit-advanced { margin-top:14px; } .fit-footnote { font-size:11px; color:var(--muted); margin-top:12px; } +.fit-scene-preparation { display:flex; align-items:center; gap:12px; flex-wrap:wrap; margin-top:12px; } +.fit-scene-preparation .fit-footnote { margin:0; } .fit-message { margin:0 17px 15px; font-size:12px; color:var(--muted); white-space:pre-wrap; overflow-wrap:anywhere; } .fit-message.error { color:var(--danger); } .fit-message.warning { color:#efc88e; } @@ -120,10 +122,12 @@ main { max-width:1680px; padding:22px 28px 12px; margin:auto; } .fit-view-controls button { padding:4px 8px; font-size:11px; margin-left:auto; } .fit-legend { display:flex; flex-wrap:wrap; gap:14px; font-size:10px; margin-bottom:8px; } .fit-legend .original { color:#fb8696; }.fit-legend .native { color:#f2b664; }.fit-legend .patched { color:#66d5bc; }.fit-legend .proposed { color:#b6a1ff; } -.fit-chart,.fit-geometry { display:block; width:100%; background:#0e1622; border:1px solid #273449; border-radius:6px; } +.fit-chart,.fit-viewer { display:block; width:100%; background:#0e1622; border:1px solid #273449; border-radius:6px; } .fit-chart { height:210px; cursor:crosshair; }.fit-error-chart { height:120px; margin-top:6px; } .fit-cursor { min-height:20px; font:10px/1.8 ui-monospace,SFMono-Regular,Consolas,monospace; color:var(--muted); } -.fit-geometry { height:280px; cursor:grab; touch-action:none; }.fit-geometry:active { cursor:grabbing; } +.fit-viewer { height:460px; }.fit-viewer[hidden] { display:none; } +.fit-legend .fit-blue { color:#2878ff; } +.fit-viewer-controls button { margin-left:0; }.fit-viewer-controls strong { margin-right:auto; } .fit-report { margin-top:12px; }.fit-report pre { max-height:320px; overflow:auto; font-size:11px; color:#aebed3; white-space:pre-wrap; overflow-wrap:anywhere; } @media (max-width:700px) { .fit-grid { grid-template-columns:1fr; }.fit-metrics { grid-template-columns:repeat(2,minmax(0,1fr)); }.fit-target,.fit-result-heading { align-items:flex-start; flex-direction:column; }.fit-inputs summary .muted { display:block; margin-left:15px; } } .editor-grid { display:grid; grid-template-columns:minmax(0,1fr) minmax(0,1fr) minmax(48px,.8fr); gap:10px; padding:0 17px; } diff --git a/examples/toolkits/skill_fitting/README.md b/examples/toolkits/skill_fitting/README.md index bb3596f09a..ac326f050a 100644 --- a/examples/toolkits/skill_fitting/README.md +++ b/examples/toolkits/skill_fitting/README.md @@ -1,9 +1,142 @@ # Video and joints to atomic motion -Run in `py312`. The production command supports explicit `pick_up`, `slide`, -`twist` and `hand_over` motion goals. It needs no object poses, object meshes, -or running simulator. Existing annotation files select the segment; automatic -video contact recognition is not part of this command. +Run in the installed project environment (for the local reconstructed episode, +`py312_bk`). The command fits `pick_up`, `slide`, `twist` and `hand_over` through +their native planners. Reconstructed scenes provide valid antipodal grasp anchors +for the single-arm skills. The original motion-only route remains available when +no scene is supplied. Fitting itself does not start a simulator. Accepted whole episodes can export a native run-task bundle. +Existing annotation files select the segment; automatic RGB contact recognition +is not part of this command. + +## Fit a reconstructed scene + +For a complete portable web project, start with only the extracted resource folder: + +```bash +conda activate py312_bk +embodichain annotate-trajectory --bundle /data/pickup-workflow.inputs --port 8898 --no-browser +``` + +Its manifest supplies the video and all joint, robot and scene defaults. Edit skill +intervals, fit the whole recording and download the run-task ZIP in the webpage. +No original recording directory is needed. The resource contract and packaging +commands are below. Individual recording inputs remain supported: + +Set `TRAJ_EP` to your recording folder. The cup and pen frame ranges below are +examples of two independent pickup intervals. + +```bash +conda activate py312_bk +TRAJ_EP=/path/to/recording + +python -m embodichain annotate-trajectory \ + --video "$TRAJ_EP/main.mp4" --annotations "$TRAJ_EP/cup.segments.json" \ + --joints "$TRAJ_EP/joints.npz" --robot-profile "$TRAJ_EP/robot_profile.json" \ + --scene "$TRAJ_EP/blender_scene/physical_export" \ + --participant auto --qpos-semantics design_reference --time-offset 0 \ + --device cpu --port 8898 --no-browser +``` + +Use the same robot profile for the right-hand cup segment `[45,148]` and the +left-hand pen segment `[196,299]`. The webpage displays the selected motion/hand +groups and object. A standalone fit of the first segment is: + +```bash +python -m embodichain fit-trajectory \ + --video "$TRAJ_EP/main.mp4" --joints "$TRAJ_EP/joints.npz" \ + --robot-profile "$TRAJ_EP/robot_profile.json" \ + --scene "$TRAJ_EP/blender_scene/physical_export" \ + --skill pickup --start-frame 45 --end-frame 148 \ + --participant auto --qpos-semantics design_reference --time-offset 0 \ + --output /tmp/cup.scene-fit.npz +``` + +Change the frame bounds to `196` and `299` for the pen. `--object-id` can pin a +scene object; otherwise the nearest mesh-bounds centre to the observed grasp is +selected. The tool never infers an object pose from the video. Without `--scene`, +single-arm fitting also discovers `blender_scene/physical_export` beside the joint +recording when that directory contains `scene_assets.yaml`. + +The directory format reads local mesh files from `objects..shape.fpath` in +`scene_assets.yaml`, and world-from-object matrices from the corresponding +`export_report.json` entry's `anchor`. A standalone YAML may instead declare +`objects..pose` as a proper 4×4 world matrix. Coordinates must be in metres +and the same world frame as the supplied robot calibration; no extra robot-root +transform is applied to already-world object poses. Optional `graspable: false` +excludes a declared obstacle from automatic target selection. + +Automatic participant selection uses the profile's explicit joint lists and URDF +ancestry to pair hands with motion groups, then finds a unique closing hand in +the selection. It does not interpret control-part names or require separate left +and right profiles. Ambiguous evidence fails explicitly; `--participant ` +selects one configured motion/hand name and `--participant configured` preserves +the profile's role bindings. HandOver continues to require configured +source/destination roles; an explicit scene request for it requires a future +time-resolved object/attachment binding and fails rather than guessing one. + +Scene grasps reuse `AntipodalAffordance.get_grasp_candidates` and the actual +open-hand collision surfaces. The supplied URDF must include the finger collision +meshes. Automatic geometry currently supports two opposing prismatic finger +joints. Contact-frame X follows their configured opening/closing travel; contact +Z follows the calibrated TCP approach axis. The canonical contact origin uses the +overlapping collision-tip bounds minus `grasp_contact_inset_m`; public candidates +are converted back to the user's unchanged TCP. This geometric convention is +recorded in the report and is not measured hardware contact calibration. + +The fitter estimates approach direction and skill motion parameters from FK, +ranks valid grasps by translation distance plus `grasp_rotation_weight_m` times +SO(3) angle, and uses the closest candidate with a complete native kinematic plan. +For Pickup this is an object-bound `GraspGoal`, followed by normal approach, +closure and lift generation. It does not require a perfectly constant observed +pre-grasp rotation or pointwise agreement with every noisy source sample. +Slide/Twist retain their native axis-motion contracts with the selected anchor. +Grasp generation tests both the nominal TCP pose and its exact local-Z half +turn (`T_tcp @ Rz(pi)`), keeping TCP position and approach Z fixed. Each physical +hand realization is independently checked against the target mesh; rotations +and translations between the contact frame and virtual TCP are included. +Full TCP-pose ranking chooses the closer qualified alternative to avoid wrist +flips. Pickup's native approach follows the selected TCP Z. There are no random +cone proposals on each fit. + +Scene fitting defaults to `grasp_approach_deviation_rad = pi/3` (60 degrees) and +`grasp_filter_ground_collision = false`. The wider sampling angle admits grasps +near the recorded orientation that the former 30-degree restriction rejected. +Disabling the inferred plane avoids rejecting candidates solely because the +gripper intersects a plane placed at the target's lowest vertex; this also +disables that conservative table proxy. Target-object surface checks and joint, +velocity, acceleration and boundary checks still apply. Complete robot/table +collision freedom remains unverified. To restore stricter sampling, pass a +`--fit-config` JSON file (or fill the web panel's JSON field), for example: + +```json +{"grasp_approach_deviation_rad": 0.5235987755982988, "grasp_filter_ground_collision": true} +``` + +The scene report records the symmetry convention, selected roll, approach angle, +and ground-filter policy. `parameters.grasp_rotation_error_rad` records the +selected grasp's rotation difference from the observed TCP. + +Each replacement stays on the recorded clock and changes only the selected +motion/hand columns. Automatic Pickup fitting may redistribute approach/lift +time within that fixed outer clock when the selected grasp needs it, while +preserving the observed closing duration. Explicit phase hints remain fixed; +`source_phases` records the observed partition and `phases` records the chosen +one. At most six schedules are checked per grasp, keeping one best path's arrays. +Quintic free-phase bridges preserve the source's outer +position/velocity/acceleration stencils and keep the contact core native. Every +changed position and derivative stencil, including the inner joins, must meet +the configured limits. Existing violations elsewhere in the source are retained +and reported separately under `source_limits`; passing this check does not +certify the entire original recording. Rejected candidates remain downloadable +without replacing `patched_qpos`. Source video, joints and object assets remain +read-only. Fitting multiple intervals does not accumulate replacements. + +Candidate generation is bounded by `grasp_sample_count`, `grasp_max_candidates` +and `max_candidates`; there is one temporary generator per fit. Scene, robot +geometry and recording content hashes are checked before returning a result. +Finite open-hand surface checks concern the target and inferred support plane; +full-arm swept collision, closing contacts and physical task success remain +unverified. ## Reproduce the Pickup round trip @@ -84,6 +217,22 @@ names are retained. Solver configurations use existing FK/IK solvers with an explicit URDF, root/end links, TCP and fixed arm base poses. Paths resolve against the profile file. Both arms of a handover need a calibrated common frame. +`hand_commands..grasp` supplies the full-close qpos target; `open` +supplies opening, in `control_parts[grasp_part]` joint order and native units. +Use the model's joint limits: a loaded gripper can stop short of the commanded +closure because of contact. Replacing the target with that measured opening +removes closing preload. Native sequence spans therefore update only bound +motion/grasp parts and hold every other joint at its previous command. DemoSkill +spans continue to use their original full-joint commands, including releases. + +`solvers..tcp` is the 4×4 **end-link-from-TCP** transform; translation +is `tcp[:3,3]` in metres, along the end-link axes. Runtime export copies this into +`task.yaml` at `robot.solver_cfg..tcp`. For the recording's Marvin +right arm, shortening the offset changes `tcp[0][3]` from 0.13 to 0.12 m; the +left arm keeps its existing calibration. This is separate from the scene's world-Z +grasp compensation. Refit/export after source calibration changes to refresh +derived grasp goals and sealed input hashes together. + `--fit-config` accepts the declared `SkillFitCfg` fields, including finite sample, candidate and wall-clock budgets and independent joint/TCP/contact tolerances. The wall-clock budget is cooperative between native plans, not a preemptive @@ -115,26 +264,106 @@ origin. Unobservable or incompatible segments fail explicitly. Tests cover all four skills using an independent Cartesian robot oracle; the recorded robot round-trip is qualified on the UR5 Pickup example. -## Standalone result +## Minimal portable scene inputs -The NPZ contains original/native/patched/proposed joints, physical timestamps, -source-derived monotone phase progress, raw native phase joints, modification -mask, and decimated FK link/TCP geometry. `metadata_json` stores parameters, -native options, phases, initial qpos, calibration/profile/annotation/source -identities, timing interpretation and independent native/patched diagnostics. -It uses numeric arrays and JSON, with pickle disabled. +The portable resource contract is `scene-skill-fitting-inputs/v1`. It needs these resources: -For plotting: +| Resource | Contents | +| --- | --- | +| `fitting.json` | Version, relative paths, fitting settings and SHA-256 inventory | +| `scene/scene_assets.yaml` | Object-local meshes, world-from-object 4×4 poses, dynamics and fitting defaults | +| `scene/*.obj` | Visual meshes and precomputed convex parts only where requested by collision configuration | +| `robot_profile.json` | Explicit joint layout, control parts, solvers, TCP/base calibration, limits and simulation overlays | +| `robot/robot.urdf`, referenced assets | Complete robot and only the mesh/texture dependencies it uses | +| `joints.npz` | Original full-joint `(T,DOF)` qpos, joint names and strictly increasing timestamps in seconds | +| `annotations.json` | Inclusive frame intervals, optional cores and original skill labels | +| `frame_clock.npz` | Actual frame presentation times and display durations; video pixels are unnecessary for fitting | +| `video/` | Original encoded video, required for web startup and declared by optional `fitting.json.frame_source` | + +Lengths are metres, angles radians, transforms are world-from-local. Grasp targets and robot calibration share one world frame. The folder needs no Blender file, export report, USDA, original directory or previously fitted trajectory. Offline fitting uses the frame clock; the web workflow additionally includes the original video, with its frame order and timestamps preserved. The installed EmbodiChain runtime, annotation extras and solver dependencies remain required for the complete workflow. -```python -from embodichain.data_pipeline.skill_fitting import load_skill_fit +Add video to an existing offline input package, without running a fit: -arrays, report = load_skill_fit("/data/episode.pickup.fit.npz") -error = arrays["patched_qpos"] - arrays["original_qpos"] +```bash +embodichain fit-trajectory --bundle /data/portable-inputs \ + --package-output /data/pickup-workflow.inputs --package-video /data/main.mp4 ``` -All original joint samples are included; visualization geometry uses the saved -`geometry_indices`. Viewing does not require the source video, joint recording, -URDF, or simulator. The annotation website also runs this pipeline and displays -video-linked joint curves, errors, rotatable FK skeletons, TCP paths and geometric -error magnification. See the [web example](../trajectory_annotation/README.md). +The packager verifies that the video content/stream matches the annotation seed. +All dependency paths are rewritten to contained relative paths and pinned by +SHA-256. Moving the resulting folder requires no source lookup. Repackaging an +existing video-enabled bundle preserves its video by default. ZIP the folder to +upload/share it; extract it before passing the folder to `--bundle`. + +`annotate-trajectory --bundle` saves mutable work to `annotations.edited.json`, +outside the sealed inventory; `--annotations` selects another work file. The web +fit uses the current edited document. The standalone `fit-trajectory --bundle` +command uses the sealed annotation seed. Do not overwrite sealed resource files +with annotation edits or generated outputs. + +Create it from an existing annotated recording: + +```bash +conda activate py312_bk +embodichain fit-trajectory \ + --video /data/main.mp4 --joints /data/joints.npz \ + --robot-profile /data/robot_profile.json --scene /data/blender_scene/physical_export \ + --annotations /data/main.segments.json --all-segments \ + --qpos-semantics measured_state --time-offset 0 \ + --output /data/whole.fit.npz --package-output /data/portable-inputs \ + --package-video /data/main.mp4 \ + --run-task-output /data/whole-run-task +``` + +Move the folder anywhere, then fit and export using only its contents: + +```bash +embodichain fit-trajectory --bundle /data/portable-inputs \ + --output /data/new.fit.npz --run-task-output /data/new-run-task +``` + +The manifest seals dependencies. Editing a sealed input requires rebuilding the bundle/inventory; checksum mismatches reject. New output paths must not replace source resources. + +## Native run-task output + +```bash +cd /data/new-run-task +embodichain run-task --gym_config task.yaml --skill-sequence skills.yaml --headless --viser +# Native EmbodiChain window, with a desktop display: +embodichain run-task --gym_config task.yaml --skill-sequence skills.yaml --no-headless +``` + +Open the printed Viser URL while execution runs. Native execution is automatic and exits at completion; use the annotation webpage's synchronized dual-robot viewer for timeline inspection. The native-window command omits `--viser`, which forces headless rendering. + +`skills.yaml` retains the chronological skill identities and source spans. Pickup exports an object ID and the fitted object-relative TCP calibration; the live object pose grounds its grasp when invoked. Native approach, closure and lift are generated by AtomicSkillEngine from the current robot state. Other supported motion-only goals retain their explicit fitted axis/pose contracts. Control parts come from the actual participant selection and preserve custom names. + +Only uncovered/explicit DemoSkill intervals retain `skills/*.execute_demo_skill.pt`, with exact original **DOF × n_frame** qpos, absolute timestamps and joint names. Native atomic spans have no trajectory PT. There is no full-episode `trajectory.pt`, `qpos.pt` or fitting NPZ in the runtime package; the separate offline NPZ retains all comparison arrays. Plain demonstration gaps do not pretend to meet the monitored ExecuteDemoSkill contact/effect contract. + +At runtime, raw demo clocks are resampled to the configured control clock without changing the saved source. Quintic joint connectors begin/end at adjacent command postures and obey the profile's velocity and acceleration limits. Atomic skills replan from observed state and update only their bound parts; unbound joints retain previous commands, including full-close targets on loaded grippers. Source frame timing is provenance; native execution duration may change. Completion writes `execution.json` with measured endpoint tracking and object heights. These observations are separate from a physical task acceptance contract. + +`--skill-sequence` also accepts `--expansion-profile`. Each exported runtime folder includes an editable `expansion.yaml`: + +```bash +embodichain run-task --gym_config task.yaml --skill-sequence skills.yaml \ + --expansion-profile expansion.yaml --expansion-output-dir ./expanded \ + --headless --device cpu +``` + +The default profile runs one nominal episode and two spatial variants, using `joint_residual` and `via_points` on native motion phases. Demo-only sequences run one nominal episode. Native contact phases, grasp joints, unbound joints and phase boundary stencils remain protected. DemoSkill tensors and their resampled command paths are identical across variants; transition connectors can change to join the preceding native posture. The simulation's measured states can differ because physics still runs. + +The profile declares `source.kind: skill_sequence`, `source_id: skills` and `source_revision: native-skill-sequence/v1`. `source.call_count` counts **all** chronological timeline spans; `trajectory_call_indices` uses those zero-based indices and defaults to native atomic spans only. Demo indices are excluded even if requested explicitly. Change `trajectory.variants_per_family` and `collection.target_episodes` together to collect more episodes. Recipe zero is nominal; subsequent recipes sample reproducible spatial variants using the profile and `--seed`. A rejected requested variant is retried, never silently replaced by nominal. + +This host supports B=1, a fixed scene and spatial variants only. Scene, affordance, timing, visual and LeRobot expansion are rejected before simulation. It writes `expansion_manifest.json` and `recipe-*/attempt-*/{execution.json,trajectory.h5}` to a **new** output directory. Each HDF5 contains `commands/qpos` with T submitted active-joint commands and `states/qpos`, `states/qvel`, root and object poses with T+1 measured states, including the initial state. Joint names and the control period are stored as attributes. The reports identify path command ranges separately from connectors/holds, retain rejected attempts, and count command completion without claiming physical task success. `expansion.yaml` is outside the sealed physical-input inventory so sampling settings remain editable. + +Scene defaults remain: + +```yaml +skill_fitting: + pick_up: + grasp_world_z_offset_m: -0.01 + replay_interpolation_factor: 2 +``` + +Omitted values are zero offset/factor one. The offset is applied before fitting and retained in object-relative grasp calibration; TCP calibration and mesh poses stay explicit. The interpolation factor retains its existing configuration spelling and becomes native duration scaling at an unchanged command period. Execution has a 200,000-command budget. + +The profile's optional `simulation` carries explicit `root_props`, `joint_drive_props`, `attrs` and `link_attrs` into the generated robot configuration. Tuned stiffness/damping/effort and mass/inertia remain simulation calibration, not manufacturer specifications. Mesh collision decompositions are prepared at import/package time, supplied through `decomposition_fpath`, and loaded directly at runtime. They are independent of an external cache on the receiving machine. diff --git a/examples/toolkits/trajectory_annotation/README.md b/examples/toolkits/trajectory_annotation/README.md index b74193e976..f724208d22 100644 --- a/examples/toolkits/trajectory_annotation/README.md +++ b/examples/toolkits/trajectory_annotation/README.md @@ -2,6 +2,16 @@ 在仓库根目录运行,默认绑定 `127.0.0.1`,自动选择端口并打开浏览器。服务运行期间保留终端,结束时按 Ctrl+C。 +含视频的最小资源包可直接启动正式网页入口,无需逐项传入原始录制路径: + +```bash +conda activate py312_bk +embodichain annotate-trajectory --bundle /data/pickup-workflow.inputs --port 8898 --no-browser +``` + +包内同时提供关节轨迹、机器人与重建场景;网页可完成区间标注、全轨迹拟合和 run-task 包导出。 +后续动态扩增使用导出包的 `skills.yaml` 和 `expansion.yaml`,详见[完整流程](../../../embodichain/toolkits/trajectory_annotator/README.md)。 + ```bash conda activate py312 # 若尚未安装网页端依赖: @@ -69,9 +79,11 @@ embodichain run-example toolkits/trajectory_annotation \ --output-dir /tmp/pickup-labels ``` -在网页中用 **I / O** 标注完整 Pickup,从技能下拉菜单选择 **Pickup** 并提交,拟合面板自动选择同一技能,点击 -**“拟合并回填选中区间”**。会显示通过/拒绝状态、参数和误差,以及与视频联动的关节曲线、 -可旋转的 FK 骨架和 TCP 路径;可下载包含完整关节轨迹的 NPZ。此示例辨识出的 `lift_height` 约为 **0.16 m**。 +在网页中用 **I / O** 标注完整 Pickup,从技能下拉菜单选择 **Pickup** 并提交,点击 +**“拟合并回填完整轨迹”**。一次处理全部标注,空白区间保留为 DemoSkill。会显示通过/拒绝状态、参数和误差,以及与视频联动的关节曲线、 +Viser 中保留原始外观的机器人与蓝色半透明的回填机器人。可播放整个回填区间或完整录制, +随视频时间同步更新两套模型的全部关节,并下载包含完整关节轨迹的 NPZ。 +此示例辨识出的 `lift_height` 约为 **0.16 m**。 同一面板也支持 Slide、Twist、HandOver。任意真实记录需提供自己的机器人配置,示例 UR5 配置不能直接用于 Piper。 区间边界较宽时,可勾选“单独设置拟合核心区间”,只重建内部完整技能,保留前后过渡采样; @@ -80,8 +92,9 @@ embodichain run-example toolkits/trajectory_annotation \ `--qpos-key`、`--timestamps-key`、`--joint-dt`、`--env-index`、`--phase-hints`、`--fit-config` 也可预填高级设置。 默认 HDF5 标注流程保持可独立使用。 -每次仅回填当前选区,以原始关节记录为基准。未通过校验时回填数组保持原始值,并保留候选; -修改标注后需重新拟合再下载。完整数据格式与边界说明见[工具说明](../../../embodichain/toolkits/trajectory_annotator/README.md)。 +每次以原始记录为基准,将全部已标注技能的核心合并回填。未通过校验的区间保持原始值,并保留候选; +修改标注后需重新拟合再下载。全部通过后可下载 run-task 运行包,包含配置、模型、原生技能的对象/参数,以及仅 DemoSkill 区间的原始 `DOF × n_frame` qpos tensor。原子技能在运行时从当前状态重新规划,不保存轨迹 PT。 +解压后按 README 用 `run-task --replay` 执行。完整数据格式与边界说明见[工具说明](../../../embodichain/toolkits/trajectory_annotator/README.md)。 ## 输入已有图像数组 diff --git a/tests/compute/test_trajectory_splicing.py b/tests/compute/test_trajectory_splicing.py index 6cfa812bc8..50c9d5fec6 100644 --- a/tests/compute/test_trajectory_splicing.py +++ b/tests/compute/test_trajectory_splicing.py @@ -21,6 +21,7 @@ from embodichain.compute.trajectory.phase_alignment import align_joint_phase from embodichain.compute.trajectory.splicing import ( + _replacement_diagnostics, splice_joint_segment, joint_path_diagnostics, ) @@ -88,6 +89,26 @@ def test_derivative_gate_detects_a_single_step_jump() -> None: assert result["peak_acceleration"] == pytest.approx([20000]) +def test_replacement_qualifies_new_stencils_without_hiding_new_violations() -> None: + times = np.arange(100) * 0.01 + source = np.zeros((100, 2)) + source[60:, 1] = 5 # Existing discontinuity on an untouched control part. + output = source.copy() + output[20:50, 0] = np.sin(np.linspace(0, np.pi, 30)) * 0.001 + limits = dict( + lower=np.full(2, -10), + upper=np.full(2, 10), + velocity_limits=np.ones(2), + acceleration_limits=np.full(2, 10), + ) + assert not joint_path_diagnostics(output, times, **limits)["accepted"] + assert _replacement_diagnostics(source, output, times, **limits)["accepted"] + output[30, 0] = 1 + result = _replacement_diagnostics(source, output, times, **limits) + assert not result["accepted"] + assert result["velocity_ratio"] > 90 + + def test_phase_clock_changes_timing_without_changing_the_native_path() -> None: native = np.column_stack((np.linspace(0, 1, 101), np.zeros(101))) progress = np.linspace(0, 1, 51) ** 2 diff --git a/tests/gym/envs/test_replay_actions.py b/tests/gym/envs/test_replay_actions.py index 1a7f9995b1..9065b806d0 100644 --- a/tests/gym/envs/test_replay_actions.py +++ b/tests/gym/envs/test_replay_actions.py @@ -236,6 +236,57 @@ def test_kinematic_replay_ignores_controller_schema(): ReplayWrapper(_ReplayEnv(), trajectory, mode="kinematic") +@pytest.mark.parametrize("mode", ["position", "position_velocity"]) +def test_named_external_trajectory_aligns_state_and_commands_without_mutating_source( + mode, +): + trajectory = _trajectory(mode=mode) + trajectory["meta"].update( + state_joint_names=["wrist", "unused", "elbow"], + active_joint_ids=[0, 2], + joint_names=["wrist", "elbow"], + ) + source_qpos = torch.tensor([[[30.0, 10.0, 20.0], [31.0, 11.0, 21.0]]]) + trajectory["states"] = TensorDict( + {"robot": {"qpos": source_qpos, "qvel": source_qpos + 100}}, batch_size=[1, 2] + ) + env = _ReplayEnv() + replay = ReplayWrapper(env, trajectory) + torch.testing.assert_close( + replay._trajectory["states"]["robot"]["qpos"][0, 0], + torch.tensor([10.0, 20.0, 30.0]), + ) + torch.testing.assert_close( + replay._trajectory["states"]["robot"]["qvel"][0, 0], + torch.tensor([110.0, 120.0, 130.0]), + ) + replay.step(None) + torch.testing.assert_close(env.last_command["qpos"], torch.tensor([[1.0, 0.0]])) + if mode == "position_velocity": + torch.testing.assert_close(env.last_command["qvel"], torch.tensor([[3.0, 2.0]])) + assert trajectory["meta"]["state_joint_names"] == ["wrist", "unused", "elbow"] + torch.testing.assert_close(trajectory["states"]["robot"]["qpos"], source_qpos) + + +@pytest.mark.parametrize( + "names,ids", + [ + (["wrist", "elbow", "missing"], [0, 1]), + (["wrist", "wrist", "elbow"], [0, 2]), + (["wrist", "unused", "elbow"], [0, 1]), + ], +) +def test_named_external_trajectory_cannot_hide_mismatched_robot_or_actuators( + names, ids +): + trajectory = _trajectory() + trajectory["meta"].update(state_joint_names=names, active_joint_ids=ids) + env = _ReplayEnv() + with pytest.raises(ValueError, match="joint"): + ReplayWrapper(env, trajectory) + assert env.step_count == 0 + + def _objective_env(): from embodichain.lab.gym.envs.objectives import ( MeasuredRigidObjectState, diff --git a/tests/lab/scripts/test_run_task_expansion.py b/tests/lab/scripts/test_run_task_expansion.py index ebe96b685f..af1d51c6ae 100644 --- a/tests/lab/scripts/test_run_task_expansion.py +++ b/tests/lab/scripts/test_run_task_expansion.py @@ -312,3 +312,79 @@ def test_cli_expansion_profile_overrides_task_expansion_policy() -> None: assert request is not None assert request[0].source.source_id == "task_program_source" + + +@pytest.mark.parametrize( + "unsupported", [None, "scene", "source", "capacity", "recipes", "video", "dataset"] +) +def test_native_skill_sequence_and_expansion_profile_coexist_without_task_program( + tmp_path, unsupported +): + import yaml + from embodichain.toolkits.skill_fitting._sequence_expansion import ( + validate_sequence_expansion, + validate_sequence_collection, + write_sequence_expansion_profile, + ) + + timeline = [ + dict(kind="demo_skill"), + dict(kind="atomic_skill"), + dict(kind="demo_skill"), + ] + manifest = tmp_path / "skills.yaml" + manifest.write_text( + yaml.safe_dump(dict(schema="native-skill-sequence/v1", timeline=timeline)) + ) + task = tmp_path / "task.yaml" + task.write_text("id: EmbodiedEnv-v1\nphysics: default\n") + write_sequence_expansion_profile(tmp_path, timeline) + profile_path = tmp_path / "expansion.yaml" + data = yaml.safe_load(profile_path.read_text()) + # A requested demo index is excluded at the native-only dispatch boundary. + data["source"]["trajectory_call_indices"] = [0, 1, 2] + if unsupported == "scene": + data["scene_randomization"]["enabled"] = True + data["scene_randomization"]["change_scope"] = "cube_initial_pose_only" + if unsupported == "source": + data["source"]["kind"] = "task_program" + profile_path.write_text(yaml.safe_dump(data)) + args = _create_parser().parse_args( + [ + "--gym_config", + str(task), + "--skill-sequence", + str(manifest), + "--expansion-profile", + str(profile_path), + "--headless", + ] + ) + resolved = _resolve_expansion_request(args, {}) + assert resolved is not None + if unsupported in {"scene", "source", "capacity"}: + with pytest.raises(ValueError): + validate_sequence_expansion( + resolved[0], manifest, 2 if unsupported == "capacity" else 1 + ) + else: + assert ( + validate_sequence_expansion(resolved[0], manifest, 1)["timeline"] + == timeline + ) + if unsupported == "recipes": + args.max_episodes = 4 + if unsupported == "video": + args.expansion_save_video = True + if unsupported == "dataset": + args.expansion_dataset_dir = str(tmp_path / "dataset") + if unsupported: + with pytest.raises(ValueError): + validate_sequence_collection( + args, {}, resolved[0], resolved[1], resolved[3] + ) + else: + plan = validate_sequence_collection( + args, {}, resolved[0], resolved[1], resolved[3] + ) + assert plan.target_episodes == 3 diff --git a/tests/sim/atomic_skills/test_fitting.py b/tests/sim/atomic_skills/test_fitting.py index eaffc58e71..3faa58060d 100644 --- a/tests/sim/atomic_skills/test_fitting.py +++ b/tests/sim/atomic_skills/test_fitting.py @@ -108,9 +108,11 @@ def compute_batch_ik(self, pose, name, joint_seed, **kwargs): return valid.reshape(pose.shape[:2]), q.reshape(*pose.shape[:2], 6) -def _engine(): +def _engine(robot=None): cfg = TrapezoidalPlannerCfg(robot_uid="analytic_cartesian_oracle") - planner = TrapezoidalPlanner(cfg, robot=CartesianRobot()) + planner = TrapezoidalPlanner( + cfg, robot=CartesianRobot() if robot is None else robot + ) generator = MotionGenerator(MotionGenCfg(planner_cfg=cfg), planner=planner) return AtomicSkillEngine( generator, @@ -169,6 +171,119 @@ def test_pickup_inverse_native_and_splice_preserve_source_and_report_rejection( assert np.ptp(arrays["native_phase_qpos"][:, 2]) > 0.1 +def test_scene_pickup_uses_object_grasp_preserves_boundary_and_unselected_joints() -> ( + None +): + """A valid object grasp may differ from rotating/noisy real TCP samples.""" + from embodichain.lab.sim.atomic_skills import ObjectSemantics + from embodichain.lab.sim.atomic_skills.affordance import AntipodalAffordance + from embodichain.lab.sim.atomic_skills.primitives import GraspGoal + + class RobotWithPassiveAxis(CartesianRobot): + dof = 8 + joint_names = tuple(f"coordinate{i}" for i in range(8)) + + source = _pickup() + qpos = np.column_stack((source.qpos, np.sin(source.timestamps) * 0.01)).astype( + np.float32 + ) + # There is no stable pre-grasp orientation, and the valid grasp differs + # from the real TCP by 3 cm (above the old 1.5 cm reconstruction gate). + qpos[:, 3] = np.linspace(0, 0.2, len(qpos)) + recording = JointRecording( + source.timestamps, qpos, RobotWithPassiveAxis.joint_names, "measured_state" + ) + anchor = torch.eye(4) + anchor[:3, 3] = torch.tensor([0.23, 0.0, 0.05]) + goal = GraspGoal( + semantics=ObjectSemantics(AntipodalAffordance(), {}, "known_object"), + object_pose=torch.eye(4), + grasp_xpos=anchor, + ) + arrays, report = fit_skill_segment( + recording, + _engine(RobotWithPassiveAxis()), + skill_id="pick_up", + endpoints={"primary": {"motion": "tool_motion", "grasp": "fingers"}}, + start=6, + stop=141, + acceleration_limits=np.full(8, 1000.0), + cfg=SkillFitCfg(max_candidates=1), + scene_grasps=(goal,), + ) + assert report["accepted"], report["failures"] + assert report["parameters"]["object_id"] == "known_object" + assert report["parameters"]["lift_height"] == pytest.approx(0.12, abs=1e-6) + close = next(p for p in report["phases"] if p["name"] == "close") + a, b = report["start"] + close["start"], report["start"] + close["stop"] + np.testing.assert_allclose( + arrays["patched_qpos"][a:b, :3], np.tile([0.23, 0, 0.05], (b - a, 1)), atol=1e-6 + ) + np.testing.assert_array_equal(arrays["patched_qpos"][:9], qpos[:9]) + np.testing.assert_array_equal(arrays["patched_qpos"][138:], qpos[138:]) + np.testing.assert_array_equal(arrays["patched_qpos"][:, 7], qpos[:, 7]) + + +@pytest.mark.parametrize("fixed_hints", [False, True]) +def test_scene_phase_retiming_respects_explicit_hints(fixed_hints) -> None: + """A late observed close leaves too little time for a changed grasp to exit.""" + from embodichain.lab.sim.atomic_skills import ObjectSemantics + from embodichain.lab.sim.atomic_skills.affordance import AntipodalAffordance + from embodichain.lab.sim.atomic_skills.primitives import GraspGoal + + class LimitedXSpeedRobot(CartesianRobot): + def get_qvel_limits(self, name=None, joint_ids=None): + ids = self.get_joint_ids(name) if joint_ids is None else joint_ids + return torch.tensor([[1.0 if i == 0 else 100.0 for i in ids]]) + + qpos = np.zeros((150, 7), dtype=np.float32) + qpos[:, 0] = 0.2 + qpos[:, 2] = np.r_[ + np.linspace(0.2, 0.05, 100), + np.full(13, 0.05), + np.linspace(0.05, 0.17, 28), + np.full(9, 0.17), + ] + qpos[:, 6] = np.r_[np.zeros(100), np.linspace(0, 0.03, 13), np.full(37, 0.03)] + recording = JointRecording( + np.arange(150) * 0.01, qpos, CartesianRobot.joint_names, "design_reference" + ) + goal_pose = torch.eye(4) + goal_pose[:3, 3] = torch.tensor([0.4, 0, 0.05]) + goal = GraspGoal( + semantics=ObjectSemantics(AntipodalAffordance(), {}, "target"), + grasp_xpos=goal_pose, + object_pose=torch.eye(4), + ) + hints = [ + dict(name="approach", start=0, stop=94), + dict(name="close", start=94, stop=107), + dict(name="lift", start=107, stop=135), + ] + arrays, report = fit_skill_segment( + recording, + _engine(LimitedXSpeedRobot()), + skill_id="pick_up", + endpoints={"primary": {"motion": "tool_motion", "grasp": "fingers"}}, + start=6, + stop=141, + acceleration_limits=np.array([10] + [1000] * 6), + cfg=SkillFitCfg(max_candidates=1), + scene_grasps=(goal,), + phase_hints=hints if fixed_hints else None, + ) + assert report["accepted"] == (not fixed_hints), report["failures"] + assert report["source_phases"] == hints + if fixed_hints: + assert report["phases"] == hints + np.testing.assert_array_equal(arrays["patched_qpos"], qpos) + else: + close = report["phases"][1] + assert close["start"] < hints[1]["start"] + assert close["stop"] - close["start"] == 13 + assert report["limits"]["velocity_ratio"] <= 1 + + @pytest.mark.parametrize("skill", ("slide", "twist")) def test_axis_skills_identify_geometric_parameters_and_use_native_motion(skill) -> None: engine = _engine() @@ -319,3 +434,74 @@ def test_moving_arm_during_closure_is_rejected_as_template_mismatch() -> None: stop=141, acceleration_limits=np.full(7, 1000.0), ) + + +@pytest.mark.parametrize("recipe,operator", [(1, "joint_residual"), (2, "via_points")]) +def test_sequence_expansion_preserves_gripper_contact_and_unbound_joints( + tmp_path, recipe, operator +): + from embodichain.lab.sim.atomic_skills import ( + PickUpMotionGoal, + PickUpOptions, + MotionPolicy, + ) + from embodichain.lab.sim.motion.expansion import load_expansion_profile + from embodichain.toolkits.skill_fitting._sequence_expansion import ( + _transform_factory, + write_sequence_expansion_profile, + ) + + class WithPassiveJoint(CartesianRobot): + uid = "independent_cartesian" + dof = 8 + joint_names = tuple(f"coordinate{i}" for i in range(8)) + + robot = WithPassiveJoint() + robot.body_data = SimpleNamespace(qpos_limits=robot.get_qpos_limits()) + engine = _engine(robot) + goal = torch.eye(4) + goal[:3, 3] = torch.tensor([0.2, 0, 0.1]) + invocation = engine.make_invocation( + "pick_up", + PickUpMotionGoal(goal), + control_parts={"primary": {"motion": "tool_motion", "grasp": "fingers"}}, + motion_policy=MotionPolicy( + strategy="motion_gen", sample_count=80, acceleration_limit=0.5 + ), + skill_options=PickUpOptions( + lift_height=0.12, pre_grasp_distance=0.1, hand_interp_steps=20 + ), + ) + context = engine.initial_context(control_dt=0.01) + baseline = engine.plan(invocation, context) + write_sequence_expansion_profile(tmp_path, [dict(kind="atomic_skill")]) + profile = load_expansion_profile(tmp_path / "expansion.yaml") + limits = SimpleNamespace( + joint_names=robot.joint_names, + velocity_limits=[100.0] * 8, + acceleration_limits=[1000.0] * 8, + ) + transform = _transform_factory(profile, "a" * 64, recipe, 0, limits)(0, engine) + expanded = engine.plan(invocation, context, plan_transform=transform) + repeated = engine.plan(invocation, context, plan_transform=transform) + original = baseline.joint_trajectory.positions + actual = expanded.joint_trajectory.positions + assert bool(expanded.plan_success.all()) + assert (actual - original).abs().max() > 1e-5 + torch.testing.assert_close(actual[:, :, 6:], original[:, :, 6:], rtol=0, atol=0) + torch.testing.assert_close(actual[:, [0, -1]], original[:, [0, -1]], rtol=0, atol=0) + for segment in baseline.segments: + if segment.name == "close": + torch.testing.assert_close( + actual[:, segment.start : segment.stop], + original[:, segment.start : segment.stop], + rtol=0, + atol=0, + ) + torch.testing.assert_close( + expanded.joint_trajectory.dt, baseline.joint_trajectory.dt, rtol=0, atol=0 + ) + torch.testing.assert_close( + repeated.joint_trajectory.positions, actual, rtol=0, atol=0 + ) + assert expanded.diagnostics.metadata["expansion"]["rows"][0]["operator"] == operator diff --git a/tests/sim/spawn/test_descriptors.py b/tests/sim/spawn/test_descriptors.py index 835e05081e..d4a025fd9d 100644 --- a/tests/sim/spawn/test_descriptors.py +++ b/tests/sim/spawn/test_descriptors.py @@ -1078,6 +1078,75 @@ def test_spawn_forwards_convex_decomposition_algorithm( assert collision.decomp_algorithm == expected +def test_precomputed_decomposition_keeps_separate_colliders_and_original_visual( + tmp_path, +) -> None: + """Two disjoint tetrahedra remain two colliders, preserving the empty gap.""" + parts = tmp_path / "parts.obj" + parts.write_text( + "o left\nv 0 0 0\nv 1 0 0\nv 0 1 0\nv 0 0 1\n" + "f 1 3 2\nf 1 2 4\nf 1 4 3\nf 2 3 4\n" + "o right\nv 3 0 0\nv 4 0 0\nv 3 1 0\nv 3 0 1\n" + "f 5 7 6\nf 5 6 8\nf 5 8 7\nf 6 7 8\n" + ) + cfg = RigidObjectCfg.from_dict( + dict( + uid="cup", + body_type="dynamic", + body_scale=[2.0, 2.0, 2.0], + shape=dict( + shape_type="Mesh", + fpath="original.glb", + collision=dict( + approximation="convex_decomposition", + max_hulls=8, + decomposition_fpath=str(parts), + ), + ), + attrs=dict(material_props=dict(static_friction=0.9, dynamic_friction=0.8)), + ) + ) + descriptor, _ = rigid_desc_from_cfg(cfg) + assert descriptor.renders[0].file_path == "original.glb" + np.testing.assert_array_equal(descriptor.body_scale, (2.0, 2.0, 2.0)) + assert len(descriptor.collisions) == 2 + for collider, expected_minimum in zip( + descriptor.collisions, ([0, 0, 0], [3, 0, 0]) + ): + assert collider.approximation == CollisionApproximation.CONVEX_HULL + assert collider.file_path is None + np.testing.assert_array_equal( + np.min(collider.vertices, axis=0), expected_minimum + ) + np.testing.assert_array_equal( + np.max(collider.vertices, axis=0), np.asarray(expected_minimum) + 1 + ) + assert collider.dexsim == descriptor.collisions[0].dexsim + assert collider.newton == descriptor.collisions[0].newton + + +def test_precomputed_decomposition_missing_or_empty_fails_without_recooking( + tmp_path, +) -> None: + parts = tmp_path / "parts.obj" + cfg = RigidObjectCfg( + uid="cup", + shape=MeshCfg( + fpath="visual.obj", + collision=MeshCollisionCfg( + approximation="convex_decomposition", + max_hulls=8, + decomposition_fpath=str(parts), + ), + ), + ) + with pytest.raises(FileNotFoundError): + rigid_desc_from_cfg(cfg) + parts.write_text("# interrupted or empty input\n") + with pytest.raises(ValueError, match="no convex parts"): + rigid_desc_from_cfg(cfg) + + def test_spawn_rejects_unsupported_convex_decomposition_method() -> None: cfg = RigidObjectCfg( uid="mesh", diff --git a/tests/sim/test_cfg.py b/tests/sim/test_cfg.py index 971838a70f..334ef12c97 100644 --- a/tests/sim/test_cfg.py +++ b/tests/sim/test_cfg.py @@ -931,6 +931,12 @@ def test_mesh_collision_cfg_rejects_both_hull_count_names() -> None: {"approximation": "sdf", "sdf_resolution": 64.5}, {"approximation": "sdf", "sdf_padding": float("nan")}, {"approximation": "sdf", "sdf_texture_format": "invalid"}, + {"approximation": "convex_hull", "decomposition_fpath": "parts.obj"}, + { + "approximation": "convex_decomposition", + "max_hulls": 8, + "decomposition_fpath": "", + }, ], ) def test_mesh_collision_cfg_rejects_invalid_numeric_types_and_values( diff --git a/tests/toolkits/test_scene_collision.py b/tests/toolkits/test_scene_collision.py new file mode 100644 index 0000000000..5e624326aa --- /dev/null +++ b/tests/toolkits/test_scene_collision.py @@ -0,0 +1,149 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Content invalidation, durable publication and concurrent scene preparation.""" + +from __future__ import annotations + +from concurrent.futures import ThreadPoolExecutor +from pathlib import Path +import threading +import time + +import numpy as np +import pytest +import yaml + +from embodichain.toolkits.skill_fitting import _collision + +pytestmark = pytest.mark.no_sim + + +@pytest.fixture +def collision_scene(tmp_path, monkeypatch): + """Two known disjoint tetrahedra, with an isolated cooking oracle/cache.""" + mesh = tmp_path / "mesh.obj" + content = ( + "o left\nv 0 0 0\nv 1 0 0\nv 0 1 0\nv 0 0 1\n" + "f 1 3 2\nf 1 2 4\nf 1 4 3\nf 2 3 4\n" + "o right\nv 3 0 0\nv 4 0 0\nv 3 1 0\nv 3 0 1\n" + "f 5 7 6\nf 5 6 8\nf 5 8 7\nf 6 7 8\n" + ) + mesh.write_text(content) + config = dict( + objects=dict( + target=dict( + pose=np.eye(4).tolist(), + shape=dict( + shape_type="Mesh", + fpath="./mesh.obj", + collision=dict(approximation="convex_decomposition", max_hulls=8), + ), + ) + ) + ) + scene = tmp_path / "scene.yaml" + scene.write_text(yaml.safe_dump(config)) + control = dict( + calls=0, entered=threading.Event(), release=threading.Event(), fail=False + ) + control["release"].set() + + def cook(vertices, triangles, recipe, directory): + control["calls"] += 1 + control["entered"].set() + assert control["release"].wait(5), "Cooking oracle was not released." + output = directory / "output.obj" + output.write_text(content) + if control["fail"]: + raise RuntimeError("native cooking failed") + return output + + monkeypatch.setattr(_collision, "_cook", cook) + return dict( + scene=scene, config=config, mesh=mesh, cache=tmp_path / "cache", control=control + ) + + +@pytest.mark.parametrize("change", ["mesh", "parameters", "engine", "corruption"]) +def test_cache_reuses_local_parts_and_invalidates_changed_inputs( + collision_scene, monkeypatch, change +): + data = collision_scene + prepare = lambda: _collision.prepare_scene_collision( + data["scene"], cache_root=data["cache"] + ) + first = prepare() + assert first["hull_count"] == 2 and first["cache_hits"] == 0 + # Placement is not part of object-local collision cooking. + data["config"]["objects"]["target"]["pose"][0][3] = 12.0 + data["scene"].write_text(yaml.safe_dump(data["config"])) + assert prepare()["cache_hits"] == 1 + assert data["control"]["calls"] == 1 + if change == "mesh": + data["mesh"].write_text(data["mesh"].read_text().replace("v 4 0 0", "v 5 0 0")) + elif change == "parameters": + data["config"]["objects"]["target"]["shape"]["collision"]["max_hulls"] = 4 + data["scene"].write_text(yaml.safe_dump(data["config"])) + elif change == "engine": + import dexsim + + monkeypatch.setattr(dexsim, "__version__", "different-engine-build") + else: + Path(first["objects"]["target"]["fpath"]).write_text("# interrupted file\n") + changed = prepare() + assert changed["cache_hits"] == 0 and changed["hull_count"] == 2 + assert data["control"]["calls"] == 2 + assert prepare()["cache_hits"] == 1 + assert data["control"]["calls"] == 2 + + +def test_concurrent_scene_imports_cook_one_complete_entry(collision_scene): + data = collision_scene + data["control"]["release"].clear() + with ThreadPoolExecutor(max_workers=2) as pool: + first = pool.submit( + _collision.prepare_scene_collision, data["scene"], cache_root=data["cache"] + ) + assert data["control"]["entered"].wait(5) + second = pool.submit( + _collision.prepare_scene_collision, data["scene"], cache_root=data["cache"] + ) + try: + time.sleep(0.05) + assert not second.done() + finally: + data["control"]["release"].set() + results = [first.result(timeout=5), second.result(timeout=5)] + assert data["control"]["calls"] == 1 + assert sorted(result["cache_hits"] for result in results) == [0, 1] + assert all(result["hull_count"] == 2 for result in results) + + +def test_failed_cooking_cannot_publish_a_partial_cache(collision_scene): + data = collision_scene + data["control"]["fail"] = True + with pytest.raises(RuntimeError, match="native cooking failed"): + _collision.prepare_scene_collision(data["scene"], cache_root=data["cache"]) + assert not list(data["cache"].rglob("manifest.json")) + data["control"]["fail"] = False + assert ( + _collision.prepare_scene_collision(data["scene"], cache_root=data["cache"])[ + "hull_count" + ] + == 2 + ) + assert data["control"]["calls"] == 2 diff --git a/tests/toolkits/test_scene_skill_fitting.py b/tests/toolkits/test_scene_skill_fitting.py new file mode 100644 index 0000000000..6971e83cd3 --- /dev/null +++ b/tests/toolkits/test_scene_skill_fitting.py @@ -0,0 +1,578 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Scene fitting and participant contracts independent of names/local assets.""" + +from __future__ import annotations + +import json +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest +import torch + +from embodichain.toolkits.skill_fitting._participants import select_participants +from embodichain.toolkits.skill_fitting._robot import RobotProfile +from embodichain.toolkits.skill_fitting._scene import ( + _load_pickup_defaults, + _hand_geometry, + _tcp_z_half_turn, + load_scene, + prepare_scene_grasps, + rank_grasps, +) + + +def test_scene_pickup_defaults_are_explicit_validated_and_digest_bound( + tmp_path, +) -> None: + import hashlib + + source = tmp_path / "scene_assets.yaml" + source.write_text("objects: {}\n") + cfg, files = _load_pickup_defaults(tmp_path) + assert cfg.grasp_world_z_offset_m == 0 and cfg.replay_interpolation_factor == 1 + source.write_text( + "objects: {}\nskill_fitting:\n pick_up:\n" + " grasp_world_z_offset_m: -0.01\n replay_interpolation_factor: 2\n" + ) + cfg, files = _load_pickup_defaults(source) + assert cfg.grasp_world_z_offset_m == -0.01 and cfg.replay_interpolation_factor == 2 + assert files == {str(source): hashlib.sha256(source.read_bytes()).hexdigest()} + for bad in [ + "replay_interpolation_factor: 0", + "grasp_world_z_offset_m: .nan", + "replay_factor_typo: 2", + ]: + source.write_text("skill_fitting:\n pick_up:\n " + bad + "\n") + with pytest.raises(ValueError): + _load_pickup_defaults(source) + + +@pytest.fixture +def dual_profile(tmp_path: Path) -> RobotProfile: + declarations = [''] + control_parts = {} + solvers = {} + commands = {} + names = [] + for arm, hand in (("boom", "clamp"), ("wrist_drive", "pincer")): + declarations += [ + f'', + f'', + ] + control_parts[arm] = [f"{arm}_joint"] + control_parts[hand] = [f"{hand}_a", f"{hand}_b"] + names.extend(control_parts[arm] + control_parts[hand]) + solvers[arm] = {"urdf_path": "robot.urdf", "end_link_name": arm} + commands[hand] = {"open": [0.0, 0.0], "grasp": [0.03, 0.03]} + for joint in control_parts[hand]: + declarations += [ + f'', + f'', + ] + (tmp_path / "robot.urdf").write_text( + '' + "".join(declarations) + "" + ) + (tmp_path / "profile.json").write_text( + json.dumps( + dict( + joint_names=names, + control_parts=control_parts, + solvers=solvers, + hand_commands=commands, + participants={"primary": {"motion": "boom", "grasp": "clamp"}}, + lower=[-1.0] * len(names), + upper=[1.0] * len(names), + velocity_limits=[1.0] * len(names), + acceleration_limits=[10.0] * len(names), + calibration_revision="independent_fixture", + ) + ) + ) + return RobotProfile.load(tmp_path / "profile.json") + + +@pytest.mark.parametrize( + "columns,expected", [((1, 2), "boom"), ((4, 5), "wrist_drive")] +) +def test_auto_participant_follows_configured_hand_event_not_default_role( + dual_profile, columns, expected +) -> None: + qpos = np.zeros((30, 6)) + event = np.r_[np.zeros(8), np.linspace(0, 0.03, 12), np.full(10, 0.03)] + qpos[:, list(columns)] = event[:, None] + selected, report = select_participants( + dual_profile, qpos, skill_id="pick_up", selection="auto", phase_hints=None + ) + assert selected["primary"]["motion"] == expected + assert report["mode"] == "auto" + + +def test_ambiguous_participants_require_explicit_selection(dual_profile) -> None: + qpos = np.zeros((30, 6)) + qpos[:, [1, 2, 4, 5]] = np.linspace(0, 0.03, 30)[:, None] + with pytest.raises(ValueError, match="Cannot automatically identify"): + select_participants( + dual_profile, qpos, skill_id="pick_up", selection="auto", phase_hints=None + ) + selected, _ = select_participants( + dual_profile, + qpos, + skill_id="pick_up", + selection="wrist_drive", + phase_hints=None, + ) + assert selected == {"primary": {"motion": "wrist_drive", "grasp": "pincer"}} + + +def test_runtime_transition_connects_postures_with_bounded_joint_derivatives( + dual_profile, +) -> None: + from embodichain.toolkits.skill_fitting._execution import _connection + + start = torch.zeros(6, dtype=torch.float64) + stop = torch.tensor([0.6, 0.02, 0.02, -0.4, 0.01, 0.01], dtype=torch.float64) + dt = 0.01 + path = _connection(start, stop, dual_profile, dt, 0.25) + torch.testing.assert_close(path[0], start) + torch.testing.assert_close(path[-1], stop) + velocity = torch.diff(path, dim=0) / dt + acceleration = torch.diff(velocity, dim=0) / dt + assert bool( + ( + velocity.abs().max(0).values <= torch.tensor(dual_profile.velocity_limits) + ).all() + ) + assert bool( + ( + acceleration.abs().max(0).values + <= torch.tensor(dual_profile.acceleration_limits) + ).all() + ) + assert velocity[[0, -1]].abs().max() < 0.002 + + +def test_native_pickup_grounds_relative_grasp_against_current_object_pose() -> None: + from embodichain.toolkits.skill_fitting._execution import _invocation + + pose = torch.eye(4) + pose[:3, :3] = torch.tensor([[0.0, -1, 0], [1, 0, 0], [0, 0, 1]]) + pose[:3, 3] = torch.tensor([0.5, -0.1, 0.72]) + relative = torch.eye(4) + relative[:3, 3] = torch.tensor([0.03, 0.02, -0.01]) + obj = SimpleNamespace( + uid="target", + get_local_pose=lambda **_: pose[None], + get_vertices=lambda **_: torch.tensor([[[0.0, 0, 0], [1, 0, 0], [0, 1, 0]]]), + get_triangles=lambda **_: torch.tensor([[[0, 1, 2]]]), + ) + engine = SimpleNamespace( + device=torch.device("cpu"), + make_invocation=lambda skill, goal, **kwargs: (goal, kwargs), + ) + goal, kwargs = _invocation( + engine, + dict( + skill_id="pick_up", + goal=dict( + kind="object_grasp", object_id="target", object_to_tcp=relative.tolist() + ), + control_parts={"primary": {"motion": "custom_arm", "grasp": "custom_hand"}}, + options=dict(lift_height=0.12), + motion_policy=dict(strategy="motion_gen", sample_count=80, control_dt=0.01), + ), + {"target": obj}, + ) + torch.testing.assert_close(goal.grasp_xpos, pose @ relative) + torch.testing.assert_close(goal.object_pose, pose) + assert goal.semantics.entity_id == "target" + assert kwargs["control_parts"]["primary"]["grasp"] == "custom_hand" + assert kwargs["skill_options"].lift_height == 0.12 + + +@pytest.mark.parametrize("both_participants", [False, True]) +def test_native_sequence_holds_loaded_gripper_command_during_other_arm_motion( + dual_profile, both_participants +) -> None: + from embodichain.toolkits.skill_fitting._execution import _bound_command_path + + # The first hand is loaded: its actual opening is 0.02, while the commanded + # full closure is 0.03. The second manipulator must not release that preload. + previous = torch.tensor([0.4, 0.03, 0.03, 0.0, 0.0, 0.0]) + planned = torch.tensor( + [[0.39, 0.02, 0.02, 0.0, 0.0, 0.0], [0.39, 0.02, 0.02, 0.2, 0.03, 0.03]] + ) + participants = {"primary": {"motion": "wrist_drive", "grasp": "pincer"}} + if both_participants: + participants["secondary"] = {"motion": "boom", "grasp": "clamp"} + actual = _bound_command_path(planned, previous, dual_profile, participants) + expected = ( + planned + if both_participants + else torch.tensor( + [[0.4, 0.03, 0.03, 0.0, 0.0, 0.0], [0.4, 0.03, 0.03, 0.2, 0.03, 0.03]] + ) + ) + torch.testing.assert_close(actual, expected, rtol=0, atol=0) + + +def test_profile_physics_does_not_override_kinematics(dual_profile, tmp_path) -> None: + profile_path = tmp_path / "profile.json" + raw = json.loads(profile_path.read_text()) + physics = dict( + root_props=dict(fixed_base=True, min_position_iters=32, min_velocity_iters=8), + joint_drive_props=dict(stiffness={"clamp": 500.0}, damping={"clamp": 50.0}), + link_attrs={ + "pad": dict( + link_names_expr=["clamp_a"], attrs=dict(mass_props=dict(mass=0.064)) + ) + }, + ) + raw["simulation"] = physics + profile_path.write_text(json.dumps(raw)) + loaded = RobotProfile.load(profile_path) + assert loaded.metadata()["simulation"] == physics + assert loaded.solvers == dual_profile.solvers + assert loaded.control_parts == dual_profile.control_parts + raw["simulation"]["init_pos"] = [0, 0, 1] + profile_path.write_text(json.dumps(raw)) + with pytest.raises(ValueError, match="simulation permits only"): + RobotProfile.load(profile_path) + + +def test_scene_keeps_local_mesh_and_exported_world_pose_separate( + tmp_path: Path, +) -> None: + import trimesh + + mesh = trimesh.creation.box(extents=(0.02, 0.03, 0.04)) + mesh.export(tmp_path / "asset.obj") + pose = [[0, -1, 0, 2], [1, 0, 0, 3], [0, 0, 1, 4], [0, 0, 0, 1]] + (tmp_path / "scene_assets.yaml").write_text( + "objects:\n target:\n shape:\n shape_type: Mesh\n fpath: asset.obj\n" + " support:\n graspable: false\n shape:\n shape_type: Mesh\n fpath: asset.obj\n" + ) + (tmp_path / "export_report.json").write_text( + json.dumps( + {"target": {"anchor": pose}, "support": {"anchor": np.eye(4).tolist()}} + ) + ) + objects, files = load_scene(tmp_path) + assert len(objects) == 1 + np.testing.assert_allclose(objects[0].vertices.min(0), [-0.01, -0.015, -0.02]) + np.testing.assert_allclose(objects[0].pose, pose, atol=1e-12) + assert set(Path(p).name for p in files) == { + "asset.obj", + "export_report.json", + "scene_assets.yaml", + } + exported, _ = load_scene(tmp_path, include_ungraspable=True) + assert [obj.identifier for obj in exported] == ["target", "support"] + np.testing.assert_allclose(exported[1].pose, np.eye(4)) + (tmp_path / "export_report.json").unlink() + with pytest.raises(ValueError, match="world-from-object"): + load_scene(tmp_path) + + +def test_grasp_ranking_excludes_invalid_and_compares_full_pose() -> None: + from embodichain.lab.sim.atomic_skills.affordance_sampling import ( + AffordancePoseCandidates, + ) + + poses = torch.eye(4).repeat(3, 1, 1) + poses[1, 0, 3] = 0.01 + poses[1, :3, :3] = torch.diag(torch.tensor([-1.0, -1.0, 1.0])) + poses[2, 0, 3] = 0.02 + candidates = AffordancePoseCandidates.from_rows( + [(poses, torch.tensor([float("inf"), 0, 0]))], device=torch.device("cpu") + ) + # The exact pose is invalid. The 2 cm candidate beats a 1 cm candidate + # rotated by pi, under the declared 5 cm/radian orientation weight. + assert rank_grasps(candidates, np.eye(4), 0.05) == [2, 1] + + +def test_half_turn_preserves_runtime_tcp_origin_and_qualifies_its_actual_surfaces() -> ( + None +): + """A tilted, offset contact calibration must not change the symmetry axis.""" + from scipy.spatial.transform import Rotation + from embodichain.lab.sim.atomic_skills.affordance_sampling import ( + AffordancePoseCandidates, + ) + from embodichain.toolkits.graspkit.pg_grasp.fixed_geometry import ( + FixedGripperCollisionGeometry, + ) + + calibration = np.eye(4) + calibration[:3, :3] = Rotation.from_euler("xyz", [0.2, 0.3, 0.4]).as_matrix() + calibration[:3, 3] = [0.01, -0.02, -0.05] + points = torch.tensor([[0.01, 0.02, 0.03], [-0.04, 0.05, -0.06]]) + geometry = FixedGripperCollisionGeometry( + points, {"grasp_from_tcp": calibration.tolist()} + ) + variant = _tcp_z_half_turn(geometry) + contact = np.eye(4) + contact[:3, :3] = Rotation.from_euler("xyz", [-0.4, 0.5, -0.6]).as_matrix() + contact[:3, 3] = [1.0, 2.0, 3.0] + original_tcp = contact @ calibration + half_turn = np.diag([-1.0, -1.0, 1.0, 1.0]) + expected_tcp = original_tcp @ half_turn + actual_tcp = variant.canonical_to_tcp_poses(torch.tensor(contact)).numpy() + np.testing.assert_allclose(actual_tcp, expected_tcp, atol=1e-12) + np.testing.assert_allclose(actual_tcp[:3, 3], original_tcp[:3, 3]) + np.testing.assert_allclose(actual_tcp[:3, 2], original_tcp[:3, 2]) + # Expected physical surface points: take the original points into TCP, + # rotate the whole hand about that local TCP Z, then place it in world. + tcp_from_contact = np.linalg.inv(calibration) + tcp_points = points.numpy() @ tcp_from_contact[:3, :3].T + tcp_from_contact[:3, 3] + expected_points = tcp_points @ expected_tcp[:3, :3].T + expected_tcp[:3, 3] + actual_points = variant.points.numpy() @ contact[:3, :3].T + contact[:3, 3] + np.testing.assert_allclose(actual_points, expected_points, atol=2e-8) + np.testing.assert_array_equal(geometry.points, points) + np.testing.assert_allclose(geometry.provenance["grasp_from_tcp"], calibration) + # An observed pose ten degrees from the half turn must choose it rather + # than the physically equivalent pose 170 degrees away, in a rotated world. + reference = expected_tcp.copy() + reference[:3, :3] = ( + expected_tcp[:3, :3] @ Rotation.from_euler("z", 10, degrees=True).as_matrix() + ) + candidates = AffordancePoseCandidates.from_rows( + [ + ( + torch.tensor(np.stack((original_tcp, actual_tcp)), dtype=torch.float32), + torch.zeros(2), + ) + ], + device=torch.device("cpu"), + ) + assert rank_grasps(candidates, reference, 0.05) == [1, 0] + + +def test_contact_grounding_preserves_a_virtual_runtime_tcp(tmp_path: Path) -> None: + """The configured TCP is 56 mm behind the geometry's contact centre.""" + parts = [ + '', + '', + ] + for name, x, axis in (("one", -0.04, 1), ("two", 0.04, -1)): + parts.append( + f'' + ) + parts.append( + f'' + ) + path = tmp_path / "hand.urdf" + path.write_text('' + "".join(parts) + "") + tcp = np.eye(4) + tcp[2, 3] = 0.10 + profile = RobotProfile( + joint_names=("pivot", "one", "two"), + control_parts={"drive": ["pivot"], "clamp": ["one", "two"]}, + solvers={ + "drive": { + "urdf_path": str(path), + "end_link_name": "hand", + "tcp": tcp.tolist(), + } + }, + hand_commands={"clamp": {"open": [0.0, 0.0], "grasp": [0.03, 0.03]}}, + ) + geometry, model = _hand_geometry( + profile, {"motion": "drive", "grasp": "clamp"}, np.zeros(3), 0.004 + ) + contact = torch.eye(4) + contact[:3, 3] = torch.tensor([1.0, 2.0, 3.0]) + tcp_pose = geometry.canonical_to_tcp_poses(contact) + np.testing.assert_allclose(tcp_pose[:3, 3], [1.0, 2.0, 2.944], atol=1e-6) + assert geometry.points[:, 2].max() == pytest.approx(0.004, abs=1e-6) + assert model.max_opening_width == pytest.approx(0.06) + + +@pytest.mark.parametrize( + "overrides,expected_angle,offset", + [ + ({}, 10.0, 0.0), + ({"grasp_filter_ground_collision": True}, 170.0, 0.0), + ({"grasp_approach_deviation_rad": np.pi / 6}, 170.0, 0.0), + ({}, 10.0, -0.01), + ], +) +def test_scene_selects_near_half_turn_only_when_its_query_accepts_it( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch, overrides, expected_angle, offset +) -> None: + """Exercise host configuration, local symmetry and validity masks together.""" + from scipy.spatial.transform import Rotation + import trimesh + from embodichain.lab.sim.atomic_skills.affordance import AntipodalAffordance + from embodichain.lab.sim.atomic_skills.affordance_sampling import ( + AffordancePoseCandidates, + ) + from embodichain.lab.sim.atomic_skills.fitting import SkillFitCfg + from embodichain.toolkits.graspkit.pg_grasp.fixed_geometry import ( + FixedGripperCollisionGeometry, + ) + from embodichain.toolkits.graspkit.pose_generator import ParallelJawGripperModelCfg + from embodichain.toolkits.skill_fitting import _scene + + mesh = trimesh.creation.box(extents=(0.02, 0.02, 0.02)) + mesh.export(tmp_path / "object.obj") + (tmp_path / "scene_assets.yaml").write_text( + json.dumps( + { + "objects": { + "target": { + "shape": {"shape_type": "Mesh", "fpath": "object.obj"}, + "pose": np.eye(4).tolist(), + } + } + } + ) + ) + calibration = np.eye(4) + calibration[:3, :3] = Rotation.from_euler("y", 0.2).as_matrix() + calibration[:3, 3] = [0.01, 0.02, -0.05] + geometry = FixedGripperCollisionGeometry( + torch.tensor([[0.01, 0.02, 0.03]]), + { + "grasp_from_tcp": calibration.tolist(), + "files_sha256": {}, + }, + ) + model = ParallelJawGripperModelCfg( + model_id="test_clamp", min_opening_width=0.001, max_opening_width=0.1 + ) + monkeypatch.setattr(_scene, "_hand_geometry", lambda *args: (geometry, model)) + + source_tcp = np.eye(4) + source_tcp[2, 3] = 0.1 + far_tcp = source_tcp.copy() + far_tcp[:3, :3] = Rotation.from_euler("z", 170, degrees=True).as_matrix() + canonical_pose = torch.tensor( + far_tcp @ np.linalg.inv(calibration), dtype=torch.float32 + ) + + def qualified_pool(self, generator, object_poses, approach_direction): + # Independent geometric-query oracle: the near roll needs a wider + # approach cone and is blocked by the optional inferred support plane. + half_turn = ( + generator.collision_geometry.provenance.get("tcp_local_z_rotation_rad") + is not None + ) + valid = not half_turn or ( + not generator.collision_cfg.filter_ground_collision + and generator.algorithm_cfg.approach_deviation_angle > np.pi / 4 + ) + pose = generator.collision_geometry.canonical_to_tcp_poses(canonical_pose) + return AffordancePoseCandidates.from_rows( + [(pose[None], torch.tensor([0.0 if valid else float("inf")]))], + device="cpu", + ) + + monkeypatch.setattr(AntipodalAffordance, "get_grasp_candidates", qualified_pool) + + class RecordedRobot: + device = torch.device("cpu") + + def get_joint_ids(self, name): + return [0] if name == "drive" else [1] + + def compute_fk(self, qpos, name): + poses = torch.eye(4).repeat(len(qpos), 1, 1) + poses[:, 2, 3] = qpos[:, 0] + return poses + + qpos = np.column_stack( + ( + np.r_[np.linspace(0.25, 0.1, 8), np.full(22, 0.1)], + np.r_[np.zeros(10), np.linspace(0, 0.03, 10), np.full(10, 0.03)], + ) + ) + profile = SimpleNamespace(hand_commands={"clamp": {"open": [0.0], "grasp": [0.03]}}) + goals, metadata, _ = prepare_scene_grasps( + tmp_path, + "target", + RecordedRobot(), + profile, + {"motion": "drive", "grasp": "clamp"}, + qpos, + SkillFitCfg(**overrides), + None, + grasp_world_z_offset_m=offset, + ) + actual = Rotation.from_matrix(goals[0].grasp_xpos.numpy()[:3, :3]).magnitude() + assert np.rad2deg(actual) == pytest.approx(expected_angle, abs=1e-5) + np.testing.assert_allclose( + goals[0].grasp_xpos.numpy()[:3, 3], + source_tcp[:3, 3] + [0, 0, offset], + atol=1e-7, + ) + np.testing.assert_allclose(goals[0].object_pose.numpy(), np.eye(4)) + np.testing.assert_allclose( + np.array(metadata["nominal_grasp_xpos"])[0, :3, 3], source_tcp[:3, 3], atol=1e-7 + ) + assert metadata["grasp_world_z_offset_m"] == offset + assert metadata["ground_collision_filter"] == overrides.get( + "grasp_filter_ground_collision", False + ) + + +@pytest.mark.parametrize("completed", [True, False]) +def test_sequence_trace_keeps_submitted_commands_and_measured_states_causal( + tmp_path, completed +) -> None: + import h5py + from embodichain.toolkits.skill_fitting._sequence_trace import _SequenceTrace + + robot = SimpleNamespace(joint_names=["a", "b"], step=0) + robot.get_qpos = lambda: torch.tensor( + [[robot.step, -robot.step]], dtype=torch.float32 + ) + robot.get_qvel = lambda: torch.tensor([[1.0, -1.0]]) + robot.get_local_pose = lambda: torch.tensor([[0.0, 0, 0, 0, 0, 0, 1.0]]) + obj = SimpleNamespace( + get_local_pose=lambda: torch.tensor( + [[0.0, 0, 0.7 + robot.step * 0.001, 0, 0, 0, 1.0]] + ) + ) + sim = SimpleNamespace( + get_rigid_object_uid_list=lambda: ["cup"], get_rigid_object=lambda uid: obj + ) + env = SimpleNamespace( + robot=robot, sim=sim, expert_action_spec=SimpleNamespace(joint_names=["a"]) + ) + trace = _SequenceTrace( + tmp_path / "trace.h5", env, SimpleNamespace(joint_names=["b", "a"]), 0.01 + ) + for index in range(1, 301): + robot.step = index + trace.append(torch.tensor([index + 1000.0])) + trace.close(completed=completed) + with h5py.File(tmp_path / "trace.h5") as data: + np.testing.assert_array_equal( + data["commands/qpos"][:, 0], np.arange(1001, 1301) + ) + np.testing.assert_array_equal(data["states/qpos"][:, 0], -np.arange(301)) + np.testing.assert_array_equal(data["states/qpos"][:, 1], np.arange(301)) + np.testing.assert_allclose( + data["states/objects/cup/pose"][:, 2], + 0.7 + np.arange(301) * 0.001, + atol=1e-7, + ) + assert data.attrs["completed"] == completed + assert data.attrs["command_count"] == 300 diff --git a/tests/toolkits/trajectory_annotator/conftest.py b/tests/toolkits/trajectory_annotator/conftest.py index 5608facd8a..36ed1db578 100644 --- a/tests/toolkits/trajectory_annotator/conftest.py +++ b/tests/toolkits/trajectory_annotator/conftest.py @@ -30,7 +30,7 @@ @pytest.fixture -def fitting_oracle(monkeypatch: pytest.MonkeyPatch) -> dict[str, Any]: +def fitting_oracle(monkeypatch: pytest.MonkeyPatch, tmp_path: Path) -> dict[str, Any]: """Isolate host lifecycle from IK with known accepted/rejected numeric bundles. Native reconstruction itself is independently covered by the Cartesian robot @@ -39,10 +39,49 @@ def fitting_oracle(monkeypatch: pytest.MonkeyPatch) -> dict[str, Any]: import numpy as np from embodichain.toolkits.skill_fitting import _pipeline + # Real lightweight geometry keeps browser playback tests independent of IK, + # while still exercising Viser's model, WebSocket and shutdown lifecycle. + urdf = tmp_path / "viewer.urdf" + urdf.write_text(""" + + + + + + + + + """) + control: dict[str, Any] = dict( accepted=True, entered=threading.Event(), release=threading.Event(), calls=[] ) control["release"].set() + import json + + times = np.arange(300, dtype=np.float64) / 100 + original = np.column_stack((times / 10, -times / 20)) + joints_path, profile_path = tmp_path / "joints.npz", tmp_path / "profile.json" + np.savez(joints_path, qpos=original, timestamps=times) + profile = dict( + joint_names=["arm_joint", "hand_joint"], + control_parts={"arm": ["arm_joint"], "hand": ["hand_joint"]}, + solvers={ + "arm": dict(urdf_path=str(urdf), root_link_name="base", end_link_name="arm") + }, + base_poses={}, + participants={}, + hand_commands={"hand": {"open": [0.0], "grasp": [-0.1]}}, + lower=[-1.0, -1.0], + upper=[1.0, 1.0], + velocity_limits=[2.0, 2.0], + acceleration_limits=[20.0, 20.0], + calibration_revision="known_fixture", + ) + profile_path.write_text(json.dumps(profile)) + control.update( + joints=str(joints_path), profile=str(profile_path), original=original + ) def fit(inputs: Any, **kwargs: Any) -> tuple[dict[str, np.ndarray], dict[str, Any]]: control["calls"].append(kwargs) @@ -53,12 +92,16 @@ def fit(inputs: Any, **kwargs: Any) -> tuple[dict[str, np.ndarray], dict[str, An if control.get("error"): raise ValueError("No observable closing event; provide phase hints.") # A 3 s recording, 100 Hz, with two known reference columns. - times = np.arange(300, dtype=np.float64) / 100 - original = np.column_stack((times / 10, -times / 20)) + start = int(np.searchsorted(times, kwargs["frame_times"][0] - 1e-7)) + stop = int( + np.searchsorted( + times, kwargs["frame_times"][-1] + kwargs["frame_durations"][-1] - 1e-7 + ) + ) native = original.copy() - native[20:84, 0] += 0.02 + native[start:stop, 0] += 0.02 proposed = original.copy() - proposed[23:81, 0] += 0.01 + proposed[start + 3 : stop - 3, 0] += 0.01 patched = proposed if control["accepted"] else original.copy() report = dict( schema_version=1, @@ -68,13 +111,24 @@ def fit(inputs: Any, **kwargs: Any) -> tuple[dict[str, np.ndarray], dict[str, An [] if control["accepted"] else ["patched path exceeds joint limits"] ), joint_names=["arm_joint", "hand_joint"], - start=20, - stop=84, - phases=[dict(name="approach", start=0, stop=64)], + start=start, + stop=stop, + phases=[dict(name="approach", start=0, stop=stop - start)], parameters=dict(lift_height=0.12), + native_goal=dict(grasp_xpos=np.eye(4).tolist()), + native_options=dict(lift_height=0.12), + native_motion_policy=dict( + strategy="motion_gen", + sample_count=80, + acceleration_limit=0.5, + control_dt=0.01, + ), native_joint_rmse=0.02, tcp_errors={"primary": {"native_tcp_rmse_m": 0.003}}, + selected_participants={"primary": {"motion": "arm", "grasp": "hand"}}, time_offset=inputs.time_offset, + robot_profile=profile, + replay_interpolation_factor=control.get("replay_interpolation_factor", 1), **kwargs["provenance"], ) arrays = dict(timestamps=times, geometry_indices=np.arange(len(times))) diff --git a/tests/toolkits/trajectory_annotator/test_annotation.py b/tests/toolkits/trajectory_annotator/test_annotation.py index a1d2f7c9fc..751c5f45e4 100644 --- a/tests/toolkits/trajectory_annotator/test_annotation.py +++ b/tests/toolkits/trajectory_annotator/test_annotation.py @@ -457,6 +457,24 @@ def test_web_fitting_vfr_clock_concurrency_export_and_stale_revisions( assert client.get("/api/frame-index?time_seconds=nan").status_code == 422 preview = client.get(f"/api/fitting/jobs/{job}/preview").json() assert preview["report"]["parameters"]["lift_height"] == 0.12 + assert "original_links" not in preview + assert preview["viewer"]["phase"] == "ready" + viewer_endpoint = f"/api/fitting/jobs/{job}/viewer" + assert ( + client.post(viewer_endpoint, json={"time_seconds": 0.5}).status_code == 403 + ) + assert ( + client.post( + viewer_endpoint, json={"time_seconds": "nan"}, headers=headers + ).status_code + == 422 + ) + assert ( + client.post( + viewer_endpoint, json={"time_seconds": 0.5}, headers=headers + ).status_code + == 200 + ) response = client.get(f"/api/fitting/jobs/{job}/bundle") assert response.status_code == 200 with np.load(io.BytesIO(response.content), allow_pickle=False) as bundle: @@ -481,6 +499,12 @@ def test_web_fitting_vfr_clock_concurrency_export_and_stale_revisions( assert client.get("/api/fitting").json()["stale"] for endpoint in ("bundle", "preview"): assert client.get(f"/api/fitting/jobs/{job}/{endpoint}").status_code == 409 + assert ( + client.post( + viewer_endpoint, json={"time_seconds": 0.5}, headers=headers + ).status_code + == 409 + ) def test_cli_fits_saved_core_and_retains_outer_annotation( @@ -542,3 +566,455 @@ def test_cli_fits_saved_core_and_retains_outer_annotation( "start_frame": 5, "end_frame": 20, } + + +@pytest.mark.parametrize("pickup_factor", [1, 2]) +def test_whole_episode_composes_both_fits_and_exports_original_demo_tensors( + make_video: Callable[..., Path], + tmp_path: Path, + fitting_oracle: dict[str, object], + monkeypatch: pytest.MonkeyPatch, + pickup_factor: int, +) -> None: + """One submission retains both independently fitted cores and all original gaps.""" + import io + import zipfile + import numpy as np + import torch + import yaml + from fastapi.testclient import TestClient + from embodichain.toolkits.trajectory_annotator._server import create_app + from embodichain.toolkits.trajectory_annotator.video import VideoFrameSource + from embodichain.lab.gym.utils.gym_utils import config_to_cfg, load_trajectory + from embodichain.toolkits.skill_fitting import _collision + + fitting_oracle["replay_interpolation_factor"] = pickup_factor + profile_path = Path(fitting_oracle["profile"]) + profile = json.loads(profile_path.read_text()) + profile["simulation"] = dict( + root_props=dict(fixed_base=True, min_position_iters=32, min_velocity_iters=8), + joint_drive_props=dict( + stiffness={"arm": 70000.0, "hand": 500.0}, + damping={"arm": 1000.0, "hand": 50.0}, + ), + ) + profile_path.write_text(json.dumps(profile)) + + # A supplied object-local convex piece exercises portable collision paths + # through the production exporter and strict deployment loader. + parts = tmp_path / "object.obj" + parts.write_text( + "o hull\nv 0 0 0\nv 1 0 0\nv 0 1 0\nv 0 0 1\nf 1 3 2\nf 1 2 4\nf 1 4 3\nf 2 3 4\n" + ) + scene = tmp_path / "scene.yaml" + scene.write_text( + yaml.safe_dump( + dict( + objects=dict( + target=dict( + pose=np.eye(4).tolist(), + shape=dict( + shape_type="Mesh", + fpath="./object.obj", + collision=dict( + approximation="convex_decomposition", + max_hulls=8, + decomposition_fpath="./object.obj", + ), + ), + ) + ) + ) + ) + ) + prepare = _collision.prepare_scene_collision + monkeypatch.setattr( + _collision, + "prepare_scene_collision", + lambda path, **kwargs: prepare(path, cache_root=tmp_path / "cache", **kwargs), + ) + + video = make_video() + app = create_app(lambda: VideoFrameSource(video), tmp_path / "whole.json") + with TestClient(app, base_url="http://127.0.0.1") as client: + deadline = time.monotonic() + 20 + while client.get("/api/status").json()["phase"] != "ready": + assert time.monotonic() < deadline + time.sleep(0.01) + headers = { + "X-Annotation-Token": client.get("/api/project").json()["session_token"] + } + segments = [ + dict(id="second", label="Pickup", start_frame=24, end_frame=29), + dict( + id="first", + label="pick_up", + start_frame=6, + end_frame=11, + core_interval=dict(start_frame=7, end_frame=10), + ), + ] + response = client.put( + "/api/annotations", + json=dict(segments=segments, expected_revision=0), + headers=headers, + ) + revision = response.json()["revision"] + payload = dict( + scope="episode", + expected_revision=revision, + inputs=dict( + joints=fitting_oracle["joints"], + robot_profile=fitting_oracle["profile"], + qpos_semantics="measured_state", + time_offset=0, + scene=str(scene), + ), + ) + response = client.post("/api/fitting/jobs", json=payload, headers=headers) + assert response.status_code == 202 + job = response.json()["job_id"] + while (state := client.get("/api/fitting").json())["phase"] == "running": + assert time.monotonic() < deadline + time.sleep(0.01) + assert state["phase"] == "done", state + assert state["accepted"] and state["run_task_available"], state + assert len(fitting_oracle["calls"]) == 2 + with np.load( + io.BytesIO(client.get(f"/api/fitting/jobs/{job}/bundle").content) + ) as result: + original, patched = result["original_qpos"], result["patched_qpos"] + expected = original.copy() + expected[27:34, 0] += 0.01 + expected[83:97, 0] += 0.01 + np.testing.assert_array_equal(patched, expected) + np.testing.assert_array_equal(original, fitting_oracle["original"]) + response = client.get(f"/api/fitting/jobs/{job}/run-task") + assert response.status_code == 200 + with zipfile.ZipFile(io.BytesIO(response.content)) as archive: + manifest = yaml.safe_load(archive.read("skills.yaml")) + timeline = manifest["timeline"] + assert [(s["skill_id"], s["start"], s["stop"]) for s in timeline] == [ + ("execute_demo_skill", 0, 20), + ("pick_up", 20, 40), + ("execute_demo_skill", 40, 80), + ("pick_up", 80, 100), + ("execute_demo_skill", 100, 300), + ] + for span in timeline: + if span["kind"] == "atomic_skill": + assert "tensor" not in span + assert span["execution"] == "native_atomic_skill" + assert span["control_parts"] == { + "primary": {"motion": "arm", "grasp": "hand"} + } + assert span["options"]["lift_height"] == 0.12 + assert span["duration_scale"] == pickup_factor + continue + data = torch.load( + io.BytesIO(archive.read(span["tensor"])), weights_only=True + ) + np.testing.assert_array_equal( + data["qpos"].numpy(), original[span["start"] : span["stop"]].T + ) + assert data["layout"] == "DOF x n_frame" + moved = tmp_path / "moved_bundle" + archive.extractall(moved) + assert not list(moved.glob("skills/*pick_up.pt")) + assert not (moved / "trajectory.pt").exists() + assert not (moved / "qpos.pt").exists() + task = moved / "task.yaml" + cfg = config_to_cfg(yaml.safe_load(task.read_text()), source_path=task) + assert cfg.robot.control_parts == {"arm": ["arm_joint"], "hand": ["hand_joint"]} + assert cfg.robot.root_props.min_position_iters == 32 + assert cfg.robot.root_props.min_velocity_iters == 8 + assert cfg.robot.joint_drive_props.stiffness == {"arm": 70000.0, "hand": 500.0} + assert ( + json.loads((moved / "robot_profile.json").read_text())["simulation"] + == profile["simulation"] + ) + assert Path(cfg.robot.fpath).is_relative_to(moved) + assert cfg.rigid_object[0].body_type == "dynamic" + collider = Path(cfg.rigid_object[0].shape.collision.decomposition_fpath) + assert collider.is_relative_to(moved) and collider.is_file() + assert manifest["scene_collision"]["target"]["hull_count"] == 1 + # Changing either annotation invalidates the whole result and both exports. + segments[0]["end_frame"] = 30 + client.put( + "/api/annotations", + json=dict(segments=segments, expected_revision=revision), + headers=headers, + ) + for endpoint in ("bundle", "run-task", "preview"): + assert client.get(f"/api/fitting/jobs/{job}/{endpoint}").status_code == 409 + + +def test_unannotated_episode_preserves_irregular_source_tensor_and_exports_control_clock( + tmp_path: Path, fitting_oracle: dict[str, object] +) -> None: + """Demo-only recordings retain raw evidence independently of replay sampling.""" + import numpy as np + import torch + from embodichain.toolkits.skill_fitting._episode import fit_episode + from embodichain.toolkits.skill_fitting._export import export_run_task + from embodichain.toolkits.skill_fitting._pipeline import FitInputs + import yaml + + times = np.arange(300, dtype=np.float64) / 100 + times[1] = 0.017 + original = fitting_oracle["original"] + recording = tmp_path / "irregular.npz" + np.savez(recording, qpos=original, timestamps=times) + inputs = FitInputs( + joints=str(recording), + robot_profile=fitting_oracle["profile"], + time_offset=0, + qpos_semantics="measured_state", + ) + document = dict( + source=dict(frame_count=90, identity="known", timing="indexed"), + revision=0, + segments=[], + ) + + def no_frame_needed(first: int, last: int): + pytest.fail("A Demo-only recording does not need a video selection clock.") + + arrays, report = fit_episode(inputs, document, no_frame_needed) + assert report["accepted"] and report["atomic_skill_count"] == 0 + assert report["demo_skill_count"] == 1 + directory = tmp_path / "demo_only" + export_run_task(directory, arrays, report, inputs) + raw = torch.load(directory / "skills/000.execute_demo_skill.pt", weights_only=True) + np.testing.assert_array_equal(raw["qpos"].numpy(), original.T) + np.testing.assert_array_equal(raw["timestamps"].numpy(), times) + manifest = yaml.safe_load((directory / "skills.yaml").read_text()) + assert manifest["schema"] == "native-skill-sequence/v1" + assert manifest["step_dt"] == pytest.approx(0.01) + assert not (directory / "trajectory.pt").exists() + document["segments"] = [dict(id="pour", label="Pour", start_frame=5, end_frame=15)] + with pytest.raises(ValueError, match="Pour.*尚不支持"): + fit_episode(inputs, document, no_frame_needed) + + +@pytest.mark.parametrize("include_video", [False, True]) +def test_portable_fitting_inputs_survive_move_without_original_resources( + tmp_path, fitting_oracle, make_video, monkeypatch, include_video +): + import shutil + import numpy as np + import pytorch_kinematics as pk + import yaml + from embodichain.toolkits.skill_fitting._bundle import ( + pack_inputs, + load_inputs, + load_annotation_inputs, + ) + from embodichain.toolkits.skill_fitting._pipeline import FitInputs + + mesh = tmp_path / "local.obj" + mesh.write_text( + "v 0 0 0\nv 1 0 0\nv 0 1 0\nv 0 0 1\nf 1 3 2\nf 1 2 4\nf 1 4 3\nf 2 3 4\n" + ) + pose = np.eye(4) + pose[:3, 3] = [0.3, -0.2, 0.7] + scene = tmp_path / "scene.yaml" + scene.write_text( + yaml.safe_dump( + dict( + objects=dict( + cube=dict( + pose=pose.tolist(), + body_type="dynamic", + shape=dict( + shape_type="Mesh", + fpath=mesh.name, + collision=dict(approximation="convex_hull"), + ), + ) + ) + ) + ) + ) + inputs = FitInputs( + joints=fitting_oracle["joints"], + robot_profile=fitting_oracle["profile"], + scene=str(scene), + time_offset=0.125, + qpos_semantics="measured_state", + ) + document = dict( + source=dict(frame_count=3, identity="vfr", timing="pts"), + revision=1, + segments=[], + ) + clock = np.array([0, 0.03, 0.08]) + durations = np.array([0.03, 0.05, 0.04]) + video = None + if include_video: + from embodichain.toolkits.trajectory_annotator.video import VideoFrameSource + from embodichain.toolkits.trajectory_annotator._cfg import AnnotationCfg + + video = make_video(count=3, variable=True, start=90) + source = VideoFrameSource(video) + try: + info = source.prepare(threading.Event(), lambda **_: None) + store = AnnotationStore( + tmp_path / "initial.json", AnnotationCfg().document_bytes + ) + store.initialize(info) + document = store.snapshot()["document"] + frames = [source.get_frame(i) for i in range(3)] + clock = np.array([f.time_seconds for f in frames]) + durations = np.array([f.delay_seconds for f in frames]) + finally: + source.close() + packed = pack_inputs( + tmp_path / "packed", inputs, document, clock, durations, video=video + ) + moved = tmp_path / "moved" + shutil.move(packed, moved) + scene.unlink() + mesh.unlink() + Path(fitting_oracle["profile"]).unlink() + Path(fitting_oracle["joints"]).unlink() + if video is not None: + video.unlink() + loaded, saved, times, display_durations = load_inputs(moved) + np.testing.assert_array_equal(times, clock) + np.testing.assert_array_equal(display_durations, durations) + assert loaded.time_offset == 0.125 + assert saved["segments"] == document["segments"] + assert saved["source"]["identity"] == document["source"]["identity"] + with np.load(loaded.joints) as data: + np.testing.assert_array_equal(data["qpos"], fitting_oracle["original"]) + scene_data = yaml.safe_load(Path(loaded.scene).read_text()) + np.testing.assert_array_equal(scene_data["objects"]["cube"]["pose"], pose) + # Exercise the actual URDF string consumer: XML encoding headers broke moved fits. + pk.build_chain_from_urdf((moved / "robot/robot.urdf").read_text()) + assert not list(moved.rglob("export_report.json")) + if include_video: + from fastapi.testclient import TestClient + from embodichain.toolkits.trajectory_annotator import _cli, app + from embodichain.toolkits.trajectory_annotator._server import create_app + + launches = [] + + def launch(factory, annotations, **kwargs): + defaults = kwargs["fitting_options"] + for key in ("joints", "robot_profile", "scene"): + assert Path(defaults[key]).is_relative_to(moved) + assert defaults["time_offset"] == 0.125 + with TestClient( + create_app(factory, annotations, fitting_options=defaults), + base_url="http://127.0.0.1", + ) as client: + deadline = time.monotonic() + 10 + while client.get("/api/status").json()["phase"] != "ready": + assert time.monotonic() < deadline + time.sleep(0.01) + project = client.get("/api/project").json() + state = project["document"] + if not launches: + response = client.put( + "/api/annotations", + json=dict( + expected_revision=state["revision"], + segments=[ + dict( + id="pickup", + label="pick_up", + start_frame=0, + end_frame=2, + ) + ], + ), + headers={"X-Annotation-Token": project["session_token"]}, + ) + assert response.status_code == 200 + else: + assert state["segments"][0]["id"] == "pickup" + launches.append(annotations) + + monkeypatch.setattr(app, "launch_annotation", launch) + for _ in range(2): + _cli.cli(["--bundle", str(moved), "--no-browser"]) + assert launches == [moved / "annotations.edited.json"] * 2 + # Editing is persistent, while the immutable seed and inventory still validate. + assert load_inputs(moved)[1]["segments"] == [] + from embodichain.toolkits.skill_fitting.cli import main as fit_cli + + repacked = tmp_path / "repacked" + fit_cli(["--bundle", str(moved), "--package-output", str(repacked)]) + assert load_annotation_inputs(repacked)[0].is_relative_to(repacked) + with pytest.raises(ValueError, match="must not overwrite"): + load_annotation_inputs(moved, moved / "joints.npz") + with pytest.raises(SystemExit): + _cli.cli(["--bundle", str(moved), "--joints", "external.npz"]) + packed_video = next((moved / "video").iterdir()) + packed_video.write_bytes(b"changed video") + with pytest.raises(ValueError, match="Bundle resource changed: video/"): + load_annotation_inputs(moved) + else: + assert not list(moved.rglob("*.mp4")) + with pytest.raises(ValueError, match="offline-only bundle has no video"): + load_annotation_inputs(moved) + (moved / "joints.npz").write_bytes(b"changed recording") + with pytest.raises(ValueError, match="Bundle resource changed: joints.npz"): + load_inputs(moved) + + +def test_scene_import_keeps_video_requests_live_and_reports_failure( + make_video, tmp_path, monkeypatch +): + """A slow scene preparation owns no decoder/request thread; errors are visible.""" + from fastapi.testclient import TestClient + from embodichain.toolkits.skill_fitting import _collision + from embodichain.toolkits.trajectory_annotator._server import create_app + from embodichain.toolkits.trajectory_annotator.video import VideoFrameSource + + source = tmp_path / "scene.yaml" + source.write_text("objects: {}\n") + entered, release = threading.Event(), threading.Event() + + def prepare(path, *, progress): + entered.set() + assert release.wait(5) + raise ValueError("invalid collision mesh") + + monkeypatch.setattr(_collision, "prepare_scene_collision", prepare) + video = make_video() + app = create_app(lambda: VideoFrameSource(video), tmp_path / "annotations.json") + with TestClient(app, base_url="http://127.0.0.1") as client: + deadline = time.monotonic() + 10 + while client.get("/api/status").json()["phase"] != "ready": + assert time.monotonic() < deadline + time.sleep(0.01) + headers = { + "X-Annotation-Token": client.get("/api/project").json()["session_token"] + } + payload = dict(scene=str(source), joints="") + try: + assert ( + client.post( + "/api/fitting/scene", json=payload, headers=headers + ).status_code + == 202 + ) + assert entered.wait(5) + assert client.get("/api/fitting/scene").json()["phase"] == "running" + assert client.get("/api/frames/5").status_code == 200 + assert ( + client.post( + "/api/fitting/scene", + json={**payload, "joints": "other"}, + headers=headers, + ).status_code + == 409 + ) + finally: + release.set() + while (state := client.get("/api/fitting/scene").json())["phase"] == "running": + assert time.monotonic() < deadline + time.sleep(0.01) + assert state["phase"] == "error" and state["error"] == "invalid collision mesh" diff --git a/tests/toolkits/trajectory_annotator/test_browser.py b/tests/toolkits/trajectory_annotator/test_browser.py index bfd3a315b3..4769fc184f 100644 --- a/tests/toolkits/trajectory_annotator/test_browser.py +++ b/tests/toolkits/trajectory_annotator/test_browser.py @@ -119,28 +119,50 @@ def test_web_fitting_comparison_download_rejection_and_invalidation( fitting_oracle["accepted"] = False open_project(page, annotation_server[0]) - expect(page.locator("#fit-run")).to_be_disabled() + expect(page.locator("#fit-run")).to_be_enabled() create_skill_interval(page) - page.locator("#fit-joints").fill("known.npz") - page.locator("#fit-profile").fill("robot.json") + page.locator("#fit-joints").fill(fitting_oracle["joints"]) + page.locator("#fit-profile").fill(fitting_oracle["profile"]) page.locator("#fit-semantics").select_option("measured_state") page.locator("#fit-offset").fill("0") page.locator("#fit-run").click() expect(page.locator("#fit-state")).to_have_text("未通过校验", timeout=15000) expect(page.locator("#fit-result")).to_be_visible() + submitted = page.request.get(annotation_server[0] + "/api/fitting").json()["inputs"] + assert submitted["scene"] == "" + assert submitted["object_id"] == "" + assert submitted["participant"] == "auto" expect(page.locator("#fit-proposed-legend")).to_be_visible() expect(page.locator("#fit-message")).to_contain_text( "patched path exceeds joint limits" ) page.locator("#fit-joint").select_option("1") expect(page.locator("#fit-cursor")).to_contain_text("hand_joint") - page.locator("#fit-exaggeration").select_option("20") + expect(page.locator("#fit-viewer")).to_be_visible() + expect(page.locator("#fit-geometry")).to_have_count(0) + page.locator("#fit-viewer").scroll_into_view_if_needed() + expect(page.frame_locator("#fit-viewer").locator("canvas").first).to_be_visible( + timeout=15000 + ) + # An empty WebGL canvas is not evidence of a loaded scene. Wait for both + # actual robot roots after the iframe enters the browser's render viewport. + for root in ("/original", "/patched"): + expect( + page.frame_locator("#fit-viewer").get_by_text(root, exact=True) + ).to_be_visible(timeout=15000) chart = page.locator("#fit-joint-chart").bounding_box() # The plot spans 0..2.99 s. This point is video t=.2 s, where the VFR # recording's displayed frame is 5 (its nonzero first PTS is normalized). x = chart["x"] + 62 + 0.2 / 2.99 * (chart["width"] - 76) page.mouse.click(x, chart["y"] + 65) expect(page.locator("#frame-current")).to_have_text("5") + expect(page.locator("#fit-viewer-status")).to_contain_text("0.200 s") + page.locator("#fit-play-segment").click() + expect(page.locator("#frame-current")).to_have_text("89", timeout=15000) + expect(page.locator("#playback-state")).to_have_text("已暂停") + page.locator("#fit-play-all").click() + expect(page.locator("#frame-current")).to_have_text("89", timeout=15000) + expect(page.locator("#playback-state")).to_have_text("已暂停") with page.expect_download() as download: page.locator("#fit-download").click() destination = tmp_path / "fit.npz" @@ -152,6 +174,11 @@ def test_web_fitting_comparison_download_rejection_and_invalidation( page.locator("#commit").click() expect(page.locator("#fit-state")).to_have_text("标注已变化") expect(page.locator("#fit-download")).to_have_attribute("aria-disabled", "true") + expect(page.locator("#fit-viewer")).to_be_hidden() + expect(page.locator("#fit-play-segment")).to_be_disabled() + expect(page.locator("#fit-download-task")).to_have_attribute( + "aria-disabled", "true" + ) # An unobservable segment must surface the error and release the job guard, # allowing a corrected configuration/selection to be fitted again. fitting_oracle["error"] = True @@ -163,7 +190,32 @@ def test_web_fitting_comparison_download_rejection_and_invalidation( page.locator("#fit-run").click() expect(page.locator("#fit-state")).to_have_text("通过校验") expect(page.locator("#fit-proposed-legend")).to_be_hidden() - expect(page.locator("#fit-parameters")).to_contain_text("lift_height") + expect(page.locator("#fit-result-title")).to_contain_text("1 段原子技能") + expect(page.locator("#fit-download-task")).to_have_attribute( + "aria-disabled", "false" + ) + with page.expect_download() as task_download: + page.locator("#fit-download-task").click() + import zipfile + import io + import torch + import yaml + + with zipfile.ZipFile(task_download.value.path()) as archive: + manifest = yaml.safe_load(archive.read("skills.yaml")) + assert [s["skill_id"] for s in manifest["timeline"]] == [ + "execute_demo_skill", + "pick_up", + "execute_demo_skill", + ] + for span in manifest["timeline"]: + data = torch.load( + io.BytesIO(archive.read(span["tensor"])), weights_only=True + ) + np.testing.assert_array_equal( + data["qpos"].numpy(), + fitting_oracle["original"][span["start"] : span["stop"]].T, + ) def test_keyboard_edit_delete_undo_and_resume( @@ -350,9 +402,7 @@ def test_skill_dropdown_core_persistence_and_demoskill( page.locator("#core-start-frame").fill("3") page.locator("#core-end-frame").fill("4") page.locator("#commit").click() - expect(page.locator("#fit-skill")).to_have_value("twist") - expect(page.locator("#fit-skill")).to_be_disabled() - expect(page.locator("#fit-selection")).to_contain_text("核心 [3, 4]") + expect(page.locator("#fit-selection")).to_contain_text("1 段已标注") page.locator("#save").click() expect(page.locator("#save-status")).to_have_text("已保存") saved = json.loads(destination.read_text()) @@ -368,7 +418,7 @@ def test_skill_dropdown_core_persistence_and_demoskill( expect(page.locator("#core-start-frame")).to_have_value("3") page.locator("#label").select_option("execute_demo_skill") page.locator("#commit").click() - expect(page.locator("#fit-run")).to_be_disabled() + expect(page.locator("#fit-run")).to_be_enabled() expect(page.locator("#fit-selection-hint")).to_contain_text("DemoSkill") page.locator("#save").click() expect(page.locator("#save-status")).to_have_text("已保存") diff --git a/tests/toolkits/trajectory_annotator/test_viewer.py b/tests/toolkits/trajectory_annotator/test_viewer.py new file mode 100644 index 0000000000..bcdc2c36cd --- /dev/null +++ b/tests/toolkits/trajectory_annotator/test_viewer.py @@ -0,0 +1,161 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Full-resolution, calibrated dual-model playback without a physics runtime.""" + +from __future__ import annotations + +from pathlib import Path +import socket +import threading +from typing import Any + +import numpy as np +import pytest + +from embodichain.toolkits.trajectory_annotator._viewer import ( + TrajectoryViewer, + _load_models, +) + +pytestmark = pytest.mark.no_sim + + +def test_models_follow_full_clock_and_recorded_mimics_and_release_server( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + """Protect pose placement, independent models, styles, time sampling and ownership.""" + import viser.extras + + path = tmp_path / "robot.urdf" + path.write_text(""" + + + + + + + + + + + + + + """) + original = np.array([[0.02, 0, 0.02], [0.03, 0.1, 0.04], [0.11, 0.5, 0.06]]) + patched = original.copy() + patched[1, 1] = 0.3 + names = ["finger2", "arm_joint", "finger1"] + report = dict( + joint_names=names, + start=1, + time_offset=10.0, + robot_profile=dict( + joint_names=names, + solvers={ + "configured_part": dict(urdf_path=str(path), root_link_name="root") + }, + base_poses={ + "configured_part": [ + [0, -1, 0, 2], + [1, 0, 0, 3], + [0, 0, 1, 4], + [0, 0, 0, 1], + ] + }, + ), + ) + _, _, world_from_model = _load_models(report)[0] + np.testing.assert_allclose(world_from_model[:3, :3], np.eye(3), atol=1e-14) + np.testing.assert_allclose(world_from_model[:3, 3], [2, 3, 3]) + + records: dict[str, Any] = {} + owners = set() + actual_urdf = viser.extras.ViserUrdf + condition = threading.Condition() + + class ObservedUrdf(actual_urdf): + def __init__(self, server: Any, model: Any, **kwargs: Any) -> None: + self.model = model + self.kind = kwargs["root_node_name"].split("/")[1] + self.color = kwargs["mesh_color_override"] + super().__init__(server, model, **kwargs) + + def update_cfg(self, configuration: Any) -> None: + super().update_cfg(configuration) + with condition: + owners.add(threading.get_ident()) + records[self.kind] = dict( + qpos=configuration, + color=self.color, + arm=self.model.get_transform("arm").copy(), + finger2=self.model.get_transform("finger2", "arm").copy(), + ) + condition.notify_all() + + monkeypatch.setattr(viser.extras, "ViserUrdf", ObservedUrdf) + viewer = TrajectoryViewer( + dict( + timestamps=np.array([10.0, 10.1, 10.4]), + original_qpos=original, + patched_qpos=patched, + ), + report, + ) + try: + state = viewer.start() + assert state["phase"] == "ready", state + viewer.set_time(0.25) + with condition: + assert condition.wait_for( + lambda: abs(records["patched"]["qpos"]["arm_joint"] - 0.4) < 1e-10, + timeout=5, + ) + np.testing.assert_allclose( + [records["original"]["qpos"][name] for name in names], [0.07, 0.3, 0.05] + ) + np.testing.assert_allclose( + [records["patched"]["qpos"][name] for name in names], [0.07, 0.4, 0.05] + ) + # Rz90 maps the prismatic arm's local X into model Y. + np.testing.assert_allclose( + records["original"]["arm"][:3, 3], [0, 0.3, 1], atol=1e-14 + ) + np.testing.assert_allclose( + records["patched"]["finger2"][:3, 3], [0, -0.07, 0], atol=1e-14 + ) + assert records["original"]["color"] is None + assert records["patched"]["color"] == (0.16, 0.47, 1.0, 0.35) + assert len(owners) == 1 and threading.get_ident() not in owners + # Scrubbing before/after the joint recording clamps to its endpoints. + viewer.set_time(-1) + with condition: + assert condition.wait_for( + lambda: records["original"]["qpos"]["arm_joint"] == 0, timeout=5 + ) + for time_seconds in np.linspace(0, 1, 100): + viewer.set_time(float(time_seconds)) + with condition: + assert condition.wait_for( + lambda: records["patched"]["qpos"]["arm_joint"] == 0.5, timeout=5 + ) + finally: + viewer.close() + assert viewer.snapshot()["phase"] == "closed" + with socket.socket() as connection: + connection.settimeout(1) + assert connection.connect_ex(("127.0.0.1", state["port"])) != 0 From db388b3b7732442dacd58a2c63c37ced1e021eea Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 17 Nov 2026 09:31:08 +0000 Subject: [PATCH 30/45] fix(expansion): retain owner-proven holds after terminal runtime A finished runtime may still have its last ordinary motion awaiting env.step. Keep the default fail-closed abort contract, but allow the joint controller owner to supply actual applied target positions when no runtime safe hold exists. Never reuse the pending motion or acknowledge before real physics consumption. Retain the interrupted phase when terminal fields are empty, and keep the last actual scalar controller metadata and row identity in cancelled diagnostics without per-step serialization. Validation: 12 focused abort, safe-hold and actual reuse cases pass on rc2; full bottle trigger and repeated five-task qualification follow on the installed revision. Independent reviews completed. --- .../topics/expansion-engine/workloads.md | 6 ++++ embodichain/lab/expansion/_workload_cursor.py | 25 ++++++++++++++-- embodichain/lab/expansion/gym_workloads.py | 14 +++++++++ .../lab/gym/envs/task_program/bridge.py | 29 ++++++++++++++++++- tests/lab/expansion/test_workloads_sim.py | 3 ++ 5 files changed, 74 insertions(+), 3 deletions(-) diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index 28f45c742d..097fb4ae48 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -115,6 +115,12 @@ pending writes plus confirmed commits can never exceed the original target. reconciliation path still owns uncertain or failed writes. Safe-stop controller values retain the interrupted invocation/phase metadata for ordinary physical validators, while their cancellation kind and reason remain explicit. +When a runtime is already terminal with a final unconsumed motion, the joint +controller owner supplies its actual already-applied position targets and zero +velocity through the abort callback's optional `controller_hold`. The bridge +prefers a runtime-generated hold, never relabels pending motion, and advances +the clock only after the fallback hold receives a real `env.step`. Without an +explicit owner-proven hold, the existing fail-closed abort behavior remains. Frozen groups can call `export_expansion_episodes` from bounded writer threads. The callback must not retain mutable live state or operate on the World. diff --git a/embodichain/lab/expansion/_workload_cursor.py b/embodichain/lab/expansion/_workload_cursor.py index 3a0c04449f..92a1633c2f 100644 --- a/embodichain/lab/expansion/_workload_cursor.py +++ b/embodichain/lab/expansion/_workload_cursor.py @@ -76,7 +76,15 @@ def next(self) -> ControllerAction | None: # Phase-aware physical validators must see that context. action = ControllerAction( action.value, - metadata={**self.last_command.metadata, **action.metadata}, + metadata={ + **self.last_command.metadata, + **{ + key: value + for key, value in action.metadata.items() + if value is not None + or key not in self.last_command.metadata + }, + }, ) self.last_command = action if self.finished and self.rejection is not None: @@ -96,7 +104,7 @@ def next(self) -> ControllerAction | None: def abort(self, reason: str) -> None: if self.finished or self.rejection is not None or self.emergency is not None: return - if not self.pending_consumed: + if not self.pending_consumed and self.last_command is None: # An unconsumed command must go through the original safe-stop # iterator; acknowledging it here would manufacture a World step. super().abort(reason) @@ -111,5 +119,18 @@ def abort(self, reason: str) -> None: if "qvel" in previous: fields["qvel"] = torch.zeros_like(hold) value = TensorDict(fields, batch_size=[len(ids)]) + if not self.pending_consumed: + self.reason = reason + self.emergency = iter( + self.segment.abort_actions( + reason, + last_action_consumed=False, + controller_hold=ControllerAction( + value, + metadata={"bridge_action_kind": "runtime_abort_safe_hold"}, + ), + ) + ) + return self.bridge.request_row_rejection(ids, ControllerAction(value), reason=reason) self.rejection = reason diff --git a/embodichain/lab/expansion/gym_workloads.py b/embodichain/lab/expansion/gym_workloads.py index 2c1d871072..c26a9d953e 100644 --- a/embodichain/lab/expansion/gym_workloads.py +++ b/embodichain/lab/expansion/gym_workloads.py @@ -362,6 +362,14 @@ def observe( for name in {a.workload_id for a in assignments}: self.groups[name]["host"]._context.journal.checkpoint() self.collector.observe(ids, action, observation) + if self.reuse_idle_rows: + row_metadata = action.metadata.get("row_actions", {}) + for row in ids: + # Retain one already-owned metadata view, rather than copying + # command histories on the control hot path. + self.active[row]["last_controller_metadata"] = row_metadata.get( + str(row), {} + ) checks = self.owner.collision return tuple( row @@ -458,6 +466,7 @@ def cancel(self, result: WorkloadRowResult, *, reason: str) -> None: ctx = group["host"]._context request = ctx.requests[row] record = { + "env_id": row, "status": "cancelled", "reason": reason, "program_accepted": result.program.accepted[0], @@ -472,6 +481,11 @@ def cancel(self, result: WorkloadRowResult, *, reason: str) -> None: return try: group["engine"].cancel_row(state["native"], reason=reason) + record["last_controller_metadata"] = { + key: value + for key, value in state.get("last_controller_metadata", {}).items() + if value is None or type(value) in (str, int, float, bool) + } measured = ( None if self.owner.collision is None diff --git a/embodichain/lab/gym/envs/task_program/bridge.py b/embodichain/lab/gym/envs/task_program/bridge.py index 5ee67cc941..ae38d02e77 100644 --- a/embodichain/lab/gym/envs/task_program/bridge.py +++ b/embodichain/lab/gym/envs/task_program/bridge.py @@ -2095,12 +2095,14 @@ def abort( reason: str, *, last_action_consumed: bool, + controller_hold: ControllerAction | None = None, ) -> Iterator[ControllerAction]: return self._abort_segment( segment, lifecycle, reason=reason, last_action_consumed=last_action_consumed, + controller_hold=controller_hold, ) return abort @@ -2112,12 +2114,28 @@ def _abort_segment( *, reason: str, last_action_consumed: bool, + controller_hold: ControllerAction | None = None, ) -> Iterator[ControllerAction]: - """Abort one segment, surfacing a safe hold only after controller activity.""" + """Abort after activity, preferring runtime-proven controller holds. + + A trusted controller owner may supply an explicit labelled hold when + the runtime became terminal before its final yielded motion was + consumed. Pending motion alone is never promoted to a safe stop. + """ if type(reason) is not str or not reason: raise ValueError("abort reason must be a non-empty string.") if type(last_action_consumed) is not bool: raise TypeError("last_action_consumed must be a bool.") + if controller_hold is not None: + if not isinstance(controller_hold, ControllerAction): + raise TypeError("controller_hold must be a ControllerAction or None.") + if ( + controller_hold.metadata.get("bridge_action_kind") + not in _SAFE_HOLD_ACTION_KINDS + ): + raise ValueError( + "controller_hold requires an explicit safe-hold action kind." + ) if lifecycle.abort_started: raise RuntimeError( f"Segment {segment.segment_id!r} abort handshake already started." @@ -2181,6 +2199,15 @@ def _abort_segment( safe_action = self._sink.drain_safe_stop_action( fallback=None if last_action_consumed else pending, ) + if safe_action is None and controller_hold is not None: + # A controller owner can prove a hold even after a runtime has + # become terminal while its final motion is still unconsumed. + # Never treat that pending motion itself as a safe stop. + held = controller_hold.snapshot() + safe_action = ControllerAction( + held.value, + metadata={**held.metadata, "abort_hold_source": "controller_owner"}, + ) if safe_action is None: raise TaskProgramBridgeError( f"Segment {segment.segment_id!r} stopped before exhaustion, but " diff --git a/tests/lab/expansion/test_workloads_sim.py b/tests/lab/expansion/test_workloads_sim.py index ed2ddaf50b..95b79fc8a7 100644 --- a/tests/lab/expansion/test_workloads_sim.py +++ b/tests/lab/expansion/test_workloads_sim.py @@ -791,6 +791,9 @@ def step(action, *args, **kwargs): record = json.loads(entry.read_text()) assert record["reason"] == "quota_fulfilled_by_peer" if "partial_rollout" in record: + assert record["env_id"] in (0, 1) + if record["steps"]: + assert record["last_controller_metadata"].get("bridge_action_kind") with np.load(entry.parent / record["partial_rollout"]["path"]) as data: assert len(data["qpos"]) == record["steps"] + 1 assert len(data.get("actions", [])) == record["steps"] From 9e29ad63abb96af1026b82ad72353f52eac38d0f Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 17 Nov 2026 09:57:29 +0000 Subject: [PATCH 31/45] fix(expansion): keep speculative retry reuse opt-in --- agent_context/topics/expansion-engine/workloads.md | 6 ++++-- embodichain/lab/expansion/task_program_jobs.py | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index 097fb4ae48..2593368236 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -64,8 +64,10 @@ unfinished groups; capacity remains exactly W times B. Queued groups keep their normal lane priority. Healthy candidates fill only their remaining quota. Each observed unsuccessful attempt permits one additional competing candidate, bounded by the existing attempt, admission-time, settling, step and pending-byte limits. -The low-level flag defaults false; the native Task Program job enables it by -default and accepts `engine.reuse_idle_rows: false` to retain fixed lanes. +The low-level flag and native Task Program job both default false. Enable +reuse explicitly with `engine.reuse_idle_rows: true`. On +CPU-bound planning, competing retries may increase total work despite higher +row occupancy; validate qualified output per wall time before enabling reuse. This mode requires a source `cancel(result, *, reason)` port. Only a confirmed receipt fills quota. Remaining live candidates use the ordinary safe-stop diff --git a/embodichain/lab/expansion/task_program_jobs.py b/embodichain/lab/expansion/task_program_jobs.py index 23d8a1a755..2fa1ad6b1e 100644 --- a/embodichain/lab/expansion/task_program_jobs.py +++ b/embodichain/lab/expansion/task_program_jobs.py @@ -403,7 +403,7 @@ def _run_operators( max_steps=settings["max_steps"], settling_steps=settings["settling_steps"], seed=seed, - reuse_idle_rows=settings.get("reuse_idle_rows", True), + reuse_idle_rows=settings.get("reuse_idle_rows", False), ) chunks = settings["grasp_nms_chunk_size"] if torch.cuda.is_available() else 2048 with pose_nms_scope(chunk_size=chunks): From 71fda76939e6be0619d573f3156bcc0d0f351fcb Mon Sep 17 00:00:00 2001 From: matafela Date: Tue, 17 Nov 2026 18:03:17 +0800 Subject: [PATCH 32/45] docs(context): route trajectory annotation commands --- agent_context/MAP.yaml | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index daffdff21c..4592bcb931 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -440,7 +440,7 @@ topics: - id: atomic-actions title: Atomic Skills aliases: [atomic skill, atomic skills, atomic action, 原子技能, 原子动作] - keywords: [fit-trajectory, --skill-sequence, scene-skill-fitting-inputs, SkillFitCfg, PickUpMotionGoal, SlideMotionGoal, TwistMotionGoal, HandOverMotionGoal, 视频关节轨迹拟合, 轨迹回填, PlanningBudget, PlanningRecovery, ExecutionPlanLifecycle, AtomicSkillEngine, run-atomic-skill, PlanningContext, ExecutionSession, ExecutionRunner, SkillInvocation, + keywords: [annotate-trajectory, trajectory_annotator, 轨迹切分, 轨迹标注, fit-trajectory, --skill-sequence, scene-skill-fitting-inputs, SkillFitCfg, PickUpMotionGoal, SlideMotionGoal, TwistMotionGoal, HandOverMotionGoal, 视频关节轨迹拟合, 轨迹回填, PlanningBudget, PlanningRecovery, ExecutionPlanLifecycle, AtomicSkillEngine, run-atomic-skill, PlanningContext, ExecutionSession, ExecutionRunner, SkillInvocation, hand_geometry_cfgs, GraspOpeningTarget, prepare_grasp_query, scoped grasp preparation, RecoveryPolicy, HeldObjectGuardRequest, PhaseEffectGateRequest, articulation_geometry, control-grid retiming, AffordanceSamplingContext, AffordancePoseCandidates, AffordanceSample, affordance sampling] @@ -449,9 +449,7 @@ topics: - embodichain/lab/sim/atomic_skills/engine.py - embodichain/lab/sim/atomic_skills/fitting.py - embodichain/toolkits/skill_fitting/ - - embodichain/toolkits/trajectory_annotator/_fitting.py - - embodichain/toolkits/trajectory_annotator/_viewer.py - - embodichain/toolkits/trajectory_annotator/web/fitting.js + - embodichain/toolkits/trajectory_annotator/ - embodichain/data_pipeline/skill_fitting.py - embodichain/lab/sim/atomic_skills/core.py - embodichain/lab/sim/atomic_skills/goals.py From 86303c7a5f9bbbc7fb6a9e1dff73ce3d66126a6a Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 17 Nov 2026 11:35:00 +0000 Subject: [PATCH 33/45] fix(expansion): compare effective scene preparation defaults Resolve scene preparation with the same precedence as EmbodiedEnv: explicit config first, registered defaults second. Normalize registered declarations through an owned FunctorCfg before shared-World compatibility checks. Extend real deployment coverage for mismatched and identical registration defaults, explicit overrides, and equivalent explicit/registered declarations. Preserve single-workload validation behavior and document the effective-hook contract. --- .../topics/expansion-engine/workloads.md | 6 +- embodichain/lab/expansion/gym_workloads.py | 61 ++++++------ tests/lab/expansion/test_gym_workloads.py | 92 ++++++++++++++----- 3 files changed, 107 insertions(+), 52 deletions(-) diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index 2593368236..511afaa956 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -43,8 +43,10 @@ World, captures the original seeded nominal scene in row zero, and composes the existing session, frame collector, episode assembler and confirmed artifact sink. Different integration/grasp policies may share that World only when their physical configuration, program, cadence and expert action schema agree. The -scene-preparation declaration and its parameters must also match, because the -common nominal scene is prepared once before constructing the World. +effective scene-preparation declaration and its parameters must also match: +explicit configuration wins over the environment's registered default, following +`EmbodiedEnv` construction. The common nominal scene is prepared once before +constructing the World. Command mode, ActionManager declarations and controlled-joint selections cannot vary between workloads in one World. Selections belong to the resolved environment configurations, not the shared registered environment implementation class. Physical/visual/prototype diff --git a/embodichain/lab/expansion/gym_workloads.py b/embodichain/lab/expansion/gym_workloads.py index c26a9d953e..e818f96574 100644 --- a/embodichain/lab/expansion/gym_workloads.py +++ b/embodichain/lab/expansion/gym_workloads.py @@ -555,6 +555,7 @@ def run_gym_expansion_workloads( from .persistence import ArtifactEpisodeSink, load_expansion_episode from .workflow import _RecordedSink from .episode_export import export_expansion_episodes + from embodichain.lab.gym.envs.managers.cfg import FunctorCfg from embodichain.lab.gym.utils.gym_utils import config_to_cfg from embodichain.lab.gym.utils.registration import REGISTERED_ENVS from embodichain.utils.utility import load_config @@ -647,43 +648,49 @@ def run_gym_expansion_workloads( ) first = configs[0][1] - def physical_key(cfg: Any) -> str: + def physical_key(env_id: str, cfg: Any) -> str: robot = deepcopy(cfg.robot.to_dict()) for binding in robot.get("gripper_bindings", {}).values(): binding["declaration"].pop("root_from_contact_frame", None) binding["defaults"].pop("root_from_contact_frame", None) binding["defaults"].get("binding", {}).pop("root_from_contact_frame", None) + physical = { + name: getattr(cfg, name) + for name in ( + "robot", + "sim_cfg", + "rigid_object", + "background", + "target_control_frequency", + "sim_steps_per_control", + "events", + "sensor", + "expert_trajectory", + "actions", + "control_parts", + "active_joint_ids", + "scene_preparation", + ) + if hasattr(cfg, name) + } + physical["robot"] = robot + # EmbodiedEnv uses the registration default only without an explicit + # hook. Normalize both routes without mutating either declaration. + if physical.get("scene_preparation") is None: + default = REGISTERED_ENVS[env_id].default_kwargs.get("scene_preparation") + if default is not None: + physical["scene_preparation"] = FunctorCfg(**deepcopy(default)) return _hash( { - name: ( - robot - if name == "robot" - else ( - getattr(cfg, name).to_dict() - if hasattr(getattr(cfg, name), "to_dict") - else getattr(cfg, name) - ) - ) - for name in ( - "robot", - "sim_cfg", - "rigid_object", - "background", - "target_control_frequency", - "sim_steps_per_control", - "events", - "sensor", - "expert_trajectory", - "actions", - "control_parts", - "active_joint_ids", - "scene_preparation", - ) - if hasattr(cfg, name) + name: value.to_dict() if hasattr(value, "to_dict") else value + for name, value in physical.items() } ) - if any(physical_key(cfg) != physical_key(first) for _, cfg in configs[1:]): + if any( + physical_key(raw["id"], cfg) != physical_key(configs[0][0]["id"], first) + for raw, cfg in configs[1:] + ): raise ValueError( "workloads must share physical environment, robot, sensors, cadence, expert action schema and scene preparation" ) diff --git a/tests/lab/expansion/test_gym_workloads.py b/tests/lab/expansion/test_gym_workloads.py index 02905565b1..0c7737e906 100644 --- a/tests/lab/expansion/test_gym_workloads.py +++ b/tests/lab/expansion/test_gym_workloads.py @@ -189,7 +189,7 @@ def test_workload_action_schema_checked_before_constructing_world( ): from embodichain.lab.expansion import ExpansionRunCfg from embodichain.lab.gym.envs.expert_trajectory import ExpertTrajectoryCfg - from embodichain.lab.gym.utils.registration import REGISTERED_ENVS + from embodichain.lab.gym.utils.registration import REGISTERED_ENVS, EnvSpec import embodichain.lab.gym.utils.gym_utils as utils import embodichain.lab.expansion.host as host @@ -244,7 +244,7 @@ class ReachedWorld(Exception): configs[name] = config # Configured tasks share this implementation class. Selection belongs # to the resolved configs above, never to artificial per-task classes. - monkeypatch.setitem(REGISTERED_ENVS, name, SimpleNamespace(cls=EmbodiedEnv)) + monkeypatch.setitem(REGISTERED_ENVS, name, EnvSpec(name, EmbodiedEnv)) cfg = ExpansionRunCfg( deployment=str(deployment), output_dir=str(tmp_path / name), @@ -285,16 +285,37 @@ class ReachedWorld(Exception): factory.assert_not_called() -@pytest.mark.parametrize("field", ["control_parts", "scene_preparation"]) -def test_real_deployments_reject_incompatible_shared_world_setup( - tmp_path, monkeypatch, field +@pytest.mark.parametrize( + "field,registered_x,explicit_x,compatible", + [ + ("control_parts", (None, None), (None, None), False), + ("scene_preparation", (None, None), (0.2, 0.8), False), + ("scene_preparation", (0.2, 0.8), (None, None), False), + ("scene_preparation", (0.2, 0.2), (None, None), True), + ("scene_preparation", (0.2, 0.8), (0.4, 0.4), True), + ("scene_preparation", (None, 0.2), (0.2, None), True), + ], + ids=[ + "control_parts", + "explicit_preparation_mismatch", + "registered_preparation_mismatch", + "registered_preparation_same", + "explicit_preparation_overrides_defaults", + "explicit_matches_registered_preparation", + ], +) +def test_real_deployments_validate_shared_world_setup( + tmp_path, monkeypatch, field, registered_x, explicit_x, compatible ): from pathlib import Path from embodichain.lab.expansion import ExpansionRunCfg from embodichain.lab.expansion.task_program_jobs import _prepare_deployment from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv from embodichain.lab.gym.utils.gym_utils import config_to_cfg - from embodichain.lab.gym.utils.registration import REGISTERED_ENVS + from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + register_env_function, + ) from embodichain.utils.utility import load_config import embodichain.lab.expansion.host as host @@ -308,17 +329,16 @@ def test_real_deployments_reject_incompatible_shared_world_setup( inputs.mkdir() base = load_config(_prepare_deployment(task, inputs)) workloads = [] - settings = ( - (("Arm", ["arm"]), ("ArmEef", ["arm", "eef"])) - if field == "control_parts" - else tuple( - (name, {"func": f"{__name__}:_prepare_cube_scene", "params": {"x": x}}) - for name, x in (("Near", 0.2), ("Far", 0.8)) - ) - ) - for name, value in settings: + + def preparation(x): + return {"func": f"{__name__}:_prepare_cube_scene", "params": {"x": x}} + + for i, name in enumerate(("first", "second")): component = load_config(task.parent / "envs/default.yaml") - component["env"][field] = value + if field == "control_parts": + component["env"][field] = ["arm"] if i == 0 else ["arm", "eef"] + elif explicit_x[i] is not None: + component["env"][field] = preparation(explicit_x[i]) physical = inputs / f"{name}.environment.yaml" physical.write_text(yaml.safe_dump(component)) declaration = { @@ -330,11 +350,27 @@ def test_real_deployments_reject_incompatible_shared_world_setup( path.write_text(json.dumps(declaration)) config = config_to_cfg(declaration, source_path=path) if field == "control_parts": - assert config.control_parts == value + assert config.control_parts == component["env"][field] + elif explicit_x[i] is None: + assert config.scene_preparation is None else: - assert config.scene_preparation.func == value["func"] - assert config.scene_preparation.params == value["params"] - assert REGISTERED_ENVS[declaration["id"]].cls is EmbodiedEnv + assert config.scene_preparation.func == preparation(explicit_x[i])["func"] + assert config.scene_preparation.params == {"x": explicit_x[i]} + spec = REGISTERED_ENVS[declaration["id"]] + # Keep the real configured integration and ordinary environment class, + # but register task-owned defaults through the supported entry point. + register_env_function( + EmbodiedEnv, + declaration["id"], + override=True, + max_episode_steps=spec.max_episode_steps, + task_program_adapter_factory=spec.task_program_adapter_factory, + **( + {"scene_preparation": preparation(registered_x[i])} + if registered_x[i] is not None + else {} + ), + ) cfg = ExpansionRunCfg( deployment=str(path), output_dir=str(tmp_path / name), @@ -353,9 +389,16 @@ def test_real_deployments_reject_incompatible_shared_world_setup( {}, ) ) - constructor = Mock(side_effect=AssertionError("World must not be constructed")) + + class ReachedWorld(Exception): + pass + + constructor = Mock(side_effect=ReachedWorld) monkeypatch.setattr(host, "GymExpansionHost", constructor) - with pytest.raises(ValueError, match="schema"): + with pytest.raises( + ReachedWorld if compatible else ValueError, + match=None if compatible else "workloads must share", + ): run_gym_expansion_workloads( workloads, WorkloadExecutionCfg( @@ -363,4 +406,7 @@ def test_real_deployments_reject_incompatible_shared_world_setup( ), output_dir=tmp_path / "run", ) - constructor.assert_not_called() + if compatible: + constructor.assert_called_once() + else: + constructor.assert_not_called() From 5c557e82b610d5ecf837209cce8966f4d0901eb1 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 18 Nov 2026 03:31:52 +0000 Subject: [PATCH 34/45] feat(expansion): add workspace pose coverage and distribution reports --- .../expansion-engine/expansion-engine.md | 15 + .../topics/expansion-engine/workloads.md | 20 + .../topics/scene-expansion/scene-expansion.md | 6 + docs/source/api_reference/public_api.rst | 15 + embodichain/lab/expansion/_distribution.html | 39 + embodichain/lab/expansion/_distribution.py | 337 +++++++++ .../lab/expansion/_distribution_pose.py | 141 ++++ embodichain/lab/expansion/_gym_context.py | 1 + embodichain/lab/expansion/_pose_cells.py | 90 +++ embodichain/lab/expansion/_pose_sampling.py | 712 ++++++++++++++++++ .../lab/expansion/_pose_sampling_cfg.py | 162 ++++ .../lab/expansion/_task_program_config.py | 14 +- embodichain/lab/expansion/_workload_cursor.py | 39 +- embodichain/lab/expansion/distribution.md | 65 ++ embodichain/lab/expansion/gym_workloads.py | 81 +- embodichain/lab/expansion/host.py | 2 + embodichain/lab/expansion/host_extensions.py | 9 +- .../lab/expansion/task_pose_coverage.py | 240 ++++++ .../lab/expansion/task_program_jobs.py | 76 +- embodichain/lab/expansion/workloads.py | 12 +- .../lab/task_program/compiler/lowering.py | 2 +- .../task_program/integrations/feasibility.py | 138 +++- .../workspace_pose_pick_place/README.md | 109 +++ .../env.obstacle.yaml | 93 +++ .../workspace_pose_pick_place/env.yaml | 81 ++ .../workspace_pose_pick_place/expansion.yaml | 201 +++++ .../task.obstacle.ur5.yaml | 19 + .../workspace_pose_pick_place/task.ur5.yaml | 19 + .../task_program/integration.yaml | 47 ++ .../task_program/program.yaml | 44 ++ .../benchmark/expansion/task_pose_coverage.py | 540 +++++++++++++ .../expansion/test_task_pose_report.py | 78 ++ tests/lab/expansion/test_distribution.py | 165 ++++ .../lab/expansion/test_task_pose_coverage.py | 595 +++++++++++++++ .../task_program/test_semantic_compiler.py | 27 + 35 files changed, 4154 insertions(+), 80 deletions(-) create mode 100644 embodichain/lab/expansion/_distribution.html create mode 100644 embodichain/lab/expansion/_distribution.py create mode 100644 embodichain/lab/expansion/_distribution_pose.py create mode 100644 embodichain/lab/expansion/_pose_cells.py create mode 100644 embodichain/lab/expansion/_pose_sampling.py create mode 100644 embodichain/lab/expansion/_pose_sampling_cfg.py create mode 100644 embodichain/lab/expansion/distribution.md create mode 100644 embodichain/lab/expansion/task_pose_coverage.py create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/README.md create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/env.obstacle.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/env.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/expansion.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task.obstacle.ur5.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task.ur5.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task_program/integration.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task_program/program.yaml create mode 100644 scripts/benchmark/expansion/task_pose_coverage.py create mode 100644 tests/benchmark/expansion/test_task_pose_report.py create mode 100644 tests/lab/expansion/test_distribution.py create mode 100644 tests/lab/expansion/test_task_pose_coverage.py diff --git a/agent_context/topics/expansion-engine/expansion-engine.md b/agent_context/topics/expansion-engine/expansion-engine.md index e900ef03d3..0928121f21 100644 --- a/agent_context/topics/expansion-engine/expansion-engine.md +++ b/agent_context/topics/expansion-engine/expansion-engine.md @@ -207,3 +207,18 @@ measured state plus explicitly labeled FK, and pure-visual children retain their physical parent's clock. Qualification requires admitted operator evidence, not just a requested profile or completed nominal episodes. Report completeness and capability acceptance are separate, including diagnostic unsupported combinations. + +## Offline distribution reports + +`python -m embodichain.lab.expansion._distribution` compares explicit before/after +confirmed job directories using the existing receipt inspector and episode loader. +It emits structured JSON/JSONL and a standalone HTML report with world-XY layout +bins, start/end joint counts, measured object paths, joint-motion summaries, and +existing geometry descriptor buckets. Version 2 adds full SO(3) orientation +features and recorded-URDF TCP FK with explicit gripper calibration. A lightweight +index drives filters; 100-record detail pages are shared by the offline viewer +and Agent JSON consumers. Matching task/robot/scene domains are required. +The report distinguishes cumulative additions from independent cohorts; observed +bins are not a feasible-volume certificate. The reader uses existing artifact +validation dependencies but creates no World. See +[distribution usage](../../../embodichain/lab/expansion/distribution.md). diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index 511afaa956..9edead8cc6 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -184,6 +184,26 @@ limitations remain explicit admission constraints. Physics devices and task qualification must be reported separately. Existing CPU deployments do not switch devices. +## Optional task-pose proposal feedback + +Task-facing expansion configs select `scene_sampling.strategy` (`bounded_pose` +or `workspace_joint`) independently of `scheduling.policy` (`fifo` or +`task_pose_coverage`). Absent settings preserve the legacy bounded sampler. +`_pose_sampling.py` owns bounded candidate snapshots, source feedback and budgets; +`task_pose_coverage.TaskPoseCoverage` implements the session coverage protocol. +Measured rigid initial/terminal poses contribute only after native sink receipts. +Separate SceneCase identities aggregate within a declared configuration/grid +fingerprint; installation changes remain separate domains. FIFO can omit coverage. + +Joint sampling uses an explicit cache or joint-uniform FK prior and repeats +full-pose IK/FK checks after planar projection. Fixed pick grasp policy and prior +must agree; unsupported dynamic grasp replacement rejects. Normal scene preparation, +planning, measured acceptance and persistence still determine success. Feedback +checkpoints do not restore a live World or replace job receipt reconciliation. +The configured acceptance task is +`extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place`; +its four-way runner is `scripts/benchmark/expansion/task_pose_coverage.py`. + ## Focused validation `tests/lab/expansion/test_workloads_sim.py` runs a real four-row task through diff --git a/agent_context/topics/scene-expansion/scene-expansion.md b/agent_context/topics/scene-expansion/scene-expansion.md index bc4ecbf6c2..f2bf1e517b 100644 --- a/agent_context/topics/scene-expansion/scene-expansion.md +++ b/agent_context/topics/scene-expansion/scene-expansion.md @@ -28,6 +28,12 @@ Joint task-conditioned layout/installation search is owned by attempts. Its configured search adapter still evaluates one B=1 deployment per attempt through the shared selected-row preparation/restoration contract. +The task-facing rolling collector also exposes selectable bounded versus +workspace-conditioned pose proposals with optional receipt-confirmed task-pose +coverage feedback; see [workload feedback](../expansion-engine/workloads.md#optional-task-pose-proposal-feedback). +This is distinct from offline atlas region validation and does not certify +continuous reachable volume. + ## Entry points | Request | Owner | diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 0c93def98d..4a931a2a43 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -6321,3 +6321,18 @@ contracts. The package-level imports above are the convenient extension API. discover_sensor_plugins get_sensor_registration list_sensor_types + +Task pose coverage +------------------ + +The task pose coverage adapter counts measured pick/place XY-yaw cells across +scene cases in one explicitly declared experiment domain. Reservations remain +pending until the ordinary ExpansionSession persistence receipt confirms them; +planned candidate poses never contribute to confirmed coverage. + +.. autosummary:: + + embodichain.lab.expansion.task_pose_coverage.TaskPoseCoverage + +.. autoclass:: embodichain.lab.expansion.task_pose_coverage.TaskPoseCoverage + :members: diff --git a/embodichain/lab/expansion/_distribution.html b/embodichain/lab/expansion/_distribution.html new file mode 100644 index 0000000000..af699051a1 --- /dev/null +++ b/embodichain/lab/expansion/_distribution.html @@ -0,0 +1,39 @@ +任务数据分布 + +
EMBODICHAIN / DATA OBSERVATORY

任务数据分布

从空间覆盖到运动多样性,检查每一轮数据补充。
确认落盘 · 实测证据

+ +

空间覆盖地图

世界坐标 XY / m;点击区域筛选实测轨迹。颜色深浅表示确认样本数。

稀疏密集 · 前后共用色标
按网格选择样本

代表轨迹联动

来源与现有预览入口
+

起点 × 终点联合分布

点击计数筛选该组合;零表示无确认数据,不代表不可行。只展示两快照中出现过的区域。

+

完整姿态分布

SO(3) 规范旋转向量三维分箱,步长 30°,非等体积;不合并物体对称姿态。

补数前后对照

数据说明与导出

本报告是 HTML 入口。分页明细位于 pages/;分享时请保留整个目录。

    Agent 结构化结果 · 前快照特征 · 后快照特征
    + diff --git a/embodichain/lab/expansion/_distribution.py b/embodichain/lab/expansion/_distribution.py new file mode 100644 index 0000000000..c2c25cc99b --- /dev/null +++ b/embodichain/lab/expansion/_distribution.py @@ -0,0 +1,337 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Offline confirmed-episode distribution analysis and linked HTML report. + +Run with ``python -m embodichain.lab.expansion._distribution --help``. +""" + +from __future__ import annotations + +import argparse +from collections import Counter +from copy import deepcopy +import hashlib +import json +from pathlib import Path +from typing import Any +import numpy as np + +__all__: list[str] = [] + + +def _digest(value: Any) -> str: + return hashlib.sha256( + json.dumps(value, sort_keys=True, allow_nan=False).encode() + ).hexdigest() + + +def _cell(pose: list[float], cell_size: float) -> str: + return ",".join(str(int(x)) for x in np.floor(np.asarray(pose[:2]) / cell_size)) + + +def _motion(points: np.ndarray, times: np.ndarray) -> dict: + if points.ndim != 2 or points.shape[0] != len(times) or len(times) < 2: + raise ValueError("Motion requires aligned T+1 points and timestamps") + if ( + not np.isfinite(points).all() + or not np.isfinite(times).all() + or np.any(np.diff(times) <= 0) + ): + raise ValueError("Motion must be finite with increasing timestamps") + lengths = np.linalg.norm(np.diff(points, axis=0), axis=1) + arc = np.r_[0.0, np.cumsum(lengths)] + keep = np.r_[True, np.diff(arc) > 1e-12] + grid = np.linspace(0, arc[-1], 32) + path = np.stack( + [np.interp(grid, arc[keep], points[keep, i]) for i in range(points.shape[1])], + axis=1, + ) + return { + "path_length": float(arc[-1]), + "duration_s": float(times[-1] - times[0]), + "speed_p50_p95": np.quantile(lengths / np.diff(times), [0.5, 0.95]).tolist(), + "path": path.tolist(), + } + + +def _summarize(rows: list[dict], size: float, minimum: int) -> dict: + unique = {} + for row in rows: + key = row["commit_id"] + if key in unique and unique[key] != row: + raise ValueError("Conflicting commit identity") + unique[key] = deepcopy(row) + rows = list(unique.values()) + pick, place, joint = Counter(), Counter(), Counter() + for row in rows: + if row["object_motion"] is None: + continue + a, b = _cell(row["initial_pose"], size), _cell(row["final_pose"], size) + pick[a] += 1 + place[b] += 1 + joint[a + "|" + b] += 1 + row["pick_cell"], row["place_cell"] = a, b + distributions = {} + for family in ("object_motion", "joint_motion", "tcp_motion"): + for metric in ("path_length", "duration_s"): + values = [ + r[family][metric] for r in rows if r.get(family) and metric in r[family] + ] + distributions[family + "." + metric] = { + "valid_count": len(values), + "missing_count": len(rows) - len(values), + "p0_p25_p50_p75_p100": ( + np.quantile(values, [0, 0.25, 0.5, 0.75, 1]).tolist() + if values + else None + ), + } + orientation_counts = {} + for phase in ("initial", "final"): + orientation_counts[phase] = dict( + Counter( + r["orientation"]["rotation_bin_" + phase] + for r in rows + if r.get("orientation") + ) + ) + return { + "orientation": orientation_counts, + "tcp_valid": sum(r.get("tcp_motion") is not None for r in rows), + "motion_distributions": distributions, + "episodes": rows, + "count": len(rows), + "layout_valid": sum(pick.values()), + "layout_missing": len(rows) - sum(pick.values()), + "pick": dict(pick), + "place": dict(place), + "joint": dict(joint), + "joint_at_minimum": sum(n >= minimum for n in joint.values()), + "unique_geometry_buckets": len( + {r["geometry_bucket"] for r in rows if r["geometry_bucket"]} + ), + "geometry_missing": sum(r["geometry_bucket"] is None for r in rows), + } + + +def _read(paths: list[Path], entity: str) -> tuple[list[dict], list[str]]: + from .persistence import ArtifactEpisodeSink, load_expansion_episode + + from ._distribution_pose import RecordedTCP, orientation + + rows, domains = [], [] + for directory in dict.fromkeys(p.resolve() for p in paths): + if not (directory / "job.json").is_file(): + raise ValueError(f"Expected confirmed job directory: {directory}") + job = json.loads((directory / "job.json").read_text()) + deployment = job.get("resolved_deployment", {}) + tcp = RecordedTCP(deployment) + for receipt in ArtifactEpisodeSink(directory).inspect(): + if not receipt.confirmed: + continue + episode = load_expansion_episode(receipt.storage_id) + if not episode.validation.accepted: + raise ValueError("Confirmed episode lacks acceptance") + meta = episode.metadata + # Fixed topology, installation and task identity; sampled object poses are features. + robot = [ + dict(uid=s["uid"], joints=list(s["joint_names"]), pose=list(s["pose"])) + for s in meta.get("initial_state", ()) + if s["kind"] == "robot" + ] + domain = _digest( + { + "robot": robot, + "instruction": meta.get("instruction"), + "backend": meta.get("physics_backend"), + "deployment_robot": deployment.get("robot"), + "scene_topology": [ + (v["uid"], v["kind"], v["body_type"]) + for v in meta.get("initial_state", ()) + ], + "scene_configuration": { + k: deployment.get(k) + for k in ("rigid_object", "background", "physics_config") + }, + } + ) + domains.append(domain) + times = episode.timestamps.cpu().numpy() + state = episode.observations.get(f"rigid/{entity}/state") + motion = ( + None if state is None else _motion(state.cpu().numpy()[:, :3], times) + ) + q = episode.observations.get("full_joint_positions") + joint_motion = None if q is None else _motion(q.cpu().numpy(), times) + descriptor = meta.get("coverage_descriptor") + bucket = None + if descriptor: + bucket = _digest( + [ + list(descriptor["phase_ids"]), + np.rint(np.asarray(descriptor["geometry"]) / 0.001) + .astype(int) + .tolist(), + ] + ) + tcp_poses = tcp.poses(episode) + rows.append( + { + "episode_id": episode.episode_id, + "commit_id": episode.commit_id, + "artifact": receipt.storage_id, + "job": str(directory), + "domain_id": domain, + "source_episode_uuid": meta.get("source_episode_uuid"), + "initial_pose": None if state is None else state[0, :7].tolist(), + "final_pose": None if state is None else state[-1, :7].tolist(), + "orientation": ( + None + if state is None + else orientation(state.cpu().numpy(), times) + ), + "tcp_motion": ( + None if tcp_poses is None else _motion(tcp_poses[:, :3], times) + ), + "tcp_orientation": ( + None if tcp_poses is None else orientation(tcp_poses, times) + ), + "tcp_evidence": ( + {"unavailable": tcp.reason} if tcp.reason else tcp.evidence + ), + "object_motion": motion, + "joint_motion": joint_motion, + "geometry_bucket": bucket, + } + ) + return rows, sorted(set(domains)) + + +def _run( + before: list[Path], + after: list[Path], + output: Path, + entity: str, + size: float, + minimum: int, +) -> None: + if not np.isfinite(size) or size <= 0 or minimum < 1: + raise ValueError("Positive cell size and minimum count required") + if output.exists(): + raise FileExistsError("Use a new output directory") + b, bd = _read(before, entity) + a, ad = _read(after, entity) + if not b or not a or bd != ad or len(bd) != 1: + raise ValueError( + "Comparison requires nonempty snapshots of one matching domain" + ) + report = { + "schema_version": 2, + "feature_version": "se3-world-object-joint-tcp-v2", + "entity": entity, + "coordinate_frame": "world", + "cell_size_m": size, + "minimum": minimum, + "domain_id": bd[0], + "before": _summarize(b, size, minimum), + "after": _summarize(a, size, minimum), + } + report["comparison_kind"] = ( + "cumulative" + if {r["commit_id"] for r in b} <= {r["commit_id"] for r in a} + else "independent_cohorts" + ) + keys = sorted(set(report["before"]["joint"]) | set(report["after"]["joint"])) + report["comparison"] = [ + { + "cell": k, + "before": report["before"]["joint"].get(k, 0), + "after": report["after"]["joint"].get(k, 0), + "deficit": max(0, minimum - report["after"]["joint"].get(k, 0)), + } + for k in keys + ] + report["limitations"] = [ + "XY bins cover observed domain only; no feasible-volume denominator.", + "TCP is recorded-URDF FK from measured qpos/root plus explicit gripper TCP calibration.", + "Orientation bins: canonical SO(3) rotation vectors, 30-degree Cartesian bins, not equal-volume; no symmetry folding.", + "Joint path length mixes joint coordinates; compare only matching domains.", + "Geometry buckets reuse existing 1e-3 descriptor quantization, not clustering.", + ] + report["snapshot_id"] = _digest(report) + output.mkdir(parents=True) + # Detail pages are shared by the web viewer and Agent; summaries retain only an index. + pages = output / "pages" + pages.mkdir() + for name in ("before", "after"): + rows = report[name]["episodes"] + for start in range(0, len(rows), 100): + chunk = rows[start : start + 100] + payload = json.dumps(chunk, ensure_ascii=False, allow_nan=False).replace( + "<", "\\u003c" + ) + key = f"{name}-{start//100}" + (pages / f"{key}.json").write_text(payload) + (pages / f"{key}.js").write_text( + f"window.distributionPage({json.dumps(key)},{payload});" + ) + with (output / f"{name}-episodes.jsonl").open("w") as f: + for row in rows: + f.write(json.dumps(row, ensure_ascii=False, allow_nan=False) + "\n") + report[name]["episodes"] = [ + dict( + episode_id=row["episode_id"], + commit_id=row["commit_id"], + initial_pose=row["initial_pose"], + final_pose=row["final_pose"], + object_motion=row["object_motion"] is not None, + pick_cell=row.get("pick_cell"), + place_cell=row.get("place_cell"), + page=f"{name}-{i//100}", + ) + for i, row in enumerate(rows) + ] + (output / "summary.json").write_text( + json.dumps(report, ensure_ascii=False, indent=2, allow_nan=False) + ) + template = Path(__file__).with_name("_distribution.html").read_text() + (output / "report.html").write_text( + template.replace( + "__REPORT__", + json.dumps(report, ensure_ascii=False, allow_nan=False).replace( + "<", "\\u003c" + ), + ) + ) + + +def _main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--before", type=Path, nargs="+", required=True) + parser.add_argument("--after", type=Path, nargs="+", required=True) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--entity", default="cube") + parser.add_argument("--cell-size", type=float, default=0.1) + parser.add_argument("--minimum", type=int, default=5) + args = parser.parse_args() + _run( + args.before, args.after, args.output, args.entity, args.cell_size, args.minimum + ) + + +if __name__ == "__main__": + _main() diff --git a/embodichain/lab/expansion/_distribution_pose.py b/embodichain/lab/expansion/_distribution_pose.py new file mode 100644 index 0000000000..9558c4ff1b --- /dev/null +++ b/embodichain/lab/expansion/_distribution_pose.py @@ -0,0 +1,141 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Full orientation features and recorded-model TCP reconstruction.""" + +from __future__ import annotations +from pathlib import Path +import hashlib +import numpy as np +from scipy.spatial.transform import Rotation + +__all__: list[str] = [] + + +def orientation(poses: np.ndarray, times: np.ndarray) -> dict: + q = np.asarray(poses, dtype=float)[:, 3:7] + if ( + q.shape != (len(times), 4) + or not np.isfinite(q).all() + or np.any(np.linalg.norm(q, axis=1) < 1e-12) + ): + raise ValueError("Orientation requires finite nonzero xyzw quaternions") + rot = Rotation.from_quat(q) + canonical = rot.as_quat(canonical=True) + vectors = rot.as_rotvec() + angle = (rot[:-1].inv() * rot[1:]).magnitude() + dt = np.diff(times) + if np.any(dt <= 0): + raise ValueError("Increasing orientation times required") + return { + "initial_xyzw": canonical[0].tolist(), + "final_xyzw": canonical[-1].tolist(), + "initial_rotvec_rad": vectors[0].tolist(), + "final_rotvec_rad": vectors[-1].tolist(), + "angular_path_rad": float(angle.sum()), + "angular_speed_p50_p95": np.quantile(angle / dt, [0.5, 0.95]).tolist(), + "net_rotation_rad": float((rot[0].inv() * rot[-1]).magnitude()), + "rotation_bin_initial": ",".join( + map(str, np.floor((vectors[0] + np.pi) / (np.pi / 6)).astype(int)) + ), + "rotation_bin_final": ",".join( + map(str, np.floor((vectors[-1] + np.pi) / (np.pi / 6)).astype(int)) + ), + } + + +class RecordedTCP: + """Cache one recorded URDF chain; reconstruct in bounded 256-frame batches.""" + + def __init__(self, deployment: dict): + import pytorch_kinematics as pk + + robot = deployment.get("robot") or {} + bindings = list((robot.get("gripper_bindings") or {}).values()) + self.reason = None + if len(bindings) != 1: + self.reason = "single_explicit_gripper_binding_required" + return + binding = bindings[0] + self.link = binding.get("root_link") + transform = binding.get("defaults", {}).get("default_root_from_tcp") + asset = robot.get("fpath") + if not self.link or transform is None or not asset or not Path(asset).is_file(): + self.reason = "recorded_tcp_calibration_or_asset_missing" + return + self.offset = np.asarray(transform, dtype=float) + if ( + self.offset.shape != (4, 4) + or not np.isfinite(self.offset).all() + or not np.allclose(self.offset[3], [0, 0, 0, 1]) + or not np.allclose( + self.offset[:3, :3].T @ self.offset[:3, :3], np.eye(3), atol=1e-6 + ) + or not np.isclose(np.linalg.det(self.offset[:3, :3]), 1) + ): + raise ValueError("Invalid TCP transform") + if robot.get("body_scale") not in (None, [1.0, 1.0, 1.0]): + self.reason = "scaled_robot_not_supported" + return + content = Path(asset).read_bytes() + self.chain = pk.build_chain_from_urdf(content) + self.names = self.chain.get_joint_parameter_names() + self.evidence = { + "method": "fk_from_measured_qpos_and_root", + "root_link": self.link, + "root_from_tcp": self.offset.tolist(), + "urdf_sha256": hashlib.sha256(content).hexdigest(), + } + + def poses(self, episode) -> np.ndarray | None: + import torch + + if self.reason: + return None + robots = [ + s for s in episode.metadata.get("initial_state", ()) if s["kind"] == "robot" + ] + if len(robots) != 1: + raise ValueError("TCP requires one recorded robot") + robot = robots[0] + fields = { + s["field"]: s["key"] + for s in episode.metadata["scene_state_fields"] + if s["entity_uid"] == robot["uid"] + } + q = episode.observations[fields["qpos"]].cpu() + roots = episode.observations[fields["pose"]].cpu().numpy() + indices = [list(robot["joint_names"]).index(n) for n in self.names] + out = [] + with torch.no_grad(): + for start in range(0, len(q), 256): + local = ( + self.chain.forward_kinematics( + q[start : start + 256, indices].float() + )[self.link] + .get_matrix() + .numpy() + @ self.offset + ) + rp = roots[start : start + 256] + world = np.broadcast_to(np.eye(4), local.shape).copy() + world[:, :3, :3] = Rotation.from_quat(rp[:, 3:]).as_matrix() + world[:, :3, 3] = rp[:, :3] + tcp = world @ local + out.append( + np.c_[tcp[:, :3, 3], Rotation.from_matrix(tcp[:, :3, :3]).as_quat()] + ) + return np.concatenate(out) diff --git a/embodichain/lab/expansion/_gym_context.py b/embodichain/lab/expansion/_gym_context.py index 0ae951c4af..70ce0d8b48 100644 --- a/embodichain/lab/expansion/_gym_context.py +++ b/embodichain/lab/expansion/_gym_context.py @@ -72,6 +72,7 @@ class GymHostContext: auxiliary_baseline: Mapping[str, torch.Tensor] | None = None profile: Real2SimTabletopCollectionCfg | None = None scene_source: JointSceneSource | None = None + feedback: Any | None = None motion_profile: CombinedExpansionProfile | None = None motion_recipes: tuple[CandidateRecipe, ...] = () names: tuple[str, ...] = () diff --git a/embodichain/lab/expansion/_pose_cells.py b/embodichain/lab/expansion/_pose_cells.py new file mode 100644 index 0000000000..2340e7cbc8 --- /dev/null +++ b/embodichain/lab/expansion/_pose_cells.py @@ -0,0 +1,90 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Pure XY/yaw grid validation and symmetric pose quantization.""" + +from __future__ import annotations +import math +from collections.abc import Mapping +import numpy as np + +__all__: list[str] = [] + + +def _grid(value: Mapping) -> dict: + if not isinstance(value, Mapping) or set(value) != { + "lower", + "upper", + "cell_size", + "yaw_period", + }: + raise ValueError("Pose grids require lower, upper, cell_size and yaw_period") + lower, upper, size = ( + np.asarray(value[k], dtype=float) for k in ("lower", "upper", "cell_size") + ) + period = value["yaw_period"] + if any(a.shape != (3,) or not np.isfinite(a).all() for a in (lower, upper, size)): + raise ValueError("Pose grid coordinates must be three finite numbers") + if type(period) not in (int, float) or not math.isfinite(period) or period <= 0: + raise ValueError("yaw_period must be finite and positive") + if period > 2 * math.pi or not math.isclose( + 2 * math.pi / period, round(2 * math.pi / period), rel_tol=0, abs_tol=1e-8 + ): + raise ValueError("yaw_period must divide a complete turn") + if ( + np.any(upper <= lower) + or np.any(size <= 0) + or not math.isclose(upper[2] - lower[2], period) + ): + raise ValueError("Pose grids need positive extents and one complete yaw period") + bins = (upper - lower) / size + if not np.allclose(bins, np.round(bins), rtol=0, atol=1e-8): + raise ValueError("Pose grid extents must be divisible by cell_size") + return { + "lower": lower.tolist(), + "upper": upper.tolist(), + "cell_size": size.tolist(), + "yaw_period": float(period), + } + + +def _pose_cell(pose, grid: Mapping) -> tuple[int, int, int] | None: + value = np.asarray(pose, dtype=float) + if value.shape == (4, 4): + x, y = value[:2, 3] + yaw = math.atan2(value[1, 0], value[0, 0]) + elif value.ndim == 1 and value.shape[0] >= 7: + x, y = value[:2] + q = value[3:7] + norm = np.linalg.norm(q) + if not math.isfinite(norm) or norm < 1e-12: + raise ValueError("Pose quaternion must be finite and nonzero") + qx, qy, qz, qw = q / norm + yaw = math.atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz)) + else: + raise ValueError("Expected a pose matrix or xyz/xyzw state") + if not np.isfinite(value).all(): + raise ValueError("Pose must be finite") + lo, hi, size = (np.asarray(grid[k]) for k in ("lower", "upper", "cell_size")) + point = np.array([x, y, (yaw - lo[2]) % grid["yaw_period"] + lo[2]]) + if np.any(point[:2] < lo[:2] - 1e-7) or np.any(point[:2] > hi[:2] + 1e-7): + return None + bins = np.rint((hi - lo) / size).astype(int) + return tuple( + np.minimum( + bins - 1, np.maximum(0, np.floor((point - lo) / size).astype(int)) + ).tolist() + ) diff --git a/embodichain/lab/expansion/_pose_sampling.py b/embodichain/lab/expansion/_pose_sampling.py new file mode 100644 index 0000000000..99e2eaf5a4 --- /dev/null +++ b/embodichain/lab/expansion/_pose_sampling.py @@ -0,0 +1,712 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Selectable bounded and workspace-guided scene proposals with owned feedback.""" + +from __future__ import annotations +from collections.abc import Mapping +from collections import Counter +from copy import deepcopy +from pathlib import Path +from typing import Any +import hashlib +import json +import math +import numpy as np +import torch + +__all__: list[str] = [] + + +class _BoundedPoseSource: + """Sample bounded arena-frame changes with task-declared support followers.""" + + def __init__( + self, env: Any, configuration: Mapping[str, Any], *, seed: int, revision: str + ) -> None: + self.configuration, self.seed, self.request_id = ( + {uid: deepcopy(dict(options)) for uid, options in configuration.items()}, + seed, + revision, + ) + self.poses = { + uid: env.sim.get_rigid_object(uid) + .get_local_pose(to_matrix=True)[0] + .detach() + .cpu() + .numpy() + .copy() + for uid in configuration + } + self.roots = { + env.robot.uid: env.robot.get_local_pose(to_matrix=True)[0] + .detach() + .cpu() + .tolist() + } + + def candidate(self, ordinal: int) -> JointSceneCandidate: + import numpy as np + from embodichain.lab.sim.scene.expansion import ScenePoseChange, SceneVariant + from embodichain.lab.sim.scene.feasibility import ( + JointSceneCandidate, + RobotDeploymentCandidate, + ) + + offsets = {} + for uid, options in self.configuration.items(): + if "follow" in options: + continue + seed = int.from_bytes( + hashlib.sha256(f"{self.seed}/{ordinal}/{uid}".encode()).digest()[:8], + "little", + ) + generator = np.random.default_rng(seed) + offsets[uid] = [ + generator.uniform(*bounds) + for bounds in (*options["translation_m"], options["yaw_rad"]) + ] + changes, parameters = [], [] + for uid, options in self.configuration.items(): + dx, dy, dz, yaw = offsets[options.get("follow", uid)] + pose = self.poses[uid].copy() + c, s = np.cos(yaw), np.sin(yaw) + pose[:3, :3] = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) @ pose[:3, :3] + pose[:3, 3] += (dx, dy, dz) + changes.append(ScenePoseChange(uid, tuple(map(tuple, pose)))) + parameters.extend((dx, dy, dz, yaw)) + scene = SceneVariant(self.request_id, self.seed, ordinal, tuple(changes)) + return JointSceneCandidate( + self.request_id, + scene, + RobotDeploymentCandidate(self.roots), + "object_pose", + tuple(parameters), + ) + + +from embodichain.lab.sim.scene.feasibility.budget import ( + FeasibilityBudget, + FeasibilityBudgetExhausted, +) + + +class _PoseBudget(FeasibilityBudget): + """Use whole-job limits, including raw proposals discarded by selection.""" + + def charge(self, kind: str, amount: int = 1) -> None: + self.check() + if type(amount) is not int or amount < 1: + raise ValueError("Budget charges must be positive integers") + limits = { + "candidates": self.cfg.max_candidates, + "ik": self.cfg.max_ik_calls, + "planning": self.cfg.max_planning_calls, + "rollouts": self.cfg.max_rollouts, + } + if self.counts[kind] + amount > limits[kind]: + self._blocked = (self.validation, kind + "_budget_exhausted") + raise FeasibilityBudgetExhausted(self._blocked[1]) + self.counts[kind] += amount + + +class _AdaptivePoseSource(_BoundedPoseSource): + """One bounded proposal stream; candidates never change after allocation.""" + + def __init__(self, env, options, *, seed, revision): + super().__init__(env, options["scene"], seed=seed, revision=revision) + from .task_pose_coverage import TaskPoseCoverage + from ._gym_utils import _hash + from embodichain.lab.sim.scene.feasibility import ( + FeasibilityBudget, + SceneRobotFeasibilityCfg, + ) + + self.env, self.options = env, deepcopy(options) + self.cfg, self.schedule = ( + self.options["scene_sampling"], + self.options["scheduling"], + ) + if self.cfg["control_part"] not in env.robot.control_parts: + raise ValueError("scene_sampling.control_part is absent from the robot") + self.coverage_cfg = self.options.get("pose_coverage") + invariant = { + "robot": env.robot.cfg.to_dict(), + "physics": env.sim.sim_config.physics_cfg.to_dict(), + "poses": self.poses, + "roots": self.roots, + "domain": self.configuration, + "coverage": self.coverage_cfg, + "validation": options["validation"], + "objects": { + uid: obj.cfg.to_dict() + for uid in getattr(env.sim, "get_rigid_object_uid_list", lambda: ())() + if (obj := env.sim.get_rigid_object(uid)) is not None + }, + } + # _hash handles JSON data only; pose arrays are owned host arrays here. + invariant.pop("poses") + self.domain_id = _hash(invariant) + self.coverage = ( + None + if self.coverage_cfg is None + else TaskPoseCoverage( + self.domain_id, self.coverage_cfg["entity"], self.coverage_cfg["grids"] + ) + ) + self.budget = _PoseBudget( + SceneRobotFeasibilityCfg( + max_candidates=self.cfg["max_candidates"], + max_ik_calls=self.cfg["max_ik_calls"], + max_planning_calls=self.cfg["max_planning_calls"], + max_rollouts=self.cfg["max_rollouts"], + max_wall_time_s=options["engine"]["max_wall_time_s"], + ) + ) + self.budget.validation = ( + True # Use the full host allocation, not search/held-out splitting. + ) + self._candidates = {} + self._checks = {} + self._predicted = {} + self._outcomes = {} + self._stats = {} + self._events = [] + self._raw_ordinal = 0 + self._root_pose = env.robot.get_local_pose(to_matrix=True)[0].detach().clone() + self._joint_seed = ( + env.robot.get_qpos(name=self.cfg["control_part"])[0:1].detach().clone() + if self.cfg["strategy"] == "workspace_joint" + else None + ) + self._local = None + self.output = None + if self.cfg["strategy"] == "workspace_joint": + self._prepare_workspace() + + def bind_program(self, adapter, program): + """Reject mismatched authored grasp branches before any candidate mutates a row.""" + if self.cfg["strategy"] != "workspace_joint": + return + profile = adapter._factory.create_robot_skill_profile() + preset = profile.presets[self.env.cfg.task_program.integration.runtime_preset] + pick = preset.skill_option_templates.get( + "pick_up", preset.skill_option_templates.get("pick") + ) + fixed = None if pick is None else pick.fixed_object_to_eef + if fixed is None: + raise ValueError( + "workspace_joint requires a fixed_object_to_eef pick policy; sampled grasp replacement is unsupported" + ) + from embodichain.lab.task_program.semantics.calls import Pick + + picked = { + entry.call.object.entity_id + for segment in program.iter_segments() + for entry in segment.calls + if type(entry.call) is Pick + } + if not picked: + raise ValueError( + "workspace_joint requires an explicit Pick in its task program" + ) + for entity in picked: + transform = self.cfg["targets"].get(entity) + if transform is None or not np.allclose( + np.array(transform), fixed.detach().cpu().numpy(), rtol=0, atol=1e-6 + ): + raise ValueError( + "Workspace grasp target must match the executed fixed_object_to_eef branch" + ) + + def _prepare_workspace(self): + robot = self.env.robot + part = self.cfg["control_part"] + n = self.cfg["sample_count"] + generator = torch.Generator(device=robot.device).manual_seed(self.seed) + if self.cfg["workspace_source"] == "cache": + cache = robot.get_workspace(part) + settings = robot.cfg.workspace_cfg[part] + ids = cache.sample_indices( + n, + strategy=settings.strategy, + min_score=settings.min_score, + generator=generator, + ) + if bool((ids < 0).any()): + raise ValueError("Workspace cache returned invalid indices") + q = cache.qpos[ids].to(robot.device) + else: + limits = robot.get_qpos_limits(name=part, env_ids=[0])[0] + if not bool(torch.isfinite(limits).all()): + raise ValueError("Workspace FK requires finite joint limits") + q = limits[:, 0] + torch.rand( + (n, len(limits)), device=robot.device, generator=generator + ) * (limits[:, 1] - limits[:, 0]) + fk = robot.compute_batch_fk(q[None], name=part, env_ids=[0], to_matrix=True)[0] + if not bool(torch.isfinite(fk).all()): + raise ValueError("Workspace FK produced nonfinite poses") + self._local = ( + (torch.linalg.inv(self._root_pose)[None] @ fk).detach().cpu().numpy().copy() + ) + + def _guided(self, candidate, rng): + from dataclasses import replace + from embodichain.lab.sim.scene.expansion import ScenePoseChange, SceneVariant + + poses = { + c.entity_uid: np.array(c.arena_pose) for c in candidate.scene.pose_changes + } + origin = "uniform_exploration" + if rng.random() >= self.cfg["uniform_fraction"]: + origin = "workspace_joint" + root = np.array(candidate.deployment.poses[self.env.robot.uid]) + for uid, transform in self.cfg["targets"].items(): + proposed = ( + root[None] @ self._local @ np.linalg.inv(np.array(transform))[None] + ) + base = self.poses[uid] + options = self.configuration[uid] + yaw = np.arctan2(proposed[:, 1, 0], proposed[:, 0, 0]) - math.atan2( + base[1, 0], base[0, 0] + ) + bounds = np.array(options["yaw_rad"]) + yaw += (2 * math.pi) * np.round((bounds.mean() - yaw) / (2 * math.pi)) + xy = proposed[:, :2, 3] - base[:2, 3] + valid = np.flatnonzero( + np.all( + (xy >= np.array(options["translation_m"])[:2, 0]) + & (xy <= np.array(options["translation_m"])[:2, 1]), + axis=1, + ) + & (yaw >= bounds[0]) + & (yaw <= bounds[1]) + ) + if not len(valid): + return candidate, "workspace_no_projected_sample" + index = int(rng.choice(valid)) + angle = float(yaw[index]) + c, s = math.cos(angle), math.sin(angle) + poses[uid][:3, :3] = ( + np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) @ base[:3, :3] + ) + poses[uid][:2, 3] = proposed[index, :2, 3] + for uid, opts in self.configuration.items(): + if "follow" in opts: + parent = opts["follow"] + poses[uid][:3, 3] = ( + self.poses[uid][:3, 3] + + poses[parent][:3, 3] + - self.poses[parent][:3, 3] + ) + poses[uid][:3, :3] = ( + poses[parent][:3, :3] + @ self.poses[parent][:3, :3].T + @ self.poses[uid][:3, :3] + ) + changes = tuple( + ScenePoseChange(uid, tuple(map(tuple, pose))) for uid, pose in poses.items() + ) + return ( + replace( + candidate, + scene=SceneVariant( + self.request_id, self.seed, candidate.scene.ordinal, changes + ), + ), + origin, + ) + + def _check(self, candidate): + from embodichain.lab.sim.motion._budget import _planning_budget_scope + + robot = self.env.robot + part = self.cfg["control_part"] + poses = { + c.entity_uid: np.array(c.arena_pose) for c in candidate.scene.pose_changes + } + for uid, transform in self.cfg["targets"].items(): + target = torch.tensor( + poses[uid] @ np.array(transform), + device=robot.device, + dtype=torch.float32, + )[None] + with _planning_budget_scope(self.budget): + ok, q = robot.compute_ik( + pose=target, joint_seed=self._joint_seed, name=part, env_ids=[0] + ) + limits = robot.get_qpos_limits(name=part, env_ids=[0]) + if ( + not bool(ok.all()) + or not bool(torch.isfinite(q).all()) + or not bool(((q >= limits[..., 0]) & (q <= limits[..., 1])).all()) + ): + return False, "pose_ik_or_joint_limits" + fk = robot.compute_fk(qpos=q, name=part, env_ids=[0], to_matrix=True) + angle = torch.acos( + ( + ( + torch.einsum( + "bii->b", fk[:, :3, :3].transpose(1, 2) @ target[:, :3, :3] + ) + - 1 + ) + / 2 + ).clamp(-1, 1) + ) + if ( + not bool(torch.isfinite(fk).all()) + or float((fk[:, :3, 3] - target[:, :3, 3]).norm()) > 1e-3 + or float(angle.max()) > 1e-2 + ): + return False, "full_pose_fk_residual" + return True, "passed" + + def candidate(self, ordinal): + from dataclasses import replace + from embodichain.lab.sim.scene.feasibility import FeasibilityBudgetExhausted + + if ordinal in self._candidates: + return self._candidates[ordinal] + if ( + type(ordinal) is not int + or not 0 <= ordinal < self.options["engine"]["max_attempts"] + ): + raise ValueError("ordinal must be within the configured attempt budget") + rng = np.random.default_rng( + int.from_bytes( + hashlib.sha256( + f"{self.seed}/{ordinal}/pose-selection".encode() + ).digest()[:8], + "little", + ) + ) + remaining = max( + 1, + self.cfg["max_rollouts"] + - self.budget.counts["rollouts"] + - len(self._predicted), + ) + quota = max(1, (self.cfg["max_candidates"] - self._raw_ordinal) // remaining) + count = ( + 1 + if self.schedule["policy"] == "fifo" + and self.cfg["strategy"] == "bounded_pose" + else min(self.schedule["pool_size"], quota) + ) + pool = [] + last = None + origin = "bounded_pose" + reason = "no_candidate" + for _ in range(count): + raw = self._raw_ordinal + # Keep the historical simple FIFO sequence exactly intact. + candidate = super().candidate( + ordinal + if count == 1 and self.cfg["strategy"] == "bounded_pose" + else raw + ) + last = candidate + try: + self.budget.charge("candidates") + self._raw_ordinal += 1 + ok = True + if self.cfg["strategy"] == "workspace_joint": + candidate, origin = self._guided(candidate, rng) + ok, reason = self._check(candidate) + else: + reason = "passed" + except FeasibilityBudgetExhausted as e: + ok = False + reason = str(e) + poses = {c.entity_uid: c.arena_pose for c in candidate.scene.pose_changes} + cells = ( + () + if self.coverage is None + else self.coverage.cells( + poses[self.coverage_cfg["entity"]], + poses[self.coverage_cfg["place_reference"]], + ) + ) + self._events.append( + { + "event": "proposal", + "ordinal": ordinal, + "raw_ordinal": raw, + "origin": origin, + "screened": ok, + "reason": reason, + "pose_changes": { + c.entity_uid: c.arena_pose for c in candidate.scene.pose_changes + }, + "installation": dict(candidate.deployment.poses), + "cells": cells, + } + ) + if ok: + pool.append((candidate, cells, origin)) + if self.schedule["policy"] == "fifo" and pool: + break + if self.budget._blocked is not None: + break + valid = bool(pool) + if pool: + if ( + self.schedule["policy"] == "fifo" + or rng.random() < self.schedule["exploration_fraction"] + ): + chosen = pool[int(rng.integers(len(pool)))] + selection = "exploration" + else: + predicted = Counter( + c for values in self._predicted.values() for c in values + ) + + def score(entry): + cells = entry[1] + success, total = self._stats.get(self._region(cells), (0, 0)) + return ( + (success + 1) + / (total + 2) + * self.coverage.gain(cells, predicted) + ) + + scores = np.array([score(c) for c in pool]) + ids = np.flatnonzero(scores == scores.max()) + chosen = pool[int(rng.choice(ids))] + selection = "coverage" + candidate, cells, origin = chosen + self._predicted[ordinal] = cells + reason = "passed" + else: + candidate = last + cells = () + selection = "rejected" + # Candidate identity includes the complete correlated vector, not just + # the raw draw before workspace conditioning. + parameters = tuple( + v + for change in candidate.scene.pose_changes + for row in change.arena_pose + for v in row + ) + candidate = replace( + candidate, + scene=replace(candidate.scene, ordinal=ordinal), + parameters=parameters, + ) + self._candidates[ordinal] = candidate + self._checks[ordinal] = (valid, reason) + self._events.append( + { + "event": "selection", + "ordinal": ordinal, + "candidate_id": candidate.candidate_id, + "selection": selection, + "origin": origin, + "cells": cells, + "screened": valid, + } + ) + self.checkpoint() + return candidate + + def validate(self, ordinal): + from .contracts import ValidationCheck, ValidationResult + + ok, reason = self._checks[ordinal] + return ValidationResult( + ( + ValidationCheck( + "scene_sampling.proposal", "passed" if ok else "failed", reason + ), + ) + ) + + @staticmethod + def _region(cells): + return tuple((cell[0], *(axis // 2 for axis in cell[1:])) for cell in cells) + + def finish(self, ordinal, episode, receipt, *, executed): + if ordinal in self._outcomes: + return + cells = self._predicted.pop(ordinal, ()) + success = episode is not None and episode.validation.accepted + if executed: + s, n = self._stats.get(self._region(cells), (0, 0)) + self._stats[self._region(cells)] = (s + int(success), n + 1) + result = { + "elapsed_s": self.budget.clock() - self.budget.started, + "proposals": self.budget.counts["candidates"], + "rollouts": self.budget.counts["rollouts"], + "event": "outcome", + "ordinal": ordinal, + "executed": executed, + "physical_accepted": success, + "committed": receipt is not None and receipt.confirmed, + "coverage": ({} if self.coverage is None else self.coverage.phase_counts()), + } + if episode is not None and "full_joint_positions" in episode.observations: + states = episode.observations["full_joint_positions"] + segments = episode.metadata.get("program_segments", ()) + pick_end = ( + min(len(states) - 1, int(segments[0]["end_step"])) if segments else 0 + ) + ids = self.env.robot.control_parts[self.cfg["control_part"]] + if ids and isinstance(ids[0], str): + ids = [self.env.robot.joint_names.index(name) for name in ids] + row = int(episode.metadata["source_env_id"]) + values = states[[pick_end, len(states) - 1]][:, ids].to( + self.env.robot.device + )[None] + tcp = self.env.robot.compute_batch_fk( + values, name=self.cfg["control_part"], env_ids=[row], to_matrix=True + )[0] + result["measured_tcp_poses"] = tcp.detach().cpu().tolist() + result["measured_tcp_indices"] = [pick_end, len(states) - 1] + self._outcomes[ordinal] = result + self._events.append(result) + self.checkpoint() + + def state_dict(self): + from dataclasses import asdict + + return deepcopy( + { + "version": 1, + "seed": self.seed, + "revision": self.request_id, + "options": self.options, + "coverage": ( + None if self.coverage is None else self.coverage.state_dict() + ), + "raw_ordinal": self._raw_ordinal, + "candidates": { + str(k): { + "request_id": v.request_id, + "scene": asdict(v.scene), + "deployment": {"poses": dict(v.deployment.poses)}, + "mode_id": v.mode_id, + "parameters": v.parameters, + } + for k, v in self._candidates.items() + }, + "checks": self._checks, + "predicted": self._predicted, + "outcomes": self._outcomes, + "events": self._events, + "statistics": [ + [cells, list(counts)] for cells, counts in self._stats.items() + ], + "budget": dict(self.budget.counts), + "elapsed_s": self.budget.clock() - self.budget.started, + "blocked": self.budget._blocked, + } + ) + + def load_state_dict(self, state): + """Restore deterministic proposal history; never promote pending writes.""" + from embodichain.lab.sim.scene.expansion import ScenePoseChange, SceneVariant + from embodichain.lab.sim.scene.feasibility import ( + JointSceneCandidate, + RobotDeploymentCandidate, + ) + + data = deepcopy(state) + if ( + self._candidates + or data.get("version") != 1 + or data.get("seed") != self.seed + or data.get("revision") != self.request_id + or data.get("options") != self.options + ): + raise ValueError("Sampling restore requires an empty, matching source") + candidates = {} + for key, value in data["candidates"].items(): + scene = dict(value["scene"]) + scene["pose_changes"] = tuple( + ScenePoseChange(**change) for change in scene["pose_changes"] + ) + candidates[int(key)] = JointSceneCandidate( + value["request_id"], + SceneVariant(**scene), + RobotDeploymentCandidate(**value["deployment"]), + value["mode_id"], + tuple(value["parameters"]), + ) + if ( + any(not 0 <= k < self.options["engine"]["max_attempts"] for k in candidates) + or not 0 <= data["raw_ordinal"] <= self.cfg["max_candidates"] + ): + raise ValueError("Sampling checkpoint exceeds declared bounds") + budget = data["budget"] + limits = { + "candidates": self.cfg["max_candidates"], + "ik": self.cfg["max_ik_calls"], + "planning": self.cfg["max_planning_calls"], + "rollouts": self.cfg["max_rollouts"], + } + if ( + set(budget) != set(limits) + or any( + type(v) is not int or not 0 <= v <= limits[k] for k, v in budget.items() + ) + or not math.isfinite(data["elapsed_s"]) + or data["elapsed_s"] < 0 + ): + raise ValueError("Malformed sampling budget checkpoint") + if self.coverage is not None: + self.coverage.load_state_dict(data["coverage"]) + elif data["coverage"] is not None: + raise ValueError("Checkpoint unexpectedly contains pose coverage") + self._candidates = candidates + self._checks = {int(k): tuple(v) for k, v in data["checks"].items()} + self._predicted = { + int(k): tuple(tuple(c) for c in v) for k, v in data["predicted"].items() + } + self._outcomes = {int(k): v for k, v in data["outcomes"].items()} + self._events = data["events"] + self._stats = { + tuple(tuple(c) for c in cells): tuple(counts) + for cells, counts in data["statistics"] + } + self._raw_ordinal = data["raw_ordinal"] + self.budget.counts = budget + self.budget.started = self.budget.clock() - data["elapsed_s"] + self.budget.deadline = self.budget.started + self.budget.cfg.max_wall_time_s + self.budget._blocked = ( + None if data["blocked"] is None else tuple(data["blocked"]) + ) + + def checkpoint(self): + if self.output is None: + return + from embodichain.data_pipeline.recording.media import _atomic_json + + _atomic_json( + self.output / "pose-sampling.json", + { + "strategy": self.cfg["strategy"], + "policy": self.schedule["policy"], + "domain_id": self.domain_id, + "budget": dict(self.budget.counts), + "events": self._events, + "coverage": ( + None if self.coverage is None else self.coverage.state_dict() + ), + "checkpoint": self.state_dict(), + }, + ) diff --git a/embodichain/lab/expansion/_pose_sampling_cfg.py b/embodichain/lab/expansion/_pose_sampling_cfg.py new file mode 100644 index 0000000000..0f996d6370 --- /dev/null +++ b/embodichain/lab/expansion/_pose_sampling_cfg.py @@ -0,0 +1,162 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Strict optional scene sampling and pose coverage declarations.""" + +from __future__ import annotations +import math +import numpy as np + +__all__: list[str] = [] + + +def validate_sampling(value: dict) -> None: + """Normalize optional settings without changing legacy configurations.""" + from ._pose_cells import _grid + + allowed = { + "strategy", + "workspace_source", + "control_part", + "sample_count", + "uniform_fraction", + "targets", + "max_rollouts", + "max_candidates", + "max_ik_calls", + "max_planning_calls", + } + if not any(k in value for k in ("scene_sampling", "scheduling", "pose_coverage")): + return + sampling = value.setdefault("scene_sampling", {}) + schedule = value.setdefault("scheduling", {}) + if not isinstance(sampling, dict) or set(sampling) - allowed: + raise ValueError("Unknown scene_sampling settings") + if not isinstance(schedule, dict) or set(schedule) - { + "policy", + "pool_size", + "exploration_fraction", + }: + raise ValueError("Unknown pose scheduling settings") + for k, v in { + "strategy": "bounded_pose", + "workspace_source": "joint_uniform_fk", + "control_part": "arm", + "sample_count": 4096, + "uniform_fraction": 0.25, + "targets": {}, + "max_candidates": 4096, + "max_rollouts": value["engine"]["max_attempts"], + "max_ik_calls": 32768, + "max_planning_calls": 4096, + }.items(): + sampling.setdefault(k, v) + for k, v in { + "policy": "fifo", + "pool_size": 64, + "exploration_fraction": 0.25, + }.items(): + schedule.setdefault(k, v) + if sampling["strategy"] not in ("bounded_pose", "workspace_joint"): + raise ValueError( + "scene_sampling.strategy must be bounded_pose or workspace_joint" + ) + if sampling["workspace_source"] not in ("cache", "joint_uniform_fk"): + raise ValueError( + "workspace_source must explicitly select cache or joint_uniform_fk" + ) + if schedule["policy"] not in ("fifo", "task_pose_coverage"): + raise ValueError("Pose scheduling supports fifo or task_pose_coverage") + for k in ( + "sample_count", + "max_candidates", + "max_rollouts", + "max_ik_calls", + "max_planning_calls", + ): + if type(sampling[k]) is not int or not 0 < sampling[k] <= 1000000: + raise ValueError(f"{k} must be a bounded positive integer") + if sampling["sample_count"] > 65536: + raise ValueError("Workspace sample_count must be <= 65536") + if sampling["max_rollouts"] < value["engine"].get("episodes_per_operator", 1): + raise ValueError("max_rollouts must cover the requested episode quota") + if sampling["max_candidates"] < value["engine"]["max_attempts"]: + raise ValueError("max_candidates must cover at least one proposal per attempt") + if type(schedule["pool_size"]) is not int or not 1 <= schedule["pool_size"] <= 64: + raise ValueError("pool_size must be between 1 and 64") + for obj, key in ( + (sampling, "uniform_fraction"), + (schedule, "exploration_fraction"), + ): + if ( + type(obj[key]) not in (int, float) + or not math.isfinite(obj[key]) + or not 0 <= obj[key] <= 1 + ): + raise ValueError(f"{key} must be a probability") + if ( + type(sampling["control_part"]) is not str + or not sampling["control_part"].strip() + ): + raise ValueError("control_part must be explicit") + if not isinstance(sampling["targets"], dict): + raise ValueError( + "workspace targets must map scene entities to object-from-TCP matrices" + ) + if sampling["strategy"] == "workspace_joint" and not sampling["targets"]: + raise ValueError( + "workspace_joint requires explicit object-local target transforms" + ) + for uid, pose in sampling["targets"].items(): + m = np.asarray(pose, dtype=float) + if ( + uid not in value["scene"] + or "follow" in value["scene"][uid] + or m.shape != (4, 4) + or not np.isfinite(m).all() + or not np.allclose(m[3], [0, 0, 0, 1]) + or not np.allclose(m[:3, :3].T @ m[:3, :3], np.eye(3), atol=1e-6) + or not np.isclose(np.linalg.det(m[:3, :3]), 1) + ): + raise ValueError( + "Workspace targets require a declared independent scene entity and rigid transform" + ) + coverage = value.get("pose_coverage") + if coverage is None: + if schedule["policy"] == "task_pose_coverage": + raise ValueError( + "task_pose_coverage scheduling requires pose_coverage grids" + ) + return + if not isinstance(coverage, dict) or set(coverage) != { + "entity", + "place_reference", + "grids", + }: + raise ValueError("pose_coverage requires entity, place_reference and grids") + if ( + any(coverage[k] not in value["scene"] for k in ("entity", "place_reference")) + or coverage["entity"] == coverage["place_reference"] + ): + raise ValueError( + "Coverage entity and place_reference must select distinct scene entities" + ) + if not isinstance(coverage["grids"], dict) or set(coverage["grids"]) != { + "pick", + "place", + }: + raise ValueError("Coverage requires pick and place grids") + coverage["grids"] = {k: _grid(v) for k, v in coverage["grids"].items()} diff --git a/embodichain/lab/expansion/_task_program_config.py b/embodichain/lab/expansion/_task_program_config.py index a4b1518890..73c464e8f2 100644 --- a/embodichain/lab/expansion/_task_program_config.py +++ b/embodichain/lab/expansion/_task_program_config.py @@ -77,10 +77,13 @@ def load_task_program_expansion(path: str | Path) -> dict[str, Any]: """ path = Path(path) value = load_mapping(path) - if ( - set(value) != {"schema_version", "engine", "scene", "trajectory", "validation"} - or value["schema_version"] != 1 - ): + if (set(value) - {"scene_sampling", "scheduling", "pose_coverage"}) != { + "schema_version", + "engine", + "scene", + "trajectory", + "validation", + } or value["schema_version"] != 1: raise ValueError("Invalid demo expansion configuration") engine, motion = value["engine"], value["trajectory"] required_engine = { @@ -365,4 +368,7 @@ def load_task_program_expansion(path: str | Path) -> dict[str, Any]: or hold[field] <= 0 ): raise ValueError(f"contact_hold.{field} must be finite and positive") + from ._pose_sampling_cfg import validate_sampling + + validate_sampling(value) return value diff --git a/embodichain/lab/expansion/_workload_cursor.py b/embodichain/lab/expansion/_workload_cursor.py index 92a1633c2f..4e35e96e73 100644 --- a/embodichain/lab/expansion/_workload_cursor.py +++ b/embodichain/lab/expansion/_workload_cursor.py @@ -32,9 +32,17 @@ class _WorkloadCursor(_Cursor): - def __init__(self, bridge: TaskProgramDemoBridge, robot: Any, seed: int) -> None: + def __init__( + self, + bridge: TaskProgramDemoBridge, + robot: Any, + seed: int, + *, + budget: Any = None, + ) -> None: super().__init__(bridge) self.robot = robot + self.budget = budget self.rejection = None self.last_command = None self.devices = ( @@ -66,7 +74,34 @@ def next(self) -> ControllerAction | None: np.random.set_state(self.numpy_rng) random.setstate(self.python_rng) try: - action = super().next() + if ( + self.budget is None + or self.rejection is not None + or self.emergency is not None + ): + action = super().next() + else: + from embodichain.lab.sim.motion._budget import ( + _planning_budget_scope, + ) + from embodichain.lab.sim.scene.feasibility import ( + FeasibilityBudgetExhausted, + ) + + try: + with _planning_budget_scope(self.budget): + action = super().next() + except FeasibilityBudgetExhausted as error: + if self.segment is None: + # No command or segment exists yet: rejection has no + # safe-stop action to acknowledge and earns no step. + self.finished = True + self.accepted = False + self.reason = str(error) + action = None + else: + self.abort(str(error)) + action = super().next() if action is not None: if ( self.rejection is not None or self.emergency is not None diff --git a/embodichain/lab/expansion/distribution.md b/embodichain/lab/expansion/distribution.md new file mode 100644 index 0000000000..d73bfb3343 --- /dev/null +++ b/embodichain/lab/expansion/distribution.md @@ -0,0 +1,65 @@ +# Task distribution report + +Run from an EmbodiChain environment with the existing artifact reader dependencies: + +```bash +python -m embodichain.lab.expansion._distribution \ + --before /absolute/job-a \ + --after /absolute/job-a /absolute/job-b \ + --entity cube --cell-size 0.1 --minimum 5 \ + --output /absolute/new-report-directory +``` + +Inputs must be job directories containing `job.json` and confirmed artifact receipts. +Repeated input paths/commit identities do not multiply samples. Conflicting commit +identities fail. Output must be new; inputs are never overwritten. Both snapshots +must have the same task, robot installation, topology and configured scene domain. + +Open `report.html`. Select a spatial region or joint-distribution cell to inspect +recorded object paths. The evidence panel identifies the full artifact and the +existing native preview command (which retains its original preload limit). +The report itself needs no server or external network. `summary.json` carries +version, snapshot hash, domain, distributions, observed-bin deficits and limitations; +`before-episodes.jsonl` and `after-episodes.jsonl` carry source references and metrics. + +Version 2 adds full orientation statistics: normalized sign-canonical xyzw, +initial/final SO(3) rotation vectors, angular path length, net rotation and angular +speed quantiles. Three-dimensional rotation-vector bins have 30-degree edges; +they are not equal-volume and no object symmetry is assumed. Original full XYZ +and quaternion poses remain in every episode record. + +TCP is explicitly derived, not directly measured: recorded full joint states and +root poses drive the recorded URDF chain, followed by the single declared gripper +root-to-TCP calibration. The record includes the asset digest, link, transform and +method. Missing/ambiguous calibration returns unavailable; malformed transforms +fail. FK runs in 256-frame batches. Scaled robots are not supported. This is the +same recorded-URDF FK approach used by the existing preview, extended with TCP +calibration; no physics World is created. + +`summary.json` now holds aggregate statistics and a lightweight episode index, +not all trajectory details. `pages/{snapshot}-{page}.json` and matching local JS +files contain 100 records per page; the latter lets the offline HTML open without +an HTTP service. Agent tools use JSON. The viewer loads details on demand, retains +at most two detail pages, and displays 50 episode options per page. Joint heatmaps +are paged in 20-by-20 blocks; orientation counts are paged in rows of 20. Filters +operate on the full index before pagination. Index memory remains O(N); extracting +features also retains O(N) bounded descriptors, not full raw trajectories. Page +files and JSONL contain identical episode records. + +World-XY maps remain the spatial projection; trajectory display switches between +object and TCP XY paths. Raw numeric evidence is never changed. Joint path length +is in joint-coordinate units and must not be interpreted as Cartesian metres. +Whole-episode metrics and existing phase-aware geometry buckets remain separate. + +Counts cover observed cells only; no continuous workspace coverage or unobserved +feasibility claim is made. Different physical parents remain separate samples. +Use physical collection roots, not visual-child datasets. Comparison is cumulative +only if every before commit occurs in after; other comparisons are independent. +No automatic collection, arbitrary LeRobot import or learned clustering is added. + +Validation dataset: the complete `formal-qualified` pick-place matrix has 40 +physical job roots (two scenes, five seeds, four sampling/scheduling variants). +For each scene, the before snapshot is variant A across five seeds; after is +the cumulative union A+B+C+D. Interrupted diagnostics and visual children are +excluded. This compares dataset composition, not a causal learning experiment. +Use all matching job roots in `--before` and `--after`; retain scene separation. diff --git a/embodichain/lab/expansion/gym_workloads.py b/embodichain/lab/expansion/gym_workloads.py index e818f96574..8c61d3c501 100644 --- a/embodichain/lab/expansion/gym_workloads.py +++ b/embodichain/lab/expansion/gym_workloads.py @@ -125,6 +125,24 @@ def __init__( for uid in self.env.sim.get_rigid_object_uid_list() } + def planning_budget(self, assignment): + feedback = self.groups[assignment.workload_id]["host"]._context.feedback + return None if feedback is None else feedback.budget + + def can_admit(self, name): + feedback = self.groups[name]["host"]._context.feedback + if feedback is None: + return True + if self.groups[name]["session"].stop_reason is not None: + return False + from embodichain.lab.sim.scene.feasibility import FeasibilityBudgetExhausted + + try: + feedback.budget.check() + except FeasibilityBudgetExhausted: + return False + return True + def prepare(self, assignments: tuple[WorkloadAssignment, ...]) -> None: from embodichain.lab.sim.scene import ( SceneView, @@ -168,9 +186,12 @@ def prepare(self, assignments: tuple[WorkloadAssignment, ...]) -> None: ) ctx.requests[assignment.env_id] = request if ctx.scene_source is not None: - candidates[assignment.env_id] = ctx.scene_source.candidate( - assignment.ordinal - ) + candidate = ctx.scene_source.candidate(assignment.ordinal) + if ( + ctx.feedback is None + or ctx.feedback.validate(assignment.ordinal).accepted + ): + candidates[assignment.env_id] = candidate if candidates: apply_joint_scene_candidates( SceneView(self.env.sim, env_ids=tuple(candidates)), candidates @@ -297,6 +318,11 @@ def activate(self, assignment: WorkloadAssignment, observation: object) -> Any: ), ) ) + if ctx.feedback is not None: + ctx.preparation_checks[row] = ValidationResult( + ctx.preparation_checks[row].checks + + ctx.feedback.validate(assignment.ordinal).checks + ) item = PreparedEpisodeWorkItem( EpisodeCandidate(identity, json.dumps(request.recipe.to_dict())), ctx.compatibility.key, @@ -346,6 +372,8 @@ def started(self, assignment: WorkloadAssignment, world_control_step: int) -> No state = self.active[row] ctx = state["group"]["host"]._context state["group"]["engine"].mark_row_started(state["native"]) + if ctx.feedback is not None: + ctx.feedback.budget.counts["rollouts"] += 1 ctx.prepared[row][4]["world_control_start"] = world_control_step ctx.journal.update( ctx.requests[row], status="running", world_control_start=world_control_step @@ -393,6 +421,10 @@ def complete(self, result: WorkloadRowResult) -> Any: status="initial_rejected", reason="wall_time_exhausted", ) + if ctx.feedback is not None: + ctx.feedback.finish( + result.assignment.ordinal, None, None, executed=False + ) return None group = state["group"] ctx = group["host"]._context @@ -451,7 +483,18 @@ def complete(self, result: WorkloadRowResult) -> Any: }, }, ) - return group["engine"].complete_row(state["native"], episode) + receipt = None + try: + receipt = group["engine"].complete_row(state["native"], episode) + return receipt + finally: + if ctx.feedback is not None: + ctx.feedback.finish( + result.assignment.ordinal, + episode, + receipt, + executed=result.world_control_start is not None, + ) finally: state["adapter"].close() self.collector.discard(row) @@ -465,6 +508,13 @@ def cancel(self, result: WorkloadRowResult, *, reason: str) -> None: group = self.groups[result.assignment.workload_id] ctx = group["host"]._context request = ctx.requests[row] + if ctx.feedback is not None: + ctx.feedback.finish( + result.assignment.ordinal, + None, + None, + executed=result.world_control_start is not None, + ) record = { "env_id": row, "status": "cancelled", @@ -513,6 +563,10 @@ def cancel(self, result: WorkloadRowResult, *, reason: str) -> None: self.collector.discard(row) def close(self) -> None: + for group in self.groups.values(): + feedback = group["host"]._context.feedback + if feedback is not None: + feedback.checkpoint() for state in self.active.values(): state["adapter"].close() self.active.clear() @@ -569,6 +623,7 @@ def run_gym_expansion_workloads( common = extensions or GymExpansionHostExtensions() if ( common.scene_source_factory + or common.feedback_factory or common.recipe_transform or common.plan_transform_factory ): @@ -784,9 +839,14 @@ def physical_key(env_id: str, cfg: Any) -> str: bridges={}, extensions=workload.extensions, scene_source=None, + feedback=None, ) if workload.extensions.scene_source_factory is not None: ctx.scene_source = workload.extensions.scene_source_factory(env) + if workload.extensions.feedback_factory is not None: + ctx.feedback = workload.extensions.feedback_factory( + env, ctx.scene_source, path + ) builder = GymRecipeBuilder(ctx) transform = workload.extensions.recipe_transform ctx.recipe = lambda ordinal, builder=builder, transform=transform: ( @@ -814,6 +874,10 @@ def physical_key(env_id: str, cfg: Any) -> str: ) adapters[id(factory)] = adapter ctx.program = adapter.compile(environment_cfg.task_program) + if ctx.feedback is not None: + bind = getattr(ctx.feedback, "bind_program", None) + if bind is not None: + bind(adapter, ctx.program) if ( workload.extensions.plan_transform_factory is not None and adapter.configured_nominal_sources @@ -839,7 +903,11 @@ def admission_clock(admitted_at=admitted_at): { "collection": { "target_committed_episodes": cfg.target_episodes, - "max_rollout_attempts": cfg.max_attempts, + "max_rollout_attempts": ( + cfg.max_attempts + if ctx.feedback is None + else ctx.feedback.budget.cfg.max_rollouts + ), "max_proposals": cfg.max_attempts, "max_wall_time_s": cfg.max_wall_time_s, }, @@ -855,6 +923,7 @@ def admission_clock(admitted_at=admitted_at): "validation": {"require_path_collision": True}, } ), + coverage=None if ctx.feedback is None else ctx.feedback.coverage, episode_namespace=journal.job_instance_id, clock=admission_clock, ) @@ -907,6 +976,8 @@ def export(name: str) -> dict[str, Any]: def done(name: str, counts: Mapping[str, object]) -> None: group = groups[name] journal = group["host"]._context.journal + if group["host"]._context.feedback is not None: + group["host"]._context.feedback.checkpoint() complete = counts["committed"] == group["host"].cfg.target_episodes journal.stage("collection", "complete" if complete else "budget_exhausted") journal.checkpoint(force=True) diff --git a/embodichain/lab/expansion/host.py b/embodichain/lab/expansion/host.py index 2bb2bcfd5b..739a4355c9 100644 --- a/embodichain/lab/expansion/host.py +++ b/embodichain/lab/expansion/host.py @@ -103,6 +103,8 @@ def __init__( self._context.extensions = extensions or GymExpansionHostExtensions() if not isinstance(self._context.extensions, GymExpansionHostExtensions): raise TypeError("extensions must be GymExpansionHostExtensions") + if self._context.extensions.feedback_factory is not None: + raise ValueError("feedback_factory requires run_gym_expansion_workloads") self._recipe_builder = GymRecipeBuilder(self._context) self._context.recipe = self.recipe self._preparer = GymCohortPreparer(self._context) diff --git a/embodichain/lab/expansion/host_extensions.py b/embodichain/lab/expansion/host_extensions.py index c7acc00d60..911e1e67b4 100644 --- a/embodichain/lab/expansion/host_extensions.py +++ b/embodichain/lab/expansion/host_extensions.py @@ -18,7 +18,7 @@ from __future__ import annotations -from dataclasses import dataclass +from dataclasses import dataclass, field from collections.abc import Callable, Mapping from pathlib import Path from typing import Any, Protocol @@ -83,6 +83,9 @@ class GymExpansionHostExtensions: plan_transform_factory: Receives environment and immutable row identity; returns an ordinary Task Program transform factory. Its record metadata and hashed plan attachments are archived by the native episode owner. + feedback_factory: Optional rolling-collection feedback owner receiving env, + scene source and artifact directory; exposes coverage, budget, validate, + finish and checkpoint without advancing physics or writing episodes. require_plan_transform: Reject completed programs without transform records. """ @@ -91,6 +94,9 @@ class GymExpansionHostExtensions: validator_factory: Callable[[Any, Path], ExpansionValidator] | None = None recipe_transform: Callable[[ExpansionRecipe], ExpansionRecipe] | None = None plan_transform_factory: Callable[[Any, ExpansionRowContext], Any] | None = None + feedback_factory: Callable[[Any, Any, Path], Any] | None = field( + default=None, kw_only=True + ) require_plan_transform: bool = False def __post_init__(self) -> None: @@ -101,6 +107,7 @@ def __post_init__(self) -> None: raise ValueError("collection_renderer must be a nonempty renderer name") for name in ( "scene_source_factory", + "feedback_factory", "validator_factory", "recipe_transform", "plan_transform_factory", diff --git a/embodichain/lab/expansion/task_pose_coverage.py b/embodichain/lab/expansion/task_pose_coverage.py new file mode 100644 index 0000000000..106495c268 --- /dev/null +++ b/embodichain/lab/expansion/task_pose_coverage.py @@ -0,0 +1,240 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Receipt-confirmed task pose coverage shared across sampled scene cases.""" + +from __future__ import annotations + +from collections import Counter +from collections.abc import Mapping +from copy import deepcopy +from typing import TYPE_CHECKING + +import numpy as np +import torch + +if TYPE_CHECKING: + from .contracts import ExpertEpisode, SceneCase + +__all__ = ["TaskPoseCoverage"] + + +from ._pose_cells import _grid, _pose_cell + + +class TaskPoseCoverage: + """Count measured pick/place cells only after ordinary sink confirmation. + + Args: + domain_id: Host-validated invariant experiment identity, including the + installation and grid, excluding sampled scene poses and RNG seed. + entity_uid: Manipulated rigid object whose measured states are counted. + grids: Explicit ``pick`` and ``place`` XY/yaw grids in arena coordinates. + + Scene cases keep their existing identities. This adapter aggregates them + within one declared experiment, never across separate instances/domains. + """ + + def __init__(self, domain_id: str, entity_uid: str, grids: Mapping) -> None: + if not all( + type(s) is str and s.strip() == s and s for s in (domain_id, entity_uid) + ): + raise ValueError("Coverage requires nonempty domain and entity identities") + if not isinstance(grids, Mapping) or set(grids) != {"pick", "place"}: + raise ValueError("Coverage requires pick and place grids") + self.domain_id, self.entity_uid = domain_id, entity_uid + self.grids = {k: _grid(v) for k, v in grids.items()} + self._cases: dict[str, tuple[str, str, str]] = {} + self._population = None + self._pending: dict[str, tuple[str, tuple]] = {} + self._confirmed: dict[str, tuple[str, tuple]] = {} + self._counts = Counter() + + def register_case( + self, + case: SceneCase, + joint_limits: torch.Tensor, + joint_names: tuple[str, ...], + reference: float | None, + ) -> None: + """Register a scene without changing its identity or normalization.""" + if reference is not None: + raise ValueError( + "Task pose coverage does not accept a manipulability reference" + ) + population = ( + case.task_id, + case.robot_profile_id, + case.calibration_id, + tuple(joint_names), + ) + if self._population is not None and population != self._population: + raise ValueError( + "Coverage domain cannot mix task/robot/calibration/joint layouts" + ) + self._population = population + identity = (case.task_id, case.robot_profile_id, case.calibration_id) + if ( + case.scene_case_id in self._cases + and self._cases[case.scene_case_id] != identity + ): + raise ValueError("Conflicting scene case registration") + self._cases[case.scene_case_id] = identity + + def reference(self, case: SceneCase) -> None: + """This coverage policy supplies no motion normalization reference.""" + if case.scene_case_id not in self._cases: + raise KeyError(case.scene_case_id) + return None + + def cells( + self, pick_pose: object, place_pose: object + ) -> tuple[tuple[str, int, int, int], ...]: + """Return ordered tagged cells; out-of-domain poses contribute no cell.""" + return tuple( + (phase, *cell) + for phase, pose in (("pick", pick_pose), ("place", place_pose)) + if (cell := _pose_cell(pose, self.grids[phase])) is not None + ) + + def reserve(self, episode: ExpertEpisode) -> bool: + """Own measured cells until the matching persistence receipt arrives.""" + case_id = episode.identity.scene_case_id + if case_id not in self._cases: + raise ValueError("Episode scene is outside this coverage domain") + if not episode.validation.accepted: + return False + states = episode.observations.get(f"rigid/{self.entity_uid}/state") + if ( + states is None + or states.ndim != 2 + or states.shape[0] != len(episode.timestamps) + or states.shape[1] < 7 + ): + raise ValueError("Task coverage requires T+1 measured rigid states") + cells = self.cells( + states[0].detach().cpu().numpy(), states[-1].detach().cpu().numpy() + ) + value = (case_id, cells) + previous = self._confirmed.get( + episode.commit_id, self._pending.get(episode.commit_id) + ) + if previous is not None and previous != value: + raise ValueError("Commit identity was reused with different measured cells") + if episode.commit_id not in self._confirmed: + self._pending[episode.commit_id] = value + return True # Duplicates are valid data; they cannot increase unique coverage. + + def confirm(self, case_id: str, commit_id: str) -> None: + """Idempotently count the reservation qualified by the session's receipt.""" + value = self._confirmed.get(commit_id, self._pending.get(commit_id)) + if value is None or value[0] != case_id: + raise ValueError("Foreign or unreserved coverage confirmation") + if commit_id in self._confirmed: + return + self._pending.pop(commit_id) + self._confirmed[commit_id] = value + self._counts.update(value[1]) + + def release(self, case_id: str, commit_id: str) -> None: + """Release unconfirmed evidence without erasing committed coverage.""" + value = self._pending.get(commit_id) + if value is not None and value[0] != case_id: + raise ValueError("Foreign coverage release") + self._pending.pop(commit_id, None) + + def gain(self, cells: tuple, predicted: Mapping | None = None) -> float: + """Score scarcity using formal counts and explicitly tentative occupancy.""" + pending = Counter( + cell for _, values in self._pending.values() for cell in values + ) + predicted = {} if predicted is None else predicted + return sum( + 1 / (1 + self._counts[cell] + pending[cell] + predicted.get(cell, 0)) + for cell in cells + ) + + def snapshot(self) -> tuple[Mapping[str, int], Mapping[str, Mapping[int, int]]]: + """Return confirmed coverage with no fabricated manipulability bands.""" + return {self.domain_id: len(self._counts)}, {} + + def phase_counts(self) -> Mapping[str, int]: + """Return owned pick/place counts separately from motion quality bands.""" + return { + phase: sum(cell[0] == phase for cell in self._counts) + for phase in self.grids + } + + def state_dict(self) -> dict: + """Return an owned receipt-derived checkpoint, including pending evidence.""" + return deepcopy( + { + "version": 1, + "domain_id": self.domain_id, + "entity_uid": self.entity_uid, + "grids": self.grids, + "population": self._population, + "cases": self._cases, + "pending": self._pending, + "confirmed": self._confirmed, + } + ) + + def load_state_dict(self, state: Mapping) -> None: + """Restore matching-domain counts; pending evidence remains unconfirmed.""" + data = deepcopy(dict(state)) + if data.get("version") != 1 or any( + data.get(k) != getattr(self, k) + for k in ("domain_id", "entity_uid", "grids") + ): + raise ValueError("Coverage checkpoint belongs to another domain/grid") + if self._pending or self._confirmed or self._cases: + raise ValueError("Restore coverage only into a new adapter") + + def decode(entries): + return { + key: (value[0], tuple(tuple(cell) for cell in value[1])) + for key, value in entries.items() + } + + pending, confirmed = decode(data["pending"]), decode(data["confirmed"]) + if pending.keys() & confirmed.keys(): + raise ValueError("Coverage checkpoint repeats pending/confirmed identities") + cases = {k: tuple(v) for k, v in data["cases"].items()} + for case, cells in (*pending.values(), *confirmed.values()): + if case not in cases or any( + len(c) != 4 + or c[0] not in self.grids + or any(type(i) is not int or i < 0 for i in c[1:]) + or any( + i >= round((hi - lo) / step) + for i, lo, hi, step in zip( + c[1:], + self.grids[c[0]]["lower"], + self.grids[c[0]]["upper"], + self.grids[c[0]]["cell_size"], + ) + ) + for c in cells + ): + raise ValueError("Malformed coverage checkpoint cells") + pop = data["population"] + self._cases = cases + self._population = None if pop is None else (*pop[:3], tuple(pop[3])) + self._pending, self._confirmed = pending, confirmed + self._counts = Counter( + cell for _, cells in confirmed.values() for cell in cells + ) diff --git a/embodichain/lab/expansion/task_program_jobs.py b/embodichain/lab/expansion/task_program_jobs.py index 2fa1ad6b1e..c92a00c296 100644 --- a/embodichain/lab/expansion/task_program_jobs.py +++ b/embodichain/lab/expansion/task_program_jobs.py @@ -55,67 +55,7 @@ def _load_deployment(path: Path) -> None: config_to_cfg(load_mapping(path), source_path=path) -class _BoundedPoseSource: - """Sample bounded arena-frame changes with task-declared support followers.""" - - def __init__( - self, env: Any, configuration: Mapping[str, Any], *, seed: int, revision: str - ) -> None: - self.configuration, self.seed, self.request_id = configuration, seed, revision - self.poses = { - uid: env.sim.get_rigid_object(uid) - .get_local_pose(to_matrix=True)[0] - .detach() - .cpu() - .numpy() - .copy() - for uid in configuration - } - self.roots = { - env.robot.uid: env.robot.get_local_pose(to_matrix=True)[0] - .detach() - .cpu() - .tolist() - } - - def candidate(self, ordinal: int) -> JointSceneCandidate: - import numpy as np - from embodichain.lab.sim.scene.expansion import ScenePoseChange, SceneVariant - from embodichain.lab.sim.scene.feasibility import ( - JointSceneCandidate, - RobotDeploymentCandidate, - ) - - offsets = {} - for uid, options in self.configuration.items(): - if "follow" in options: - continue - seed = int.from_bytes( - hashlib.sha256(f"{self.seed}/{ordinal}/{uid}".encode()).digest()[:8], - "little", - ) - generator = np.random.default_rng(seed) - offsets[uid] = [ - generator.uniform(*bounds) - for bounds in (*options["translation_m"], options["yaw_rad"]) - ] - changes, parameters = [], [] - for uid, options in self.configuration.items(): - dx, dy, dz, yaw = offsets[options.get("follow", uid)] - pose = self.poses[uid].copy() - c, s = np.cos(yaw), np.sin(yaw) - pose[:3, :3] = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) @ pose[:3, :3] - pose[:3, 3] += (dx, dy, dz) - changes.append(ScenePoseChange(uid, tuple(map(tuple, pose)))) - parameters.extend((dx, dy, dz, yaw)) - scene = SceneVariant(self.request_id, self.seed, ordinal, tuple(changes)) - return JointSceneCandidate( - self.request_id, - scene, - RobotDeploymentCandidate(self.roots), - "object_pose", - tuple(parameters), - ) +from ._pose_sampling import _BoundedPoseSource, _AdaptivePoseSource def _task_expansion_extensions(*, options, operator, seed, report): @@ -126,11 +66,20 @@ def _task_expansion_extensions(*, options, operator, seed, report): def source(env): nonlocal pose_source - pose_source = _BoundedPoseSource( - env, options["scene"], seed=seed, revision=report["semantic_id"] + pose_source = ( + _AdaptivePoseSource(env, options, seed=seed, revision=report["semantic_id"]) + if "scene_sampling" in options + else _BoundedPoseSource( + env, options["scene"], seed=seed, revision=report["semantic_id"] + ) ) return pose_source + def feedback(env, source, path): + source.output = path + source.checkpoint() + return source + def recipe_transform(recipe): operations = [] for operation in recipe.operations: @@ -163,6 +112,7 @@ def plans(env, row): return GymExpansionHostExtensions( scene_source_factory=source, + feedback_factory=feedback if "scene_sampling" in options else None, recipe_transform=recipe_transform, plan_transform_factory=plans, require_plan_transform=operator != "object_pose", diff --git a/embodichain/lab/expansion/workloads.py b/embodichain/lab/expansion/workloads.py index b655d02175..bbb27180bc 100644 --- a/embodichain/lab/expansion/workloads.py +++ b/embodichain/lab/expansion/workloads.py @@ -305,7 +305,8 @@ def world_info(self) -> Mapping[str, object]: def _eligible(self, name: str) -> bool: workload, counts = self.workloads[name], self.counts[name] return ( - counts["committed"] < workload.target_episodes + getattr(self.source, "can_admit", lambda name: True)(name) + and counts["committed"] < workload.target_episodes and counts["attempts"] < workload.max_attempts and ( name not in self.started_at @@ -464,7 +465,14 @@ def _activate(self) -> None: ) self.live[row] = { **state, - "cursor": _WorkloadCursor(bridge, self.env.robot, assignment.seed), + "cursor": _WorkloadCursor( + bridge, + self.env.robot, + assignment.seed, + budget=getattr( + self.source, "planning_budget", lambda assignment: None + )(assignment), + ), "start": None, } diff --git a/embodichain/lab/task_program/compiler/lowering.py b/embodichain/lab/task_program/compiler/lowering.py index 3b6ecdbe3c..013eb09d03 100644 --- a/embodichain/lab/task_program/compiler/lowering.py +++ b/embodichain/lab/task_program/compiler/lowering.py @@ -2028,7 +2028,7 @@ def _lower_place( option_template = self._skill_option_template(analyzed, PlaceOptions) if call.at is not None: object_target = self._broadcast_pose( - call.at.to_matrix(), + self._ground_object_target(SemanticObjectTarget(pose=call.at), context), context, name="Place.at", ) diff --git a/embodichain/lab/task_program/integrations/feasibility.py b/embodichain/lab/task_program/integrations/feasibility.py index 8f688a15da..f422c48727 100644 --- a/embodichain/lab/task_program/integrations/feasibility.py +++ b/embodichain/lab/task_program/integrations/feasibility.py @@ -23,6 +23,8 @@ from pathlib import Path from typing import Any +import math +import json import numpy as np from scipy.spatial.transform import Rotation @@ -98,12 +100,28 @@ def __init__( self._previous = [] self._order_ok = True self.receipts = [] + self._transport = {} @staticmethod def _validate(value: Mapping[str, object]) -> dict: kinds = { "pose_near": {"entity", "position", "tolerance_m"}, + "pose_relative": { + "entity", + "other", + "position_tolerance", + "yaw_tolerance_rad", + "yaw_period_rad", + "tilt_tolerance_rad", + }, "lifted": {"entity", "height_m"}, + "transport_guard": { + "entity", + "other", + "lift_height_m", + "minimum_transit_height_m", + "goal_tolerance_m", + }, "joint_range": {"joint", "lower", "upper"}, "speed_below": {"entity", "linear_m_s", "angular_rad_s"}, "axis_relation": {"entity", "other", "axis", "lower", "upper"}, @@ -114,7 +132,13 @@ def _validate(value: Mapping[str, object]) -> dict: "Unsupported measured rigid-task predicate" ) kind = value["kind"] - data = _closed(value, kinds[kind] | {"kind", "negate"}, "predicate") + data = _closed( + value, + kinds[kind] + | {"kind", "negate"} + | ({"position_offset"} if kind == "pose_relative" else set()), + "predicate", + ) if ( not kinds[kind] <= data.keys() or type(data.get("negate", False)) is not bool @@ -153,6 +177,51 @@ def _validate(value: Mapping[str, object]) -> dict: raise ValueError( "Axis relation requires an arena axis and finite ordered bounds" ) + if kind == "pose_relative": + offset = np.asarray(data.get("position_offset", (0, 0, 0)), dtype=float) + if offset.shape != (3,) or not np.isfinite(offset).all(): + raise ValueError("position_offset must have three finite values") + tolerance = np.asarray(data["position_tolerance"], dtype=float) + if ( + tolerance.shape != (3,) + or not np.isfinite(tolerance).all() + or np.any(tolerance <= 0) + ): + raise ValueError( + "position_tolerance must contain three positive finite values" + ) + for key in ("yaw_tolerance_rad", "yaw_period_rad", "tilt_tolerance_rad"): + if ( + type(data[key]) not in (int, float) + or not math.isfinite(data[key]) + or data[key] <= 0 + ): + raise ValueError(f"{key} must be finite and positive") + if data["yaw_tolerance_rad"] > data[ + "yaw_period_rad" + ] / 2 or not math.isclose( + 2 * math.pi / data["yaw_period_rad"], + round(2 * math.pi / data["yaw_period_rad"]), + ): + raise ValueError( + "Yaw symmetry must divide a full turn and tolerance cannot exceed half a period" + ) + if kind == "transport_guard": + for key in ( + "lift_height_m", + "minimum_transit_height_m", + "goal_tolerance_m", + ): + if ( + type(data[key]) not in (int, float) + or not math.isfinite(data[key]) + or data[key] <= 0 + ): + raise ValueError(f"{key} must be finite and positive") + if data["minimum_transit_height_m"] >= data["lift_height_m"]: + raise ValueError( + "Transit threshold must be below the verified lift height" + ) return data def reset(self, env: Any) -> None: @@ -166,6 +235,7 @@ def reset(self, env: Any) -> None: self._previous = [False] * len(self.events) self._order_ok = True self.receipts = [] + self._transport = {} def predicate(self, env: Any, condition: Mapping[str, object]) -> bool: """Read one explicitly declared condition from current physical state. @@ -184,6 +254,12 @@ def predicate(self, env: Any, condition: Mapping[str, object]) -> bool: if key in cached: return bool(cached[key][self.env_id]) kind = condition["kind"] + if kind == "transport_guard": + lifted, dropped = self._transport.get( + json.dumps(dict(condition), sort_keys=True), (False, False) + ) + passed = lifted and not dropped + return bool(not passed if condition.get("negate", False) else passed) if kind == "joint_range": index = env.robot.joint_names.index(condition["joint"]) value = float(env.robot.get_qpos()[self.env_id, index]) @@ -195,7 +271,40 @@ def predicate(self, env: Any, condition: Mapping[str, object]) -> bool: position = ( obj.get_local_pose()[self.env_id, :3].detach().cpu().double().numpy() ) - if kind == "pose_near": + if kind == "pose_relative": + other = env.sim.get_rigid_object(condition["other"]) + if other is None: + raise ValueError("Relative target entity is absent") + actual = ( + obj.get_local_pose(to_matrix=True)[self.env_id] + .detach() + .cpu() + .double() + .numpy() + ) + target = ( + other.get_local_pose(to_matrix=True)[self.env_id] + .detach() + .cpu() + .double() + .numpy() + ) + delta = actual[:3, 3] - ( + target[:3, 3] + + target[:3, :3] + @ np.asarray(condition.get("position_offset", (0, 0, 0))) + ) + relative = target[:3, :3].T @ actual[:3, :3] + yaw = math.atan2(relative[1, 0], relative[0, 0]) + period = condition["yaw_period_rad"] + yaw_error = abs((yaw + period / 2) % period - period / 2) + tilt = math.acos(float(np.clip(relative[2, 2], -1, 1))) + passed = ( + bool(np.all(np.abs(delta) <= condition["position_tolerance"])) + and yaw_error <= condition["yaw_tolerance_rad"] + and tilt <= condition["tilt_tolerance_rad"] + ) + elif kind == "pose_near": passed = ( np.linalg.norm(position - condition["position"]) <= condition["tolerance_m"] @@ -325,6 +434,31 @@ def batch_scope( def observe(self, env: Any, action: Any) -> None: """Record consecutive event predicates at actual control boundaries.""" self._steps += 1 + for condition in self.final_checks: + if condition["kind"] != "transport_guard": + continue + key = json.dumps(dict(condition), sort_keys=True) + lifted, dropped = self._transport.get(key, (False, False)) + pose = env.sim.get_rigid_object(condition["entity"]).get_local_pose()[ + self.env_id, :3 + ] + goal = env.sim.get_rigid_object(condition["other"]).get_local_pose()[ + self.env_id, :3 + ] + if ( + not np.isfinite(pose.detach().cpu().numpy()).all() + or not np.isfinite(goal.detach().cpu().numpy()).all() + ): + self._transport[key] = (lifted, True) + continue + height = float(pose[2]) - self.initial_heights[condition["entity"]] + lifted = lifted or height >= condition["lift_height_m"] + dropped = dropped or ( + lifted + and height < condition["minimum_transit_height_m"] + and float((pose[:2] - goal[:2]).norm()) > condition["goal_tolerance_m"] + ) + self._transport[key] = (lifted, dropped) expected = self._index for i, (name, conditions, required) in enumerate(self.events): current = all(self.predicate(env, c) for c in conditions) diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/README.md new file mode 100644 index 0000000000..dc771fbc51 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/README.md @@ -0,0 +1,109 @@ +# Workspace-conditioned task pose coverage + +This acceptance task picks a 6×4×4 cm cuboid, lifts it, places it at a live +scene-relative XY/yaw target, releases it and waits for stability. Default CPU +physics, the UR5/DH gripper embodiment and the original controller/validation +owners are retained. The nominal robot installation is fixed. The non-colliding +goal reference is below the support plane; its declared +0.12 m offset identifies +the intended object centre. The two opposing physical gripper fingers have +explicit contact permissions, including the mimic finger. + +`expansion.yaml` selects two independent dimensions: + +```yaml +scene_sampling: + strategy: bounded_pose # bounded_pose | workspace_joint +scheduling: + policy: fifo # fifo | task_pose_coverage +``` + +Existing configurations without these settings retain the original sampler and +sequence. `workspace_joint` requires explicit object-to-TCP target matrices and +selects `joint_uniform_fk` or `cache` as its prior source. It does not silently +substitute a cache when none is configured. Both sources retain the same task +execution, measured validation and sink receipt path. Coverage is optional for +FIFO; feedback scheduling requires explicit `pose_coverage` grids. + +The task uses one declared fixed object-to-EEF grasp for all four comparisons. +The joint source verifies that its grasp transform matches the executed policy. +Workspace samples are only proposals: projected XY/yaw is checked again at the +actual height and orientation using IK, joint limits and FK residuals. Runtime +planning and the measured collision/effect gates remain authoritative. A prior +miss retains a labelled uniform fallback; it is not an infeasibility certificate. + +The joint candidate remains immutable for its logical ordinal. Feedback affects +only future candidates. Candidate RNG is independent of physical slot assignment; +adaptive selection also depends on the recorded feedback order, so different +parallel schedules need not produce identical future candidate sequences. + +## Coverage and accounting + +Each fully accepted and persisted episode contributes the measured initial and +terminal cuboid XY/yaw to separate pick/place grids. The cuboid's pi yaw symmetry +is canonicalized. Pick has 144 declared cells; place has 54. These are declared +domain cells, not a certified continuous feasible-volume denominator. Trajectory +variants of the same measured task poses do not increase unique task-pose coverage. + +A measured transport guard latches a failure if, after the first 5 cm lift, +the object falls below 2.5 cm above its initial height while more than 3 cm +from the placement goal in XY. Reaching the target later cannot erase that +failure. This control-boundary guard complements collision/effect validation; +it is not a continuous-contact or substep retention certificate. + +The domain fingerprint includes robot/installation, physical object configuration, +validation settings, ranges and grids. It excludes seed, sampling strategy and +individual candidate poses. SceneCase identities remain distinct. Existing session +receipts own acceptance: pending or failed writes do not increase confirmed counts. +Success-rate feedback records actual executions; preflight failures remain a +separate class. Observed TCP poses are auxiliary measurements, not extra points in +the primary score. + +`pose-sampling.json` retains raw proposals, provenance, selections, outcome order, +resource counts, confirmed coverage and a source checkpoint. Checkpoint loading +reproduces proposal state but is **not** a full live-World or job-resume facility; +ordinary session/sink reconciliation must resolve pending writes before continuing. +The collector still requires a fresh output directory. + +The source scores 75% of eligible choices by Beta(1,1) success estimate times +scarcity, with coarse paired pick/place regions; 25% use the underlying proposal +stream. The workspace prior independently reserves 25% uniform proposals. These +are different probabilities and are recorded separately. Pools are bounded at 64; +all generated candidates and decorated IK/planning calls consume budgets, including +unselected candidates. Physical rollouts have a separate cap from preparations. +Calls are cooperatively bounded, not forcibly preempted inside native code. + +## Acceptance runs + +From the repository root: + +```bash +python -m scripts.benchmark.expansion.task_pose_coverage --suite pilot --output outputs/pose-pilot +python -m scripts.benchmark.expansion.task_pose_coverage --suite smoke --output outputs/pose-smoke +python -m scripts.benchmark.expansion.task_pose_coverage --suite formal --output outputs/pose-formal +python -m scripts.benchmark.expansion.task_pose_coverage --suite installation --output outputs/pose-installations +python -m scripts.benchmark.expansion.task_pose_coverage --suite parallel --output outputs/pose-b4 +python -m scripts.benchmark.expansion.task_pose_coverage --suite trajectory --output outputs/pose-trajectories +python -m scripts.benchmark.expansion.task_pose_coverage --output outputs/pose-formal --rebuild +``` + +A/B/C/D are bounded+FIFO, bounded+feedback, workspace+FIFO, workspace+feedback. +Pilot uses fixed nominal poses and must pass physical acceptance before broad-domain +claims. Smoke uses 12 rollouts per group. Formal runs use both open/obstacle scenes, +five paired seeds, 4096 raw candidates, 128 rollouts, 32768 IK calls, 4096 planning +calls and 1800 seconds per cell. Initialization and prior preparation are included +in end-to-end wall time; pose budgets begin with source construction. Process +startup/import is recorded by the orchestrator log but excluded from worker wall +windows. No warmup is subtracted from these cold measurements. + +The target is a median D/C unique-cell gain >=15%, positive in >=4/5 seeds, +without reduced median pick or place coverage or more than 5 percentage points +acceptance-rate loss. A failed target remains failed; no thresholds are relaxed. +Installation experiments are separate domains; they cannot inflate the nominal +installation score. Obstacle placement and physical criteria are fixed before +formal runs. Run `--media` only for separate illustrative replay; numerical T+1 +evidence and confirmed artifact/LeRobot export are always retained. + +`--suite parallel` verifies real B=4 operation but is not an assertion of identical +adaptive sample order to B=1. Deterministic source checkpoint tests cover feedback +replay and owned candidate reuse. Report wall time, physics outcomes, failures and +coverage separately; never infer physical success from program completion alone. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/env.obstacle.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/env.obstacle.yaml new file mode 100644 index 0000000000..3c71af7a46 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/env.obstacle.yaml @@ -0,0 +1,93 @@ +environment_id: workspace_pose_pick_place-obstacle +physics: default +max_episodes: 1 +max_episode_steps: 1200 +num_envs: 1 +arena_space: 2.5 +simulation: + light: + direct: + - uid: main_light + light_type: sun + color: + - 0.6 + - 0.6 + - 0.6 + intensity: 5.0 + direction: + - 0.0 + - 0.0 + - -1.0 + - uid: rect_light + light_type: rect + color: + - 1.0 + - 1.0 + - 1.0 + intensity: 8.0 + init_pos: + - -0.42 + - 0.08 + - 0.8 + direction: + - 0.0 + - 0.0 + - -1.0 + rect_width: 0.5 + rect_height: 0.5 + background: + - uid: goal + shape: + shape_type: Cube + size: + - 0.01 + - 0.01 + - 0.001 + body_type: static + init_pos: + - -0.4 + - 0.48 + - -0.1 + attrs: + collision_props: + collision_enabled: false + - uid: obstacle + shape: + shape_type: Cube + size: + - 0.08 + - 0.08 + - 0.12 + body_type: static + init_pos: + - -0.75 + - -0.2 + - 0.06 + rigid_object: + - uid: cube + shape: + shape_type: Cube + size: + - 0.06 + - 0.04 + - 0.04 + body_type: dynamic + init_pos: + - -0.42 + - -0.08 + - 0.02 + attrs: + mass_props: + mass: 0.05 + rigid_props: + linear_damping: 0.2 + angular_damping: 0.2 + material_props: + dynamic_friction: 0.97 + static_friction: 0.99 + rigid_object_group: [] + articulation: [] +env: + sim_steps_per_control: 4 + events: {} + dataset: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/env.yaml new file mode 100644 index 0000000000..3639166932 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/env.yaml @@ -0,0 +1,81 @@ +environment_id: workspace_pose_pick_place +physics: default +max_episodes: 1 +max_episode_steps: 1200 +num_envs: 1 +arena_space: 2.5 +simulation: + light: + direct: + - uid: main_light + light_type: sun + color: + - 0.6 + - 0.6 + - 0.6 + intensity: 5.0 + direction: + - 0.0 + - 0.0 + - -1.0 + - uid: rect_light + light_type: rect + color: + - 1.0 + - 1.0 + - 1.0 + intensity: 8.0 + init_pos: + - -0.42 + - 0.08 + - 0.8 + direction: + - 0.0 + - 0.0 + - -1.0 + rect_width: 0.5 + rect_height: 0.5 + background: + - uid: goal + shape: + shape_type: Cube + size: + - 0.01 + - 0.01 + - 0.001 + body_type: static + init_pos: + - -0.4 + - 0.48 + - -0.1 + attrs: + collision_props: + collision_enabled: false + rigid_object: + - uid: cube + shape: + shape_type: Cube + size: + - 0.06 + - 0.04 + - 0.04 + body_type: dynamic + init_pos: + - -0.42 + - -0.08 + - 0.02 + attrs: + mass_props: + mass: 0.05 + rigid_props: + linear_damping: 0.2 + angular_damping: 0.2 + material_props: + dynamic_friction: 0.97 + static_friction: 0.99 + rigid_object_group: [] + articulation: [] +env: + sim_steps_per_control: 4 + events: {} + dataset: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/expansion.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/expansion.yaml new file mode 100644 index 0000000000..74fa2ef3fe --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/expansion.yaml @@ -0,0 +1,201 @@ +schema_version: 1 +engine: + episodes_per_operator: 128 + max_attempts: 4096 + max_steps: 1200 + max_wall_time_s: 1800.0 + settling_steps: 30 + capture_media: false + collision_backend: newton + collision_device: cpu +scene: + cube: + translation_m: + - - -0.48 + - 0.12 + - - -0.22 + - 0.18 + - - 0 + - 0 + yaw_rad: + - -1.5707963267948966 + - 1.5707963267948966 + goal: + translation_m: + - - -0.2 + - 0.1 + - - -0.18 + - 0.12 + - - 0 + - 0 + yaw_rad: + - -1.5707963267948966 + - 1.5707963267948966 +trajectory: + operators: + - object_pose + joint_offset_scale: 0.005 + via_count: 1 + max_proposals: 64 + minimum_joint_delta: 0.002 + velocity_limit: 0.5 + acceleration_limit: 0.5 + nullspace_task_rows: + - 0 + - 1 + - 2 + - 3 + - 4 + - 5 + nullspace_translation_tolerance_m: 0.001 + nullspace_rotation_tolerance_rad: 0.01 + free_phases: + place: + - retract +validation: + initial_checks: + - kind: support_bounds + entity: cube + lower: + - -1 + - -0.4 + - -0.003 + upper: + - -0.2 + - 0.7 + - 0.05 + final_checks: + - &id001 + kind: pose_relative + entity: cube + other: goal + position_tolerance: + - 0.02 + - 0.02 + - 0.01 + yaw_tolerance_rad: 0.17453292519943295 + yaw_period_rad: 3.141592653589793 + tilt_tolerance_rad: 0.17453292519943295 + position_offset: + - 0 + - 0 + - 0.12 + - &id002 + kind: joint_range + joint: gripper_finger1_joint_1 + lower: -0.001 + upper: 0.005 + - &id003 + kind: speed_below + entity: cube + linear_m_s: 0.03 + angular_rad_s: 0.2 + - kind: transport_guard + entity: cube + other: goal + lift_height_m: 0.05 + minimum_transit_height_m: 0.025 + goal_tolerance_m: 0.03 + events: + - event_id: lift + conditions: + - kind: lifted + entity: cube + height_m: 0.05 + - event_id: placed_and_released + conditions: + - *id001 + - *id002 + - *id003 + consecutive_steps: 3 + allowed_contacts: + - - cube + - UR5/gripper_finger1_link_1 + - - cube + - UR5/gripper_finger2_link_1 + - - cube + - default_plane + mounting_contacts: + - - '@root' + - default_plane +scene_sampling: + strategy: bounded_pose + workspace_source: joint_uniform_fk + control_part: arm + sample_count: 4096 + uniform_fraction: 0.25 + max_candidates: 4096 + max_rollouts: 128 + max_ik_calls: 32768 + max_planning_calls: 4096 + targets: + cube: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.0 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + goal: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.12 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 +scheduling: + policy: fifo + pool_size: 64 + exploration_fraction: 0.25 +pose_coverage: + entity: cube + place_reference: goal + grids: + pick: + lower: + - -0.9 + - -0.3 + - -1.5707963267948966 + upper: + - -0.3 + - 0.1 + - 1.5707963267948966 + cell_size: + - 0.1 + - 0.1 + - 0.5235987755982988 + yaw_period: 3.141592653589793 + place: + lower: + - -0.6 + - 0.3 + - -1.5707963267948966 + upper: + - -0.3 + - 0.6 + - 1.5707963267948966 + cell_size: + - 0.1 + - 0.1 + - 0.5235987755982988 + yaw_period: 3.141592653589793 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task.obstacle.ur5.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task.obstacle.ur5.yaml new file mode 100644 index 0000000000..c9ba26d82f --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task.obstacle.ur5.yaml @@ -0,0 +1,19 @@ +id: TaskProgramWorkspacePosePickPlaceObstacle-v1 +environment: + component: env.obstacle.yaml +task_program: + program: task_program/program.yaml + integration: task_program/integration.yaml + execution_policy: ../../../components/execution_policies/trajectory_open_loop.yaml +embodiment: + component: ../../../components/embodiments/ur5_dh_pgi_140_80.yaml + overrides: + init_qpos: + - 0 + - -1.57 + - 1.57 + - -1.57 + - -1.57 + - 0 + - 0 + - 0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task.ur5.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task.ur5.yaml new file mode 100644 index 0000000000..a70cff410f --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task.ur5.yaml @@ -0,0 +1,19 @@ +id: TaskProgramWorkspacePosePickPlace-v1 +environment: + component: env.yaml +task_program: + program: task_program/program.yaml + integration: task_program/integration.yaml + execution_policy: ../../../components/execution_policies/trajectory_open_loop.yaml +embodiment: + component: ../../../components/embodiments/ur5_dh_pgi_140_80.yaml + overrides: + init_qpos: + - 0 + - -1.57 + - 1.57 + - -1.57 + - -1.57 + - 0 + - 0 + - 0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task_program/integration.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task_program/integration.yaml new file mode 100644 index 0000000000..baa11842f9 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task_program/integration.yaml @@ -0,0 +1,47 @@ +integration_id: workspace_pose_pick_place_v1 +program_id: workspace_pose_pick_place +requires: + scene_contract: workspace_pose_pick_place_scene_v1 + embodiment_contract: single_arm_parallel_gripper +scene_binding: + contract_id: workspace_pose_pick_place_scene_v1 + registry_id: task_program_workspace_pose_pick_place + rigid_objects: + - entity_id: cube + dynamics: dynamic + semantic_type: cube + affordances: + - entity_id: cube_grasp + kind: antipodal_grasp + - entity_id: goal + dynamics: static + semantic_type: placement_reference +profile: + defaults: + pick_up: + primary: primary_manipulator + place: + primary: primary_manipulator + skill_options: + pick: + kind: pick_up + fixed_object_to_eef: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - -1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - -1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + place: + kind: place + effect_monitors: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task_program/program.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task_program/program.yaml new file mode 100644 index 0000000000..610056960c --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/task_program/program.yaml @@ -0,0 +1,44 @@ +program_id: workspace_pose_pick_place +instruction: Pick up the cuboid and place it at the scene-relative goal pose. +targets: + drop_pose: + kind: scene_relative + entity: goal + frame: parent + position: + - 0 + - 0 + - 0.12 + quaternion_xyzw: + - 0 + - 0 + - 0 + - 1 +program: + kind: repeat + count: 1 + body: + kind: sequence + items: + - kind: segment + name: pick_cube + instruction: Grasp and lift the cube. + steps: + kind: invoke + call: + kind: pick + object: cube + - kind: segment + name: place_cube + instruction: Place the cube at the next target position and release it. + steps: + kind: invoke + call: + kind: place + object: cube + at: + kind: target_ref + target: drop_pose + post: + - kind: wait_stable + entity: cube diff --git a/scripts/benchmark/expansion/task_pose_coverage.py b/scripts/benchmark/expansion/task_pose_coverage.py new file mode 100644 index 0000000000..154f438866 --- /dev/null +++ b/scripts/benchmark/expansion/task_pose_coverage.py @@ -0,0 +1,540 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Compare bounded/workspace sampling and FIFO/coverage scheduling. + +Run: python -m scripts.benchmark.expansion.task_pose_coverage --output DIR +Use --suite formal only after the smoke scene qualification succeeds. +The worker uses real physics, measured acceptance and native persistence. +""" + +from __future__ import annotations + +import argparse +from copy import deepcopy +import importlib.metadata +import json +import math +import os +from pathlib import Path +import resource +import subprocess +import sys +import time +import statistics +import yaml + +ROOT = Path(__file__).resolve().parents[3] +TASK = ( + ROOT + / "extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place" +) +GROUPS = { + "A": ("bounded_pose", "fifo"), + "B": ("bounded_pose", "task_pose_coverage"), + "C": ("workspace_joint", "fifo"), + "D": ("workspace_joint", "task_pose_coverage"), +} + + +def _write(path: Path, value: object) -> None: + path.write_text(json.dumps(value, indent=2, default=str) + "\n") + + +def _worker(args) -> None: + sys.path[:0] = [ + str(ROOT), + str(ROOT / "extensions/embodichain_tasks"), + str(ROOT / "extensions/embodichain_robots"), + ] + from embodichain.lab.expansion.task_program_jobs import run_task_program_expansion + + args.output.mkdir(parents=True, exist_ok=False) + cfg = yaml.safe_load((TASK / "expansion.yaml").read_text()) + cfg["scene_sampling"]["strategy"], cfg["scheduling"]["policy"] = GROUPS[args.group] + cfg["engine"].update( + episodes_per_operator=args.rollouts, + max_attempts=args.candidates, + batch_size=args.rows, + max_wall_time_s=args.seconds, + ) + cfg["scene_sampling"].update( + max_candidates=args.candidates, + max_rollouts=args.rollouts, + max_ik_calls=args.ik_calls, + max_planning_calls=args.planning_calls, + ) + if args.nominal: + for v in cfg["scene"].values(): + v.update(translation_m=[[0, 0], [0, 0], [0, 0]], yaw_rad=[0, 0]) + cfg["trajectory"]["operators"] = [args.operator] + config = args.output / "configuration.yaml" + config.write_text(yaml.safe_dump(cfg, sort_keys=False)) + deployment = TASK / ( + "task.obstacle.ur5.yaml" if args.scene == "obstacle" else "task.ur5.yaml" + ) + raw = yaml.safe_load(deployment.read_text()) + # Resolve every component before writing an installation-specific deployment. + for key in ("environment", "embodiment"): + raw[key]["component"] = str((TASK / raw[key]["component"]).resolve()) + for key in ("program", "integration", "execution_policy"): + raw["task_program"][key] = str((TASK / raw["task_program"][key]).resolve()) + if args.installation != "nominal": + sign = 1 if args.installation == "positive" else -1 + raw["embodiment"]["overrides"].update( + init_pos=[sign * 0.05, 0, 0], init_rot=[0, 0, sign * 10] + ) + if args.media: + cfg["engine"]["capture_media"] = True + config.write_text(yaml.safe_dump(cfg, sort_keys=False)) + dep = args.output / "deployment.yaml" + dep.write_text(yaml.safe_dump(raw, sort_keys=False)) + import hashlib + + source_paths = subprocess.check_output( + ["git", "ls-files", "--cached", "--others", "--exclude-standard"], + cwd=ROOT, + text=True, + ).splitlines() + selected = [ + ROOT / path + for path in source_paths + if path.startswith( + ( + "embodichain/lab/expansion/", + "embodichain/lab/task_program/", + "extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/", + "scripts/benchmark/expansion/task_pose_coverage.py", + ) + ) + and (ROOT / path).is_file() + ] + _write( + args.output / "source-manifest.json", + { + str(p.relative_to(ROOT)): hashlib.sha256(p.read_bytes()).hexdigest() + for p in selected + }, + ) + start = time.perf_counter() + result = None + error = None + try: + result = run_task_program_expansion( + dep, config, args.output / "collection", seed=args.seed + ) + except Exception as exc: + error = f"{type(exc).__name__}: {exc}" + import traceback + + traceback.print_exc() + samples = list((args.output / "collection").rglob("pose-sampling.json")) + data = json.loads(samples[0].read_text()) if samples else {} + events = data.get("events", []) + outcomes = [e for e in events if e["event"] == "outcome"] + confirmed = data.get("coverage", {}).get("confirmed", {}) + cells = {tuple(cell) for _, values in confirmed.values() for cell in values} + counts = {p: sum(c[0] == p for c in cells) for p in ("pick", "place")} + failure = {} + for record in (args.output / "collection").rglob("attempt.json"): + d = json.loads(record.read_text()) + if d.get("status") != "committed": + reason = d.get("reason", d.get("status", "unknown")) + failure[reason] = failure.get(reason, 0) + 1 + versions = { + name: importlib.metadata.version(name) for name in ("torch", "dexsim-engine") + } + summary = { + "source_digest": hashlib.sha256( + (args.output / "source-manifest.json").read_bytes() + ).hexdigest(), + "suite": args.suite, + "declared_budget": { + "rollouts": args.rollouts, + "candidates": args.candidates, + "ik_calls": args.ik_calls, + "planning_calls": args.planning_calls, + "seconds": args.seconds, + }, + "group": args.group, + "scene": args.scene, + "installation": args.installation, + "seed": args.seed, + "rows": args.rows, + "operator": args.operator, + "nominal": args.nominal, + "media": args.media, + "execution_status": "error" if error else "completed", + "error": error, + "wall_s": time.perf_counter() - start, + "peak_rss_mib": resource.getrusage(resource.RUSAGE_SELF).ru_maxrss / 1024, + "committed": len(confirmed), + "physical_accepted": sum(e["physical_accepted"] for e in outcomes), + "executed": sum(e["executed"] for e in outcomes), + "coverage": counts, + "unique_cells": len(cells), + "budget": data.get("budget", {}), + "failures": failure, + "versions": versions, + "result": result, + "coverage_domain": data.get("domain_id"), + "path": str(args.output), + } + _write(args.output / "metrics.json", summary) + print(json.dumps(summary), flush=True) + if error: + raise SystemExit(1) + + +def _report(output: Path, rows: list[dict]) -> None: + _write(output / "results.json", rows) + lines = [ + "# Task pose coverage acceptance", + "", + "Default CPU physics; cold end-to-end preparation, planning, rollout, validation and export. " + "Grasp classification may use CUDA. RSS is a process high-water mark; GPU memory unmeasured. " + "Only receipt-confirmed, physically validated episodes increase coverage.", + "", + "## Time & Memory", + "", + "|Case|Group|Seed|cost_time_ms|peak RSS MiB|cpu_delta_mb|gpu_delta_mb|peak_gpu_mb|", + "|---|---|---:|---:|---:|---|---|---|", + ] + for r in rows: + lines.append( + f"|{r['scene']}/{r['installation']}/B={r['rows']}|{r['group']}|{r['seed']}|{r['wall_s']*1000:.1f}|{r['peak_rss_mib']:.1f}|N/A|N/A|N/A|" + ) + lines += [ + "", + "## Success & Other Metrics", + "", + "|Case|Group|Executed|Physically accepted|Committed|Pick cells|Place cells|Outcome|", + "|---|---|---:|---:|---:|---:|---:|---|", + ] + for r in rows: + lines.append( + f"|{r['scene']}/{r['installation']}|{r['group']}|{r['executed']}|{r['physical_accepted']}|{r['committed']}|{r['coverage']['pick']}|{r['coverage']['place']}|{r['error'] or 'completed'}|" + ) + lines += [ + "", + "## Leaderboard", + "", + "Ranked by measured acceptance rate; coverage gates are evaluated separately.", + "", + "|Group|Accepted / executed|Acceptance rate|Confirmed unique cells (sum across separate runs)|", + "|---|---|---:|---:|", + ] + aggregates = [] + for group in GROUPS: + selected = [r for r in rows if r["group"] == group] + if selected: + n = sum(r["executed"] for r in selected) + k = sum(r["physical_accepted"] for r in selected) + aggregates.append((group, k, n, sum(r["unique_cells"] for r in selected))) + for g, k, n, u in sorted( + aggregates, key=lambda v: v[1] / max(v[2], 1), reverse=True + ): + lines.append(f"|{g}|{k}/{n}|{k/max(n,1):.3f}|{u}|") + gates = {} + for scene in ("open", "obstacle"): + pairs = [] + for seed in (7, 17, 27, 37, 47): + matched = { + r["group"]: r + for r in rows + if r["scene"] == scene + and r["seed"] == seed + and r["installation"] == "nominal" + and not r["nominal"] + and not r["media"] + and r["rows"] == 1 + and r["operator"] == "object_pose" + and r.get("suite") == "formal" + and r.get("execution_status") == "completed" + and r.get("error") is None + and r.get("declared_budget") + == { + "rollouts": 128, + "candidates": 4096, + "ik_calls": 32768, + "planning_calls": 4096, + "seconds": 1800, + } + } + if ( + set(matched) >= {"C", "D"} + and matched["C"].get("coverage_domain") + == matched["D"].get("coverage_domain") + and matched["C"].get("versions") == matched["D"].get("versions") + and matched["C"].get("source_digest") + == matched["D"].get("source_digest") + and bool(matched["C"].get("source_digest")) + ): + pairs.append((matched["C"], matched["D"])) + if len(pairs) != 5 or any(c["unique_cells"] == 0 for c, d in pairs): + gates[scene] = { + "status": "unqualified", + "reason": "Need five complete paired seeds with nonzero baseline coverage", + } + continue + gains = [d["unique_cells"] / c["unique_cells"] - 1 for c, d in pairs] + phase_ok = all( + statistics.median(d["coverage"][p] for c, d in pairs) + >= statistics.median(c["coverage"][p] for c, d in pairs) + for p in ("pick", "place") + ) + rate = lambda r: r["physical_accepted"] / max(1, r["executed"]) + passed = ( + statistics.median(gains) >= 0.15 + and sum(g > 0 for g in gains) >= 4 + and phase_ok + and statistics.median(rate(d) - rate(c) for c, d in pairs) >= -0.05 + ) + gates[scene] = { + "status": "passed" if passed else "failed", + "median_gain": statistics.median(gains), + "positive_seeds": sum(g > 0 for g in gains), + } + _write(output / "acceptance.json", gates) + lines += [ + "", + "## Acceptance gates", + "", + json.dumps(gates, indent=2), + "", + "Raw proposal/outcome logs and receipt checkpoints remain in each run directory. Single-point nominal pilots do not qualify broad-domain improvement.", + ] + (output / "report.md").write_text("\n".join(lines) + "\n") + # Offline plot, without importing simulation during report rebuild. + try: + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + fig, axes = plt.subplots(1, 3, figsize=(15, 4)) + for r in rows: + logs = list(Path(r["path"]).rglob("pose-sampling.json")) + if not logs: + continue + events = [ + e + for e in json.loads(logs[0].read_text())["events"] + if e["event"] == "outcome" + ] + for ax, key in zip(axes, ("proposals", "rollouts", "elapsed_s")): + ax.step( + [e.get(key, 0) for e in events], + [sum(e["coverage"].values()) for e in events], + where="post", + label=f"{r['group']}/{r['scene']}/{r['seed']}", + ) + ax.set_xlabel(key) + ax.set_ylabel("Confirmed task pose cells") + if axes[-1].lines: + axes[-1].legend(fontsize=6) + fig.tight_layout() + fig.savefig(output / "coverage.svg") + plt.close(fig) + except ImportError: + pass + + +def main() -> None: + """Run isolated comparable trials or rebuild reports from their artifacts.""" + p = argparse.ArgumentParser(description=__doc__) + p.add_argument("--output", type=Path, required=True) + p.add_argument("--worker", action="store_true") + p.add_argument( + "--seeds", + default=None, + help="Comma-separated seed subset; omitted uses suite defaults", + ) + p.add_argument( + "--scenes", default=None, help="Comma-separated open/obstacle subset" + ) + p.add_argument( + "--suite", + choices=("pilot", "smoke", "formal", "installation", "parallel", "trajectory"), + default="smoke", + ) + p.add_argument("--rebuild", action="store_true") + p.add_argument("--group", choices=tuple(GROUPS), default="A") + p.add_argument("--scene", choices=("open", "obstacle"), default="open") + p.add_argument( + "--installation", choices=("nominal", "positive", "negative"), default="nominal" + ) + p.add_argument("--seed", type=int, default=7) + p.add_argument("--rows", type=int, default=1) + p.add_argument("--rollouts", type=int, default=12) + p.add_argument("--candidates", type=int, default=384) + p.add_argument("--ik-calls", type=int, default=32768) + p.add_argument("--planning-calls", type=int, default=4096) + p.add_argument("--seconds", type=float, default=300) + p.add_argument("--nominal", action="store_true") + p.add_argument("--media", action="store_true") + p.add_argument( + "--operator", choices=("object_pose", "joint_residual"), default="object_pose" + ) + args = p.parse_args() + args.output = args.output.resolve() + if args.worker: + _worker(args) + return + if args.rebuild: + _report( + args.output, + [ + json.loads(f.read_text()) + for f in sorted(args.output.glob("*/metrics.json")) + ], + ) + return + args.output.mkdir(parents=True, exist_ok=False) + seeds = ( + [7, 17, 27, 37, 47] + if args.suite == "formal" + else [7, 17, 27] if args.suite == "installation" else [7] + ) + scenes = ["open", "obstacle"] if args.suite in ("formal", "pilot") else [args.scene] + if args.seeds is not None: + seeds = [int(value) for value in args.seeds.split(",")] + if args.scenes is not None: + scenes = args.scenes.split(",") + if not set(scenes) <= {"open", "obstacle"}: + p.error("Unknown scene") + installations = ( + ["positive", "negative"] if args.suite == "installation" else ["nominal"] + ) + groups = ["C", "D"] if args.suite == "installation" else list(GROUPS) + rows = [] + _write( + args.output / "provenance.json", + { + "head": subprocess.check_output( + ["git", "rev-parse", "HEAD"], cwd=ROOT, text=True + ).strip(), + "diff": subprocess.check_output(["git", "diff"], cwd=ROOT, text=True), + "scope": vars(args), + "cpu_affinity": sorted(os.sched_getaffinity(0)), + }, + ) + for scene in scenes: + for installation in installations: + for i, seed in enumerate(seeds): + for group in ( + groups + if ( + (7, 17, 27, 37, 47).index(seed) + if seed in (7, 17, 27, 37, 47) + else i + ) + % 2 + == 0 + else groups[::-1] + ): + destination = args.output / f"{scene}-{installation}-{group}-{seed}" + n = ( + 128 + if args.suite == "formal" + else ( + 64 + if args.suite == "installation" + else 1 if args.suite == "pilot" else args.rollouts + ) + ) + cap = ( + 4096 + if args.suite == "formal" + else 2048 if args.suite == "installation" else args.candidates + ) + seconds = ( + 1800 + if args.suite == "formal" + else 900 if args.suite == "installation" else args.seconds + ) + command = [ + sys.executable, + str(Path(__file__).resolve()), + "--worker", + "--suite", + args.suite, + "--output", + str(destination), + "--group", + group, + "--scene", + scene, + "--seed", + str(seed), + "--installation", + installation, + "--rows", + str(4 if args.suite == "parallel" else args.rows), + "--rollouts", + str(n), + "--candidates", + str(cap), + "--seconds", + str(seconds), + "--operator", + ( + "joint_residual" + if args.suite == "trajectory" + else args.operator + ), + ] + if args.suite == "installation": + command += ["--ik-calls", "16384", "--planning-calls", "2048"] + if args.suite == "pilot" or args.nominal: + command += ["--nominal"] + if args.media: + command += ["--media"] + with (args.output / (destination.name + ".log")).open("w") as log: + try: + result = subprocess.run( + command, + cwd=ROOT, + env={ + **os.environ, + "OMP_NUM_THREADS": "1", + "MKL_NUM_THREADS": "1", + "SDL_VIDEODRIVER": "offscreen", + }, + stdout=log, + stderr=subprocess.STDOUT, + timeout=seconds + 180, + ) + except subprocess.TimeoutExpired: + raise RuntimeError( + f"Worker timeout; preserve log {log.name}" + ) + if (destination / "metrics.json").exists(): + rows.append( + json.loads((destination / "metrics.json").read_text()) + ) + _report(args.output, rows) + if result.returncode: + raise RuntimeError(f"Worker failed; inspect {destination}") + if args.suite == "pilot" and not rows[-1]["committed"]: + raise RuntimeError( + "Nominal scene qualification failed; do not relax acceptance or narrow the declared domain" + ) + print(args.output / "report.md") + + +if __name__ == "__main__": + main() diff --git a/tests/benchmark/expansion/test_task_pose_report.py b/tests/benchmark/expansion/test_task_pose_report.py new file mode 100644 index 0000000000..46ef649d19 --- /dev/null +++ b/tests/benchmark/expansion/test_task_pose_report.py @@ -0,0 +1,78 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Formal evidence gates cannot be satisfied by smoke or incompatible results.""" + +from __future__ import annotations +import json +import pytest +from scripts.benchmark.expansion.task_pose_coverage import _report + + +@pytest.mark.parametrize( + "change", + [ + {"suite": "smoke"}, + {"execution_status": "error", "error": "crash"}, + {"declared_budget": {"rollouts": 12}}, + {"coverage_domain": "other"}, + {"source_digest": "other"}, + ], +) +def test_formal_gate_rejects_incomplete_or_incompatible_records(tmp_path, change): + rows = [] + for seed in (7, 17, 27, 37, 47): + for group in ("C", "D"): + count = 10 if group == "C" else 20 + r = { + "suite": "formal", + "declared_budget": { + "rollouts": 128, + "candidates": 4096, + "ik_calls": 32768, + "planning_calls": 4096, + "seconds": 1800, + }, + "scene": "open", + "installation": "nominal", + "seed": seed, + "group": group, + "rows": 1, + "operator": "object_pose", + "nominal": False, + "media": False, + "execution_status": "completed", + "error": None, + "coverage_domain": "domain", + "versions": {"sdk": "v1"}, + "source_digest": "same", + "coverage": {"pick": count, "place": count}, + "unique_cells": 2 * count, + "executed": 128, + "physical_accepted": 128, + "committed": 128, + "wall_s": 100, + "peak_rss_mib": 100, + "path": str(tmp_path / "missing"), + } + if group == "D": + r.update(change) + rows.append(r) + _report(tmp_path, rows) + assert ( + json.loads((tmp_path / "acceptance.json").read_text())["open"]["status"] + == "unqualified" + ) diff --git a/tests/lab/expansion/test_distribution.py b/tests/lab/expansion/test_distribution.py new file mode 100644 index 0000000000..3e6d07a7a3 --- /dev/null +++ b/tests/lab/expansion/test_distribution.py @@ -0,0 +1,165 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations +import numpy as np +import pytest +from embodichain.lab.expansion._distribution import _motion, _summarize + + +def test_motion_density_and_time_are_separate(): + a = _motion(np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]]), np.array([0.0, 2.0])) + b = _motion( + np.array([[0.0, 0.0, 0.0], [0.5, 0.0, 0.0], [1.0, 0.0, 0.0]]), + np.array([0.0, 2.0, 4.0]), + ) + assert a["path_length"] == b["path_length"] == 1 + assert a["duration_s"] == 2 and b["duration_s"] == 4 + np.testing.assert_allclose(a["path"], b["path"]) + with pytest.raises(ValueError): + _motion(np.zeros((2, 3)), np.array([1.0, 1.0])) + + +def test_joint_distribution_and_idempotency(): + rows = [ + dict( + commit_id=str(i), + initial_pose=[x, 0, 0], + final_pose=[x, 0, 0], + object_motion={}, + geometry_bucket=None, + ) + for i, x in enumerate([-0.15, 0.15]) + ] + r = _summarize(rows + rows, 0.1, 1) + assert r["count"] == 2 + assert r["joint"] == {"-2,0|-2,0": 1, "1,0|1,0": 1} + assert r["joint_at_minimum"] == 2 + with pytest.raises(ValueError): + _summarize([rows[0], dict(rows[0], final_pose=[9, 0, 0])], 0.1, 1) + + +def test_missing_layout_is_not_zero(): + r = _summarize( + [dict(commit_id="a", object_motion=None, geometry_bucket=None)], 0.1, 5 + ) + assert r["layout_missing"] == 1 and r["layout_valid"] == 0 + assert r["joint"] == {} + + +def test_orientation_sign_and_out_of_plane_rotation(): + from embodichain.lab.expansion._distribution_pose import orientation + + poses = np.array([[0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 1, 0, 0, 0]], dtype=float) + a = orientation(poses, np.array([0.0, 2.0])) + poses[:, 3:] *= -1 + b = orientation(poses, np.array([0.0, 2.0])) + assert a == b + assert a["angular_path_rad"] == pytest.approx(np.pi) + assert a["angular_speed_p50_p95"] == pytest.approx([np.pi / 2] * 2) + assert a["initial_rotvec_rad"] == [0.0, 0.0, 0.0] + assert abs(a["final_rotvec_rad"][0]) == pytest.approx(np.pi) + poses[:, 3:] = 0 + with pytest.raises(ValueError): + orientation(poses, np.array([0.0, 2.0])) + + +def test_tcp_root_transform_and_calibration(tmp_path): + import torch + from types import SimpleNamespace + from embodichain.lab.expansion._distribution_pose import RecordedTCP + + asset = tmp_path / "test.urdf" + asset.write_text( + """""" + ) + offset = np.eye(4) + offset[2, 3] = 0.15 + tcp = RecordedTCP( + { + "robot": { + "fpath": str(asset), + "gripper_bindings": { + "eef": { + "root_link": "tool", + "defaults": {"default_root_from_tcp": offset.tolist()}, + } + }, + } + } + ) + e = SimpleNamespace( + metadata={ + "initial_state": [{"kind": "robot", "uid": "r", "joint_names": ["slide"]}], + "scene_state_fields": [ + {"entity_uid": "r", "field": "pose", "key": "root"}, + {"entity_uid": "r", "field": "qpos", "key": "q"}, + ], + }, + observations={ + "root": torch.tensor([[1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]] * 2), + "q": torch.tensor([[0.0], [1.0]]), + }, + ) + p = tcp.poses(e) + np.testing.assert_allclose(p[:, :3], [[1, 2, 3.15], [2, 2, 3.15]]) + np.testing.assert_allclose(p[:, 3:], [[0, 0, 0, 1]] * 2) + + +def test_detail_pages_match_agent_records_and_summary(tmp_path, monkeypatch): + import json + from embodichain.lab.expansion import _distribution as module + + rows = [] + for i in range(101): + rows.append( + dict( + episode_id=f"e{i}", + commit_id=f"c{i}", + initial_pose=[0, 0, 0, 0, 0, 0, 1], + final_pose=[1, 0, 0, 0, 0, 0, 1], + object_motion={ + "path_length": 1.0, + "duration_s": 2.0, + "path": [[0, 0, 0], [1, 0, 0]], + }, + joint_motion=None, + tcp_motion=None, + orientation={ + "rotation_bin_initial": "6,6,6", + "rotation_bin_final": "6,6,6", + }, + geometry_bucket=None, + ) + ) + monkeypatch.setattr(module, "_read", lambda paths, entity: (rows, ["domain"])) + output = tmp_path / "result" + module._run([], [], output, "cube", 0.1, 5) + summary = json.loads((output / "summary.json").read_text()) + detail = [ + json.loads(line) + for line in (output / "after-episodes.jsonl").read_text().splitlines() + ] + pages = [ + json.loads((output / "pages" / f"after-{n}.json").read_text()) for n in range(2) + ] + assert [len(p) for p in pages] == [100, 1] + assert pages[0] + pages[1] == detail + assert summary["after"]["orientation"]["initial"] == {"6,6,6": 101} + assert summary["after"]["joint"] == {"0,0|10,0": 101} + assert summary["after"]["episodes"][100]["page"] == "after-1" + assert summary["after"]["episodes"][0]["object_motion"] is True + assert rows[0].get("pick_cell") is None diff --git a/tests/lab/expansion/test_task_pose_coverage.py b/tests/lab/expansion/test_task_pose_coverage.py new file mode 100644 index 0000000000..2ffb4b10e1 --- /dev/null +++ b/tests/lab/expansion/test_task_pose_coverage.py @@ -0,0 +1,595 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Measured-cell accounting, sampling compatibility and feedback regressions.""" + +from __future__ import annotations +from copy import deepcopy +from types import SimpleNamespace +import json +import math +import numpy as np +import pytest +import torch +from embodichain.lab.expansion.task_pose_coverage import TaskPoseCoverage +from embodichain.lab.expansion.contracts import SceneCase +from embodichain.lab.expansion._pose_sampling import ( + _BoundedPoseSource, + _AdaptivePoseSource, +) +from embodichain.lab.expansion._pose_sampling_cfg import validate_sampling +from embodichain.lab.sim.scene.feasibility import FeasibilityBudgetExhausted + + +def grids(): + return { + phase: { + "lower": [-1, -1, -math.pi / 2], + "upper": [1, 1, math.pi / 2], + "cell_size": [0.5, 0.5, math.pi / 2], + "yaw_period": math.pi, + } + for phase in ("pick", "place") + } + + +def coverage(): + cov = TaskPoseCoverage("domain", "object", grids()) + for name in ("scene-a", "scene-b"): + cov.register_case( + SceneCase(name, "initial", "signature", "task", "robot"), + torch.tensor([[-1.0, 1.0]]), + ("joint",), + None, + ) + return cov + + +def episode(commit="c1", case="scene-a", x=0.1, yaw=0.0, accepted=True): + state = torch.tensor( + [ + [x, 0.1, 0.02, 0, 0, math.sin(yaw / 2), math.cos(yaw / 2)], + [x + 0.1, 0.1, 0.02, 0, 0, math.sin(yaw / 2), math.cos(yaw / 2)], + ] + ) + return SimpleNamespace( + commit_id=commit, + identity=SimpleNamespace(scene_case_id=case), + validation=SimpleNamespace(accepted=accepted), + timestamps=torch.tensor([0.0, 0.1]), + observations={"rigid/object/state": state}, + ) + + +def test_receipts_aggregate_cases_without_double_credit_and_own_cells(): + cov = coverage() + e = episode() + assert cov.reserve(e) + assert cov.snapshot()[0] == {"domain": 0} + e.observations["rigid/object/state"].fill_(99) + cov.confirm("scene-a", "c1") + cov.confirm("scene-a", "c1") + assert cov.phase_counts() == {"pick": 1, "place": 1} + assert cov.snapshot()[1] == {} + assert cov.reserve(episode("c2", "scene-b", yaw=math.pi)) + cov.confirm("scene-b", "c2") + assert cov.snapshot()[0] == { + "domain": 2 + } # Symmetry and scene changes aren't novelty. + assert not cov.reserve(episode("bad", accepted=False)) + cov.reserve(episode("c3", x=-0.9)) + cov.release("scene-a", "c3") + with pytest.raises(ValueError): + cov.confirm("scene-a", "c3") + assert cov.snapshot()[0] == {"domain": 2} + + +def test_checkpoint_keeps_pending_unconfirmed_and_rejects_foreign_domain(): + cov = coverage() + cov.reserve(episode()) + cov.confirm("scene-a", "c1") + cov.reserve(episode("pending", x=-0.9)) + state = json.loads(json.dumps(cov.state_dict())) + restored = TaskPoseCoverage("domain", "object", grids()) + restored.load_state_dict(state) + assert restored.snapshot() == cov.snapshot() + restored.confirm("scene-a", "pending") + assert restored.snapshot()[0] == {"domain": 4} + assert cov.snapshot()[0] == {"domain": 2} + with pytest.raises(ValueError): + TaskPoseCoverage("another", "object", grids()).load_state_dict(state) + with pytest.raises(ValueError): + restored.confirm("scene-b", "pending") + + +def test_cells_boundaries_periodicity_and_measured_not_requested_pose(): + cov = coverage() + assert cov.cells([-1, -1, 0, 0, 0, 0, 1], [1, 1, 0, 0, 0, 0, 1]) == ( + ("pick", 0, 0, 1), + ("place", 3, 3, 1), + ) + assert cov.cells([-1.1, 0, 0, 0, 0, 0, 1], [1.1, 0, 0, 0, 0, 0, 1]) == () + with pytest.raises(ValueError): + cov.cells([0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1]) + e = episode(x=-0.9) + e.metadata = {"requested_pose": [0.9, 0.9, 0]} + cov.reserve(e) + cov.confirm("scene-a", "c1") + assert cov.gain((("pick", 0, 2, 1),)) == 0.5 + + +class _Robot: + control_parts = {"arm": [0, 1, 2]} + device = torch.device("cpu") + uid = "robot" + cfg = SimpleNamespace(to_dict=lambda: {"uid": "robot"}) + + def get_local_pose(self, to_matrix=False): + return torch.eye(4)[None] + + def get_qpos(self, **kwargs): + return torch.tensor([[0.0, 0.0, 0.02]]) + + def get_qpos_limits(self, **kwargs): + return torch.tensor([[[-1.0, 1.0], [-1.0, 1.0], [0.02, 0.02]]]) + + def compute_batch_fk(self, qpos, **kwargs): + pose = torch.eye(4).expand(*qpos.shape[:2], 4, 4).clone() + pose[..., :3, 3] = qpos + return pose + + def compute_ik(self, pose, **kwargs): + return torch.ones(len(pose), dtype=torch.bool), pose[:, :3, 3].clone() + + def compute_fk(self, qpos, **kwargs): + pose = torch.eye(4).repeat(len(qpos), 1, 1) + pose[:, :3, 3] = qpos + return pose + + +def environment(): + pose = torch.eye(4)[None] + pose[:, 2, 3] = 0.02 + obj = SimpleNamespace(get_local_pose=lambda to_matrix=True: pose.clone()) + return SimpleNamespace( + robot=_Robot(), + sim=SimpleNamespace( + get_rigid_object=lambda uid: obj, + sim_config=SimpleNamespace( + physics_cfg=SimpleNamespace(to_dict=lambda: {"physics": "default"}) + ), + ), + ) + + +def options(strategy="bounded_pose", policy="fifo"): + value = { + "engine": {"max_attempts": 8, "max_wall_time_s": 120}, + "validation": {}, + "scene": { + uid: { + "translation_m": [[-0.9, 0.9], [-0.9, 0.9], [0, 0]], + "yaw_rad": [0, 0], + } + for uid in ("object", "goal") + }, + "scene_sampling": { + "strategy": strategy, + "sample_count": 64, + "max_candidates": 64, + "targets": ( + {uid: np.eye(4).tolist() for uid in ("object", "goal")} + if strategy == "workspace_joint" + else {} + ), + }, + "scheduling": {"policy": policy, "pool_size": 8}, + "pose_coverage": { + "entity": "object", + "place_reference": "goal", + "grids": grids(), + }, + } + validate_sampling(value) + return value + + +def test_legacy_config_and_simple_fifo_keep_identical_poses(): + old = {"engine": {}, "scene": {}} + expected = deepcopy(old) + validate_sampling(old) + assert old == expected + env = environment() + opts = options() + simple = _BoundedPoseSource(env, opts["scene"], seed=7, revision="revision") + new = _AdaptivePoseSource(env, opts, seed=7, revision="revision") + for ordinal in range(4): + assert ( + new.candidate(ordinal).scene.pose_changes + == simple.candidate(ordinal).scene.pose_changes + ) + + +@pytest.mark.parametrize("strategy", ["bounded_pose", "workspace_joint"]) +@pytest.mark.parametrize("policy", ["fifo", "task_pose_coverage"]) +def test_candidate_is_frozen_after_feedback_and_source_is_seeded(strategy, policy): + opts = options(strategy, policy) + a = _AdaptivePoseSource(environment(), opts, seed=7, revision="revision") + b = _AdaptivePoseSource(environment(), opts, seed=7, revision="revision") + first = a.candidate(0) + assert first == b.candidate(0) + assert a.validate(0).accepted + a.finish(0, None, None, executed=True) + assert a.candidate(0) is first + assert a.budget.counts["candidates"] <= 64 + assert len(a._predicted) == 0 + + +def test_bounded_budget_never_bypasses_candidate_quota(): + a = _AdaptivePoseSource(environment(), options(), seed=7, revision="r") + a.budget.charge("candidates", 64) + with pytest.raises(FeasibilityBudgetExhausted): + a.budget.charge("candidates") + assert a.budget.counts["candidates"] == 64 + + +def test_invalid_joint_config_does_not_fall_back(): + opt = options() + opt["scene_sampling"]["strategy"] = "workspace_joint" + with pytest.raises(ValueError, match="target transforms"): + validate_sampling(opt) + + +def test_sampler_checkpoint_replays_choices_after_feedback(): + opts = options("workspace_joint", "task_pose_coverage") + a = _AdaptivePoseSource(environment(), opts, seed=7, revision="r") + first = a.candidate(0) + a.finish(0, None, None, executed=True) + state = json.loads(json.dumps(a.state_dict())) + b = _AdaptivePoseSource(environment(), opts, seed=7, revision="r") + b.load_state_dict(state) + assert b.candidate(0) == first + assert a.candidate(1) == b.candidate(1) + assert a.budget.counts == b.budget.counts + + +def test_relative_pose_validation_preserves_yaw_symmetry_and_tilt(): + from embodichain.lab.task_program.integrations.feasibility import ( + MeasuredTaskMonitor, + ) + + actual = torch.eye(4).repeat(2, 1, 1) + target = actual.clone() + target[:, 2, 3] = -0.1 + actual[:, 2, 3] = 0.02 + actual[0, 0, 0] = actual[0, 1, 1] = -1 + actual[1, 0, 0] = actual[1, 1, 1] = 0 + actual[1, 0, 1] = -1 + actual[1, 1, 0] = 1 + + class Entity: + def __init__(self, poses): + self.poses = poses + + def get_local_pose(self, to_matrix=False): + if to_matrix: + return self.poses.clone() + return self.poses[:, :3, 3].clone() + + objects = {"object": Entity(actual), "target": Entity(target)} + env = SimpleNamespace(sim=SimpleNamespace(get_rigid_object=objects.get)) + condition = { + "kind": "pose_relative", + "entity": "object", + "other": "target", + "position_offset": [0, 0, 0.12], + "position_tolerance": [0.02, 0.02, 0.01], + "yaw_period_rad": math.pi, + "yaw_tolerance_rad": 0.18, + "tilt_tolerance_rad": 0.18, + } + monitor = MeasuredTaskMonitor([condition], [], env_id=0) + assert monitor.predicate(env, condition) + monitor.env_id = 1 + assert not monitor.predicate(env, condition) + actual[1] = actual[0] + actual[1, 2, 3] += 0.02 + assert not monitor.predicate(env, condition) + with pytest.raises(ValueError): + MeasuredTaskMonitor([{**condition, "yaw_period_rad": 0}], []) + + +def test_workspace_fifo_does_not_require_coverage_policy(): + opts = options("workspace_joint", "fifo") + del opts["pose_coverage"] + validate_sampling(opts) + source = _AdaptivePoseSource(environment(), opts, seed=7, revision="r") + assert source.coverage is None + source.candidate(0) + assert source.validate(0).accepted + source.finish(0, None, None, executed=False) + opts["scheduling"]["policy"] = "task_pose_coverage" + with pytest.raises(ValueError, match="grids"): + validate_sampling(opts) + + +def test_checkpoint_rejects_out_of_grid_cell_without_partial_restore(): + cov = coverage() + cov.reserve(episode()) + state = cov.state_dict() + state["pending"]["c1"] = ("scene-a", (("pick", 99, 0, 0),)) + restored = TaskPoseCoverage("domain", "object", grids()) + with pytest.raises(ValueError): + restored.load_state_dict(state) + assert not restored._cases and not restored._pending + + +def test_sampling_proposal_budget_counts_discarded_pool_candidates(): + opts = options("bounded_pose", "task_pose_coverage") + source = _AdaptivePoseSource(environment(), opts, seed=7, revision="r") + source.candidate(0) + assert source.budget.counts["candidates"] == 8 + assert len(source._candidates) == 1 + # Re-reading the chosen immutable candidate consumes no extra budget. + source.candidate(0) + assert source.budget.counts["candidates"] == 8 + + +def test_coverage_gain_uses_confirmed_pending_and_predicted_separately(): + cov = coverage() + e = episode() + cells = cov.cells( + e.observations["rigid/object/state"][0], + e.observations["rigid/object/state"][-1], + ) + assert cov.gain(cells) == 2 + cov.reserve(e) + assert cov.gain(cells) == 1 + cov.release("scene-a", "c1") + assert cov.gain(cells) == 2 + assert cov.gain(cells, {c: 1 for c in cells}) == 1 + + +def test_workspace_branch_must_match_execution_policy(): + source = _AdaptivePoseSource( + environment(), options("workspace_joint"), seed=7, revision="r" + ) + source.env.cfg = SimpleNamespace( + task_program=SimpleNamespace( + integration=SimpleNamespace(runtime_preset="trajectory") + ) + ) + pick = SimpleNamespace(fixed_object_to_eef=torch.eye(4)) + profile = SimpleNamespace( + presets={ + "trajectory": SimpleNamespace(skill_option_templates={"pick_up": pick}) + } + ) + adapter = SimpleNamespace( + _factory=SimpleNamespace(create_robot_skill_profile=lambda: profile) + ) + from embodichain.lab.task_program.semantics.calls import Pick + from embodichain.lab.task_program.semantics.scene import SceneObjectRef + + program = SimpleNamespace( + iter_segments=lambda: [ + SimpleNamespace( + calls=[SimpleNamespace(call=Pick(object=SceneObjectRef("object")))] + ) + ] + ) + source.bind_program(adapter, program) + pick.fixed_object_to_eef[0, 3] = 0.1 + with pytest.raises(ValueError, match="match"): + source.bind_program(adapter, program) + pick.fixed_object_to_eef = None + with pytest.raises(ValueError, match="fixed_object"): + source.bind_program(adapter, program) + + +def test_budget_expiry_before_first_command_has_no_fabricated_hold(): + from embodichain.lab.expansion._workload_cursor import _WorkloadCursor + + class Bridge: + def __iter__(self): + raise AssertionError("Expired planning must not enter the bridge") + yield + + class Budget: + def check(self): + raise FeasibilityBudgetExhausted("ik_budget_exhausted") + + cursor = _WorkloadCursor( + Bridge(), SimpleNamespace(device=torch.device("cpu")), 7, budget=Budget() + ) + assert cursor.next() is None + assert cursor.finished and not cursor.accepted and cursor.length == 0 + assert cursor.reason == "ik_budget_exhausted" and cursor.last_command is None + + +def test_coverage_rejects_nonphysical_yaw_period_and_identifies_branch_cut(): + from embodichain.lab.expansion._pose_cells import _grid + + with pytest.raises(ValueError, match="complete turn"): + _grid( + { + "lower": [-1, -1, -0.5], + "upper": [1, 1, 0.5], + "cell_size": [0.5, 0.5, 0.1], + "yaw_period": 1, + } + ) + cov = coverage() + a = [0, 0, 0, 0, 0, 1, 0] + b = [0, 0, 0, 0, 0, -1, 0] + assert cov.cells(a, a) == cov.cells(b, b) + + +def test_cache_configuration_is_used_and_empty_qualified_cache_fails(): + from unittest.mock import Mock + + env = environment() + opts = options("workspace_joint") + opts["scene_sampling"]["workspace_source"] = "cache" + cache = SimpleNamespace( + sample_indices=Mock(side_effect=ValueError("No samples meet min_score")) + ) + env.robot.cfg = SimpleNamespace( + to_dict=lambda: {"uid": "robot"}, + workspace_cfg={"arm": SimpleNamespace(strategy="point_uniform", min_score=0.8)}, + ) + env.robot.get_workspace = lambda part: cache + with pytest.raises(ValueError, match="min_score"): + _AdaptivePoseSource(env, opts, seed=7, revision="r") + assert cache.sample_indices.call_args.kwargs["strategy"] == "point_uniform" + assert cache.sample_indices.call_args.kwargs["min_score"] == 0.8 + + +def test_fifo_without_coverage_still_checks_grasp_correspondence(): + from embodichain.lab.task_program.semantics.calls import Pick + from embodichain.lab.task_program.semantics.scene import SceneObjectRef + + opts = options("workspace_joint") + del opts["pose_coverage"] + source = _AdaptivePoseSource(environment(), opts, seed=7, revision="r") + source.env.cfg = SimpleNamespace( + task_program=SimpleNamespace( + integration=SimpleNamespace(runtime_preset="trajectory") + ) + ) + fixed = torch.eye(4) + fixed[2, 3] = 0.2 + profile = SimpleNamespace( + presets={ + "trajectory": SimpleNamespace( + skill_option_templates={ + "pick_up": SimpleNamespace(fixed_object_to_eef=fixed) + } + ) + } + ) + adapter = SimpleNamespace( + _factory=SimpleNamespace(create_robot_skill_profile=lambda: profile) + ) + program = SimpleNamespace( + iter_segments=lambda: [ + SimpleNamespace( + calls=[SimpleNamespace(call=Pick(object=SceneObjectRef("object")))] + ) + ] + ) + with pytest.raises(ValueError, match="match"): + source.bind_program(adapter, program) + + +def test_transport_guard_remembers_a_drop_and_resets_per_episode(): + from embodichain.lab.task_program.integrations.feasibility import ( + MeasuredTaskMonitor, + ) + + state = torch.tensor([[0.0, 0.0, 0.02]]) + goal = torch.tensor([[0.5, 0.0, 0.02]]) + objects = { + "object": SimpleNamespace(get_local_pose=lambda: state.clone()), + "goal": SimpleNamespace(get_local_pose=lambda: goal.clone()), + } + env = SimpleNamespace( + sim=SimpleNamespace( + get_rigid_object=objects.get, + get_rigid_object_uid_list=lambda: tuple(objects), + ) + ) + condition = { + "kind": "transport_guard", + "entity": "object", + "other": "goal", + "lift_height_m": 0.05, + "minimum_transit_height_m": 0.025, + "goal_tolerance_m": 0.03, + } + monitor = MeasuredTaskMonitor([condition], []) + monitor.reset(env) + assert not monitor.validation(env).accepted + state[0, 2] = 0.08 + monitor.observe(env, None) + assert monitor.validation(env).accepted + state[0, 2] = 0.02 + monitor.observe(env, None) + state[0] = goal[0] + monitor.observe(env, None) + assert not monitor.validation(env).accepted + state[0] = torch.tensor([0.0, 0.0, 0.02]) + monitor.reset(env) + state[0, 2] = 0.08 + monitor.observe(env, None) + state[0] = goal[0] + monitor.observe(env, None) + assert monitor.validation(env).accepted + + +def test_new_feedback_hook_preserves_legacy_positional_extension_constructor(): + from embodichain.lab.expansion.host_extensions import GymExpansionHostExtensions + + factory = lambda env, row: None + extensions = GymExpansionHostExtensions(None, None, None, None, factory, True) + assert ( + extensions.require_plan_transform + and extensions.plan_transform_factory is factory + ) + assert extensions.feedback_factory is None + + +def test_bounded_source_owns_input_ranges(): + cfg = options()["scene"] + source = _BoundedPoseSource(environment(), cfg, seed=7, revision="r") + expected = source.candidate(3) + cfg["object"]["translation_m"][0][:] = [0.5, 0.5] + assert source.candidate(3) == expected + + +def test_transport_guard_rejects_nonfinite_measured_state(): + from embodichain.lab.task_program.integrations.feasibility import ( + MeasuredTaskMonitor, + ) + + state = torch.tensor([[0.0, 0.0, 0.02]]) + goal = torch.tensor([[0.5, 0.0, 0.02]]) + objects = { + "object": SimpleNamespace(get_local_pose=lambda: state.clone()), + "goal": SimpleNamespace(get_local_pose=lambda: goal.clone()), + } + env = SimpleNamespace( + sim=SimpleNamespace( + get_rigid_object=objects.get, + get_rigid_object_uid_list=lambda: tuple(objects), + ) + ) + condition = { + "kind": "transport_guard", + "entity": "object", + "other": "goal", + "lift_height_m": 0.05, + "minimum_transit_height_m": 0.025, + "goal_tolerance_m": 0.03, + } + monitor = MeasuredTaskMonitor([condition], []) + monitor.reset(env) + state[0, 2] = 0.08 + monitor.observe(env, None) + assert monitor.validation(env).accepted + state[0, 2] = float("nan") + monitor.observe(env, None) + assert not monitor.validation(env).accepted diff --git a/tests/lab/task_program/test_semantic_compiler.py b/tests/lab/task_program/test_semantic_compiler.py index 001e32598e..2d9b436506 100644 --- a/tests/lab/task_program/test_semantic_compiler.py +++ b/tests/lab/task_program/test_semantic_compiler.py @@ -2373,3 +2373,30 @@ def test_occurrence_policy_rejects_unknown_preset_and_assurance_changes( (Pick(object=SceneObjectRef("cube")),), preset_ids=(preset_id,) ) assert all(provider.calls == 0 for provider in providers) + + +def test_place_grounds_scene_relative_pose_from_current_observation() -> None: + from embodichain.lab.task_program.semantics.calls import SemanticRelativePose + + registry, (_, table) = _scene_registry() + compiler, _ = _compiler(registry) + pick = compiler.analyze((Pick(object=SceneObjectRef("cube")),)) + semantics = compiler.ground(pick, 0, _context(registry)).invocation.goal.semantics + object_to_eef = torch.eye(4).repeat(2, 1, 1) + object_to_eef[:, 2, 3] = 0.12 + target = SemanticRelativePose("table", (0.1, 0.0, 0.02), (0, 0, 0, 1)) + workflow = compiler.analyze((Place(object=SceneObjectRef("cube"), at=target),)) + first = compiler.ground( + workflow, 0, _held_context(registry, semantics, object_to_eef) + ) + expected = table.pose.clone() + expected[:, 0, 3] += 0.1 + expected[:, 2, 3] += 0.14 + torch.testing.assert_close(first.invocation.goal.xpos, expected) + table.pose[:, 1, 3] += 0.2 + second = compiler.ground( + workflow, 0, _held_context(registry, semantics, object_to_eef) + ) + expected[:, 1, 3] += 0.2 + torch.testing.assert_close(second.invocation.goal.xpos, expected) + assert not torch.equal(first.invocation.goal.xpos, second.invocation.goal.xpos) From 910db94634a55c58d59a86869296ec85196f8542 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 18 Nov 2026 03:34:59 +0000 Subject: [PATCH 35/45] feat(expansion): add bidirectional conditional layout sampling Generate layouts from authored anchors or workspace samples and alternate proposal directions within the existing budget. Retain the default bounded sampler, full task screening and independent measured acceptance. Compare all four strategies on pick/place, stacking and dual-arm RGB sorting. --- .../topics/expansion-engine/workloads.md | 41 ++ .../topics/scene-expansion/scene-expansion.md | 21 +- .../embodichain.lab.sim.scene.expansion.rst | 26 + embodichain/compute/geometry/_hierarchical.py | 21 +- embodichain/lab/expansion/_pose_sampling.py | 22 +- .../lab/expansion/_pose_sampling_cfg.py | 166 ++++- .../lab/expansion/_robot_first_sampling.py | 207 ++++++ .../lab/expansion/_scene_first_sampling.py | 377 +++++++++++ embodichain/lab/expansion/gym_workloads.py | 6 + .../lab/expansion/task_program_jobs.py | 9 +- .../lab/sim/scene/expansion/__init__.py | 2 + .../lab/sim/scene/expansion/conditions.py | 183 +++++ embodichain/lab/sim/scene/expansion/layout.py | 24 + .../lab/sim/scene/feasibility/proposals.py | 157 +++-- .../acceptance/scene_first/README.md | 20 + .../acceptance/scene_first/env.yaml | 125 ++++ .../acceptance/scene_first/expansion.yaml | 555 +++++++++++++++ .../acceptance/scene_first/integration.yaml | 82 +++ .../acceptance/scene_first/program.yaml | 86 +++ .../acceptance/scene_first/task.yaml | 18 + .../acceptance/scene_first/README.md | 9 + .../acceptance/scene_first/env.yaml | 99 +++ .../acceptance/scene_first/expansion.yaml | 296 ++++++++ .../acceptance/scene_first/integration.yaml | 69 ++ .../acceptance/scene_first/program.yaml | 44 ++ .../acceptance/scene_first/task.yaml | 19 + .../acceptance/scene_first/README.md | 9 + .../acceptance/scene_first/env.yaml | 93 +++ .../acceptance/scene_first/expansion.yaml | 295 ++++++++ .../acceptance/scene_first/integration.yaml | 47 ++ .../acceptance/scene_first/program.yaml | 44 ++ .../acceptance/scene_first/task.yaml | 19 + .../benchmark/expansion/README.scene_first.md | 101 +++ scripts/benchmark/expansion/scene_first.py | 635 ++++++++++++++++++ .../expansion/test_scene_first_report.py | 161 +++++ .../expansion/test_scene_first_sampling.py | 428 ++++++++++++ tests/sim/scene/expansion/test_conditions.py | 238 +++++++ 37 files changed, 4666 insertions(+), 88 deletions(-) create mode 100644 embodichain/lab/expansion/_robot_first_sampling.py create mode 100644 embodichain/lab/expansion/_scene_first_sampling.py create mode 100644 embodichain/lab/sim/scene/expansion/conditions.py create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/README.md create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/env.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/expansion.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/integration.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/program.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/task.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/README.md create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/env.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/expansion.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/integration.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/program.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/task.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/README.md create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/env.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/expansion.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/integration.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/program.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/task.yaml create mode 100644 scripts/benchmark/expansion/README.scene_first.md create mode 100644 scripts/benchmark/expansion/scene_first.py create mode 100644 tests/benchmark/expansion/test_scene_first_report.py create mode 100644 tests/lab/expansion/test_scene_first_sampling.py create mode 100644 tests/sim/scene/expansion/test_conditions.py diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index 9edead8cc6..a3a245238c 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -204,6 +204,47 @@ The configured acceptance task is `extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place`; its four-way runner is `scripts/benchmark/expansion/task_pose_coverage.py`. +## Conditional layout proposals and task screening + +`scene_sampling.strategy: scene_first` adds a prepared support-tree layout source +before ordinary native execution. `_scene_first_sampling.py` reuses shared scene +preparation/conditional generation and `RobotFeasibilityBackend`/`plan_task_mode`; +it creates no physics World and owns only the isolated collision-query resources. +`layout` and `task_mode` are explicit, validated task declarations. Root layout +variables currently use XY only; selected nested members retain declared XY/yaw. +B=1 is checked before runtime creation. A `bounded_pose` job may opt into the +same layout/task-mode screens for a controlled comparison without changing +ordinary simple-sampling defaults. Screening budgets are bounded and separate +from fixed physical quality thresholds. Prepared candidates stay immutable by +ordinal; failed screening cannot turn into an accepted simple fallback. + +`robot_first` anchors one movable support-tree root per proposal from the +workspace of a declared `robot_anchors` phase/part target. It uses the target's +initial rigid relation to the root and projects current-arena TCP XY onto the +root's allowed XY domain, preserving authored height and rotation. Per-part FK +populations are prepared once and selected across occupied cells. The layout +generator reserves the anchor and completes the remaining groups. A prior +without an in-domain sample is a budgeted rejection. + +`bidirectional` alternates scene-first and robot-first by raw ordinal, including +rejections. Attempt budgets, FIFO or task-pose coverage selection, execution and +receipt rules stay in their shared owners. The source records the direction of +each draw and selection, and records the requested anchor separately from the +generated and measured poses. + +The current geometry, joint limits, full-pose FK residuals, full-state/mimic +branches and synchronized path collision checks remain in their existing owners. +Actual execution replans and measured acceptance/persistence remain mandatory. +`scene-first-checks.json` retains phase results, workspace prior and anchor +evidence, and the unchanged World clock; +the query is closed before the owner environment, including setup failures. +The reproducible cross-task runner is +`scripts/benchmark/expansion/scene_first.py`; per-task fixtures are under the +existing grab/place, stack and dual-arm RGB sorting task configurations. The +sorting program operates the two arms sequentially and retains its original +segment validators; receipt-based coverage counts both manipulated blocks and +does not turn fixed-goal tracking noise into placement diversity. + ## Focused validation `tests/lab/expansion/test_workloads_sim.py` runs a real four-row task through diff --git a/agent_context/topics/scene-expansion/scene-expansion.md b/agent_context/topics/scene-expansion/scene-expansion.md index f2bf1e517b..0086ea2467 100644 --- a/agent_context/topics/scene-expansion/scene-expansion.md +++ b/agent_context/topics/scene-expansion/scene-expansion.md @@ -11,6 +11,12 @@ imported from `embodichain.lab.sim.scene.expansion`. movable permissions. `prepare_scene_randomization` prepares geometry; `generate_scene_variants` returns addressed CPU/CUDA candidates; `apply_scene_variants` preflights selected rigid poses without stepping. +`SceneLayoutCondition` restricts prepared generation to explicit arena-pose +anchors, with root XY/member XY-yaw permissions retained. Moving ancestors +must also be anchored. Anchored roots reserve space before unanchored groups +are sampled. Invalid domains reject before generation; unsupported +geometry returns budget exhaustion rather than silently moving the anchors. +The source/policy remain reusable; empty conditions preserve the old stream. `SimulationSceneExpansionHost` prepares/restores ordered rows with independent validation. Offline scheduling and confirmed persistence belong to the [Expansion Engine](../expansion-engine/expansion-engine.md). @@ -31,8 +37,19 @@ attempt through the shared selected-row preparation/restoration contract. The task-facing rolling collector also exposes selectable bounded versus workspace-conditioned pose proposals with optional receipt-confirmed task-pose coverage feedback; see [workload feedback](../expansion-engine/workloads.md#optional-task-pose-proposal-feedback). -This is distinct from offline atlas region validation and does not certify -continuous reachable volume. +The collector's optional scene-first, robot-first and bidirectional strategies +compose support-tree layout generation with the existing full-state task +feasibility backend. Robot-first projects a declared target's workspace TCPs to +root XY conditions before generating the rest of the layout. See the +[conditional layout source contract](../expansion-engine/workloads.md#conditional-layout-proposals-and-task-screening) +for selectors and direction scheduling. Explicit +`layout` and `task_mode` declarations bind support parents, permissions and +synchronized task stages. Its current path screening requires B=1, including +for a bounded-sampling baseline that opts into the same screens. Declared +workspace caches supply seeds; no cache hit certifies a pose or task. Pure +layout queries do not advance the owning World; topology changes invalidate +the prepared geometry. This is distinct from offline atlas region validation +and does not certify continuous reachable volume. ## Entry points diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.expansion.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.expansion.rst index 70132c16ba..84f4e27f9d 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.expansion.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.expansion.rst @@ -135,6 +135,7 @@ proposals. Hosts retain stepping, cache refresh and task-acceptance ownership. :nosignatures: SceneLayoutInput + SceneLayoutCondition PreparedSceneRandomization SceneRandomizationBatch SceneVariantApplicationResult @@ -154,6 +155,22 @@ proposals. Hosts retain stepping, cache refresh and task-acceptance ownership. .. autoclass:: SceneLayoutInput :members: +Conditional layout generation +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +``generate_scene_variants(..., conditions=SceneLayoutCondition(poses))`` fixes +arena-space anchors while completing the rest of the layout through the same +support and clearance checks. Group-root anchors currently restrict XY only; +selected members retain their support-frame XY/yaw domain. Anchor every moving +ancestor of an absolute member anchor. Conditions cannot change permissions, +height, authored tilt, topology or physics. Invalid domains reject before sampling; +geometric infeasibility consumes the bounded proposal budget without repair. +The prepared source and original policy stay reusable and independently owned. +Omitting conditions, or providing an empty condition, retains the legacy sequence. + +.. autoclass:: SceneLayoutCondition + :members: + .. autoclass:: PreparedSceneRandomization :members: @@ -192,3 +209,12 @@ proposals. Hosts retain stepping, cache refresh and task-acceptance ownership. .. autofunction:: save_scene_variants .. autofunction:: load_scene_variants + +Condition implementation module +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. currentmodule:: embodichain.lab.sim.scene.expansion.conditions + +.. autoclass:: SceneLayoutCondition + :members: + :no-index: diff --git a/embodichain/compute/geometry/_hierarchical.py b/embodichain/compute/geometry/_hierarchical.py index 68acbad096..88587d2934 100644 --- a/embodichain/compute/geometry/_hierarchical.py +++ b/embodichain/compute/geometry/_hierarchical.py @@ -212,6 +212,7 @@ def _sample_hierarchy( support_gap: float, movable_roots: tuple[int, ...] | None = None, root_offset_bounds: dict[int, torch.Tensor] | None = None, + anchored_roots: tuple[int, ...] = (), ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: """One proposal per ordinal, with no per-member retry loops.""" local = [matrix.expand(count, 4, 4).clone() for matrix in relative] @@ -299,7 +300,7 @@ def _sample_hierarchy( world.append( local[node] if parent < 0 else _matrix_product(world[parent], local[node]) ) - root_boxes, deltas = [], [] + root_boxes, deltas = [], {} fixed_boxes = {} if randomize_groups and movable_roots is not None: for root in root_keys: @@ -327,7 +328,15 @@ def _sample_hierarchy( ) fixed_boxes[root] = box root_boxes.append(box) - for root, key in root_keys.items(): + # Place exact anchors before variable groups, so their space is already + # reserved when choosing the free region of an unanchored sibling. + roots = ( + sorted(root_keys, key=lambda root: root not in anchored_roots) + if anchored_roots + else root_keys + ) + for root in roots: + key = root_keys[root] points = ( _rotate_points( _subtree_points(root, local, corners, parents), world[root][:, :3, :3] @@ -336,8 +345,8 @@ def _sample_hierarchy( ) box = torch.stack((points[:, :, :2].amin(1), points[:, :, :2].amax(1)), dim=1) if root in fixed_boxes: - deltas.append( - torch.zeros((count, 2), dtype=relative.dtype, device=relative.device) + deltas[root] = torch.zeros( + (count, 2), dtype=relative.dtype, device=relative.device ) continue if randomize_groups: @@ -392,9 +401,9 @@ def _sample_hierarchy( >= group_margin - 1e-10 ) root_boxes.append(box) - deltas.append(delta) + deltas[root] = delta return ( - torch.stack(deltas, dim=1), + torch.stack([deltas[root] for root in root_keys], dim=1), ( torch.stack([sampled[node] for node in sorted(targets)], dim=1) if targets diff --git a/embodichain/lab/expansion/_pose_sampling.py b/embodichain/lab/expansion/_pose_sampling.py index 99e2eaf5a4..85c41306fe 100644 --- a/embodichain/lab/expansion/_pose_sampling.py +++ b/embodichain/lab/expansion/_pose_sampling.py @@ -375,6 +375,16 @@ def _check(self, candidate): return False, "full_pose_fk_residual" return True, "passed" + def _offer(self, candidate, rng, raw): + if self.cfg["strategy"] == "workspace_joint": + candidate, origin = self._guided(candidate, rng) + ok, reason = self._check(candidate) + return candidate, origin, ok, reason + return candidate, "bounded_pose", True, "passed" + + def _offer_origin(self, raw): + return self.cfg["strategy"] + def candidate(self, ordinal): from dataclasses import replace from embodichain.lab.sim.scene.feasibility import FeasibilityBudgetExhausted @@ -420,15 +430,12 @@ def candidate(self, ordinal): else raw ) last = candidate + origin = self._offer_origin(raw) try: self.budget.charge("candidates") self._raw_ordinal += 1 ok = True - if self.cfg["strategy"] == "workspace_joint": - candidate, origin = self._guided(candidate, rng) - ok, reason = self._check(candidate) - else: - reason = "passed" + candidate, origin, ok, reason = self._offer(candidate, rng, raw) except FeasibilityBudgetExhausted as e: ok = False reason = str(e) @@ -457,7 +464,7 @@ def candidate(self, ordinal): } ) if ok: - pool.append((candidate, cells, origin)) + pool.append((candidate, cells, origin, raw)) if self.schedule["policy"] == "fifo" and pool: break if self.budget._blocked is not None: @@ -488,7 +495,7 @@ def score(entry): ids = np.flatnonzero(scores == scores.max()) chosen = pool[int(rng.choice(ids))] selection = "coverage" - candidate, cells, origin = chosen + candidate, cells, origin, raw = chosen self._predicted[ordinal] = cells reason = "passed" else: @@ -514,6 +521,7 @@ def score(entry): { "event": "selection", "ordinal": ordinal, + "raw_ordinal": raw, "candidate_id": candidate.candidate_id, "selection": selection, "origin": origin, diff --git a/embodichain/lab/expansion/_pose_sampling_cfg.py b/embodichain/lab/expansion/_pose_sampling_cfg.py index 0f996d6370..c0ae0999ba 100644 --- a/embodichain/lab/expansion/_pose_sampling_cfg.py +++ b/embodichain/lab/expansion/_pose_sampling_cfg.py @@ -22,6 +22,9 @@ __all__: list[str] = [] +_ROBOT_LAYOUT_STRATEGIES = ("robot_first", "bidirectional") +_LAYOUT_STRATEGIES = ("scene_first", *_ROBOT_LAYOUT_STRATEGIES) + def validate_sampling(value: dict) -> None: """Normalize optional settings without changing legacy configurations.""" @@ -29,6 +32,10 @@ def validate_sampling(value: dict) -> None: allowed = { "strategy", + "layout", + "task_mode", + "screening", + "robot_anchors", "workspace_source", "control_part", "sample_count", @@ -70,9 +77,14 @@ def validate_sampling(value: dict) -> None: "exploration_fraction": 0.25, }.items(): schedule.setdefault(k, v) - if sampling["strategy"] not in ("bounded_pose", "workspace_joint"): + if sampling["strategy"] not in ( + "bounded_pose", + "workspace_joint", + *_LAYOUT_STRATEGIES, + ): raise ValueError( - "scene_sampling.strategy must be bounded_pose or workspace_joint" + "scene_sampling.strategy must be bounded_pose, workspace_joint, " + "scene_first, robot_first or bidirectional" ) if sampling["workspace_source"] not in ("cache", "joint_uniform_fk"): raise ValueError( @@ -134,6 +146,156 @@ def validate_sampling(value: dict) -> None: raise ValueError( "Workspace targets require a declared independent scene entity and rigid transform" ) + if sampling["strategy"] == "workspace_joint" and "layout" in sampling: + raise ValueError( + "workspace_joint does not consume layout conditions; choose scene_first or a screened bounded_pose baseline" + ) + if sampling["strategy"] in _LAYOUT_STRATEGIES or "layout" in sampling: + if ( + value["engine"].get("batch_size", 1) + * value["engine"].get("workload_count", 1) + != 1 + ): + raise ValueError("Layout task-path screening currently requires B=1") + layout = sampling.get("layout") + if ( + not isinstance(layout, dict) + or set(layout) - {"support_uid", "parent_by_uid", "policy", "conditions"} + or not {"support_uid", "parent_by_uid", "policy"} <= layout.keys() + ): + raise ValueError( + "Layout sampling requires support_uid, parent_by_uid and policy" + ) + if ( + not isinstance(layout["support_uid"], str) + or not layout["support_uid"].strip() + or not isinstance(layout["parent_by_uid"], dict) + or not layout["parent_by_uid"] + or any( + type(k) is not str or type(v) is not str + for k, v in layout["parent_by_uid"].items() + ) + ): + raise ValueError( + "layout requires an explicit support UID and parent mapping" + ) + if not isinstance(layout["policy"], dict): + raise ValueError("layout.policy must be a mapping") + mode = sampling.get("task_mode") + if ( + not isinstance(mode, dict) + or set(mode) - {"mode_id", "phases"} + or not {"mode_id", "phases"} <= mode.keys() + or not isinstance(mode["phases"], list) + or not mode["phases"] + ): + raise ValueError( + "Layout sampling requires a declared task_mode phase graph" + ) + screening = sampling.setdefault("screening", {}) + if not isinstance(screening, dict) or set(screening) - { + "beam_width", + "ik_seeds", + "max_rrt_nodes", + "planning_timeout_s", + }: + raise ValueError("Unknown scene-first screening budgets") + for key, default in { + "beam_width": 2, + "ik_seeds": 2, + "max_rrt_nodes": 64, + "planning_timeout_s": 0.5, + }.items(): + screening.setdefault(key, default) + for key in ("beam_width", "ik_seeds", "max_rrt_nodes"): + if type(screening[key]) is not int or not 1 <= screening[key] <= 4096: + raise ValueError("Screening counts must be positive and bounded") + if ( + type(screening["planning_timeout_s"]) not in (int, float) + or not math.isfinite(screening["planning_timeout_s"]) + or not 0 < screening["planning_timeout_s"] <= 60 + ): + raise ValueError("Screening timeout must be positive and bounded") + from embodichain.lab.sim.scene.expansion.layout import _policy_data + from embodichain.lab.sim.scene.feasibility import request_from_mapping + + normalized_policy = _policy_data(layout["policy"]) + if any( + "follow" in value["scene"].get(uid, {}) + for uid in normalized_policy["intra_group"]["targets"] + ): + raise ValueError( + "Rigid scene followers cannot also be independent intra-group targets" + ) + parents = layout["parent_by_uid"] + support = layout["support_uid"] + for uid in parents: + seen = set() + current = uid + while current != support: + if current in seen or current not in parents: + raise ValueError("Layout parents must form one rooted support tree") + seen.add(current) + current = parents[current] + identities = {support, *parents, *value["scene"]} + request = request_from_mapping( + dict( + task_id="preflight", + robot_profile_id="preflight", + policy_id="preflight", + validator_id="preflight", + parent_scene_id="preflight", + entity_poses={uid: np.eye(4).tolist() for uid in identities}, + variables=[], + modes=[mode], + ) + ) + for uid, parent in parents.items(): + if ( + parent == support + and uid in value["scene"] + and "follow" not in value["scene"][uid] + ): + bounds = value["scene"][uid] + if bounds["yaw_rad"] != [0, 0] or bounds["translation_m"][2] != [0, 0]: + raise ValueError("Root layout domains currently support XY only") + conditions = layout.get("conditions", {}) + if not isinstance(conditions, dict): + raise ValueError("layout.conditions must map UIDs to arena poses") + for uid, m in conditions.items(): + m = np.asarray(m, dtype=float) + if ( + m.shape != (4, 4) + or not np.isfinite(m).all() + or not np.allclose(m[3], [0, 0, 0, 1]) + or not np.allclose(m[:3, :3].T @ m[:3, :3], np.eye(3), atol=1e-6) + or not np.isclose(np.linalg.det(m[:3, :3]), 1) + ): + raise ValueError("layout conditions require proper rigid matrices") + if ( + "robot_anchors" in sampling + or sampling["strategy"] in _ROBOT_LAYOUT_STRATEGIES + ): + from ._robot_first_sampling import _anchor_targets + + anchors = _anchor_targets( + request.modes[0], + layout, + normalized_policy, + sampling.get("robot_anchors"), + ) + if any( + a.root_uid not in value["scene"] + or "follow" in value["scene"][a.root_uid] + for a in anchors + ): + raise ValueError( + "A robot anchor requires an independently declared scene root" + ) + elif "task_mode" in sampling: + raise ValueError("task_mode screening requires an explicit layout") + elif "robot_anchors" in sampling: + raise ValueError("robot_anchors requires an explicit layout and task_mode") coverage = value.get("pose_coverage") if coverage is None: if schedule["policy"] == "task_pose_coverage": diff --git a/embodichain/lab/expansion/_robot_first_sampling.py b/embodichain/lab/expansion/_robot_first_sampling.py new file mode 100644 index 0000000000..1cbd0cdabe --- /dev/null +++ b/embodichain/lab/expansion/_robot_first_sampling.py @@ -0,0 +1,207 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Owned workspace priors for conditional layout completion.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from dataclasses import dataclass +from typing import TYPE_CHECKING, Any + +import numpy as np + +from embodichain.lab.sim.scene.expansion import SceneLayoutCondition +from embodichain.lab.sim.scene.feasibility.proposals import _workspace_fk_samples + +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + from embodichain.lab.sim.scene.expansion import PreparedSceneRandomization + from embodichain.lab.sim.scene.feasibility import ( + FeasibilityBudget, + PhaseTarget, + TaskOperationMode, + ) + +__all__: list[str] = [] + + +@dataclass(frozen=True) +class _AnchorTarget: + phase_id: str + target: PhaseTarget + root_uid: str + + +def _anchor_targets( + mode: TaskOperationMode, layout: Mapping, policy: Mapping, selections: object +) -> tuple[_AnchorTarget, ...]: + """Resolve each named target to a rigid frame in the initial support tree.""" + if not isinstance(selections, list) or not selections: + raise ValueError( + "robot_anchors requires a nonempty list of phase/part selectors" + ) + targets = {} + moved = set() + support, parents = layout["support_uid"], layout["parent_by_uid"] + variable_members = policy["intra_group"]["targets"] + for phase in mode.phases: + for target in phase.targets: + targets[phase.phase_id, target.control_part] = (target, moved.copy()) + moved.update(held.entity_uid for held in phase.held_objects) + moved.update(change.entity_uid for change in phase.resulting_poses) + result, seen = [], set() + for sel in selections: + if ( + not isinstance(sel, Mapping) + or set(sel) != {"phase_id", "control_part"} + or any(type(value) is not str for value in sel.values()) + ): + raise ValueError("robot_anchors selects explicit phase_id and control_part") + key = (sel["phase_id"], sel["control_part"]) + if key not in targets or key in seen: + raise ValueError("robot_anchors must select unique declared phase targets") + seen.add(key) + target, earlier_moves = targets[key] + root = target.frame_uid + if root not in parents: + raise ValueError("A robot anchor target needs a frame in the support tree") + while True: + if root in variable_members or root in earlier_moves: + raise ValueError( + "A robot anchor needs an initial frame rigidly attached to its layout root" + ) + if parents[root] == support: + break + root = parents[root] + groups = policy["group_targets"] + if not policy["randomize_groups"] or ( + groups is not None and root not in groups + ): + raise ValueError("A robot anchor requires a movable layout root") + if root in layout.get("conditions", {}): + raise ValueError("A robot anchor cannot replace an authored root condition") + result.append(_AnchorTarget(key[0], target, root)) + return tuple(result) + + +@dataclass(frozen=True) +class _AnchorPool: + selection: _AnchorTarget + base: np.ndarray + buckets: tuple[np.ndarray, ...] + + def condition( + self, visit: int, fixed: SceneLayoutCondition + ) -> SceneLayoutCondition | None: + """Cycle over occupied cells and shuffle within each cell once per job.""" + if not self.buckets: + return None + cycle, cell = divmod(visit, len(self.buckets)) + points = self.buckets[cell] + pose = self.base.copy() + pose[:2, 3] = points[cycle % len(points)] + return SceneLayoutCondition({**fixed.poses, self.selection.root_uid: pose}) + + def metrics(self) -> dict[str, object]: + return { + "phase_id": self.selection.phase_id, + "control_part": self.selection.target.control_part, + "root_uid": self.selection.root_uid, + "samples_in_domain": sum(map(len, self.buckets)), + "occupied_xy_cells": len(self.buckets), + } + + +def _prepare_anchor_pools( + robot: Robot, + budget: FeasibilityBudget, + prepared: PreparedSceneRandomization, + config: Mapping[str, Any], + selections: Sequence[_AnchorTarget], + scene: Mapping[str, Any], + nominal: Mapping[str, np.ndarray], + *, + seed: int, +) -> tuple[_AnchorPool, ...]: + """Precompute at most sample_count FK rows per arm, retaining only legal XY.""" + policy = prepared.policy + movable_roots = { + root + for root, parent in prepared.source.parent_by_uid.items() + if parent == prepared.source.support_uid and root in prepared.movable_uids + } + nodes_by_part: dict[str, list[tuple[int, _AnchorTarget]]] = {} + for index, selection in enumerate(selections): + root = selection.root_uid + if root not in movable_roots or root not in scene or "follow" in scene[root]: + raise ValueError( + f"Robot anchor root {root!r} must have independent XY bounds" + ) + nodes_by_part.setdefault(selection.target.control_part, []).append( + (index, selection) + ) + + pools = {} + for part, entries in nodes_by_part.items(): + part_index = entries[0][0] + fk = _workspace_fk_samples( + robot, + budget, + control_part=part, + source=config["workspace_source"], + sample_count=config["sample_count"], + seed=seed + part_index, + ) + for index, selection in entries: + root, target = selection.root_uid, selection.target + base = np.array(prepared.source.poses[root], dtype=float) + bounds = np.asarray(scene[root]["translation_m"], dtype=float)[:2].T + bounds = bounds + np.asarray(nominal[root])[:2, 3] + offsets = policy["group_xy_offset_bounds_m"].get(root) + if offsets is not None: + layout_bounds = base[:2, 3] + np.asarray(offsets) + bounds[0] = np.maximum(bounds[0], layout_bounds[0]) + bounds[1] = np.minimum(bounds[1], layout_bounds[1]) + lower, upper = bounds + if np.any(upper < lower): + raise ValueError(f"Robot anchor root {root!r} has disjoint XY bounds") + + # Preserve sampled TCP XY under the root's allowed rotation/height. + # Sampled TCP height/orientation are not retained by this projection. + frame = np.asarray(prepared.source.poses[target.frame_uid]) + root_from_tcp = np.linalg.solve(base, frame @ np.asarray(target.pose)) + xy = fk[:, :2, 3] - (base[:3, :3] @ root_from_tcp[:3, 3])[:2] + xy[:, lower == upper] = lower[lower == upper] + xy = xy[np.all((lower <= xy) & (xy <= upper), axis=1)] + # Balance across occupied cells, so a dense set of joint samples in + # one region cannot dominate every proposal. No extrapolation. + size = budget.cfg.archive_bins + scaled = np.divide( + xy - lower, upper - lower, out=np.zeros_like(xy), where=upper > lower + ) + cells = np.minimum((scaled * size).astype(int), size - 1) + occupied = cells[:, 0] * size + cells[:, 1] + rng = np.random.default_rng(np.random.SeedSequence([seed, index])) + buckets = [] + for cell in rng.permutation(np.unique(occupied)): + points = rng.permutation(xy[occupied == cell]) + points.setflags(write=False) + buckets.append(points) + base.setflags(write=False) + pools[index] = _AnchorPool(selection, base, tuple(buckets)) + budget.check() + return tuple(pools[index] for index in range(len(selections))) diff --git a/embodichain/lab/expansion/_scene_first_sampling.py b/embodichain/lab/expansion/_scene_first_sampling.py new file mode 100644 index 0000000000..f9a7c20838 --- /dev/null +++ b/embodichain/lab/expansion/_scene_first_sampling.py @@ -0,0 +1,377 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Conditional layout proposals with shared full task screening.""" + +from __future__ import annotations + +from dataclasses import replace +import math +import time +import numpy as np + +from ._pose_sampling import _AdaptivePoseSource +from ._pose_sampling_cfg import _ROBOT_LAYOUT_STRATEGIES +from ._robot_first_sampling import _anchor_targets, _prepare_anchor_pools +from embodichain.lab.sim.scene.expansion import ( + SceneLayoutCondition, + scene_layout_input_from_sim, + prepare_scene_randomization, + generate_scene_variants, + ScenePoseChange, + SceneVariant, +) + +__all__: list[str] = [] + + +class _LayoutPoseSource(_AdaptivePoseSource): + """Generate or certify complete layouts before checking task branches. + + The numerical layout source owns no World. The existing task backend owns + workspace seed selection, full-state IK and collision-checked path screening. + Runtime execution still replans and independently validates measured outcomes. + """ + + def __init__(self, env, options, *, seed, revision): + if env.num_envs != 1: + raise ValueError("Layout task-path screening currently requires B=1") + before = self._world_clock(env.sim) + self._collision = None + super().__init__(env, options, seed=seed, revision=revision) + for key, value in self.cfg["screening"].items(): + setattr(self.budget.cfg, key, value) + self.budget.cfg.__post_init__() + layout = self.cfg["layout"] + source = scene_layout_input_from_sim( + env.sim, + support_uid=layout["support_uid"], + parent_by_uid=layout["parent_by_uid"], + movable_entity_ids=tuple( + uid for uid in options["scene"] if uid in layout["parent_by_uid"] + ), + scene_id=revision, + ) + self._prepared_layout = prepare_scene_randomization(source, layout["policy"]) + self._fixed_layout = { + uid: ScenePoseChange(uid, source.poses[uid]) + for uid in self.configuration + if uid in source.poses and uid not in self._prepared_layout.movable_uids + } + self._conditions = SceneLayoutCondition(layout.get("conditions", {})) + # Compile/check condition syntax and permissions before any row is reset. + from embodichain.lab.sim.scene.expansion.conditions import _condition_prepared + + _condition_prepared(self._prepared_layout, self._conditions) + nodes = self._prepared_layout._geometry["nodes"] + for n in nodes: + if n["uid"] in options["scene"] and n["parent"] < 0: + bounds = options["scene"][n["uid"]] + if "follow" not in bounds and ( + bounds["yaw_rad"] != [0, 0] or bounds["translation_m"][2] != [0, 0] + ): + raise ValueError( + "Root layout sampling supports XY only; put member yaw in a declared intra-group domain" + ) + from embodichain.lab.sim.scene.feasibility import request_from_mapping + + entities = { + uid: obj.get_local_pose(to_matrix=True)[0].detach().cpu().double().tolist() + for uid in env.sim.get_rigid_object_uid_list() + if (obj := env.sim.get_rigid_object(uid)) is not None + } + entities[env.robot.uid] = self.roots[env.robot.uid] + if getattr(env.sim, "_default_plane", None) is not None: + entities["default_plane"] = np.eye(4).tolist() + request = request_from_mapping( + dict( + task_id=revision, + robot_profile_id=env.robot.uid, + policy_id=revision, + validator_id=revision, + parent_scene_id=revision, + entity_poses=entities, + variables=[], + modes=[self.cfg["task_mode"]], + ) + ) + self._mode = request.modes[0] + for phase in self._mode.phases: + for target in phase.targets: + if ( + target.control_part not in env.robot.control_parts + or env.robot.get_solver(name=target.control_part) is None + ): + raise ValueError( + f"Task phase names unavailable control part {target.control_part!r}" + ) + self._entities = {k: np.array(v) for k, v in entities.items()} + self._anchor_pools = () + self._workspace_seconds = self._screening_seconds = 0.0 + if self.cfg["strategy"] in _ROBOT_LAYOUT_STRATEGIES: + started = time.perf_counter() + selections = _anchor_targets( + self._mode, + layout, + self._prepared_layout.policy, + self.cfg["robot_anchors"], + ) + self._anchor_pools = _prepare_anchor_pools( + env.robot, + self.budget, + self._prepared_layout, + self.cfg, + selections, + self.configuration, + self.poses, + seed=seed, + ) + self._workspace_seconds = time.perf_counter() - started + + from embodichain.lab.task_program.integrations.simulation.feasibility_geometry import ( + collision_bodies_from_simulation, + ) + from .acceptance import _GeometryRow + from embodichain.lab.sim.scene.feasibility import NewtonCollisionChecker + + bodies = collision_bodies_from_simulation(env) + known = {b.frame_uid for b in bodies} + ignored, _ = _GeometryRow.contact_permissions(env, options["validation"], known) + try: + self._collision = NewtonCollisionChecker( + bodies, ignored_pairs=ignored, device="cpu", max_shapes=512 + ) + self._check_clock(env.sim, before) + except BaseException: + self.close() + raise + self._scene_identity = ( + (id(env.sim.spawn_result), env.sim.spawn_result.topology_revision) + if hasattr(env.sim, "spawn_result") + else None + ) + self._task_checks = {} + self._anchor_checks = {} + + def bind_program(self, adapter, program): + """Keep the declared phase graph and live runtime independent authorities.""" + if not tuple(program.iter_segments()): + raise ValueError("Layout sampling requires a nonempty task program") + + @staticmethod + def _world_clock(sim): + return getattr(sim, "physics_step_index", None), getattr( + sim, "simulation_time", None + ) + + @classmethod + def _check_clock(cls, sim, before): + if before != cls._world_clock(sim): + raise RuntimeError("Scene proposal screening advanced the owning World") + + def _offer_origin(self, raw): + if self.cfg["strategy"] == "bidirectional": + return "robot_first" if (raw + self.seed) % 2 else "scene_first" + return super()._offer_origin(raw) + + def _offer(self, candidate, rng, raw): + sim = self.env.sim + if self._scene_identity is not None and self._scene_identity != ( + id(sim.spawn_result), + sim.spawn_result.topology_revision, + ): + raise RuntimeError( + "Prepared layout geometry is stale after a topology change" + ) + before = self._world_clock(sim) + started = time.perf_counter() + try: + return self._offer_layout(candidate, raw, self._offer_origin(raw)) + finally: + self._screening_seconds += time.perf_counter() - started + self._check_clock(sim, before) + self._task_checks.setdefault(raw, []).append( + dict( + stage="screening.clock", + status="passed", + reason="Owning World step/time unchanged", + phase_id=None, + ) + ) + + def _offer_layout(self, candidate, raw, origin): + if origin != "bounded_pose": + conditions = self._conditions + if origin == "robot_first": + ordinal = raw // 2 if self.cfg["strategy"] == "bidirectional" else raw + visit, index = divmod(ordinal, len(self._anchor_pools)) + pool = self._anchor_pools[index] + conditions = pool.condition(visit, conditions) + self._anchor_checks[raw] = { + "root_uid": pool.selection.root_uid, + "phase_id": pool.selection.phase_id, + "control_part": pool.selection.target.control_part, + "pose": ( + None + if conditions is None + else conditions.poses[pool.selection.root_uid] + ), + } + if conditions is None: + return candidate, origin, False, "no_workspace_anchor" + batch = generate_scene_variants( + self._prepared_layout, + seed=self.seed, + candidate_count=1, + max_attempts=1, + start_ordinal=raw, + conditions=conditions, + ) + if not batch.variants: + return candidate, origin, False, "layout_budget_exhausted" + generated = batch.variants[0] + # References outside the support tree (e.g. a numerical goal) + # retain their declared independent bounded sampling. + changes = { + c.entity_uid: self._fixed_layout.get(c.entity_uid, c) + for c in candidate.scene.pose_changes + } + changes.update({c.entity_uid: c for c in generated.pose_changes}) + candidate = replace( + candidate, + scene=SceneVariant( + candidate.scene.parent_scene_id, + self.seed, + raw, + tuple(changes.values()), + generated.metadata, + ), + ) + else: + # Equal downstream geometric/kinematic gates for the simple baseline. + for change in candidate.scene.pose_changes: + fixed = self._fixed_layout.get(change.entity_uid) + if fixed is not None and not np.allclose( + change.arena_pose, fixed.arena_pose, rtol=0, atol=1e-9 + ): + return candidate, origin, False, "layout_moves_fixed_entity" + anchors = { + c.entity_uid: c.arena_pose + for c in candidate.scene.pose_changes + if c.entity_uid in self._prepared_layout.movable_uids + } + for uid, pose in self._conditions.poses.items(): + actual = anchors.get(uid, self._prepared_layout.source.poses[uid]) + if not np.allclose(actual, pose, rtol=0, atol=1e-9): + return candidate, "bounded_pose", False, "condition_mismatch" + batch = generate_scene_variants( + self._prepared_layout, + seed=self.seed, + candidate_count=1, + max_attempts=1, + start_ordinal=raw, + conditions=SceneLayoutCondition(anchors), + ) + if not batch.variants: + return candidate, "bounded_pose", False, "layout_budget_exhausted" + poses = { + **self._entities, + **{ + c.entity_uid: np.array(c.arena_pose) + for c in candidate.scene.pose_changes + }, + **{uid: np.array(pose) for uid, pose in candidate.deployment.poses.items()}, + } + # Never expand explicit task-facing ranges while applying layout policy. + for c in candidate.scene.pose_changes: + opts = self.configuration[c.entity_uid] + if "follow" in opts: + continue + delta = np.asarray(c.arena_pose)[:3, 3] - self.poses[c.entity_uid][:3, 3] + bounds = np.asarray(opts["translation_m"]) + rotation = np.asarray(c.arena_pose)[:3, :3] @ np.linalg.inv( + self.poses[c.entity_uid][:3, :3] + ) + yaw = math.atan2(rotation[1, 0], rotation[0, 0]) + low, high = opts["yaw_rad"] + yaw += 2 * math.pi * round(((low + high) / 2 - yaw) / (2 * math.pi)) + if ( + np.any(delta < bounds[:, 0] - 1e-6) + or np.any(delta > bounds[:, 1] + 1e-6) + or yaw < low - 1e-6 + or yaw > high + 1e-6 + ): + return ( + candidate, + origin, + False, + "layout_outside_task_domain", + ) + from embodichain.lab.sim.scene.feasibility import ( + RobotFeasibilityBackend, + plan_task_mode, + ) + + backend = RobotFeasibilityBackend( + self.env.robot, + self._collision, + self.budget.cfg, + poses, + seed=self.seed + raw, + ) + plan = plan_task_mode(self._mode, backend, poses, self.budget) + self._task_checks[raw] = [ + { + "stage": c.stage, + "status": c.status, + "reason": c.reason, + "phase_id": c.phase_id, + } + for c in plan.checks + ] + reason = ( + "passed" + if plan.accepted + else next( + (c.reason for c in plan.checks if c.status != "passed"), + "task_path_unverified", + ) + ) + return candidate, origin, plan.accepted, reason + + def checkpoint(self): + super().checkpoint() + if self.output is not None and hasattr(self, "_task_checks"): + from embodichain.data_pipeline.recording.media import _atomic_json + + _atomic_json( + self.output / "scene-first-checks.json", + dict( + phase_checks=self._task_checks, + layout_source=self._prepared_layout.source.fingerprint, + conditions=self._conditions.to_dict(), + workspace_prior=[pool.metrics() for pool in self._anchor_pools], + workspace_anchor_checks=self._anchor_checks, + workspace_prepare_wall_s=self._workspace_seconds, + proposal_screen_wall_s=self._screening_seconds, + ), + ) + + def close(self): + """Release only query resources, without modifying the owning World.""" + if self._collision is not None: + self._collision.close() + self._collision = None diff --git a/embodichain/lab/expansion/gym_workloads.py b/embodichain/lab/expansion/gym_workloads.py index 8c61d3c501..c8e32ac457 100644 --- a/embodichain/lab/expansion/gym_workloads.py +++ b/embodichain/lab/expansion/gym_workloads.py @@ -751,6 +751,7 @@ def physical_key(env_id: str, cfg: Any) -> str: ) owner = source = None + feedback_closers = [] groups: dict[str, Any] = {} adapters: dict[int, Any] = {} futures: dict[str, Any] = {} @@ -847,6 +848,9 @@ def physical_key(env_id: str, cfg: Any) -> str: ctx.feedback = workload.extensions.feedback_factory( env, ctx.scene_source, path ) + close = getattr(ctx.feedback, "close", None) + if close is not None: + feedback_closers.append(close) builder = GymRecipeBuilder(ctx) transform = workload.extensions.recipe_transform ctx.recipe = lambda ordinal, builder=builder, transform=transform: ( @@ -1064,5 +1068,7 @@ def done(name: str, counts: Mapping[str, object]) -> None: adapter.close() for group in groups.values(): group["host"]._context.journal.checkpoint(force=True, state="finished") + for close in reversed(feedback_closers): + close() if owner is not None: owner.close() diff --git a/embodichain/lab/expansion/task_program_jobs.py b/embodichain/lab/expansion/task_program_jobs.py index c92a00c296..55af8d08d0 100644 --- a/embodichain/lab/expansion/task_program_jobs.py +++ b/embodichain/lab/expansion/task_program_jobs.py @@ -66,8 +66,15 @@ def _task_expansion_extensions(*, options, operator, seed, report): def source(env): nonlocal pose_source + from ._scene_first_sampling import _LayoutPoseSource + + source_type = ( + _LayoutPoseSource + if "layout" in options.get("scene_sampling", {}) + else _AdaptivePoseSource + ) pose_source = ( - _AdaptivePoseSource(env, options, seed=seed, revision=report["semantic_id"]) + source_type(env, options, seed=seed, revision=report["semantic_id"]) if "scene_sampling" in options else _BoundedPoseSource( env, options["scene"], seed=seed, revision=report["semantic_id"] diff --git a/embodichain/lab/sim/scene/expansion/__init__.py b/embodichain/lab/sim/scene/expansion/__init__.py index 32758cf991..35b0707b5c 100644 --- a/embodichain/lab/sim/scene/expansion/__init__.py +++ b/embodichain/lab/sim/scene/expansion/__init__.py @@ -27,10 +27,12 @@ from .cfg import SceneExpansionCfg from .contracts import ScenePoseChange, SceneVariant +from .conditions import SceneLayoutCondition from .source import FixedSceneCandidateSource __all__ = [ "SceneExpansionCfg", + "SceneLayoutCondition", "ScenePoseChange", "SceneVariant", "FixedSceneCandidateSource", diff --git a/embodichain/lab/sim/scene/expansion/conditions.py b/embodichain/lab/sim/scene/expansion/conditions.py new file mode 100644 index 0000000000..2b70be330d --- /dev/null +++ b/embodichain/lab/sim/scene/expansion/conditions.py @@ -0,0 +1,183 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Owned arena-space anchors that restrict an existing prepared layout domain.""" + +from __future__ import annotations + +from collections.abc import Mapping +from copy import deepcopy +from dataclasses import dataclass, replace +import math +from types import MappingProxyType +from typing import TYPE_CHECKING + +import numpy as np + +from .._validation import _identifier, _rigid_pose + +if TYPE_CHECKING: + from .layout import PreparedSceneRandomization + +__all__ = ["SceneLayoutCondition"] + + +@dataclass(frozen=True) +class SceneLayoutCondition: + """Fix declared entities to arena poses without enlarging their motion domain. + + Args: + poses: Native UID to rigid arena-space pose. Inputs are independently + owned immutable tuples. Group roots retain their authored rotation + and height; selected members retain their declared support-frame + XY/yaw constraints. A moving ancestor of a member anchor must also + be anchored, so an absolute anchor cannot drift with its parent. + + Conditions do not authorize new motion, topology changes or physics steps. + Geometric infeasibility yields an empty/budget-exhausted generation result; + malformed or unsupported conditions reject before candidate generation. + """ + + poses: Mapping[str, object] + + def __post_init__(self) -> None: + if not isinstance(self.poses, Mapping): + raise TypeError("Condition poses must be a UID-to-pose mapping") + owned = {} + for uid, pose in self.poses.items(): + _identifier(uid, "condition UID") + owned[uid] = _rigid_pose(pose) + object.__setattr__(self, "poses", MappingProxyType(dict(sorted(owned.items())))) + + def to_dict(self) -> dict[str, list[list[float]]]: + """Return a detached JSON-compatible projection of the anchors.""" + return {uid: [list(row) for row in pose] for uid, pose in self.poses.items()} + + +def _condition_prepared( + prepared: PreparedSceneRandomization, condition: SceneLayoutCondition +) -> PreparedSceneRandomization: + """Compile anchors to zero-width bounds, keeping existing geometry checks.""" + if not isinstance(condition, SceneLayoutCondition): + raise TypeError("conditions must be SceneLayoutCondition") + if not condition.poses: + return prepared + source, policy = prepared.source, prepared.policy + unknown = set(condition.poses) - set(source.poses) + if unknown: + raise ValueError(f"Unknown condition entities: {sorted(unknown)}") + nodes = prepared._geometry["nodes"] + index = {n["uid"]: i for i, n in enumerate(nodes)} + roots = {nodes[i]["uid"] for i in prepared._movable_roots} + targets = policy["intra_group"]["targets"] + variable = roots | set(targets) + world = {source.support_uid: np.asarray(source.poses[source.support_uid])} + for n in nodes: + uid = n["uid"] + original = np.asarray(source.poses[uid]) + parent_uid = source.parent_by_uid[uid] + anchored = uid in condition.poses + if anchored: + parent = parent_uid + while parent != source.support_uid: + if parent in variable and parent not in condition.poses: + raise ValueError( + f"Anchor {uid!r} requires its moving ancestor {parent!r} to be anchored" + ) + parent = source.parent_by_uid[parent] + parent_world = world[parent_uid] + if n["parent"] < 0: + if not anchored: + world[uid] = original + continue + wanted = np.asarray(condition.poses[uid]) + if ( + not np.allclose(wanted[:3, :3], original[:3, :3], rtol=0, atol=1e-9) + or abs(wanted[2, 3] - original[2, 3]) > 1e-9 + ): + raise ValueError( + "Root anchors support declared XY translation only; rotation/height must remain authored" + ) + delta = wanted[:2, 3] - original[:2, 3] + if uid not in roots: + if not np.allclose(delta, 0, rtol=0, atol=1e-9): + raise ValueError(f"Condition cannot move fixed group {uid!r}") + else: + bounds = policy["group_xy_offset_bounds_m"].get(uid) + if bounds is not None and ( + np.any(delta < np.asarray(bounds)[0] - 1e-9) + or np.any(delta > np.asarray(bounds)[1] + 1e-9) + ): + raise ValueError("Anchor lies outside declared root XY bounds") + policy["group_xy_offset_bounds_m"][uid] = [ + delta.tolist(), + delta.tolist(), + ] + world[uid] = wanted + continue + relative = np.asarray(n["relative_pose"]) + if not anchored: + world[uid] = parent_world @ relative + continue + wanted = np.asarray(condition.poses[uid]) + if uid not in targets: + if not np.allclose(wanted, parent_world @ relative, rtol=0, atol=1e-9): + raise ValueError(f"Condition cannot move undeclared member {uid!r}") + world[uid] = wanted + continue + spec = targets[uid] + support = prepared._geometry["supports"][parent_uid] + inv_frame = np.linalg.inv(np.asarray(support["parent_from_support"])) + baseline = inv_frame @ relative + local = inv_frame @ np.linalg.inv(parent_world) @ wanted + rotation = local[:3, :3] @ np.linalg.inv(baseline[:3, :3]) + angle = math.degrees(math.atan2(rotation[1, 0], rotation[0, 0])) + c, s = math.cos(math.radians(angle)), math.sin(math.radians(angle)) + yaw_rotation = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) + low, high = spec["yaw_delta_range_deg"] + angle += 360 * math.floor(((low + high) / 2 - angle) / 360 + 0.5) + height = ( + policy["intra_group"]["support_gap_m"] + + support.get("height_ceiling_m", 0) + - support["root_min_z"][uid] + ) + if ( + not np.allclose(rotation, yaw_rotation, rtol=0, atol=1e-9) + or abs(local[2, 3] - height) > 1e-9 + ): + raise ValueError( + "Member anchor violates its authored tilt or support height" + ) + if angle < low - 1e-8 or angle > high + 1e-8: + raise ValueError("Member anchor lies outside declared yaw bounds") + offset = local[:2, 3] - baseline[:2, 3] + bounds = spec["xy_offset_bounds_m"] + if bounds is not None and ( + np.any(offset < np.asarray(bounds)[0] - 1e-9) + or np.any(offset > np.asarray(bounds)[1] + 1e-9) + ): + raise ValueError("Member anchor lies outside declared XY bounds") + spec["xy_offset_bounds_m"] = [offset.tolist(), offset.tolist()] + spec["yaw_delta_range_deg"] = [angle, angle] + world[uid] = wanted + if source.support_uid in condition.poses and not np.allclose( + condition.poses[source.support_uid], + source.poses[source.support_uid], + rtol=0, + atol=1e-9, + ): + raise ValueError("The main support is fixed") + return replace(prepared, _policy=deepcopy(policy)) diff --git a/embodichain/lab/sim/scene/expansion/layout.py b/embodichain/lab/sim/scene/expansion/layout.py index 479946ce70..14ec5af156 100644 --- a/embodichain/lab/sim/scene/expansion/layout.py +++ b/embodichain/lab/sim/scene/expansion/layout.py @@ -37,6 +37,7 @@ from ..geometry import SupportSurfaceConfig, detect_support_region from ..query import _backend_tensor from .contracts import ScenePoseChange, SceneVariant +from .conditions import SceneLayoutCondition, _condition_prepared from ._layout_numeric import ( _hierarchy_batch_limit, _numeric_geometry, @@ -571,6 +572,7 @@ def generate_scene_variants( candidate_count: int = 10, max_attempts: int = 1000, start_ordinal: int = 0, + conditions: SceneLayoutCondition | None = None, ) -> SceneRandomizationBatch: """Generate a batch without loading meshes or constructing simulation objects. @@ -580,10 +582,17 @@ def generate_scene_variants( candidate_count: Requested valid layouts. max_attempts: Whole-layout proposal budget. start_ordinal: First logical proposal; allows bounded continuation. + conditions: Optional owned arena-pose anchors. They restrict existing + root XY/member XY-yaw domains and never authorize extra motion. + Moving ancestors must be anchored explicitly. Empty conditions + preserve the ordinary sequence. Infeasible geometry exhausts the + declared proposal budget without moving or repairing anchors. Returns: Complete or budget-exhausted proposals with exact continuation ordinal. """ + if conditions is not None: + prepared = _condition_prepared(prepared, conditions) _validate_generation_parameters( seed=seed, candidate_count=candidate_count, @@ -600,6 +609,15 @@ def generate_scene_variants( ): raise ValueError("Requested CUDA device is unavailable.") spec = _specification(prepared._geometry, policy, device) + anchored_roots = ( + tuple( + i + for i in prepared._movable_roots + if prepared._geometry["nodes"][i]["uid"] in conditions.poses + ) + if conditions is not None + else () + ) limit = ( _hierarchy_batch_limit(policy["batch_size"], len(spec[-1])) if policy["randomize_groups"] @@ -619,6 +637,7 @@ def generate_scene_variants( group_margin=policy["collision_margin_m"], support_gap=policy["intra_group"]["support_gap_m"], movable_roots=prepared._movable_roots, + anchored_roots=anchored_roots, root_offset_bounds={ i: torch.tensor( policy["group_xy_offset_bounds_m"][node["uid"]], @@ -656,6 +675,11 @@ def generate_scene_variants( ("generator", _ALGORITHM), ("source_sha256", prepared.source.fingerprint), ("geometry_sha256", prepared.geometry_sha256), + *( + (("conditions_sha256", _digest(conditions.to_dict())),) + if conditions is not None and conditions.poses + else () + ), ), ) return SceneRandomizationBatch( diff --git a/embodichain/lab/sim/scene/feasibility/proposals.py b/embodichain/lab/sim/scene/feasibility/proposals.py index 06c01176da..5bf8281d82 100644 --- a/embodichain/lab/sim/scene/feasibility/proposals.py +++ b/embodichain/lab/sim/scene/feasibility/proposals.py @@ -37,6 +37,83 @@ __all__ = ["RobotWorkspaceProposalSource"] +def _workspace_fk_samples( + robot: Robot, + budget: FeasibilityBudget, + *, + control_part: str, + source: str, + sample_count: int, + seed: int, +) -> np.ndarray: + """Return owned current-arena FK poses from a bounded workspace population.""" + generator = torch.Generator(device=robot.device).manual_seed(seed) + budget.check() + if source == "cache": + workspace = robot.get_workspace(control_part) + settings = robot.cfg.workspace_cfg[control_part] + indices = workspace.sample_indices( + sample_count, + strategy=settings.strategy, + min_score=settings.min_score, + generator=generator, + ) + if ( + indices.shape != (sample_count,) + or indices.dtype != torch.int64 + or bool(((indices < 0) | (indices >= workspace.num_samples)).any()) + ): + raise ValueError("workspace sampler returned invalid cache indices") + qpos = workspace.qpos[indices].to(device=robot.device) + else: + limits = robot.get_qpos_limits(name=control_part)[0] + if ( + not bool(torch.isfinite(limits).all()) + or bool((limits[:, 1] < limits[:, 0]).any()) + or bool((limits.abs() > 1e6).any()) + ): + raise UnsupportedFeasibilityError( + "Joint-uniform workspace preparation requires explicit finite joint bounds" + ) + qpos = limits[:, 0] + torch.rand( + (sample_count, len(limits)), device=robot.device, generator=generator + ) * (limits[:, 1] - limits[:, 0]) + if ( + qpos.ndim != 2 + or len(qpos) != sample_count + or not bool(torch.isfinite(qpos).all()) + ): + raise ValueError( + "workspace proposals require finite selected joint configurations" + ) + poses = np.empty((sample_count, 4, 4), dtype=np.float64) + for start in range(0, sample_count, budget.cfg.batch_size): + budget.check() + stop = min(start + budget.cfg.batch_size, sample_count) + batch = robot.compute_batch_fk( + qpos=qpos[None, start:stop], + name=control_part, + env_ids=[0], + to_matrix=True, + ) + if batch.shape != (1, stop - start, 4, 4): + raise ValueError("workspace FK proposals must match the requested batch") + poses[start:stop] = batch[0].detach().cpu().numpy() + if not np.isfinite(poses).all(): + raise ValueError("workspace FK proposals must be finite rigid matrices") + rotations = poses[:, :3, :3] + if ( + not np.allclose(poses[:, 3, :], [0.0, 0.0, 0.0, 1.0], atol=1e-5, rtol=0) + or not np.allclose( + rotations.transpose(0, 2, 1) @ rotations, np.eye(3), atol=1e-5, rtol=0 + ) + or not np.allclose(np.linalg.det(rotations), 1.0, atol=1e-5, rtol=0) + ): + raise ValueError("workspace FK proposals must be proper rigid transforms") + budget.check() + return poses + + class RobotWorkspaceProposalSource: """Copy current-model FK samples into an independent numerical proposal prior. @@ -112,76 +189,16 @@ def __init__( np.linalg.inv(np.array(target.pose)), np.linalg.inv(np.array(variable.members[target.frame_uid])), ) - generator = torch.Generator(device=robot.device).manual_seed(budget.cfg.seed) - budget.check() - if control_part in (robot.cfg.workspace_cfg or {}): - workspace = robot.get_workspace(control_part) - settings = robot.cfg.workspace_cfg[control_part] - indices = workspace.sample_indices( - sample_count, - strategy=settings.strategy, - min_score=settings.min_score, - generator=generator, - ) - if ( - indices.shape != (sample_count,) - or indices.dtype != torch.int64 - or bool(((indices < 0) | (indices >= workspace.num_samples)).any()) - ): - raise ValueError("workspace sampler returned invalid cache indices") - qpos = workspace.qpos[indices].to(device=robot.device) - origin = "configured_qpos_cache" - else: - limits = robot.get_qpos_limits(name=control_part)[0] - if ( - not bool(torch.isfinite(limits).all()) - or bool((limits[:, 1] < limits[:, 0]).any()) - or bool((limits.abs() > 1e6).any()) - ): - raise UnsupportedFeasibilityError( - "Joint-uniform workspace preparation requires explicit finite joint bounds" - ) - qpos = limits[:, 0] + torch.rand( - (sample_count, len(limits)), device=robot.device, generator=generator - ) * (limits[:, 1] - limits[:, 0]) - origin = "joint_uniform_fk" - if ( - qpos.ndim != 2 - or len(qpos) != sample_count - or not bool(torch.isfinite(qpos).all()) - ): - raise ValueError( - "workspace proposals require finite selected joint configurations" - ) - batches = [] - for start in range(0, sample_count, budget.cfg.batch_size): - budget.check() - batch = ( - robot.compute_batch_fk( - qpos=qpos[None, start : start + budget.cfg.batch_size], - name=control_part, - env_ids=[0], - to_matrix=True, - )[0] - .detach() - .cpu() - .double() - .numpy() - .copy() - ) - batches.append(batch) - poses = np.concatenate(batches) - if poses.ndim != 3 or poses.shape[1:] != (4, 4) or not np.isfinite(poses).all(): - raise ValueError("workspace FK proposals must be finite rigid matrices") - rotations = poses[:, :3, :3] - if ( - not np.allclose(poses[:, 3, :], [0.0, 0.0, 0.0, 1.0], atol=1e-5, rtol=0) - or not np.allclose( - rotations.transpose(0, 2, 1) @ rotations, np.eye(3), atol=1e-5, rtol=0 - ) - or not np.allclose(np.linalg.det(rotations), 1.0, atol=1e-5, rtol=0) - ): - raise ValueError("workspace FK proposals must be proper rigid transforms") + has_cache = control_part in (robot.cfg.workspace_cfg or {}) + poses = _workspace_fk_samples( + robot, + budget, + control_part=control_part, + source="cache" if has_cache else "joint_uniform_fk", + sample_count=sample_count, + seed=budget.cfg.seed, + ) + origin = "configured_qpos_cache" if has_cache else "joint_uniform_fk" self._local = np.linalg.inv(self._root)[None] @ poses self._uniform_fraction = uniform_fraction self._rng = np.random.default_rng(budget.cfg.seed) diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/README.md new file mode 100644 index 0000000000..898a52473c --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/README.md @@ -0,0 +1,20 @@ +# Dual-arm RGB sorting acceptance + +This scene-first fixture reuses the original RGB block-sorting program unchanged: +right arm moves red, left arm moves blue, green stays stationary. It selects the +existing dual-UR5 profile and open-loop dual-arm execution policy. The two arms +act sequentially; this case does not demonstrate simultaneous manipulation. + +Default physics, sensors disabled, B=1. The supported primitive tabletop has its +top at 0.835 m, with the 5 cm blocks resting at 0.86 m. Red/blue nominal poses +are the midpoints of the original reset domains, with the same ±2.5 cm X / ±3 cm +Y ranges. Green is +a fixed reference at its original domain midpoint. Geometry/robot topology stays +fixed. Both source strategies share the same layout and task-path screens. + +All original lift (5 cm), XY placement (3.5 / 3 cm), and wait-stable validators +remain unchanged. Independent measured acceptance additionally requires both +objects at table height, both hands released, green within 5 mm of its initial +reference, and stable terminal speeds. Coverage records both manipulated blocks. + +See [the comparison guide](../../../../../../../../../scripts/benchmark/expansion/README.scene_first.md). diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/env.yaml new file mode 100644 index 0000000000..bd36d9f6c1 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/env.yaml @@ -0,0 +1,125 @@ +environment_id: conditional_dual_arm_sort +physics: default +max_episodes: 1 +max_episode_steps: 2400 +simulation: + light: + direct: + - uid: light_1 + light_type: sun + color: + - 1.0 + - 1.0 + - 1.0 + intensity: 5.0 + direction: + - 0.0 + - 0.0 + - -1.0 + background: + - uid: support_surface + shape: + shape_type: Cube + size: + - 1.0 + - 1.0 + - 0.1 + body_type: static + init_pos: + - 0.725 + - 0.0 + - 0.785 + rigid_object: + - uid: block_1 + shape: + shape_type: Cube + size: + - 0.05 + - 0.05 + - 0.05 + attrs: + mass_props: + mass: 0.05 + rigid_props: + max_depenetration_velocity: 10.0 + min_position_iters: 32 + min_velocity_iters: 8 + collision_props: + contact_offset: 0.003 + rest_offset: 0.001 + material_props: + static_friction: 0.5 + dynamic_friction: 0.5 + restitution: 0.0 + init_pos: + - 0.625 + - -0.15 + - 0.86 + body_scale: + - 1 + - 1 + - 1 + - uid: block_2 + shape: + shape_type: Cube + size: + - 0.05 + - 0.05 + - 0.05 + attrs: + mass_props: + mass: 0.05 + rigid_props: + max_depenetration_velocity: 10.0 + min_position_iters: 32 + min_velocity_iters: 8 + collision_props: + contact_offset: 0.003 + rest_offset: 0.001 + material_props: + static_friction: 0.5 + dynamic_friction: 0.5 + restitution: 0.0 + init_pos: + - 0.675 + - 0.0 + - 0.86 + body_scale: + - 1 + - 1 + - 1 + - uid: block_3 + shape: + shape_type: Cube + size: + - 0.05 + - 0.05 + - 0.05 + attrs: + mass_props: + mass: 0.05 + rigid_props: + max_depenetration_velocity: 10.0 + min_position_iters: 32 + min_velocity_iters: 8 + collision_props: + contact_offset: 0.003 + rest_offset: 0.001 + material_props: + static_friction: 0.5 + dynamic_friction: 0.5 + restitution: 0.0 + init_pos: + - 0.725 + - 0.15 + - 0.86 + body_scale: + - 1 + - 1 + - 1 +env: + sim_steps_per_control: 4 + events: {} + dataset: {} +num_envs: 1 +arena_space: 3.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/expansion.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/expansion.yaml new file mode 100644 index 0000000000..1d4d02c893 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/expansion.yaml @@ -0,0 +1,555 @@ +schema_version: 1 +engine: + episodes_per_operator: 4 + max_attempts: 64 + max_steps: 3200 + max_wall_time_s: 600.0 + settling_steps: 30 + capture_media: false + collision_backend: newton + collision_device: cpu +scene: + block_1: + translation_m: + - - -0.025 + - 0.025 + - - -0.03 + - 0.03 + - - 0 + - 0 + yaw_rad: + - 0 + - 0 + block_3: + translation_m: + - - -0.025 + - 0.025 + - - -0.03 + - 0.03 + - - 0 + - 0 + yaw_rad: + - 0 + - 0 +trajectory: + operators: + - object_pose + joint_offset_scale: 0.005 + via_count: 1 + max_proposals: 64 + minimum_joint_delta: 0.002 + velocity_limit: 0.5 + acceleration_limit: 0.5 + nullspace_task_rows: + - 0 + - 1 + - 2 + - 3 + - 4 + - 5 + nullspace_translation_tolerance_m: 0.001 + nullspace_rotation_tolerance_rad: 0.01 + free_phases: + place: + - retract +validation: + initial_checks: + - kind: support_bounds + entity: block_1 + lower: + - 0.2 + - -0.5 + - 0.82 + upper: + - 1.2 + - 0.5 + - 0.95 + - kind: support_bounds + entity: block_2 + lower: + - 0.2 + - -0.5 + - 0.82 + upper: + - 1.2 + - 0.5 + - 0.95 + - kind: support_bounds + entity: block_3 + lower: + - 0.2 + - -0.5 + - 0.82 + upper: + - 1.2 + - 0.5 + - 0.95 + final_checks: &id001 + - kind: axis_relation + entity: block_1 + other: block_2 + axis: 0 + lower: -0.115 + upper: -0.045 + - kind: axis_relation + entity: block_1 + other: block_2 + axis: 1 + lower: -0.03 + upper: 0.03 + - kind: axis_relation + entity: block_1 + other: block_2 + axis: 2 + lower: -0.012 + upper: 0.012 + - kind: speed_below + entity: block_1 + linear_m_s: 0.03 + angular_rad_s: 0.2 + - kind: axis_relation + entity: block_3 + other: block_2 + axis: 0 + lower: 0.045 + upper: 0.115 + - kind: axis_relation + entity: block_3 + other: block_2 + axis: 1 + lower: -0.03 + upper: 0.03 + - kind: axis_relation + entity: block_3 + other: block_2 + axis: 2 + lower: -0.012 + upper: 0.012 + - kind: speed_below + entity: block_3 + linear_m_s: 0.03 + angular_rad_s: 0.2 + - kind: joint_range + joint: left_gripper_finger1_joint_1 + lower: -0.001 + upper: 0.005 + - kind: joint_range + joint: right_gripper_finger1_joint_1 + lower: -0.001 + upper: 0.005 + - kind: pose_near + entity: block_2 + position: + - 0.675 + - 0.0 + - 0.86 + tolerance_m: 0.005 + - kind: speed_below + entity: block_2 + linear_m_s: 0.03 + angular_rad_s: 0.2 + events: + - event_id: block_1_lifted + conditions: + - kind: lifted + entity: block_1 + height_m: 0.05 + - event_id: block_3_lifted + conditions: + - kind: lifted + entity: block_3 + height_m: 0.05 + - event_id: both_sorted_released_stable + conditions: *id001 + consecutive_steps: 3 + allowed_contacts: &id002 + - - block_1 + - support_surface + - - block_1 + - default_plane + - - block_2 + - support_surface + - - block_2 + - default_plane + - - block_3 + - support_surface + - - block_3 + - default_plane + - - block_1 + - DualUR5/right_gripper_finger1_link_1 + - - block_1 + - DualUR5/right_gripper_finger2_link_1 + - - block_3 + - DualUR5/left_gripper_finger1_link_1 + - - block_3 + - DualUR5/left_gripper_finger2_link_1 + mounting_contacts: + - - '@root' + - default_plane + - - '@root' + - support_surface +scene_sampling: + strategy: scene_first + workspace_source: joint_uniform_fk + control_part: right_arm + sample_count: 16384 + robot_anchors: + - phase_id: block_1_grasp + control_part: right_arm + - phase_id: block_3_grasp + control_part: left_arm + uniform_fraction: 0.25 + max_candidates: 256 + max_rollouts: 4 + max_ik_calls: 16384 + max_planning_calls: 2048 + targets: {} + layout: + support_uid: support_surface + parent_by_uid: + block_1: support_surface + block_2: support_surface + block_3: support_surface + policy: + group_targets: + - block_1 + - block_3 + batch_size: 1 + collision_margin_m: 0.002 + group_xy_offset_bounds_m: + block_1: + - - -0.025 + - -0.03 + - - 0.025 + - 0.03 + block_3: + - - -0.025 + - -0.03 + - - 0.025 + - 0.03 + task_mode: + mode_id: sort_with_both_arms + phases: + - phase_id: block_1_approach + dependencies: [] + targets: + - control_part: right_arm + frame_uid: block_1 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.11 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + - phase_id: block_1_grasp + dependencies: + - block_1_approach + targets: + - control_part: right_arm + frame_uid: block_1 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - -0.01 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + - phase_id: block_1_lift + dependencies: + - block_1_grasp + targets: + - control_part: right_arm + frame_uid: block_1 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.14 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + held_objects: + - entity_uid: block_1 + control_part: right_arm + tcp_from_object: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - -0.01 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + - phase_id: block_1_place + dependencies: + - block_1_lift + targets: + - control_part: right_arm + frame_uid: block_2 + pose: + - - 1.0 + - 0.0 + - 0.0 + - -0.08 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.07 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + held_objects: + - entity_uid: block_1 + control_part: right_arm + tcp_from_object: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - -0.01 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + - phase_id: block_1_retreat + dependencies: + - block_1_place + targets: + - control_part: right_arm + frame_uid: block_2 + pose: + - - 1.0 + - 0.0 + - 0.0 + - -0.08 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.22 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + - phase_id: block_3_approach + dependencies: + - block_1_retreat + targets: + - control_part: left_arm + frame_uid: block_3 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.11 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + - phase_id: block_3_grasp + dependencies: + - block_3_approach + targets: + - control_part: left_arm + frame_uid: block_3 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - -0.01 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + - phase_id: block_3_lift + dependencies: + - block_3_grasp + targets: + - control_part: left_arm + frame_uid: block_3 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.14 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + held_objects: + - entity_uid: block_3 + control_part: left_arm + tcp_from_object: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - -0.01 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + - phase_id: block_3_place + dependencies: + - block_3_lift + targets: + - control_part: left_arm + frame_uid: block_2 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.08 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.07 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + held_objects: + - entity_uid: block_3 + control_part: left_arm + tcp_from_object: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - -0.01 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + - phase_id: block_3_retreat + dependencies: + - block_3_place + targets: + - control_part: left_arm + frame_uid: block_2 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.08 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.22 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 +scheduling: + policy: fifo + pool_size: 8 + exploration_fraction: 0.25 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/integration.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/integration.yaml new file mode 100644 index 0000000000..63e3ac866d --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/integration.yaml @@ -0,0 +1,82 @@ +integration_id: blocks_ranking_rgb_scene_first_dual_ur5 +program_id: rank_blocks_rgb +requires: + scene_contract: blocks_ranking_rgb_scene_v1 + embodiment_contract: dual_arm_parallel_gripper +scene_binding: + contract_id: blocks_ranking_rgb_scene_v1 + registry_id: task_program_blocks_ranking_rgb + rigid_objects: + - entity_id: block_1 + dynamics: dynamic + semantic_type: block + affordances: + - entity_id: block_1_grasp + kind: antipodal_grasp + - entity_id: block_2 + dynamics: dynamic + semantic_type: block + affordances: + - entity_id: red_placement + kind: support_surface + native_name: red_neighbor + object_target_frame: world_axes + object_target_pose: + position: + - -0.08 + - 0.0 + - 0.08 + quaternion_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - entity_id: blue_placement + kind: support_surface + native_name: blue_neighbor + object_target_frame: world_axes + object_target_pose: + position: + - 0.08 + - 0.0 + - 0.08 + quaternion_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - entity_id: block_3 + dynamics: dynamic + semantic_type: block + affordances: + - entity_id: block_3_grasp + kind: antipodal_grasp +profile: + defaults: + pick_up: + primary: primary_manipulator + place: + primary: primary_manipulator + skill_options: + pick: + kind: pick_up + pre_grasp_distance: 0.12 + lift_height: 0.15 + hand_interp_steps: 10 + grasp_settle_steps: 45 + fixed_object_to_eef: + position: + - 0.0 + - 0.0 + - -0.01 + quaternion_xyzw: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + place: + kind: place + lift_height: 0.15 + hand_interp_steps: 10 + preserve_current_object_orientation: true + effect_monitors: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/program.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/program.yaml new file mode 100644 index 0000000000..9f8ecd558b --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/program.yaml @@ -0,0 +1,86 @@ +program_id: rank_blocks_rgb +instruction: Arrange the red, green, and blue blocks from left to right while keeping the green block + stationary. +targets: {} +program: +- segment: pick_red_block + instruction: Grasp and lift red block clear of the table. + steps: + pick: + object: block_1 + grasp: block_1_grasp + resources: + primary: primary_manipulator + validators: + - kind: object_lifted + object: block_1 + minimum_lift: 0.05 +- segment: place_red_block + instruction: Place the red block left of the green block, release it, and let it settle. + steps: + place: + object: block_1 + 'on': red_placement + resources: + primary: primary_manipulator + post: + - kind: wait_stable + entity: block_1 + validators: + - kind: object_relative_position + object: block_1 + reference: block_2 + position_offset: + - -0.08 + - 0.0 + - 0.0 + position_tolerance: + - 0.035 + - 0.03 + - null +- segment: pick_blue_block + instruction: Grasp and lift blue block clear of the table. + steps: + pick: + object: block_3 + grasp: block_3_grasp + resources: + primary: secondary_manipulator + validators: + - kind: object_lifted + object: block_3 + minimum_lift: 0.05 +- segment: place_blue_block + instruction: Place the blue block right of the green block, release it, and let it settle. + steps: + place: + object: block_3 + 'on': blue_placement + resources: + primary: secondary_manipulator + post: + - kind: wait_stable + entity: block_3 + validators: + - kind: object_relative_position + object: block_3 + reference: block_2 + position_offset: + - 0.08 + - 0.0 + - 0.0 + position_tolerance: + - 0.035 + - 0.03 + - null + - kind: object_relative_position + object: block_1 + reference: block_2 + position_offset: + - -0.08 + - 0.0 + - 0.0 + position_tolerance: + - 0.035 + - 0.03 + - null diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/task.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/task.yaml new file mode 100644 index 0000000000..f63099d29e --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/blocks_ranking_rgb/acceptance/scene_first/task.yaml @@ -0,0 +1,18 @@ +id: ConditionalSceneBlocksRankingRGB-DualUR5-v1 +environment: + component: env.yaml +task_program: + program: program.yaml + integration: integration.yaml + execution_policy: ../../../../../../components/execution_policies/trajectory_open_loop_dual_arm_manipulation.yaml +embodiment: + component: ../../../../../../components/embodiments/dual_ur5_dh_pgi_140_80.yaml + overrides: + init_pos: + - 1.683 + - 0.0 + - 0.5275 + init_rot: + - 0.0 + - 0.0 + - -90.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/README.md new file mode 100644 index 0000000000..458e189193 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/README.md @@ -0,0 +1,9 @@ +# Conditional scene-first task fixture + +This deployment keeps the task's declared controller, physical acceptance and +confirmed persistence gates while comparing bounded versus scene-first XY layouts. +`expansion.yaml` owns explicit support parents, sampling ranges, the task phase +graph and screening effort. It does not change the ordinary deployment defaults. + +See the [cross-task acceptance guide](../../../../../../../../../scripts/benchmark/expansion/README.scene_first.md) for commands, conditional +anchor semantics, scope restrictions and result interpretation. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/env.yaml new file mode 100644 index 0000000000..a2ca89425d --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/env.yaml @@ -0,0 +1,99 @@ +environment_id: conditional_stack +physics: default +max_episodes: 1 +max_episode_steps: 2400 +num_envs: 1 +arena_space: 2.5 +simulation: + light: + direct: + - uid: main_light + light_type: sun + color: + - 0.6 + - 0.6 + - 0.6 + intensity: 5.0 + direction: + - 0.0 + - 0.0 + - -1.0 + - uid: rect_light + light_type: rect + color: + - 1.0 + - 1.0 + - 1.0 + intensity: 8.0 + init_pos: + - -0.42 + - 0.08 + - 0.8 + direction: + - 0.0 + - 0.0 + - -1.0 + rect_width: 0.5 + rect_height: 0.5 + background: + - uid: support_surface + shape: + shape_type: Cube + size: + - 2.0 + - 2.0 + - 0.1 + body_type: static + init_pos: + - 0.0 + - 0.0 + - -0.05 + rigid_object: + - uid: block_1 + shape: + shape_type: Cube + size: + - 0.05 + - 0.05 + - 0.05 + body_type: dynamic + init_pos: + - -0.4 + - 0.48 + - 0.025 + attrs: + mass_props: + mass: 0.05 + rigid_props: + linear_damping: 0.2 + angular_damping: 0.2 + material_props: + dynamic_friction: 0.97 + static_friction: 0.99 + - uid: block_2 + shape: + shape_type: Cube + size: + - 0.05 + - 0.05 + - 0.05 + body_type: dynamic + init_pos: + - -0.42 + - -0.08 + - 0.025 + attrs: + mass_props: + mass: 0.05 + rigid_props: + linear_damping: 0.2 + angular_damping: 0.2 + material_props: + dynamic_friction: 0.97 + static_friction: 0.99 + rigid_object_group: [] + articulation: [] +env: + sim_steps_per_control: 4 + events: {} + dataset: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/expansion.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/expansion.yaml new file mode 100644 index 0000000000..f42b1e302c --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/expansion.yaml @@ -0,0 +1,296 @@ +schema_version: 1 +engine: + episodes_per_operator: 4 + max_attempts: 64 + max_steps: 2400 + max_wall_time_s: 600.0 + settling_steps: 30 + capture_media: false + collision_backend: newton + collision_device: cpu +scene: + block_1: + translation_m: + - - -0.06 + - 0.06 + - - -0.06 + - 0.06 + - - 0 + - 0 + yaw_rad: + - 0 + - 0 + block_2: + translation_m: + - - -0.06 + - 0.06 + - - -0.06 + - 0.06 + - - 0 + - 0 + yaw_rad: + - 0 + - 0 +trajectory: + operators: + - object_pose + joint_offset_scale: 0.005 + via_count: 1 + max_proposals: 64 + minimum_joint_delta: 0.002 + velocity_limit: 0.5 + acceleration_limit: 0.5 + nullspace_task_rows: + - 0 + - 1 + - 2 + - 3 + - 4 + - 5 + nullspace_translation_tolerance_m: 0.001 + nullspace_rotation_tolerance_rad: 0.01 + free_phases: + place: + - retract +validation: + initial_checks: + - kind: support_bounds + entity: block_2 + lower: + - -1 + - -1 + - -0.003 + upper: + - 1 + - 1 + - 0.1 + final_checks: &id001 + - kind: axis_relation + entity: block_2 + other: block_1 + axis: 2 + lower: 0.04 + upper: 0.062 + - kind: speed_below + entity: block_2 + linear_m_s: 0.03 + angular_rad_s: 0.2 + - kind: speed_below + entity: block_1 + linear_m_s: 0.03 + angular_rad_s: 0.2 + - kind: joint_range + joint: gripper_finger1_joint_1 + lower: -0.001 + upper: 0.005 + events: + - event_id: lift + conditions: + - kind: lifted + entity: block_2 + height_m: 0.05 + - event_id: stacked + conditions: *id001 + consecutive_steps: 3 + allowed_contacts: &id002 + - - block_1 + - default_plane + - - block_1 + - support_surface + - - block_1 + - UR5/gripper_finger1_link_1 + - - block_1 + - UR5/gripper_finger2_link_1 + - - block_2 + - default_plane + - - block_2 + - support_surface + - - block_2 + - UR5/gripper_finger1_link_1 + - - block_2 + - UR5/gripper_finger2_link_1 + - - block_1 + - block_2 + mounting_contacts: + - - '@root' + - default_plane + - - '@root' + - support_surface +scene_sampling: + strategy: scene_first + workspace_source: joint_uniform_fk + control_part: arm + sample_count: 16384 + robot_anchors: + - phase_id: grasp + control_part: arm + uniform_fraction: 0.25 + max_candidates: 256 + max_rollouts: 4 + max_ik_calls: 16384 + max_planning_calls: 2048 + targets: {} + layout: + support_uid: support_surface + parent_by_uid: + block_1: support_surface + block_2: support_surface + policy: + group_targets: + - block_1 + - block_2 + batch_size: 1 + collision_margin_m: 0.02 + group_xy_offset_bounds_m: + block_1: + - - -0.06 + - -0.06 + - - 0.06 + - 0.06 + block_2: + - - -0.06 + - -0.06 + - - 0.06 + - 0.06 + task_mode: + mode_id: runtime + phases: + - phase_id: approach + dependencies: [] + targets: + - control_part: arm + frame_uid: block_2 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.1 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + - phase_id: grasp + dependencies: + - approach + targets: + - control_part: arm + frame_uid: block_2 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.0 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + - phase_id: lift + dependencies: + - grasp + targets: + - control_part: arm + frame_uid: block_2 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.1 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + held_objects: + - entity_uid: block_2 + control_part: arm + tcp_from_object: + - - 1 + - 0 + - 0 + - 0 + - - 0 + - -1 + - 0 + - 0 + - - 0 + - 0 + - -1 + - 0 + - - 0 + - 0 + - 0 + - 1 + - phase_id: place + dependencies: + - lift + targets: + - control_part: arm + frame_uid: block_1 + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.05 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id002 + held_objects: + - entity_uid: block_2 + control_part: arm + tcp_from_object: + - - 1 + - 0 + - 0 + - 0 + - - 0 + - -1 + - 0 + - 0 + - - 0 + - 0 + - -1 + - 0 + - - 0 + - 0 + - 0 + - 1 +scheduling: + policy: fifo + pool_size: 8 + exploration_fraction: 0.25 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/integration.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/integration.yaml new file mode 100644 index 0000000000..fb0de39013 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/integration.yaml @@ -0,0 +1,69 @@ +integration_id: stack_blocks_two_v1 +program_id: stack_two_blocks +requires: + scene_contract: stack_blocks_two_scene_v1 + embodiment_contract: single_arm_parallel_gripper +scene_binding: + contract_id: stack_blocks_two_scene_v1 + registry_id: task_program_stack_blocks_two + rigid_objects: + - entity_id: block_1 + dynamics: dynamic + semantic_type: block + affordances: + - entity_id: block_1_top + kind: support_surface + native_name: top + object_target_frame: world_axes + object_target_pose: + position: + - 0.0 + - 0.0 + - 0.05 + quaternion_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - entity_id: block_2 + dynamics: dynamic + semantic_type: block + affordances: + - entity_id: block_2_grasp + kind: antipodal_grasp +profile: + defaults: + pick_up: + primary: primary_manipulator + place: + primary: primary_manipulator + skill_options: + pick: + kind: pick_up + pre_grasp_distance: 0.12 + lift_height: 0.15 + hand_interp_steps: 10 + grasp_settle_steps: 45 + fixed_object_to_eef: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - -1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - -1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + place: + kind: place + lift_height: 0.1 + hand_interp_steps: 10 + preserve_current_object_orientation: false + effect_monitors: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/program.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/program.yaml new file mode 100644 index 0000000000..8ef78b03e3 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/program.yaml @@ -0,0 +1,44 @@ +program_id: stack_two_blocks +instruction: Stack the two blocks into a stable tower with block 2 on top of block + 1. +targets: {} +program: +- segment: pick_block_2 + instruction: Grasp and lift block 2 clear of the table. + steps: + pick: + object: block_2 + grasp: block_2_grasp + validators: + - kind: object_lifted + object: block_2 + minimum_lift: 0.05 +- segment: place_block_2_on_block_1 + instruction: Place block 2 on top of block 1, release it, and let the stack settle. + steps: + place: + object: block_2 + 'on': block_1_top + post: + - kind: wait_stable + entity: block_2 + - kind: wait_stable + entity: block_1 + validators: + - kind: object_relative_position + object: block_2 + reference: block_1 + position_offset: + - 0.0 + - 0.0 + - 0.05 + position_tolerance: + - 0.025 + - 0.025 + - 0.012 + - kind: object_upright + object: block_1 + maximum_tilt_degrees: 45.0 + - kind: object_upright + object: block_2 + maximum_tilt_degrees: 45.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/task.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/task.yaml new file mode 100644 index 0000000000..a371b22ba4 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/stack_blocks_two/acceptance/scene_first/task.yaml @@ -0,0 +1,19 @@ +id: ConditionalSceneStack-UR5-v1 +environment: + component: env.yaml +task_program: + program: program.yaml + integration: integration.yaml + execution_policy: ../../../../../../components/execution_policies/trajectory_open_loop.yaml +embodiment: + component: ../../../../../../components/embodiments/ur5_dh_pgi_140_80.yaml + overrides: + init_qpos: + - 0 + - -1.57 + - 1.57 + - -1.57 + - -1.57 + - 0 + - 0 + - 0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/README.md new file mode 100644 index 0000000000..de725d51be --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/README.md @@ -0,0 +1,9 @@ +# Conditional scene-first task fixture + +This deployment keeps the task's declared controller, physical acceptance and +confirmed persistence gates while comparing bounded versus scene-first XY layouts. +`expansion.yaml` owns explicit support parents, sampling ranges, the task phase +graph and screening effort. It does not change the ordinary deployment defaults. + +See the [cross-task acceptance guide](../../../../../../../../scripts/benchmark/expansion/README.scene_first.md) for commands, conditional +anchor semantics, scope restrictions and result interpretation. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/env.yaml new file mode 100644 index 0000000000..212bf07c3a --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/env.yaml @@ -0,0 +1,93 @@ +environment_id: conditional_pick_place +physics: default +max_episodes: 1 +max_episode_steps: 2400 +num_envs: 1 +arena_space: 2.5 +simulation: + light: + direct: + - uid: main_light + light_type: sun + color: + - 0.6 + - 0.6 + - 0.6 + intensity: 5.0 + direction: + - 0.0 + - 0.0 + - -1.0 + - uid: rect_light + light_type: rect + color: + - 1.0 + - 1.0 + - 1.0 + intensity: 8.0 + init_pos: + - -0.42 + - 0.08 + - 0.8 + direction: + - 0.0 + - 0.0 + - -1.0 + rect_width: 0.5 + rect_height: 0.5 + background: + - uid: goal + shape: + shape_type: Cube + size: + - 0.01 + - 0.01 + - 0.001 + body_type: static + init_pos: + - -0.4 + - 0.48 + - -0.1 + attrs: + collision_props: + collision_enabled: false + - uid: support_surface + shape: + shape_type: Cube + size: + - 2.0 + - 2.0 + - 0.1 + body_type: static + init_pos: + - 0.0 + - 0.0 + - -0.05 + rigid_object: + - uid: cube + shape: + shape_type: Cube + size: + - 0.06 + - 0.04 + - 0.04 + body_type: dynamic + init_pos: + - -0.42 + - -0.08 + - 0.02 + attrs: + mass_props: + mass: 0.05 + rigid_props: + linear_damping: 0.2 + angular_damping: 0.2 + material_props: + dynamic_friction: 0.97 + static_friction: 0.99 + rigid_object_group: [] + articulation: [] +env: + sim_steps_per_control: 4 + events: {} + dataset: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/expansion.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/expansion.yaml new file mode 100644 index 0000000000..73e8e9df09 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/expansion.yaml @@ -0,0 +1,295 @@ +schema_version: 1 +engine: + episodes_per_operator: 4 + max_attempts: 64 + max_steps: 2400 + max_wall_time_s: 600.0 + settling_steps: 30 + capture_media: false + collision_backend: newton + collision_device: cpu +scene: + cube: + translation_m: + - - -0.06 + - 0.06 + - - -0.06 + - 0.06 + - - 0 + - 0 + yaw_rad: + - 0 + - 0 + goal: + translation_m: + - - -0.06 + - 0.06 + - - -0.06 + - 0.06 + - - 0 + - 0 + yaw_rad: + - 0 + - 0 +trajectory: + operators: + - object_pose + joint_offset_scale: 0.005 + via_count: 1 + max_proposals: 64 + minimum_joint_delta: 0.002 + velocity_limit: 0.5 + acceleration_limit: 0.5 + nullspace_task_rows: + - 0 + - 1 + - 2 + - 3 + - 4 + - 5 + nullspace_translation_tolerance_m: 0.001 + nullspace_rotation_tolerance_rad: 0.01 + free_phases: + place: + - retract +validation: + initial_checks: + - kind: support_bounds + entity: cube + lower: + - -1 + - -0.4 + - -0.003 + upper: + - -0.2 + - 0.7 + - 0.05 + final_checks: + - &id001 + kind: pose_relative + entity: cube + other: goal + position_tolerance: + - 0.02 + - 0.02 + - 0.01 + yaw_tolerance_rad: 0.17453292519943295 + yaw_period_rad: 3.141592653589793 + tilt_tolerance_rad: 0.17453292519943295 + position_offset: + - 0 + - 0 + - 0.12 + - &id002 + kind: joint_range + joint: gripper_finger1_joint_1 + lower: -0.001 + upper: 0.005 + - &id003 + kind: speed_below + entity: cube + linear_m_s: 0.03 + angular_rad_s: 0.2 + - kind: transport_guard + entity: cube + other: goal + lift_height_m: 0.05 + minimum_transit_height_m: 0.025 + goal_tolerance_m: 0.03 + events: + - event_id: lift + conditions: + - kind: lifted + entity: cube + height_m: 0.05 + - event_id: placed_and_released + conditions: + - *id001 + - *id002 + - *id003 + consecutive_steps: 3 + allowed_contacts: &id004 + - - cube + - default_plane + - - cube + - support_surface + - - cube + - UR5/gripper_finger1_link_1 + - - cube + - UR5/gripper_finger2_link_1 + mounting_contacts: + - - '@root' + - default_plane + - - '@root' + - support_surface +scene_sampling: + strategy: scene_first + workspace_source: joint_uniform_fk + control_part: arm + sample_count: 16384 + robot_anchors: + - phase_id: grasp + control_part: arm + uniform_fraction: 0.25 + max_candidates: 256 + max_rollouts: 4 + max_ik_calls: 16384 + max_planning_calls: 2048 + targets: {} + layout: + support_uid: support_surface + parent_by_uid: + cube: support_surface + policy: + group_targets: + - cube + batch_size: 1 + collision_margin_m: 0.02 + group_xy_offset_bounds_m: + cube: + - - -0.06 + - -0.06 + - - 0.06 + - 0.06 + task_mode: + mode_id: runtime + phases: + - phase_id: approach + dependencies: [] + targets: + - control_part: arm + frame_uid: cube + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.1 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id004 + - phase_id: grasp + dependencies: + - approach + targets: + - control_part: arm + frame_uid: cube + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.0 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id004 + - phase_id: lift + dependencies: + - grasp + targets: + - control_part: arm + frame_uid: cube + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.1 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id004 + held_objects: + - entity_uid: cube + control_part: arm + tcp_from_object: + - - 1 + - 0 + - 0 + - 0 + - - 0 + - -1 + - 0 + - 0 + - - 0 + - 0 + - -1 + - 0 + - - 0 + - 0 + - 0 + - 1 + - phase_id: place + dependencies: + - lift + targets: + - control_part: arm + frame_uid: goal + pose: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - -1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - -1.0 + - 0.12 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + allowed_contacts: *id004 + held_objects: + - entity_uid: cube + control_part: arm + tcp_from_object: + - - 1 + - 0 + - 0 + - 0 + - - 0 + - -1 + - 0 + - 0 + - - 0 + - 0 + - -1 + - 0 + - - 0 + - 0 + - 0 + - 1 +scheduling: + policy: fifo + pool_size: 8 + exploration_fraction: 0.25 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/integration.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/integration.yaml new file mode 100644 index 0000000000..baa11842f9 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/integration.yaml @@ -0,0 +1,47 @@ +integration_id: workspace_pose_pick_place_v1 +program_id: workspace_pose_pick_place +requires: + scene_contract: workspace_pose_pick_place_scene_v1 + embodiment_contract: single_arm_parallel_gripper +scene_binding: + contract_id: workspace_pose_pick_place_scene_v1 + registry_id: task_program_workspace_pose_pick_place + rigid_objects: + - entity_id: cube + dynamics: dynamic + semantic_type: cube + affordances: + - entity_id: cube_grasp + kind: antipodal_grasp + - entity_id: goal + dynamics: static + semantic_type: placement_reference +profile: + defaults: + pick_up: + primary: primary_manipulator + place: + primary: primary_manipulator + skill_options: + pick: + kind: pick_up + fixed_object_to_eef: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - -1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - -1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + place: + kind: place + effect_monitors: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/program.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/program.yaml new file mode 100644 index 0000000000..610056960c --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/program.yaml @@ -0,0 +1,44 @@ +program_id: workspace_pose_pick_place +instruction: Pick up the cuboid and place it at the scene-relative goal pose. +targets: + drop_pose: + kind: scene_relative + entity: goal + frame: parent + position: + - 0 + - 0 + - 0.12 + quaternion_xyzw: + - 0 + - 0 + - 0 + - 1 +program: + kind: repeat + count: 1 + body: + kind: sequence + items: + - kind: segment + name: pick_cube + instruction: Grasp and lift the cube. + steps: + kind: invoke + call: + kind: pick + object: cube + - kind: segment + name: place_cube + instruction: Place the cube at the next target position and release it. + steps: + kind: invoke + call: + kind: place + object: cube + at: + kind: target_ref + target: drop_pose + post: + - kind: wait_stable + entity: cube diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/task.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/task.yaml new file mode 100644 index 0000000000..73df857927 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/task.yaml @@ -0,0 +1,19 @@ +id: ConditionalScenePickPlace-UR5-v1 +environment: + component: env.yaml +task_program: + program: program.yaml + integration: integration.yaml + execution_policy: ../../../../../components/execution_policies/trajectory_open_loop.yaml +embodiment: + component: ../../../../../components/embodiments/ur5_dh_pgi_140_80.yaml + overrides: + init_qpos: + - 0 + - -1.57 + - 1.57 + - -1.57 + - -1.57 + - 0 + - 0 + - 0 diff --git a/scripts/benchmark/expansion/README.scene_first.md b/scripts/benchmark/expansion/README.scene_first.md new file mode 100644 index 0000000000..655dcc7f5a --- /dev/null +++ b/scripts/benchmark/expansion/README.scene_first.md @@ -0,0 +1,101 @@ +# Conditional layout sampling acceptance + +Run the three native task fixtures with: + +```bash +python -m scripts.benchmark.expansion.scene_first --pilot --seeds 7 --rollouts 2 --candidates 8 --output outputs/layout-pilot +python -m scripts.benchmark.expansion.scene_first --seeds 7,18,29,40 --rollouts 6 --candidates 48 --seconds 300 --output outputs/layout-comparison +python -m scripts.benchmark.expansion.scene_first --output outputs/layout-comparison --rebuild +``` + +The collector accepts four strategies for layout comparisons. + +| `scene_sampling.strategy` | Proposal | +| --- | --- | +| `bounded_pose` | Draw each declared independent pose in its bounds. A comparison opts into the same layout and task-path screening by providing `layout` and `task_mode`. | +| `scene_first` | Generate the declared layout before checking the robot's task path. | +| `robot_first` | Use a workspace sample to anchor one support group, then generate the remaining layout. | +| `bidirectional` | Alternate scene-first and robot-first by raw ordinal under the same candidate budget. Seed parity chooses the first direction. | + +The legacy default stays `bounded_pose`. Use `--cases` and `--strategies` to +select subsets. The four-seed run balances the order within each task and uses +one raw draw per attempt. The report merges coverage cells across seeds and +retains per-seed timings, rejected proposals and source counts in `results.json`. + +`layout.support_uid` and `parent_by_uid` select real prepared rigid entities and +an acyclic support tree. The shared scene generator prepares scaled geometry and +conservative support/clearance data once. A proposal fixes optional arena-space +`layout.conditions`, then samples the rest of the allowed layout. It does not +step physics, resize a World or mutate runtime asset topology. It preserves +complete support groups rather than independently mixing their coordinates. + +Current group roots support XY translation with authored rotation/height; +member XY/yaw requires an explicitly declared intra-group policy. Root yaw or +Z expansion is rejected rather than silently ignored. Exact member anchors +require their moving ancestors to be anchored. Anchors restrict an existing +domain, not create a new placement permission. No-condition layouts retain the +old addressed RNG stream and continuation behavior. `SceneLayoutCondition` +is also available directly in `embodichain.lab.sim.scene.expansion`. + +Robot-first and bidirectional jobs declare `scene_sampling.robot_anchors`. Each +entry selects a `phase_id` and `control_part` from `task_mode`. The target must +refer to an existing, movable layout root or to a member rigidly attached to one. +Earlier task phases must leave that relation intact. For example, the sorting +fixture alternates the right-arm red pickup and left-arm blue pickup as anchors. + +The proposal source prepares `sample_count` FK samples per selected control +part, from the declared `workspace_source` (`cache` or `joint_uniform_fk`). +It projects each TCP's XY through the target's authored offset to the initial +group, intersects task and layout bounds, and balances selection across +occupied XY cells. The room occupied by the anchor is reserved during layout +completion. An empty prior spends one raw proposal and rejects that draw. The +next draw in the bidirectional stream still follows its assigned direction. +Height and orientation come from the authored root. The projected pose goes +through full-pose IK and task-path collision screening. + +`task_mode` uses existing TaskOperationMode/TaskPhase declarations. Targets name +control parts explicitly and can describe multiple simultaneous arms. Held-object +transforms belong to each incoming phase and must match the intended task; they +are never inferred from a name. The existing feasibility backend selects current +or cached workspace seeds, checks IK, current joint limits and full-pose FK +residuals, and searches collision-checked full-state phase connections. A cache +miss is not a reachability rejection; bounded direct seeds remain available. +A no-solution result means this attempt exhausted its declared search, not proof +that the complete region is unreachable. Every actual Task Program still grounds +and plans again from the measured prepared scene. + +`screening` bounds beam_width, ik_seeds, max_rrt_nodes and planning_timeout_s; +it changes preflight effort, not acceptance thresholds. All raw proposals, +discarded candidates and IK/planning calls consume the declared budgets. Failed +layout or task screening remains preparation rejection and never earns physical +success or a sink receipt. The B=1 path is explicit; batch settings reject before +World construction. Collision queries own no physical World and are released on +normal completion and startup failures. A topology revision invalidates the +prepared source instead of silently retaining stale geometry. + +The fixtures reuse native Task Program execution, physical geometry, controllers, +segment validators and measured terminal/event acceptance. The grab/place and +stack fixtures use a rectangular primitive support with the existing UR5 +embodiment. The dual-arm RGB sorting fixture reuses its original program and +dual-UR5 profile: right arm moves red, left arm moves blue, with the green +reference fixed. The arms act sequentially; this does not measure simultaneous +bimanual execution. Its original 5 cm lift and 3.5 / 3 cm XY placement bounds +remain unchanged. Independent acceptance additionally checks both release +states, terminal heights/speeds and the stationary green reference. All +movable topology and physical parameters stay fixed. Only declared XY layouts +vary in the controlled run. Both source strategies share the same task ranges, +path checks, physical thresholds and persistence gate. An initial geometry/path +pass never substitutes physical task completion. + +Reports keep proposed/prepared/executed/accepted/persisted counts separately, +record full failed attempts and numerical T+1 evidence, and audit read-back from +confirmed episode artifacts. Declared scene-grid coverage is not a certified +continuous feasible-volume fraction. Task pose diversity is counted separately; +each fixed sorting placement contributes one category rather than novelty from +tracking noise. Both manipulated blocks contribute initial/placement coverage. +For each strategy the report shows cold wall time per seed and per receipt, +including World preparation, screening, execution and export. The separate +screening figure helps identify where time was spent. + +The acceptance path rejects unsupported phases or geometry and uses the original +task validation thresholds. RGB sorting exercises both arms in the comparison. diff --git a/scripts/benchmark/expansion/scene_first.py b/scripts/benchmark/expansion/scene_first.py new file mode 100644 index 0000000000..e63342f3b8 --- /dev/null +++ b/scripts/benchmark/expansion/scene_first.py @@ -0,0 +1,635 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Compare four layout proposal strategies on three native Task Programs. + +Run: python -m scripts.benchmark.expansion.scene_first --output DIR +Use --pilot first. Each case uses a fresh process, Default CPU physics, the +same full-state geometric/task-path screens and measured acceptance/receipts. +""" + +from __future__ import annotations + +import argparse +from collections import Counter, defaultdict +import hashlib +import importlib.metadata +import json +import os +from pathlib import Path +import resource +from statistics import median +import subprocess +import sys +import time +import numpy as np +import yaml + +ROOT = Path(__file__).resolve().parents[3] +TASKS = ROOT / "extensions/embodichain_tasks/configs/tasks/manipulation" +CASES = { + "pick_place": TASKS / "workspace_pose_pick_place/acceptance/scene_first", + "stack": TASKS / "tableware/stack_blocks_two/acceptance/scene_first", + "dual_arm_sort": TASKS / "tableware/blocks_ranking_rgb/acceptance/scene_first", +} +STRATEGIES = ("bounded_pose", "scene_first", "robot_first", "bidirectional") +PLACEMENTS = { + "pick_place": {"cube": ("goal", (0.0, 0.0))}, + "stack": {"block_2": ("block_1", (0.0, 0.0))}, + "dual_arm_sort": { + "block_1": ("block_2", (-0.08, 0.0)), + "block_3": ("block_2", (0.08, 0.0)), + }, +} + + +def _write(path, value): + path.write_text(json.dumps(value, indent=2, default=str) + "\n") + + +def _xy_cell(pose, origin, bounds): + """Four fixed XY bins per authored nonzero axis; never fit bins to results.""" + delta = np.asarray(pose)[:2] - np.asarray(origin)[:2] + lo, hi = np.array(bounds)[:2, 0], np.array(bounds)[:2, 1] + if np.any(delta < lo - 1e-5) or np.any(delta > hi + 1e-5): + return None + return tuple( + min(3, max(0, int(np.floor(4 * (v - a) / (b - a))))) if b > a else 0 + for v, a, b in zip(delta, lo, hi) + ) + + +def _confirmed_episodes(root): + """Read only receipt-confirmed evidence; a staged NPZ is not a commit.""" + from embodichain.lab.expansion.persistence import ( + ArtifactEpisodeSink, + load_expansion_episode, + ) + + if not root.exists(): + return () + return tuple( + load_expansion_episode(receipt.storage_id) + for receipt in ArtifactEpisodeSink(root).inspect() + if receipt.confirmed + ) + + +def _task_pose_cells(episode, case, cfg, nominal): + """Count each manipulated object on frozen initial and placement domains.""" + cells = set() + for uid, (goal, offset) in PLACEMENTS[case].items(): + values = episode.observations[f"rigid/{uid}/state"].cpu().numpy() + initial_uid = cfg["scene"][uid].get("follow", uid) + initial = _xy_cell( + values[0], nominal[uid], cfg["scene"][initial_uid]["translation_m"] + ) + if initial is not None: + cells.add((uid, "initial", *initial)) + bounds = cfg["scene"].get(goal, {}).get("translation_m", [[0, 0]] * 3) + if all(pair[0] == pair[1] for pair in bounds[:2]): + # A fixed target contributes one category after measured acceptance; + # tracking error inside its tolerance cannot create pose diversity. + cells.add((uid, "placed-fixed")) + else: + origin = np.asarray(nominal[goal], dtype=float).copy() + origin[:2] += offset + placed = _xy_cell(values[-1], origin, bounds) + if placed is not None: + cells.add((uid, "placed", *placed)) + return cells + + +def _strategy_order(strategies: list[str], seed_index: int) -> list[str]: + """Balance position and preceding strategy over four seeds in the full comparison.""" + width = len(strategies) + order = [0, *(i // 2 + 1 if i % 2 else width - i // 2 for i in range(1, width))] + return [strategies[(index + seed_index) % width] for index in order] + + +def _source_breakdown(events, checks): + """Attribute screened and executed outcomes to the actual raw source.""" + origins = defaultdict(Counter) + selections = {} + rejected = Counter() + for e in events: + if e["event"] == "proposal": + source = origins[e["origin"]] + source["proposed"] += 1 + source["screened"] += int(e["screened"]) + if not e["screened"]: + rejected[e["reason"]] += 1 + elif e["origin"] == "robot_first": + anchor = checks["workspace_anchor_checks"][str(e["raw_ordinal"])] + actual = e["pose_changes"][anchor["root_uid"]] + if not np.allclose(actual, anchor["pose"], atol=1e-9, rtol=0): + raise ValueError( + "Conditioned layout did not preserve the workspace anchor" + ) + elif e["event"] == "selection": + selections[e["ordinal"]] = e["origin"] + elif e["event"] == "outcome": + source = origins[selections[e["ordinal"]]] + for key in ("executed", "physical_accepted", "committed"): + source[key] += int(e[key]) + return {k: dict(v) for k, v in origins.items()}, dict(rejected) + + +def worker(args): + """Execute one isolated source/case/seed with complete failure denominators.""" + sys.path[:0] = [ + str(ROOT), + str(ROOT / "extensions/embodichain_tasks"), + str(ROOT / "extensions/embodichain_robots"), + ] + from embodichain.lab.expansion.task_program_jobs import run_task_program_expansion + + directory = args.output + directory.mkdir(parents=True, exist_ok=False) + cfg = yaml.safe_load((CASES[args.case] / "expansion.yaml").read_text()) + cfg["scene_sampling"]["strategy"] = args.strategy + cfg["engine"].update( + episodes_per_operator=args.rollouts, + max_attempts=args.candidates, + max_wall_time_s=args.seconds, + ) + cfg["scene_sampling"].update( + max_candidates=args.candidates, max_rollouts=args.rollouts + ) + # One raw draw per attempt across all four variants. The screening budgets + # and measured acceptance remain those declared by the fixture. + cfg["scheduling"]["pool_size"] = 1 + if args.pilot: + for value in cfg["scene"].values(): + if "follow" not in value: + value["translation_m"] = [[0, 0]] * 3 + for key in cfg["scene_sampling"]["layout"]["policy"][ + "group_xy_offset_bounds_m" + ]: + cfg["scene_sampling"]["layout"]["policy"]["group_xy_offset_bounds_m"][ + key + ] = [[0, 0], [0, 0]] + config = directory / "expansion.yaml" + config.write_text(yaml.safe_dump(cfg, sort_keys=False)) + files = subprocess.check_output( + ["git", "ls-files", "--cached", "--others", "--exclude-standard"], + cwd=ROOT, + text=True, + ).splitlines() + paths = [ + p + for p in files + if p.startswith( + ( + "embodichain/lab/expansion/", + "embodichain/lab/sim/scene/", + "embodichain/compute/geometry/", + "extensions/embodichain_tasks/configs/tasks/manipulation/", + ) + ) + and (ROOT / p).is_file() + ] + paths.append(str(Path(__file__).resolve().relative_to(ROOT))) + _write( + directory / "source-manifest.json", + {p: hashlib.sha256((ROOT / p).read_bytes()).hexdigest() for p in paths}, + ) + started = time.perf_counter() + result = None + error = None + try: + result = run_task_program_expansion( + CASES[args.case] / "task.yaml", + config, + directory / "collection", + seed=args.seed, + ) + except Exception as exc: + error = f"{type(exc).__name__}: {exc}" + import traceback + + traceback.print_exc() + run_wall_s = time.perf_counter() - started + root = directory / "collection/dataset/object_pose" + source = root / "pose-sampling.json" + data = json.loads(source.read_text()) if source.exists() else {} + events = data.get("events", []) + outcomes = [e for e in events if e["event"] == "outcome"] + screen_path = root / "scene-first-checks.json" + screens = json.loads(screen_path.read_text()) if screen_path.exists() else {} + origins, proposal_rejections = _source_breakdown(events, screens) + failures = {} + for path in root.glob("attempts/*/attempt.json"): + d = json.loads(path.read_text()) + if d.get("status") != "committed": + key = d.get("reason", d.get("status", "unavailable")) + failures[key] = failures.get(key, 0) + 1 + nominal_path = directory / "collection/reference-scene.json" + nominal = ( + json.loads(nominal_path.read_text())["poses_xyzw"] + if nominal_path.exists() + else {} + ) + independent = [uid for uid, opts in cfg["scene"].items() if "follow" not in opts] + joint = set() + marginal = set() + task = set() + audits = [] + committed = 0 + for episode in _confirmed_episodes(root): + committed += 1 + row = [] + for uid in independent: + states = episode.observations[f"rigid/{uid}/state"] + cell = _xy_cell( + states[0].cpu().numpy(), + nominal[uid], + cfg["scene"][uid]["translation_m"], + ) + if cell is not None: + marginal.add((uid, *cell)) + row.append((uid, cell)) + if all(cell is not None for _, cell in row): + joint.add(tuple((uid, *cell) for uid, cell in row)) + task.update(_task_pose_cells(episode, args.case, cfg, nominal)) + steps = len(episode.actions) + if ( + not episode.validation.accepted + or len(episode.timestamps) != steps + 1 + or any(len(v) != steps + 1 for v in episode.observations.values()) + or episode.metadata["source_env_id"] != 0 + ): + raise ValueError("Invalid measured episode evidence at confirmed readback") + arm_motion = {} + if args.case == "dual_arm_sort": + q = episode.observations["full_joint_positions"].cpu().numpy() + names = episode.metadata["joint_names"] + for arm in ("left", "right"): + columns = [ + i for i, name in enumerate(names) if name.startswith(f"{arm}_joint") + ] + arm_motion[arm] = float(np.ptp(q[:, columns], axis=0).max()) + if min(arm_motion.values()) <= 0.1: + raise ValueError( + "Dual-arm sorting requires measured motion in both arms" + ) + audits.append( + { + "episode_id": episode.episode_id, + "T": len(episode.actions), + "observations": len(episode.timestamps), + "source_row": episode.metadata["source_env_id"], + "validation": episode.validation.accepted, + "arm_joint_range_rad": arm_motion, + } + ) + slots_per_uid = { + uid: int( + np.prod( + [ + 4 if pair[1] > pair[0] else 1 + for pair in cfg["scene"][uid]["translation_m"][:2] + ] + ) + ) + for uid in independent + } + metric = { + "case": args.case, + "strategy": args.strategy, + "seed": args.seed, + "pilot": args.pilot, + "budget": data.get("budget", {}), + "declared_budget": { + "candidates": args.candidates, + "rollouts": args.rollouts, + "seconds": args.seconds, + }, + "cpu_affinity": sorted(os.sched_getaffinity(0)), + "cuda_visible_devices": os.environ.get("CUDA_VISIBLE_DEVICES"), + "execution": ( + json.loads((directory / "collection/execution.json").read_text()) + if (directory / "collection/execution.json").exists() + else None + ), + "error": error, + "wall_s": run_wall_s, + "peak_rss_mib": resource.getrusage(resource.RUSAGE_SELF).ru_maxrss / 1024, + "executed": sum(bool(e["executed"]) for e in outcomes), + "physically_accepted": sum(bool(e["physical_accepted"]) for e in outcomes), + "committed": committed, + "scene_joint_cells": len(joint), + "scene_joint_declared_cells": int(np.prod(list(slots_per_uid.values()))), + "scene_marginal_cells": len(marginal), + "scene_marginal_declared_cells": sum(slots_per_uid.values()), + "task_pose_cells": len(task), + "coverage_cells": { + "scene_joint": sorted(joint), + "scene_marginal": sorted(marginal), + "task": sorted(task), + }, + "preparation_rejected": sum(not e["executed"] for e in outcomes), + "proposal_rejections": proposal_rejections, + "origins": origins, + "workspace_prior": screens.get("workspace_prior", []), + "workspace_prepare_wall_s": screens.get("workspace_prepare_wall_s"), + "proposal_screen_wall_s": screens.get("proposal_screen_wall_s"), + "failures": failures, + "episode_audit": audits, + "versions": { + name: importlib.metadata.version(name) + for name in ["torch", "dexsim-engine"] + }, + "result": result, + } + _write(directory / "metrics.json", metric) + print(json.dumps(metric), flush=True) + if error: + raise SystemExit(1) + + +def _aggregate(rows): + """Combine unique receipt-confirmed coverage across seeds in a fixed domain.""" + groups = defaultdict(list) + for row in rows: + groups[row["case"], row["strategy"]].append(row) + result = [] + for (case, strategy), runs in sorted(groups.items()): + origins = defaultdict(Counter) + for run in runs: + for origin, values in run.get("origins", {}).items(): + origins[origin].update(values) + coverage = {} + for kind in ("scene_joint", "scene_marginal", "task"): + if all("coverage_cells" in run for run in runs): + coverage[kind] = len( + { + json.dumps(cell) + for run in runs + for cell in run["coverage_cells"][kind] + } + ) + else: + coverage[kind] = None + totals = { + k: sum(run[k] for run in runs) + for k in ( + "executed", + "physically_accepted", + "committed", + "preparation_rejected", + ) + } + proposed = [run["budget"].get("candidates") for run in runs] + result.append( + dict( + case=case, + strategy=strategy, + seeds=[r["seed"] for r in runs], + raw_proposals=( + None if any(v is None for v in proposed) else sum(proposed) + ), + median_wall_s=median(run["wall_s"] for run in runs), + total_wall_s=sum(run["wall_s"] for run in runs), + median_screen_s=median( + [ + run["proposal_screen_wall_s"] + for run in runs + if run.get("proposal_screen_wall_s") is not None + ] + or [0.0] + ), + peak_rss_mib=max(run["peak_rss_mib"] for run in runs), + scene_joint_declared=max( + run["scene_joint_declared_cells"] for run in runs + ), + scene_marginal_declared=max( + run["scene_marginal_declared_cells"] for run in runs + ), + coverage=coverage, + origins={key: dict(val) for key, val in origins.items()}, + failed_runs=[ + f'{run["seed"]}: {run["error"]}' for run in runs if run["error"] + ], + **totals, + ) + ) + return result + + +def report(output, rows): + """Write aggregated comparisons and preserve seed-level results separately.""" + aggregate = _aggregate(rows) + _write(output / "results.json", rows) + _write(output / "aggregate.json", aggregate) + lines = [ + "# Conditional layout sampling across three tasks", + "", + "Default CPU physics, B=1, sensors disabled. Each strategy uses the same", + "task domains, layout and path checks, runtime replanning, physical", + "acceptance and artifact/LeRobot export. CUDA grasp classification may be", + "used. Cold wall time covers the expansion call, including initialization", + "and export, excluding process startup and benchmark readback. RSS is the", + "process high-water mark; incremental CPU/GPU allocations are unmeasured.", + "", + "## Time & Memory", + "", + "|Task|Strategy|Seeds|Median cost_time_ms|Median screen s|Wall s / receipt|Peak RSS MiB|cpu_delta_mb|gpu_delta_mb|peak_gpu_mb|", + "|---|---|---:|---:|---:|---:|---:|---|---|---|", + ] + for r in aggregate: + per_episode = ( + f'{r["total_wall_s"]/r["committed"]:.2f}' if r["committed"] else "N/A" + ) + lines.append( + f"|{r['case']}|{r['strategy']}|{len(r['seeds'])}|{r['median_wall_s']*1000:.1f}|" + f"{r['median_screen_s']:.2f}|{per_episode}|{r['peak_rss_mib']:.1f}|N/A|N/A|N/A|" + ) + lines += [ + "", + "## Success & Other Metrics", + "", + "|Task|Strategy|Raw proposals|Prep rejected|Accepted / executed|Committed|Marginal XY cells|Joint XY cells|Task pose cells|Failed runs|", + "|---|---|---:|---:|---:|---:|---:|---:|---:|---:|", + ] + for r in aggregate: + coverage = r["coverage"] + number = lambda x: "N/A" if x is None else str(x) + lines.append( + f"|{r['case']}|{r['strategy']}|{number(r['raw_proposals'])}|{r['preparation_rejected']}|" + f"{r['physically_accepted']}/{r['executed']}|{r['committed']}|" + f"{number(coverage['scene_marginal'])}/{r['scene_marginal_declared']}|" + f"{number(coverage['scene_joint'])}/{r['scene_joint_declared']}|" + f"{number(coverage['task'])}|{len(r['failed_runs'])}|" + ) + lines += [ + "", + "## Leaderboard", + "", + "Order within each task by physical acceptance per executed rollout.", + "", + "|Task|Strategy|Physical acceptance|Confirmed episodes|", + "|---|---|---:|---:|", + ] + for r in sorted( + aggregate, + key=lambda r: ( + r["case"], + -r["physically_accepted"] / r["executed"] if r["executed"] else 1.0, + ), + ): + rate = ( + f"{r['physically_accepted']/r['executed']:.3f}" + if r["executed"] + else "N/A (no rollout)" + ) + lines.append(f"|{r['case']}|{r['strategy']}|{rate}|{r['committed']}|") + lines += [ + "", + "Scene coverage is the union of fixed XY grid cells from receipt-confirmed", + "measured initial layouts. Task cells use measured initial/terminal object", + "positions; each fixed placement is a single category. The counted cells", + "describe observed sample diversity in the fixture domains.", + "See results.json for each seed, proposal failures, source usage, recorded", + "budgets, hardware affinity, and readback audit. Aggregated wall times", + "include all recorded runs, including errors.", + ] + (output / "report.md").write_text("\n".join(lines) + "\n") + + +def main(): + """Run fresh worker processes sequentially, alternating source order by seed.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--worker", action="store_true") + parser.add_argument("--pilot", action="store_true") + parser.add_argument("--rebuild", action="store_true") + parser.add_argument("--cases", default=",".join(CASES)) + parser.add_argument("--strategies", default=",".join(STRATEGIES)) + parser.add_argument("--case", choices=tuple(CASES), default="pick_place") + parser.add_argument("--strategy", choices=STRATEGIES, default="scene_first") + parser.add_argument("--seeds", default="7,17") + parser.add_argument("--seed", type=int, default=7) + parser.add_argument("--rollouts", type=int, default=4) + parser.add_argument("--candidates", type=int, default=64) + parser.add_argument("--seconds", type=float, default=600) + args = parser.parse_args() + args.output = args.output.resolve() + if args.worker: + worker(args) + return + if args.rebuild: + report( + args.output, + [ + json.loads(p.read_text()) + for p in sorted(args.output.rglob("metrics.json")) + ], + ) + return + cases = args.cases.split(",") + strategies = args.strategies.split(",") + if any(case not in CASES for case in cases) or len(set(cases)) != len(cases): + parser.error("Select distinct task cases from " + ",".join(CASES)) + if any(s not in STRATEGIES for s in strategies) or len(set(strategies)) != len( + strategies + ): + parser.error("Select distinct strategies from " + ",".join(STRATEGIES)) + args.output.mkdir(parents=True, exist_ok=False) + rows = [] + for case in cases: + for index, seed in enumerate(map(int, args.seeds.split(","))): + for strategy in _strategy_order(strategies, index): + directory = args.output / f"{case}-{strategy}-{seed}" + command = [ + sys.executable, + str(Path(__file__).resolve()), + "--worker", + "--output", + str(directory), + "--case", + case, + "--strategy", + strategy, + "--seed", + str(seed), + "--rollouts", + str(args.rollouts), + "--candidates", + str(args.candidates), + "--seconds", + str(args.seconds), + ] + if args.pilot: + command += ["--pilot"] + attempt_started = time.perf_counter() + env = { + **os.environ, + "OMP_NUM_THREADS": "1", + "MKL_NUM_THREADS": "1", + "SDL_VIDEODRIVER": "offscreen", + } + with (args.output / (directory.name + ".log")).open("w") as log: + try: + proc = subprocess.run( + command, + cwd=ROOT, + env=env, + stdout=log, + stderr=subprocess.STDOUT, + timeout=args.seconds + 180, + ) + worker_error = f"Worker exited with status {proc.returncode}" + except subprocess.TimeoutExpired: + worker_error = "Worker exceeded the hard process timeout" + metrics = directory / "metrics.json" + if not metrics.exists(): + directory.mkdir(exist_ok=True) + _write( + metrics, + { + "case": case, + "strategy": strategy, + "seed": seed, + "budget": {}, + "declared_budget": { + "candidates": args.candidates, + "rollouts": args.rollouts, + "seconds": args.seconds, + }, + "error": worker_error, + "wall_s": time.perf_counter() - attempt_started, + "peak_rss_mib": 0, + "executed": 0, + "physically_accepted": 0, + "committed": 0, + "scene_joint_declared_cells": 0, + "scene_marginal_declared_cells": 0, + "preparation_rejected": 0, + }, + ) + row = json.loads(metrics.read_text()) + rows.append(row) + report(args.output, rows) + if row["error"]: + print( + f"Worker failed: {directory}; diagnostics retained", flush=True + ) + print(args.output / "report.md", flush=True) + + +if __name__ == "__main__": + main() diff --git a/tests/benchmark/expansion/test_scene_first_report.py b/tests/benchmark/expansion/test_scene_first_report.py new file mode 100644 index 0000000000..5bf5e69be2 --- /dev/null +++ b/tests/benchmark/expansion/test_scene_first_report.py @@ -0,0 +1,161 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Receipt and frozen-domain semantics for cross-task scene acceptance reports.""" + +from __future__ import annotations +from types import SimpleNamespace +import numpy as np +import torch +from scripts.benchmark.expansion.scene_first import ( + STRATEGIES, + _aggregate, + _confirmed_episodes, + _strategy_order, + _task_pose_cells, + _xy_cell, +) + + +def test_each_strategy_runs_once_at_each_position_and_after_each_peer(): + strategies = list(STRATEGIES) + orders = [_strategy_order(strategies, i) for i in range(4)] + assert all(set(column) == set(strategies) for column in zip(*orders)) + assert len({pair for order in orders for pair in zip(order, order[1:])}) == 12 + + +def test_cross_seed_coverage_counts_each_measured_cell_once(): + run = dict( + case="stack", + strategy="bidirectional", + executed=2, + physically_accepted=2, + committed=2, + preparation_rejected=1, + budget={"candidates": 3}, + wall_s=30.0, + peak_rss_mib=2000, + scene_joint_declared_cells=256, + scene_marginal_declared_cells=32, + error=None, + ) + shared = ["a", 0, 0] + rows = [ + dict( + run, + seed=seed, + coverage_cells={ + "scene_joint": [[shared, ["b", seed, 0]]], + "scene_marginal": [shared, ["b", seed, 0]], + "task": [["a", "initial", 0, 0], ["a", "placed", seed, 0]], + }, + ) + for seed in (1, 2) + ] + (result,) = _aggregate(rows) + assert result["coverage"] == {"scene_joint": 2, "scene_marginal": 3, "task": 3} + assert (result["committed"], result["raw_proposals"]) == (4, 6) + + +def test_staged_numerical_file_without_receipt_has_no_coverage_credit(tmp_path): + folder = tmp_path / "episodes" / "staged" + folder.mkdir(parents=True) + (folder / "episode.npz").write_bytes(b"not committed") + assert _confirmed_episodes(tmp_path) == () + + +def test_confirmed_readback_uses_verified_receipt_storage(monkeypatch, tmp_path): + from unittest.mock import Mock + import embodichain.lab.expansion.persistence as persistence + + episode = object() + load = Mock(return_value=episode) + monkeypatch.setattr(persistence, "load_expansion_episode", load) + monkeypatch.setattr( + persistence.ArtifactEpisodeSink, + "inspect", + lambda self: ( + SimpleNamespace(confirmed=True, storage_id="confirmed.npz"), + SimpleNamespace(confirmed=False, storage_id="pending.npz"), + ), + ) + assert _confirmed_episodes(tmp_path) == (episode,) + load.assert_called_once_with("confirmed.npz") + + +def test_xy_bins_are_fixed_and_reject_outside_domain(): + bounds = [[-0.1, 0.1], [-0.1, 0.1], [0, 0]] + assert _xy_cell([-0.1, -0.1, 0], [0, 0, 0], bounds) == (0, 0) + assert _xy_cell([0.1, 0.1, 0], [0, 0, 0], bounds) == (3, 3) + assert _xy_cell([0.11, 0, 0], [0, 0, 0], bounds) is None + assert _xy_cell([0, 0, 0], [0, 0, 0], [[0, 0], [0, 0], [0, 0]]) == (0, 0) + + +def test_both_arms_count_but_fixed_goal_tracking_noise_adds_no_diversity(): + nominal = { + "block_1": [0.625, -0.15, 0.86], + "block_2": [0.675, 0, 0.86], + "block_3": [0.725, 0.15, 0.86], + } + cfg = { + "scene": { + uid: {"translation_m": [[-0.025, 0.025], [-0.03, 0.03], [0, 0]]} + for uid in ("block_1", "block_3") + } + } + episode = SimpleNamespace( + observations={ + "rigid/block_1/state": torch.tensor( + [[0.62, -0.155, 0.86], [0.59, -0.005, 0.86]] + ), + "rigid/block_3/state": torch.tensor( + [[0.72, 0.145, 0.86], [0.76, 0.005, 0.86]] + ), + } + ) + cells = _task_pose_cells(episode, "dual_arm_sort", cfg, nominal) + assert cells == { + ("block_1", "initial", 1, 1), + ("block_3", "initial", 1, 1), + ("block_1", "placed-fixed"), + ("block_3", "placed-fixed"), + } + for value in episode.observations.values(): + value[-1, :2] += 0.01 + assert _task_pose_cells(episode, "dual_arm_sort", cfg, nominal) == cells + + +def test_relative_placement_offsets_use_the_reference_sampling_domain(): + nominal = { + "block_1": [0.625, -0.15, 0.86], + "block_2": [0.675, 0, 0.86], + "block_3": [0.725, 0.15, 0.86], + } + bounds = [[-0.025, 0.025], [-0.03, 0.03], [0, 0]] + cfg = {"scene": {uid: {"translation_m": bounds} for uid in nominal}} + episode = SimpleNamespace( + observations={ + "rigid/block_1/state": torch.tensor( + [nominal["block_1"], [0.575, -0.02, 0.86]] + ), + "rigid/block_3/state": torch.tensor( + [nominal["block_3"], [0.775, 0.02, 0.86]] + ), + } + ) + cells = _task_pose_cells(episode, "dual_arm_sort", cfg, nominal) + assert ("block_1", "placed", 0, 0) in cells + assert ("block_3", "placed", 3, 3) in cells diff --git a/tests/lab/expansion/test_scene_first_sampling.py b/tests/lab/expansion/test_scene_first_sampling.py new file mode 100644 index 0000000000..6564b3eb9d --- /dev/null +++ b/tests/lab/expansion/test_scene_first_sampling.py @@ -0,0 +1,428 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""All layout sources share conservative screening and own selected poses.""" + +from __future__ import annotations +from types import SimpleNamespace +from copy import deepcopy +import json +import numpy as np +import pytest +import torch + +from scene_layout_support import make_source +from embodichain.lab.expansion._scene_first_sampling import _LayoutPoseSource +from embodichain.lab.expansion._pose_sampling_cfg import validate_sampling +from embodichain.lab.sim.scene.feasibility import FeasibilityBudgetExhausted + + +def setup_source( + monkeypatch, + strategy="scene_first", + accepted=True, + member_yaw=False, + workspace=None, + bounded_fixed_root=False, +): + import embodichain.lab.expansion._scene_first_sampling as module + + source = make_source() + + class Object: + def __init__(self, uid): + self.uid = uid + self.cfg = SimpleNamespace(to_dict=lambda: {"uid": uid}) + + def get_local_pose(self, to_matrix=True): + return torch.tensor(np.array(source.poses[self.uid]))[None] + + objects = {uid: Object(uid) for uid in source.poses} + robot = SimpleNamespace( + uid="robot", + device=torch.device("cpu"), + control_parts={"custom_arm": [0]}, + cfg=SimpleNamespace(to_dict=lambda: {"uid": "robot"}), + get_local_pose=lambda to_matrix=True: torch.eye(4)[None], + get_solver=lambda name: object(), + get_qpos_limits=lambda name: torch.tensor([[[0.0, 1.0]]]), + compute_batch_fk=lambda **kw: workspace[None], + ) + env = SimpleNamespace( + num_envs=1, + robot=robot, + sim=SimpleNamespace( + num_envs=1, + get_rigid_object_uid_list=lambda: tuple(objects), + get_rigid_object=objects.get, + sim_config=SimpleNamespace( + physics_cfg=SimpleNamespace(to_dict=lambda: {"backend": "default"}) + ), + ), + ) + checked = [] + closed = [] + monkeypatch.setattr(module, "scene_layout_input_from_sim", lambda *a, **kw: source) + monkeypatch.setattr( + "embodichain.lab.task_program.integrations.simulation.feasibility_geometry.collision_bodies_from_simulation", + lambda env: (), + ) + monkeypatch.setattr( + "embodichain.lab.expansion.acceptance._GeometryRow.contact_permissions", + lambda *a: ((), ()), + ) + monkeypatch.setattr( + "embodichain.lab.sim.scene.feasibility.NewtonCollisionChecker", + lambda *a, **kw: SimpleNamespace(close=lambda: closed.append(True)), + ) + monkeypatch.setattr( + "embodichain.lab.sim.scene.feasibility.RobotFeasibilityBackend", + lambda *a, **kw: None, + ) + + def screen(mode, backend, poses, budget): + checked.append(deepcopy(poses)) + return SimpleNamespace( + accepted=accepted, + checks=( + SimpleNamespace( + stage="motion", + status="passed" if accepted else "failed", + reason="checked task path", + phase_id="reach", + ), + ), + ) + + monkeypatch.setattr("embodichain.lab.sim.scene.feasibility.plan_task_mode", screen) + opts = { + "engine": { + "max_attempts": 8, + "max_wall_time_s": 120.0, + "episodes_per_operator": 1, + }, + "scene": { + "tray": { + "translation_m": [[-0.1, 0.1], [-0.1, 0.1], [0, 0]], + "yaw_rad": [0, 0], + }, + "mug": {"follow": "tray"}, + }, + "validation": {}, + "scene_sampling": { + "strategy": strategy, + "control_part": "custom_arm", + "max_candidates": 64, + "max_rollouts": 8, + "layout": { + "support_uid": "workbench", + "parent_by_uid": dict(source.parent_by_uid), + "policy": { + "group_targets": ["tray"], + "group_xy_offset_bounds_m": {"tray": [[-0.1, -0.1], [0.1, 0.1]]}, + }, + "conditions": {"tray": np.array(source.poses["tray"]).tolist()}, + }, + "task_mode": { + "mode_id": "runtime", + "phases": [ + { + "phase_id": "reach", + "targets": [ + { + "control_part": "custom_arm", + "frame_uid": "mug", + "pose": np.eye(4).tolist(), + } + ], + } + ], + }, + }, + "scheduling": {"policy": "fifo", "pool_size": 4}, + } + if strategy in ("robot_first", "bidirectional"): + sampling = opts["scene_sampling"] + sampling["sample_count"] = len(workspace) + sampling["robot_anchors"] = [ + {"phase_id": "reach", "control_part": "custom_arm"} + ] + sampling["layout"]["conditions"] = { + "workbench": source.poses["workbench"], + } + sampling["task_mode"]["phases"][0]["targets"][0]["pose"][0][3] = 0.04 + opts["scene"]["z_target"] = { + "translation_m": [[-0.1, 0.1], [-0.1, 0.1], [0, 0]], + "yaw_rad": [0, 0], + } + sampling["layout"]["policy"]["group_targets"].append("z_target") + sampling["layout"]["policy"]["group_xy_offset_bounds_m"]["z_target"] = [ + [-0.1, -0.1], + [0.1, 0.1], + ] + if bounded_fixed_root: + opts["scene_sampling"]["layout"]["conditions"] = {} + opts["scene"]["z_target"] = { + "translation_m": [[-0.1, 0.1], [-0.1, 0.1], [0, 0]], + "yaw_rad": [0, 0], + } + if member_yaw: + opts["scene"]["mug"] = { + "translation_m": [[-0.15, 0.15], [-0.15, 0.15], [0, 0]], + "yaw_rad": [0, 0], + } + opts["scene_sampling"]["layout"]["policy"]["intra_group"] = { + "targets": {"mug": {"yaw_delta_range_deg": [30, 30]}} + } + validate_sampling(opts) + return ( + _LayoutPoseSource(env, opts, seed=7, revision="test-source"), + checked, + closed, + ) + + +def test_scene_first_anchors_and_workspace_task_checks_are_owned_and_cached( + monkeypatch, tmp_path +): + source, checked, closed = setup_source(monkeypatch) + source.output = tmp_path + candidate = source.candidate(0) + assert source.validate(0).accepted and len(checked) == 1 + poses = {c.entity_uid: np.array(c.arena_pose) for c in candidate.scene.pose_changes} + np.testing.assert_allclose(poses["tray"], source._conditions.poses["tray"]) + np.testing.assert_allclose(poses["mug"], checked[0]["mug"]) + checked[0]["mug"][0, 3] = 99 + assert candidate is source.candidate(0) + assert source.budget.counts["candidates"] == 1 + assert ( + json.loads((tmp_path / "scene-first-checks.json").read_text())["phase_checks"][ + "0" + ][0]["stage"] + == "motion" + ) + source.close() + source.close() + assert closed == [True] + + +@pytest.mark.parametrize("strategy", ["scene_first", "robot_first", "bidirectional"]) +def test_failed_task_screening_never_becomes_a_successful_fallback( + monkeypatch, strategy +): + source, checked, _ = setup_source( + monkeypatch, strategy=strategy, accepted=False, workspace=_workspace() + ) + source.candidate(0) + assert not source.validate(0).accepted and len(checked) == 4 + assert all(not e["screened"] for e in source._events if e["event"] == "proposal") + + +def _workspace() -> torch.Tensor: + # Tool XY is +4 cm along the support group's X. The first three TCP samples + # share one cell, the fourth is in a different cell, and the last is illegal. + poses = torch.eye(4, dtype=torch.float64).repeat(5, 1, 1) + poses[:, :2, 3] = torch.tensor( + [[-0.309, -0.04], [-0.310, -0.04], [-0.311, -0.04], [-0.19, 0.05], [0.9, 0.0]], + dtype=poses.dtype, + ) + # Keep authored root height and orientation despite the projected FK pose. + poses[:, 2, 3] = 0.7 + poses[:, 1, 1] = poses[:, 2, 2] = -1 + return poses + + +def test_robot_first_balances_prior_cells_and_completes_other_groups(monkeypatch): + workspace = _workspace() + source, checked, _ = setup_source(monkeypatch, "robot_first", workspace=workspace) + workspace.fill_(77) + first = source.candidate(0) + second = source.candidate(1) + assert source.validate(0).accepted and source.validate(1).accepted + assert source.candidate(0) is first + x_coords, other_poses = [], [] + for index, candidate in enumerate((first, second)): + poses = { + c.entity_uid: np.array(c.arena_pose) for c in candidate.scene.pose_changes + } + x_coords.append(poses["tray"][0, 3]) + other_poses.append(poses["z_target"]) + np.testing.assert_allclose(poses["tray"][:3, :3], np.eye(3)) + assert poses["tray"][2, 3] == pytest.approx(0.061) + np.testing.assert_allclose(poses["mug"][:2, 3], poses["tray"][:2, 3]) + np.testing.assert_allclose(checked[index]["tray"], poses["tray"]) + assert min(x_coords) == pytest.approx(-0.35, abs=0.0011) + assert max(x_coords) == pytest.approx(-0.23) + assert not np.allclose(*other_poses) + + +@pytest.mark.parametrize("strategy", ["robot_first", "bidirectional"]) +def test_empty_workspace_draw_costs_a_candidate_and_never_runs_task_screen( + monkeypatch, strategy +): + workspace = _workspace() + workspace[:, 0, 3] = 99 + source, checked, _ = setup_source(monkeypatch, strategy, workspace=workspace) + source.candidate(0) + proposals = [e for e in source._events if e["event"] == "proposal"] + assert not proposals[0]["screened"] + assert proposals[0]["reason"] == "no_workspace_anchor" + if strategy == "robot_first": + assert not source.validate(0).accepted and checked == [] + else: + assert source.validate(0).accepted and len(checked) == 1 + assert [p["origin"] for p in proposals] == ["robot_first", "scene_first"] + assert source._events[-1]["raw_ordinal"] == 1 + assert source.budget.counts["candidates"] == len(proposals) + + +def test_mixed_draws_keep_direction_and_layout_after_checkpoint_restore(monkeypatch): + source, _, _ = setup_source(monkeypatch, "bidirectional", workspace=_workspace()) + first = source.candidate(0) + restored, _, _ = setup_source(monkeypatch, "bidirectional", workspace=_workspace()) + restored.load_state_dict(source.state_dict()) + assert restored.candidate(0) == first + assert restored.candidate(1) == source.candidate(1) + proposals = [e for e in restored._events if e["event"] == "proposal"] + assert [(e["raw_ordinal"], e["origin"]) for e in proposals] == [ + (0, "robot_first"), + (1, "scene_first"), + ] + + +def test_simple_baseline_keeps_identical_task_screening_and_conditions(monkeypatch): + source, checked, _ = setup_source(monkeypatch, strategy="bounded_pose") + # Its simple random draw does not satisfy the exact anchor, and must be + # rejected before any workspace/path computation. + source.candidate(0) + assert not source.validate(0).accepted and checked == [] + assert any(e.get("reason") == "condition_mismatch" for e in source._events) + + +def test_source_obeys_hard_raw_candidate_budget(monkeypatch): + source, checked, _ = setup_source(monkeypatch) + source.budget.charge("candidates", source.cfg["max_candidates"]) + source.candidate(0) + assert not source.validate(0).accepted and checked == [] + assert source.budget.counts["candidates"] == 64 + + +def test_scene_first_config_rejects_batch_and_unknown_phase_fields_before_runtime(): + # Reuse a valid source's owned config, without constructing a new World. + import yaml + from pathlib import Path + + root = Path(__file__).resolve().parents[3] + path = ( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/expansion.yaml" + ) + cfg = yaml.safe_load(path.read_text()) + cfg["engine"]["batch_size"] = 4 + with pytest.raises(ValueError, match="B=1"): + validate_sampling(cfg) + cfg["engine"]["batch_size"] = 1 + cfg["scene_sampling"]["task_mode"]["phases"][0]["unknown"] = True + with pytest.raises(ValueError, match="Unknown TaskPhase"): + validate_sampling(cfg) + + +@pytest.mark.parametrize( + "invalid, error", + [ + ("unknown_stage", "declared phase targets"), + ("moved_frame", "initial frame"), + ("pinned_root", "authored root condition"), + ], +) +def test_robot_anchors_reject_invalid_frame_provenance(invalid, error): + import yaml + from pathlib import Path + + root = Path(__file__).resolve().parents[3] + path = root / ( + "extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/" + "acceptance/scene_first/expansion.yaml" + ) + cfg = yaml.safe_load(path.read_text()) + sampling = cfg["scene_sampling"] + sampling["strategy"] = "robot_first" + if invalid == "unknown_stage": + sampling["robot_anchors"][0]["phase_id"] = "unknown" + elif invalid == "moved_frame": + sampling["task_mode"]["phases"][0]["held_objects"] = [ + {"entity_uid": "cube", "control_part": "arm"} + ] + else: + sampling["layout"]["conditions"] = {"cube": np.eye(4).tolist()} + with pytest.raises(ValueError, match=error): + validate_sampling(cfg) + + +def test_source_rejects_stale_geometry_and_does_not_refresh_silently(monkeypatch): + source, checked, _ = setup_source(monkeypatch) + source.env.sim.spawn_result = SimpleNamespace(topology_revision=2) + source._scene_identity = (id(source.env.sim.spawn_result), 1) + with pytest.raises(RuntimeError, match="stale"): + source.candidate(0) + assert checked == [] + + +def test_fixed_layout_geometry_cannot_be_moved_by_a_bounded_draw(monkeypatch): + source, checked, _ = setup_source( + monkeypatch, strategy="bounded_pose", bounded_fixed_root=True + ) + source.candidate(0) + assert not source.validate(0).accepted and not checked + assert source._events[0]["reason"] == "layout_moves_fixed_entity" + + +def test_member_layout_yaw_cannot_expand_the_task_domain(monkeypatch): + source, checked, _ = setup_source(monkeypatch, member_yaw=True) + source.candidate(0) + assert not source.validate(0).accepted and checked == [] + reasons = [e["reason"] for e in source._events if e["event"] == "proposal"] + assert reasons and set(reasons) == {"layout_outside_task_domain"} + + +def test_screening_must_not_step_the_owning_world(monkeypatch): + source, _, _ = setup_source(monkeypatch) + source.env.sim.physics_step_index = 0 + + def bad_query(*args): + source.env.sim.physics_step_index += 1 + return SimpleNamespace(accepted=True, checks=()) + + monkeypatch.setattr( + "embodichain.lab.sim.scene.feasibility.plan_task_mode", bad_query + ) + with pytest.raises(RuntimeError, match="advanced"): + source.candidate(0) + + +def test_legacy_workspace_strategy_cannot_silently_ignore_new_layout(): + import yaml + from pathlib import Path + + path = ( + Path(__file__).resolve().parents[3] + / "extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/expansion.yaml" + ) + value = yaml.safe_load(path.read_text()) + value["scene_sampling"]["strategy"] = "workspace_joint" + value["scene_sampling"]["targets"] = {"cube": np.eye(4).tolist()} + with pytest.raises(ValueError, match="does not consume layout"): + validate_sampling(value) diff --git a/tests/sim/scene/expansion/test_conditions.py b/tests/sim/scene/expansion/test_conditions.py new file mode 100644 index 0000000000..2496b2bb2a --- /dev/null +++ b/tests/sim/scene/expansion/test_conditions.py @@ -0,0 +1,238 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Anchored layout restrictions preserve geometry, ownership and addressed RNG.""" + +from __future__ import annotations +from dataclasses import replace +import numpy as np +import pytest + +from embodichain.lab.sim.scene.expansion import ( + SceneLayoutCondition, + prepare_scene_randomization, + generate_scene_variants, + save_scene_variants, + load_scene_variants, +) +from scene_layout_support import make_source, make_policy + + +def _poses(variant): + return {c.entity_uid: np.asarray(c.arena_pose) for c in variant.pose_changes} + + +def test_condition_owns_anchors_and_leaves_prepared_and_other_samples_independent(): + prepared = prepare_scene_randomization(make_source(), make_policy()) + anchor = np.array(prepared.source.poses["tray"]) + anchor[0, 3] += 0.1 + condition = SceneLayoutCondition({"tray": anchor}) + anchor[0, 3] = 99 + original_policy = prepared.policy + result = generate_scene_variants( + prepared, conditions=condition, seed=7, candidate_count=4 + ) + assert result.status == "completed" + for variant in result.variants: + np.testing.assert_allclose( + _poses(variant)["tray"], condition.poses["tray"], rtol=0, atol=1e-12 + ) + assert not np.allclose( + _poses(result.variants[0])["mug"], _poses(result.variants[-1])["mug"] + ) + assert prepared.policy == original_policy + projection = condition.to_dict() + projection["tray"][0][3] = 77 + assert condition.to_dict()["tray"][0][3] == pytest.approx(-0.2) + with pytest.raises(TypeError): + condition.poses["tray"] = np.eye(4) + + +def test_member_anchor_requires_pinned_ancestors_and_preserves_full_pose(): + prepared = prepare_scene_randomization(make_source(), make_policy()) + first = generate_scene_variants(prepared, seed=9, candidate_count=1).variants[0] + poses = _poses(first) + with pytest.raises(ValueError, match="ancestor"): + generate_scene_variants( + prepared, conditions=SceneLayoutCondition({"mug": poses["mug"]}) + ) + condition = SceneLayoutCondition(poses) + batch = generate_scene_variants( + prepared, conditions=condition, candidate_count=2, max_attempts=10 + ) + assert batch.status == "completed" + for variant in batch.variants: + for uid, pose in poses.items(): + np.testing.assert_allclose(_poses(variant)[uid], pose, rtol=0, atol=1e-10) + + +@pytest.mark.parametrize("size", [1, 7, 64]) +def test_condition_prefix_continuation_and_persistence(size, tmp_path): + source = make_source() + condition = SceneLayoutCondition({"tray": source.poses["tray"]}) + prepared = prepare_scene_randomization(source, make_policy(batch_size=size)) + one = generate_scene_variants( + prepared, conditions=condition, seed=13, candidate_count=3 + ) + two = generate_scene_variants( + prepared, + conditions=condition, + seed=13, + candidate_count=2, + start_ordinal=one.next_ordinal, + ) + together = generate_scene_variants( + prepared, conditions=condition, seed=13, candidate_count=5 + ) + assert tuple(one.variants) + tuple(two.variants) == tuple(together.variants) + reference = generate_scene_variants( + prepare_scene_randomization(source, make_policy(batch_size=1)), + conditions=condition, + seed=13, + candidate_count=5, + ) + assert tuple(reference.variants) == tuple(together.variants) + saved = save_scene_variants(together, tmp_path / "layouts.json") + loaded = load_scene_variants(saved) + assert tuple(loaded.variants) == tuple(together.variants) + + +def test_bad_anchor_rejects_without_relaxing_domain_or_permissions(): + source = make_source() + prepared = prepare_scene_randomization( + source, + make_policy(group_xy_offset_bounds_m={"tray": [[-0.1, -0.1], [0.1, 0.1]]}), + ) + outside = np.array(source.poses["tray"]) + outside[0, 3] += 0.2 + with pytest.raises(ValueError, match="bounds"): + generate_scene_variants( + prepared, conditions=SceneLayoutCondition({"tray": outside}) + ) + tilted = np.array(source.poses["tray"]) + tilted[0, 0] = -1 + tilted[1, 1] = -1 + with pytest.raises(ValueError, match="rotation"): + generate_scene_variants( + prepared, conditions=SceneLayoutCondition({"tray": tilted}) + ) + fixed = np.array(source.poses["z_target"]) + fixed[0, 3] += 0.01 + with pytest.raises(ValueError, match="fixed"): + generate_scene_variants( + prepared, conditions=SceneLayoutCondition({"z_target": fixed}) + ) + with pytest.raises(ValueError, match="Unknown"): + generate_scene_variants( + prepared, conditions=SceneLayoutCondition({"missing": np.eye(4)}) + ) + + +def test_geometrically_impossible_anchor_returns_exact_exhaustion_without_repair(): + source = make_source() + prepared = prepare_scene_randomization(source, make_policy()) + overlap = np.array(source.poses["tray"]) + overlap[:2, 3] = np.array(source.poses["z_target"])[:2, 3] + result = generate_scene_variants( + prepared, + conditions=SceneLayoutCondition({"tray": overlap}), + candidate_count=1, + max_attempts=9, + ) + assert result.status == "budget_exhausted" and not result.variants + assert result.attempts == 9 and result.next_ordinal == 9 + + +def test_anchor_reserves_room_before_sampling_an_earlier_sibling(): + source = make_source() + prepared = prepare_scene_randomization( + source, + make_policy( + group_targets=["tray", "z_target"], + intra_group={}, + collision_margin_m=0.02, + group_xy_offset_bounds_m={ + "tray": [[0, -0.04], [0.35, 0.04]], + "z_target": [[-0.8, -0.4], [0.4, 0.4]], + }, + ), + ) + target = np.array(source.poses["z_target"]) + target[:2, 3] = [-0.3, 0.0] + result = generate_scene_variants( + prepared, + conditions=SceneLayoutCondition({"z_target": target}), + seed=11, + candidate_count=8, + max_attempts=8, + ) + assert result.status == "completed" and len(result.variants) == 8 + for variant in result.variants: + poses = _poses(variant) + np.testing.assert_allclose(poses["z_target"], target, rtol=0, atol=1e-10) + # Their Y ranges always overlap. Clearance is possible only in X: + # 0.15 m tray half-width + 0.05 m block half-width + 0.02 m margin. + assert poses["tray"][0, 3] - target[0, 3] >= 0.22 - 1e-10 + + +def test_no_condition_and_empty_condition_preserve_legacy_exact_sequence(): + prepared = prepare_scene_randomization(make_source(), make_policy()) + a = generate_scene_variants(prepared, seed=41, candidate_count=4) + b = generate_scene_variants( + prepared, seed=41, candidate_count=4, conditions=SceneLayoutCondition({}) + ) + assert tuple(a.variants) == tuple(b.variants) and a.attempts == b.attempts + + +@pytest.mark.gpu +def test_cuda_anchors_keep_cpu_geometry_certificate_and_selected_pose(): + import torch + + if not torch.cuda.is_available(): + pytest.skip("CUDA unavailable") + source = make_source() + condition = SceneLayoutCondition({"tray": source.poses["tray"]}) + prepared = prepare_scene_randomization( + source, make_policy(device="cuda:0", batch_size=7) + ) + result = generate_scene_variants( + prepared, conditions=condition, seed=13, candidate_count=3 + ) + assert result.status == "completed" + for candidate in result.variants: + np.testing.assert_allclose( + _poses(candidate)["tray"], source.poses["tray"], rtol=0, atol=1e-10 + ) + + +def test_member_anchor_under_fixed_parent_and_invalid_height_are_explicit(): + prepared = prepare_scene_randomization( + make_source(), make_policy(randomize_groups=False) + ) + initial = generate_scene_variants(prepared, seed=19, candidate_count=1).variants[0] + pose = _poses(initial)["mug"] + result = generate_scene_variants( + prepared, conditions=SceneLayoutCondition({"mug": pose}), candidate_count=1 + ) + assert result.status == "completed" + np.testing.assert_allclose( + _poses(result.variants[0])["mug"], pose, rtol=0, atol=1e-10 + ) + pose[2, 3] += 0.01 + with pytest.raises(ValueError, match="height"): + generate_scene_variants( + prepared, conditions=SceneLayoutCondition({"mug": pose}) + ) From 97493f37bb76cc046d1e57367b3eb38fc48b9a70 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 18 Nov 2026 03:46:48 +0000 Subject: [PATCH 36/45] fix(expansion): address point-IK and idle-row review feedback --- agent_context/topics/expansion-engine/expansion-engine.md | 4 ++-- tests/sim/motion/_cartesian_segment_fixture.py | 2 +- tests/sim/motion/test_explicit_interpolation.py | 1 + 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/agent_context/topics/expansion-engine/expansion-engine.md b/agent_context/topics/expansion-engine/expansion-engine.md index 0928121f21..84c13178ee 100644 --- a/agent_context/topics/expansion-engine/expansion-engine.md +++ b/agent_context/topics/expansion-engine/expansion-engine.md @@ -160,8 +160,8 @@ Optional idle-row reuse keeps the same W×B capacity while competing retry candidates use otherwise idle slots. Native write reservations still enforce the original quota; peers are cancelled only after a confirmed receipt, using measured partial evidence and the bridge's ordinary stop handshake. The low-level -source must supply an explicit cancellation port. Native Task Program jobs enable -this policy by default without adding processes or Worlds. +source must supply an explicit cancellation port. Native Task Program jobs opt in +with the [idle-row reuse setting](workloads.md#source-ports-and-native-accounting). Native numerical evidence has T submitted actions and T+1 measured states. It includes all represented dynamic scene entities, source/action semantics, physical parameters diff --git a/tests/sim/motion/_cartesian_segment_fixture.py b/tests/sim/motion/_cartesian_segment_fixture.py index e448a779b3..9360717571 100644 --- a/tests/sim/motion/_cartesian_segment_fixture.py +++ b/tests/sim/motion/_cartesian_segment_fixture.py @@ -27,7 +27,7 @@ def segment_fixture(case): calls = [] - robot = NS(device=torch.device("cpu")) + robot = NS(device=torch.device("cpu"), get_solver=lambda name=None: None) def fk(qpos, **kwargs): result = torch.eye(4).expand(qpos.shape[0], 4, 4).clone() diff --git a/tests/sim/motion/test_explicit_interpolation.py b/tests/sim/motion/test_explicit_interpolation.py index 807b26271a..f32bfa2436 100644 --- a/tests/sim/motion/test_explicit_interpolation.py +++ b/tests/sim/motion/test_explicit_interpolation.py @@ -77,6 +77,7 @@ def solve(*, pose, joint_seed, name): get_qpos_limits=lambda **kwargs: torch.tensor([[[-1.0, 1.0]]]).expand( batch, -1, -1 ), + get_solver=lambda name=None: None, compute_ik=solve, ) return generator, seeds From 5b5a30706fdbbecc2c011d46247d8ea10d390095 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 18 Nov 2026 04:26:23 +0000 Subject: [PATCH 37/45] docs: refresh scene expansion architecture evidence --- docs/architecture/curated.json | 6 +- docs/architecture/generated/architecture.json | 198 +++++++------- docs/architecture/generated/summary.md | 248 +++++++++--------- 3 files changed, 226 insertions(+), 226 deletions(-) diff --git a/docs/architecture/curated.json b/docs/architecture/curated.json index 23cce03d11..faefda5424 100644 --- a/docs/architecture/curated.json +++ b/docs/architecture/curated.json @@ -1275,9 +1275,9 @@ { "path": "embodichain/lab/sim/scene/expansion/__init__.py", "symbol": "", - "start_line": 25, - "end_line": 27, - "excerpt": "from .cfg import SceneExpansionCfg\nfrom .contracts import ScenePoseChange, SceneVariant\nfrom .source import FixedSceneCandidateSource" + "start_line": 28, + "end_line": 31, + "excerpt": "from .cfg import SceneExpansionCfg\nfrom .contracts import ScenePoseChange, SceneVariant\nfrom .conditions import SceneLayoutCondition\nfrom .source import FixedSceneCandidateSource" }, { "path": "embodichain/lab/sim/scene/expansion/source.py", diff --git a/docs/architecture/generated/architecture.json b/docs/architecture/generated/architecture.json index a7bce18684..0cb0f66d19 100644 --- a/docs/architecture/generated/architecture.json +++ b/docs/architecture/generated/architecture.json @@ -3,8 +3,8 @@ "kind": "static-architecture", "coverage": "curated-sample", "repository": "DexForce/EmbodiChain", - "revision": "87d8d9454cd18f19d24434380bcd26bcd0dd376e", - "source_ref": "87d8d9454cd18f19d24434380bcd26bcd0dd376e", + "revision": "cfdc9f3cf510432ceb8c19af996c746f798a4eb3", + "source_ref": "HEAD", "package_version": "0.3.2", "topic_index": "agent_context/MAP.yaml", "limitations": [ @@ -496,8 +496,8 @@ { "path": "embodichain/lab/gym/envs/task_program/bridge.py", "symbol": "BufferedGymCommandSink", - "start_line": 864, - "end_line": 864, + "start_line": 892, + "end_line": 892, "excerpt": "class BufferedGymCommandSink:" } ], @@ -1418,8 +1418,8 @@ { "path": "embodichain/toolkits/graspkit/pg_grasp/gripper_collision_checker.py", "symbol": "GripperCollisionChecker", - "start_line": 39, - "end_line": 39, + "start_line": 96, + "end_line": 96, "excerpt": "class GripperCollisionChecker:" } ], @@ -1556,8 +1556,8 @@ { "path": "embodichain/lab/gym/envs/task_program/bridge.py", "symbol": "TaskProgramDemoBridge", - "start_line": 1149, - "end_line": 1149, + "start_line": 1177, + "end_line": 1177, "excerpt": "class TaskProgramDemoBridge:" } ], @@ -1591,8 +1591,8 @@ { "path": "embodichain/lab/task_program/integrations/catalog.py", "symbol": "TaskProgramIntegrationCatalog", - "start_line": 611, - "end_line": 611, + "start_line": 617, + "end_line": 617, "excerpt": "class TaskProgramIntegrationCatalog:" } ], @@ -1765,8 +1765,8 @@ { "path": "embodichain/lab/sim/motion/motion_generator.py", "symbol": "MotionGenerator", - "start_line": 310, - "end_line": 310, + "start_line": 334, + "end_line": 334, "excerpt": "class MotionGenerator:" } ], @@ -2310,8 +2310,8 @@ { "path": "embodichain/lab/sim/objects/rigid_object.py", "symbol": "RigidObject", - "start_line": 430, - "end_line": 430, + "start_line": 434, + "end_line": 434, "excerpt": "class RigidObject(BatchEntity):" } ], @@ -2607,8 +2607,8 @@ "path": "embodichain/lab/sim/scene/expansion/__init__.py", "symbol": "", "start_line": 28, - "end_line": 30, - "excerpt": "from .cfg import SceneExpansionCfg\nfrom .contracts import ScenePoseChange, SceneVariant\nfrom .source import FixedSceneCandidateSource" + "end_line": 31, + "excerpt": "from .cfg import SceneExpansionCfg\nfrom .contracts import ScenePoseChange, SceneVariant\nfrom .conditions import SceneLayoutCondition\nfrom .source import FixedSceneCandidateSource" }, { "path": "embodichain/lab/sim/scene/expansion/source.py", @@ -3116,8 +3116,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager", - "start_line": 562, - "end_line": 562, + "start_line": 566, + "end_line": 566, "excerpt": "class SimulationManager:" } ], @@ -3349,8 +3349,8 @@ { "path": "embodichain/gen_sim/task_engine/semantic_plan_validation.py", "symbol": "_validate_interpreted_plan", - "start_line": 824, - "end_line": 831, + "start_line": 838, + "end_line": 845, "excerpt": " \"status\": \"covered\",\n \"semantic_status\": \"valid\",\n \"visual_status\": (\"complete\" if observed == total else \"pending\"),\n \"pending_bindings\": len(bindings),\n \"visual_observed_entities\": observed,\n \"visual_total_entities\": total,\n \"execution_ready\": False,\n \"runtime_evidence_status\": \"not_run\"," } ], @@ -4001,8 +4001,8 @@ { "path": "embodichain/lab/gym/envs/task_program/bridge.py", "symbol": "TaskProgramDemoBridge.__init__", - "start_line": 1287, - "end_line": 1287, + "start_line": 1315, + "end_line": 1315, "excerpt": " self._clock = clock" } ] @@ -4019,8 +4019,8 @@ { "path": "embodichain/lab/gym/envs/task_program/bridge.py", "symbol": "TaskProgramDemoBridge.__init__", - "start_line": 1285, - "end_line": 1285, + "start_line": 1313, + "end_line": 1313, "excerpt": " self._runtime = runtime" }, { @@ -4044,8 +4044,8 @@ { "path": "embodichain/lab/gym/envs/task_program/bridge.py", "symbol": "TaskProgramDemoBridge.__init__", - "start_line": 1284, - "end_line": 1284, + "start_line": 1312, + "end_line": 1312, "excerpt": " self._program = program" }, { @@ -4069,8 +4069,8 @@ { "path": "embodichain/lab/gym/envs/task_program/bridge.py", "symbol": "TaskProgramDemoBridge.__init__", - "start_line": 1286, - "end_line": 1286, + "start_line": 1314, + "end_line": 1314, "excerpt": " self._sink = command_sink" } ] @@ -4087,8 +4087,8 @@ { "path": "embodichain/lab/gym/envs/task_program/bridge.py", "symbol": "TaskProgramDemoBridge._segment_actions", - "start_line": 1995, - "end_line": 1995, + "start_line": 2023, + "end_line": 2023, "excerpt": " self._sink.pop()," } ] @@ -4604,8 +4604,8 @@ { "path": "pyproject.toml", "symbol": "", - "start_line": 100, - "end_line": 101, + "start_line": 101, + "end_line": 102, "excerpt": "[project.entry-points.\"embodichain.tasks\"]\nembodichain_tasks = \"embodichain_tasks\"" }, { @@ -5251,8 +5251,8 @@ { "path": "embodichain/lab/sim/objects/rigid_object.py", "symbol": "RigidObject", - "start_line": 430, - "end_line": 430, + "start_line": 434, + "end_line": 434, "excerpt": "class RigidObject(BatchEntity):" } ] @@ -5819,15 +5819,15 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.add_robot", - "start_line": 4395, - "end_line": 4396, + "start_line": 4389, + "end_line": 4390, "excerpt": " robot = self._declare_spawn_articulation(cfg, Robot)\n self._robots[uid] = robot" }, { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager._declare_spawn_articulation", - "start_line": 4435, - "end_line": 4438, + "start_line": 4429, + "end_line": 4432, "excerpt": " facade = facade_type(\n cfg=cfg,\n device=self.device,\n )" } ] @@ -5844,8 +5844,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.start_visualization", - "start_line": 1194, - "end_line": 1199, + "start_line": 1198, + "end_line": 1203, "excerpt": " runtime = VisualizationRuntime(\n SceneExporter(self, visualization_cfg),\n visualization_cfg,\n )\n self._visualization_runtime = runtime\n runtime.start()" } ] @@ -5862,8 +5862,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.add_robot", - "start_line": 4396, - "end_line": 4401, + "start_line": 4390, + "end_line": 4395, "excerpt": " self._robots[uid] = robot\n if was_materialized:\n self.prepare()\n for sensor_cfg in mounted_sensors:\n self.add_sensor(sensor_cfg)\n return robot" } ] @@ -5880,8 +5880,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.enable_gizmo", - "start_line": 4591, - "end_line": 4591, + "start_line": 4585, + "end_line": 4585, "excerpt": " gizmo = Gizmo(target, gizmo_cfg, control_part)" } ] @@ -5898,8 +5898,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.create_rigid_constraint", - "start_line": 4041, - "end_line": 4041, + "start_line": 4051, + "end_line": 4051, "excerpt": " constraint = RigidConstraint(" } ] @@ -5916,8 +5916,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.add_rigid_object_group", - "start_line": 4182, - "end_line": 4182, + "start_line": 4192, + "end_line": 4192, "excerpt": " group = RigidObjectGroup(" } ] @@ -5934,8 +5934,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager._declare_rigid_objects", - "start_line": 3462, - "end_line": 3462, + "start_line": 3472, + "end_line": 3472, "excerpt": " rigid_obj = RigidObject(cfg=cfg, device=self.device)" } ] @@ -5952,8 +5952,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.add_articulation", - "start_line": 4283, - "end_line": 4283, + "start_line": 4293, + "end_line": 4293, "excerpt": " self._articulations[uid] = articulation" } ] @@ -5970,8 +5970,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.add_deformable_object", - "start_line": 3841, - "end_line": 3841, + "start_line": 3851, + "end_line": 3851, "excerpt": " self._deformable_objects[uid] = deformable" } ] @@ -5988,8 +5988,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager.add_light", - "start_line": 2973, - "end_line": 2973, + "start_line": 2983, + "end_line": 2983, "excerpt": " self._lights[uid] = batch_lights" } ] @@ -6006,8 +6006,8 @@ { "path": "embodichain/lab/gym/envs/task_program/bridge.py", "symbol": "BufferedGymCommandSink.__init__", - "start_line": 880, - "end_line": 881, + "start_line": 908, + "end_line": 909, "excerpt": " self._encoder = encoder\n self._clock = clock" } ] @@ -6030,24 +6030,6 @@ } ] }, - { - "id": "static-imports-01a9592b5f6f", - "source": "simulation-manager", - "target": "configuration", - "relation": "imports", - "description": "The selected source module declares an absolute import associated with this target.", - "provenance": "static-extracted", - "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", - "evidence": [ - { - "path": "embodichain/lab/sim/sim_manager.py", - "symbol": "", - "start_line": 166, - "end_line": 166, - "excerpt": "from embodichain.utils import configclass, logger" - } - ] - }, { "id": "static-imports-023d746bdd8d", "source": "scene-generation", @@ -6606,6 +6588,24 @@ } ] }, + { + "id": "static-imports-3d3109273aad", + "source": "simulation-manager", + "target": "configuration", + "relation": "imports", + "description": "The selected source module declares an absolute import associated with this target.", + "provenance": "static-extracted", + "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", + "evidence": [ + { + "path": "embodichain/lab/sim/sim_manager.py", + "symbol": "", + "start_line": 170, + "end_line": 170, + "excerpt": "from embodichain.utils import configclass, logger" + } + ] + }, { "id": "static-imports-3d4c7a13ce4e", "source": "environment-adapter", @@ -7488,6 +7488,24 @@ } ] }, + { + "id": "static-imports-9ccfb0f3bdcf", + "source": "simulation-manager", + "target": "physics", + "relation": "imports", + "description": "The selected source module declares an absolute import associated with this target.", + "provenance": "static-extracted", + "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", + "evidence": [ + { + "path": "embodichain/lab/sim/sim_manager.py", + "symbol": "", + "start_line": 141, + "end_line": 141, + "excerpt": "from embodichain.lab.sim.physics import make_physics_backend" + } + ] + }, { "id": "static-imports-a225c2b26ae1", "source": "environment-adapter", @@ -7848,24 +7866,6 @@ } ] }, - { - "id": "static-imports-c84985d043b9", - "source": "simulation-manager", - "target": "physics", - "relation": "imports", - "description": "The selected source module declares an absolute import associated with this target.", - "provenance": "static-extracted", - "scope": "Module-level declaration. Not a runtime call or proof that every class in the file uses the imported symbol.", - "evidence": [ - { - "path": "embodichain/lab/sim/sim_manager.py", - "symbol": "", - "start_line": 137, - "end_line": 137, - "excerpt": "from embodichain.lab.sim.physics import make_physics_backend" - } - ] - }, { "id": "static-imports-c8cfa7225b4a", "source": "embodied-env", @@ -8604,8 +8604,8 @@ "static-imports-735fdeb87797", "static-imports-30e2933cfbeb", "static-imports-a38ea1f6aa90", - "static-imports-c84985d043b9", - "static-imports-01a9592b5f6f", + "static-imports-9ccfb0f3bdcf", + "static-imports-3d3109273aad", "static-imports-3d4c7a13ce4e", "static-imports-14722eceaa11", "static-imports-84f40c9ef00a", @@ -9060,8 +9060,8 @@ "static-imports-735fdeb87797", "static-imports-30e2933cfbeb", "static-imports-a38ea1f6aa90", - "static-imports-c84985d043b9", - "static-imports-01a9592b5f6f" + "static-imports-9ccfb0f3bdcf", + "static-imports-3d3109273aad" ], "groups": [ { @@ -9363,7 +9363,7 @@ "static-imports-a954b58f9edc", "static-imports-17df33c96f97", "static-imports-9bc131d4e099", - "static-imports-01a9592b5f6f" + "static-imports-3d3109273aad" ], "groups": [ { diff --git a/docs/architecture/generated/summary.md b/docs/architecture/generated/summary.md index 6336b00c5b..e15c9ac680 100644 --- a/docs/architecture/generated/summary.md +++ b/docs/architecture/generated/summary.md @@ -1,139 +1,139 @@ # Architecture overview -Source revision: `87d8d9454cd18f19d24434380bcd26bcd0dd376e` · package 0.3.2 +Source revision: `cfdc9f3cf510432ceb8c19af996c746f798a4eb3` · package 0.3.2 Curated static relationships; not a complete dependency graph or runtime trace. | Module | Responsibility | Evidence and documentation | | --- | --- | --- | -| CLI | Dispatches commands and lazily loads task discovery, environment, and training entry points. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/cli/main.py#L17-L17) · [CLI reference](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/guides/cli.md) | -| Environment launcher | Runs task discovery, configuration loading, and Gym environment creation. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/scripts/run_task.py#L2015-L2015) · [Environment launch guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/guides/run_task.md) | -| Task discovery | Imports installed task packages through entry points to populate the environment registry. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/utils/registration.py#L446-L446) · [Task discovery and supported tasks](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/resources/task/index.rst) | -| Official tasks | Registers official task entry points and organises reusable robot tasks. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/extensions/embodichain_tasks/embodichain_tasks/__init__.py#L17-L23) · [Supported tasks](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/resources/task/index.rst) | -| Config loader | Loads deployment files, resolves components, and assembles the typed environment configuration. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/utils/gym_utils.py#L1954-L1954) · [Configuration guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/guides/configuration.md) | -| Component composition | Resolves environment and embodiment components into runnable Gym configurations. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/utils/_component_composition.py#L17-L17) · [Configuration guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/guides/configuration.md) | -| EmbodiedEnv | Assembles scenes, managers, and demonstrations with optional Task Program integration. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/embodied_env.py#L518-L518) · [EmbodiedEnv documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/gym/env.md) | -| BaseEnv | Owns the Gym loop, simulation initialisation, and environment step lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/base_env.py#L118-L118) · [BaseEnv documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/gym/env.md) | -| Managers | Organises observations, actions, rewards, events, and data recording within the environment lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/managers/__init__.py#L17-L20) · [Gym managers and environment lifecycle](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/gym/index.rst) | -| Configuration utilities | Defines typed configuration copying, serialization, and required-field validation. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/utils/configclass.py#L15-L15) · [Configuration utilities documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.utils.rst) | -| Domain randomization | Event functors randomize physical, visual, spatial, and geometric properties. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/managers/randomization/__init__.py#L17-L20) · [Domain randomization documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst) | -| DifferentiableEnv | Preserves the Gym lifecycle while bridging task-defined kinematics to autograd. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/differentiable_env.py#L48-L48) · [DifferentiableEnv documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst) | -| EventManager | Dispatches configured event functors with lifecycle scheduling and scoped seeds. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/managers/event_manager.py#L97-L97) · [EventManager documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst) | -| TaskProgramCompiler | Compiles task configuration into an analysable program using a static scene manifest. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/compiler/program.py#L1613-L1613) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [TaskProgramCompiler documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst) | -| SemanticCallCompiler | Analyses and lowers Semantic Calls against the bound scene and skill integration. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/compiler/lowering.py#L1033-L1033) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [SemanticCallCompiler documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst) | -| SemanticCallExecutor | Lowers and executes semantic calls, creating an execution session for each call. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/runtime/executor.py#L258-L258) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [SemanticCallExecutor documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.task_program.runtime.rst) | -| TaskProgramEnvironmentAdapter | Validates integration choices and assembles the semantic runtime and Gym bridge. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/integrations/environment.py#L315-L315) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [TaskProgramEnvironmentAdapter documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.rst) | -| TaskProgramDemoBridge | Converts program segments into lazy Gym demonstrations with a shared runtime, buffer, and clock. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/task_program/bridge.py#L1149-L1149) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [TaskProgramDemoBridge documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.task_program.rst) | -| Scene expansion attempts | Prepares selected scene candidates and executes a shared Task Program with per-row measured acceptance. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/integrations/scene_expansion.py#L241-L241) · [Task Program scene preparation and measured acceptance](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.scene_expansion.rst) | -| MLLM frontend | Decodes untrusted model JSON and compiles it through the existing Task Program adapter. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/agents/mllm/task_program.py#L17-L17) · [MLLM frontend documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) | -| AtomicSkillEngine | Manages planning resources and atomic skills to create executable action sessions. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/atomic_skills/engine.py#L85-L85) · [Atomic Skills guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/atomic_skills/index.md) · [AtomicSkillEngine documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_skills.rst) | -| Motion benchmarks | Evaluates motion planners and Atomic Skills using installed suites and external adapters. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/extensions/embodichain_benchmarks/motion_generation/__init__.py#L17-L21) · [Benchmark API and external adapters](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/benchmarks.rst) | -| MotionGenerator | Composes planning backends behind a unified robot motion generation interface. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/motion/motion_generator.py#L310-L310) · [Motion generation guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/motion/motion_generator.md) · [MotionGenerator documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.motion.motion_generator.rst) | -| Planners | Provides motion planning backends that generate constrained robot trajectories. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/motion/planners/__init__.py#L17-L22) · [Motion planner guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/motion/planners/index.rst) | -| Solvers | Provides inverse kinematics backends and stateful robot solving capabilities. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/motion/solvers/__init__.py#L17-L20) · [Kinematic solver guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/motion/solvers/index.rst) | -| Robot workspace | Loads reachability caches and samples robot joint configurations and poses. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/motion/workspace/runtime.py#L53-L53) · [Robot workspace documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/workspace_analyzer/runtime.md) | -| Expansion session | Owns bounded candidate lifecycles, collection budgets and final commit receipts. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/expansion/session.py#L121-L121) · [Trajectory augmentation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.motion.expansion.rst) | -| SimulationManager | Manages the world, robots, sensors, physics, and visualisation lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/sim_manager.py#L562-L562) · [SimulationManager documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/sim_manager.md) | -| Scene expansion contracts | Defines immutable arena-local scene proposals and bounded authored candidate sources. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/scene/expansion/__init__.py#L28-L30) · [Scene expansion contracts and fixed candidates](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.expansion.rst) | -| Robot | Represents a simulated robot with control parts, joint state, and kinematics. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/robot.py#L17-L17) · [Robots and articulations](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/sim_articulation.md) | -| Sensors | Provides camera, stereo, and contact observations for the environment. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/sensors/__init__.py#L17-L20) · [Sensor guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/sensors/index.rst) | -| Physics backends | Constructs the configured physics backend and connects simulation state to physics solving. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/physics/__init__.py#L16-L21) · [Physics backend guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/sim_manager/physics/index.md) | -| Browser visualization | Publishes simulation snapshots and interactive controls through a browser runtime. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/visualization/runtime.py#L17-L17) · [Browser visualization guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/visualization/viser.md) | -| Rigid objects | Represents rigid scene objects and their physical state. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/rigid_object.py#L430-L430) · [Rigid objects documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Rigid object groups | Groups multiple rigid bodies into a batched scene entity. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/rigid_object_group.py#L187-L187) · [Rigid object group guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/sim_rigid_object_group.md) · [Object API reference](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Articulations | Represents jointed scene entities and articulated state. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/articulation.py#L724-L724) · [Articulations documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Deformable objects | Represents Newton-backed deformable particle-set entities. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/deformable/base.py#L49-L49) · [Deformable objects documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Lights | Controls light color, intensity, falloff, and pose through a batched facade. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/light.py#L40-L40) · [Lights documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Rigid constraints | Wraps per-arena fixed constraints between two rigid objects. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/constraint.py#L36-L36) · [Rigid constraints documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Gizmo | Provides native robot IK and browser manipulation controls for simulation targets. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/gizmo.py#L458-L458) · [Gizmo documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Compute | Shared numerical algorithms for kinematics, trajectories, geometry, and images. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/compute/__init__.py#L17-L21) · [Compute API reference](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.compute.rst) | -| Trajectory computation | Shared interpolation, resampling, timing, and trajectory warping algorithms. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/compute/trajectory/__init__.py#L17-L20) · [Trajectory computation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.compute.rst) | -| Kinematics computation | Shared numerical kinematics and manipulability algorithms. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/compute/kinematics/__init__.py#L17-L24) · [Kinematics computation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.compute.rst) | -| DatasetManager | Coordinates dataset functors for frame collection, saving, and finalization. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/managers/dataset_manager.py#L46-L46) · [DatasetManager documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/gym/dataset_functors.md) | -| LeRobot recording | Persists demonstrations and metadata in LeRobot datasets with optional depth sidecars. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/managers/datasets.py#L167-L167) · [LeRobot recording documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/gym/dataset_functors.md) | -| Async recording | Moves durable dataset writes to one background worker. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/managers/async_datasets.py#L58-L58) · [Async recording documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/gym/dataset_functors.md) | -| Depth sidecar writer | Coordinates depth-video files and metadata alongside recorded demonstrations. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/data_pipeline/depth_video/writer.py#L226-L226) · [Depth video documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.data_pipeline.depth_video.rst) | -| Depth sidecar reader | Loads depth metadata and decodes episode/sensor depth video frames. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/data_pipeline/depth_video/reader.py#L17-L23) · [Depth sidecar reader documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.data_pipeline.depth_video.rst) | -| Depth quantization | Converts depth values to and from the sidecar video representation. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/data_pipeline/depth_video/depth_utils.py#L24-L35) · [Depth quantization documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.data_pipeline.depth_video.rst) | -| OnlineDataEngine | Online data engine managing workers and their lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/data_pipeline/engine/data.py#L17-L17) · [Online data streaming guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/online_data.md) | -| OnlineDataset | Exposes live engine samples as an infinite PyTorch IterableDataset. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/data_pipeline/datasets/online_data.py#L39-L39) · [OnlineDataset documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.data_pipeline.datasets.rst) | -| Chunk samplers | Selects fixed-shape time windows using configurable chunk-length distributions. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/data_pipeline/datasets/sampler.py#L17-L17) · [Chunk samplers documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.data_pipeline.datasets.rst) | -| Data assets | Resolves asset paths and coordinates registered dataset downloads and caches. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/data/dataset.py#L17-L17) · [Data assets documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/resources/assets.md) · [Asset catalog and task role contracts](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/asset_generalization.rst) | -| Scene generation | Generates scene state from input images for scene asset workflows. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/scene_engine/pipeline/generate.py#L17-L17) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/generative_sim/scene_engine.md) | -| Scene understanding | Interprets an image, segments objects, and initializes a validated support graph. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/scene_engine/pipeline/generation/scene_understanding.py#L260-L260) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/generative_sim/scene_engine.md) | -| Asset generation & refinement | Generates geometry and articulated assets, then refines scene placement. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/scene_engine/pipeline/generation/scene_generation.py#L96-L96) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/generative_sim/scene_engine.md) | -| Scene asset preparation | Prepares generated Scene Engine geometry for layout and export. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/scene_engine/pipeline/utils/simready_processor.py#L90-L90) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/generative_sim/scene_engine.md) | -| Scene editing | Edits an existing portable scene using a typed edit plan. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/scene_engine/pipeline/edit.py#L17-L17) · [Scene editing documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/generative_sim/scene_engine.md) | -| Scene edit layout | Reconstructs scene placement against an edited goal graph. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/scene_engine/pipeline/editing/scene_edit_layout_generation.py#L32-L32) · [Scene editing documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/generative_sim/scene_engine.md) | -| Scene import | Reconstructs an editable scene and graph from a portable scene export. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/scene_engine/pipeline/utils/scene_importer.py#L50-L50) · [Scene export documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/generative_sim/scene_engine.md) | -| Scene export | Writes portable scene metadata and assets for downstream simulation preparation. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/scene_engine/pipeline/utils/scene_exporter.py#L53-L53) · [Scene export documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/generative_sim/scene_engine.md) | -| Asset Engine | Prepares an existing mesh through source conversion, semantic/physics inference, and mesh/USD export. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/asset_engine/pipeline/ingest.py#L172-L178) · [Asset Engine guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/generative_sim/asset_engine.md) | -| Asset parser stages | Dispatches inspector, geometry, physics, USD, and internal asset parsers. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/asset_engine/parser/base.py#L58-L64) · [Asset Engine guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/generative_sim/asset_engine.md) | -| GenSim UI sessions | Tracks one replaceable subprocess per Gradio session. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/gradio_ui/app_processes.py#L109-L109) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/generative_sim/scene_engine.md) | -| Training assembly | Builds environments, policies, algorithms, and the compatible trainer from configuration. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/train.py#L17-L17) · [Training assembly documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/rl/train_script.md) | -| RL algorithms | Registers PPO, GRPO, and APG with their rollout contracts. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/algo/__init__.py#L17-L17) · [RL algorithms documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/rl/algorithm.md) | -| PPO | Optimizes an actor-critic policy from standard TensorDict rollouts. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/algo/ppo.py#L52-L52) · [PPO documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst) | -| GRPO | Optimizes policies with group-relative advantages from standard rollouts. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/algo/grpo.py#L48-L48) · [GRPO documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst) | -| APG | Optimizes policy parameters through differentiable rollout rewards. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/algo/apg.py#L119-L119) · [APG documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst) | -| Policy models | Defines policy action inference and model interfaces. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/policies/policy.py#L35-L35) · [Policy models documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/rl/models.md) | -| ActorCritic | Provides Gaussian actions and value estimates with separate actor and critic inputs. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/policies/actor_critic.py#L33-L33) · [ActorCritic documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.learning.rl.policies.rst) | -| ActorOnly | Provides Gaussian actions without a learned value network. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/policies/actor_only.py#L31-L31) · [ActorOnly documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.learning.rl.policies.rst) | -| Learning environments | Defines lightweight vector-environment protocols and factory registration. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/env.py#L17-L17) · [Learning environments documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.learning.rl.rst) | -| Standard trainer | Coordinates standard rollout collection, algorithm updates, evaluation, and checkpoints. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/utils/trainer.py#L17-L17) · [Reinforcement learning overview](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/rl/index.rst) | -| SyncCollector | Collects standard vector-environment rollouts into the training buffer. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/collector/sync_collector.py#L33-L33) · [SyncCollector documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.learning.rl.collector.rst) | -| Rollout buffer | Stores standard rollout transitions and bootstrap state in TensorDict buffers. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/buffer/standard_buffer.py#L27-L27) · [Rollout buffer documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/rl/buffer.md) | -| DifferentiableTrainer | Coordinates graph-connected rollouts and analytic policy-gradient updates. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/differentiable_trainer.py#L77-L77) · [DifferentiableTrainer documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.learning.rl.rst) | -| Differentiable collector | Collects differentiable trajectories while preserving their autograd graph. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/collector/differentiable.py#L145-L145) · [Differentiable collector documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.learning.rl.collector.rst) | -| Running observation normalization | Normalizes continuous observation fields with explicit running-statistic updates. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/normalization.py#L29-L29) · [Running observation normalization documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.learning.rl.rst) | -| Policy evaluation | Runs trained policy evaluation in headless or viewer workflows. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/policy_evaluation/cli.py#L17-L17) · [Policy evaluation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.learning.rl.policy_evaluation.rst) | -| Devices | Defines start, stop, and controller-state interfaces for robot input devices. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/devices/device.py#L17-L17) · [Laboratory and device API reference](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.rst) | -| Device controller | Maps input-device state to robot joint commands with joint-limit handling. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/devices/device_controller.py#L29-L29) · [Device controller documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.rst) | -| GraspKit | Defines parallel-jaw gripper models and grasp-pose generation interfaces. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/graspkit/pose_generator.py#L17-L17) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/grasp_generator.rst) | -| URDF assembly | Assembles URDF components, connections, sensors, and mesh resources. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/urdf_assembly/urdf_assembly_manager.py#L81-L81) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/urdf_assembly.md) | -| Convex decomposition | Prepares convex collision meshes for URDF assets. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/acd/cli.py#L17-L17) · [Convex decomposition documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/convex_decomposition.md) | -| Shared scene services | Composes explicit configurations, proposes layouts, and operates on caller-owned physical state. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/scene/__init__.py#L17-L22) · [Scene services architecture and capabilities](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/scene/index.md) · [Scene construction and preparation workflows](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/scene/workflows.md) · [Production scene service integrations](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/scene/integrations.md) · [Scene composition, layout, state and validation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.rst) · [Shared numerical geometry and layout](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.compute.scene_geometry.rst) | -| NeRD model extension | Bundles the causal NeRD network, physical codec, trajectory windows and supervised recipe in the main wheel. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/extensions/embodichain_models/embodichain_models/dynamics/nerd/model.py#L201-L201) · [Official model library](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/model_catalogs.rst) · [NeRD data and training workflow](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/neural_dynamics/nerd.md) | -| Supervised learning (SL) | Shares structured batches, objective-owned losses, optimizer updates and offline recovery across policy and dynamics fitting. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/sl/learner.py#L52-L52) · [SL batches, updates and recovery](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.learning.sl.rst) | -| NeRD showcase | Prepares pinned external resources and routes default NeRD training through shared SL. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/examples/learning/nerd/__main__.py#L81-L81) · [NeRD showcase commands](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/neural_dynamics/nerd.md) | -| Robot world model (RWM) | Owns action-conditioned robot-state prediction, differentiable rollout objectives and bounded transition replay. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/extensions/embodichain_models/embodichain_models/dynamics/rwm/model.py#L77-L77) · [RWM state, objectives, prediction, replay and SL recipe](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/rwm.rst) · [RWM principles and usage](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/neural_dynamics/rwm.md) | -| TaskSpec | Defines simulator-independent task requirements, milestones, semantic identity and evidence evaluation. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/task_spec/spec.py#L335-L335) · [TaskSpec protocol API](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/public_api.rst) | -| Task Engine | Interprets instructions and image evidence into candidates, TaskSpec requirements and canonical Semantic Call graphs. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/task_engine/agent.py#L72-L72) · [Task Engine workflow and artifact guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/generative_sim/task_engine.md) · [Task Engine semantic contracts](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.gen_sim.task_engine.rst) | +| CLI | Dispatches commands and lazily loads task discovery, environment, and training entry points. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/cli/main.py#L17-L17) · [CLI reference](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/guides/cli.md) | +| Environment launcher | Runs task discovery, configuration loading, and Gym environment creation. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/scripts/run_task.py#L2015-L2015) · [Environment launch guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/guides/run_task.md) | +| Task discovery | Imports installed task packages through entry points to populate the environment registry. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/utils/registration.py#L446-L446) · [Task discovery and supported tasks](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/resources/task/index.rst) | +| Official tasks | Registers official task entry points and organises reusable robot tasks. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/extensions/embodichain_tasks/embodichain_tasks/__init__.py#L17-L23) · [Supported tasks](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/resources/task/index.rst) | +| Config loader | Loads deployment files, resolves components, and assembles the typed environment configuration. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/utils/gym_utils.py#L1954-L1954) · [Configuration guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/guides/configuration.md) | +| Component composition | Resolves environment and embodiment components into runnable Gym configurations. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/utils/_component_composition.py#L17-L17) · [Configuration guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/guides/configuration.md) | +| EmbodiedEnv | Assembles scenes, managers, and demonstrations with optional Task Program integration. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/embodied_env.py#L518-L518) · [EmbodiedEnv documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/gym/env.md) | +| BaseEnv | Owns the Gym loop, simulation initialisation, and environment step lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/base_env.py#L118-L118) · [BaseEnv documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/gym/env.md) | +| Managers | Organises observations, actions, rewards, events, and data recording within the environment lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/managers/__init__.py#L17-L20) · [Gym managers and environment lifecycle](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/gym/index.rst) | +| Configuration utilities | Defines typed configuration copying, serialization, and required-field validation. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/utils/configclass.py#L15-L15) · [Configuration utilities documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.utils.rst) | +| Domain randomization | Event functors randomize physical, visual, spatial, and geometric properties. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/managers/randomization/__init__.py#L17-L20) · [Domain randomization documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst) | +| DifferentiableEnv | Preserves the Gym lifecycle while bridging task-defined kinematics to autograd. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/differentiable_env.py#L48-L48) · [DifferentiableEnv documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst) | +| EventManager | Dispatches configured event functors with lifecycle scheduling and scoped seeds. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/managers/event_manager.py#L97-L97) · [EventManager documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst) | +| TaskProgramCompiler | Compiles task configuration into an analysable program using a static scene manifest. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/compiler/program.py#L1613-L1613) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [TaskProgramCompiler documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst) | +| SemanticCallCompiler | Analyses and lowers Semantic Calls against the bound scene and skill integration. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/compiler/lowering.py#L1033-L1033) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [SemanticCallCompiler documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst) | +| SemanticCallExecutor | Lowers and executes semantic calls, creating an execution session for each call. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/runtime/executor.py#L258-L258) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [SemanticCallExecutor documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.task_program.runtime.rst) | +| TaskProgramEnvironmentAdapter | Validates integration choices and assembles the semantic runtime and Gym bridge. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/integrations/environment.py#L315-L315) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [TaskProgramEnvironmentAdapter documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.rst) | +| TaskProgramDemoBridge | Converts program segments into lazy Gym demonstrations with a shared runtime, buffer, and clock. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/task_program/bridge.py#L1177-L1177) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [TaskProgramDemoBridge documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.task_program.rst) | +| Scene expansion attempts | Prepares selected scene candidates and executes a shared Task Program with per-row measured acceptance. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/integrations/scene_expansion.py#L241-L241) · [Task Program scene preparation and measured acceptance](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.scene_expansion.rst) | +| MLLM frontend | Decodes untrusted model JSON and compiles it through the existing Task Program adapter. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/agents/mllm/task_program.py#L17-L17) · [MLLM frontend documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) | +| AtomicSkillEngine | Manages planning resources and atomic skills to create executable action sessions. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/atomic_skills/engine.py#L85-L85) · [Atomic Skills guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/atomic_skills/index.md) · [AtomicSkillEngine documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_skills.rst) | +| Motion benchmarks | Evaluates motion planners and Atomic Skills using installed suites and external adapters. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/extensions/embodichain_benchmarks/motion_generation/__init__.py#L17-L21) · [Benchmark API and external adapters](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/benchmarks.rst) | +| MotionGenerator | Composes planning backends behind a unified robot motion generation interface. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/motion/motion_generator.py#L334-L334) · [Motion generation guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/motion/motion_generator.md) · [MotionGenerator documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.motion.motion_generator.rst) | +| Planners | Provides motion planning backends that generate constrained robot trajectories. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/motion/planners/__init__.py#L17-L22) · [Motion planner guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/motion/planners/index.rst) | +| Solvers | Provides inverse kinematics backends and stateful robot solving capabilities. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/motion/solvers/__init__.py#L17-L20) · [Kinematic solver guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/motion/solvers/index.rst) | +| Robot workspace | Loads reachability caches and samples robot joint configurations and poses. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/motion/workspace/runtime.py#L53-L53) · [Robot workspace documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/workspace_analyzer/runtime.md) | +| Expansion session | Owns bounded candidate lifecycles, collection budgets and final commit receipts. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/expansion/session.py#L121-L121) · [Trajectory augmentation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.motion.expansion.rst) | +| SimulationManager | Manages the world, robots, sensors, physics, and visualisation lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/sim_manager.py#L566-L566) · [SimulationManager documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/sim_manager.md) | +| Scene expansion contracts | Defines immutable arena-local scene proposals and bounded authored candidate sources. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/scene/expansion/__init__.py#L28-L31) · [Scene expansion contracts and fixed candidates](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.expansion.rst) | +| Robot | Represents a simulated robot with control parts, joint state, and kinematics. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/robot.py#L17-L17) · [Robots and articulations](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/sim_articulation.md) | +| Sensors | Provides camera, stereo, and contact observations for the environment. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/sensors/__init__.py#L17-L20) · [Sensor guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/sensors/index.rst) | +| Physics backends | Constructs the configured physics backend and connects simulation state to physics solving. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/physics/__init__.py#L16-L21) · [Physics backend guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/sim_manager/physics/index.md) | +| Browser visualization | Publishes simulation snapshots and interactive controls through a browser runtime. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/visualization/runtime.py#L17-L17) · [Browser visualization guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/visualization/viser.md) | +| Rigid objects | Represents rigid scene objects and their physical state. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/rigid_object.py#L434-L434) · [Rigid objects documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Rigid object groups | Groups multiple rigid bodies into a batched scene entity. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/rigid_object_group.py#L187-L187) · [Rigid object group guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/sim_rigid_object_group.md) · [Object API reference](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Articulations | Represents jointed scene entities and articulated state. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/articulation.py#L724-L724) · [Articulations documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Deformable objects | Represents Newton-backed deformable particle-set entities. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/deformable/base.py#L49-L49) · [Deformable objects documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Lights | Controls light color, intensity, falloff, and pose through a batched facade. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/light.py#L40-L40) · [Lights documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Rigid constraints | Wraps per-arena fixed constraints between two rigid objects. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/constraint.py#L36-L36) · [Rigid constraints documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Gizmo | Provides native robot IK and browser manipulation controls for simulation targets. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/gizmo.py#L458-L458) · [Gizmo documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Compute | Shared numerical algorithms for kinematics, trajectories, geometry, and images. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/compute/__init__.py#L17-L21) · [Compute API reference](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.compute.rst) | +| Trajectory computation | Shared interpolation, resampling, timing, and trajectory warping algorithms. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/compute/trajectory/__init__.py#L17-L20) · [Trajectory computation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.compute.rst) | +| Kinematics computation | Shared numerical kinematics and manipulability algorithms. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/compute/kinematics/__init__.py#L17-L24) · [Kinematics computation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.compute.rst) | +| DatasetManager | Coordinates dataset functors for frame collection, saving, and finalization. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/managers/dataset_manager.py#L46-L46) · [DatasetManager documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/gym/dataset_functors.md) | +| LeRobot recording | Persists demonstrations and metadata in LeRobot datasets with optional depth sidecars. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/managers/datasets.py#L167-L167) · [LeRobot recording documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/gym/dataset_functors.md) | +| Async recording | Moves durable dataset writes to one background worker. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/managers/async_datasets.py#L58-L58) · [Async recording documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/gym/dataset_functors.md) | +| Depth sidecar writer | Coordinates depth-video files and metadata alongside recorded demonstrations. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/data_pipeline/depth_video/writer.py#L226-L226) · [Depth video documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.data_pipeline.depth_video.rst) | +| Depth sidecar reader | Loads depth metadata and decodes episode/sensor depth video frames. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/data_pipeline/depth_video/reader.py#L17-L23) · [Depth sidecar reader documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.data_pipeline.depth_video.rst) | +| Depth quantization | Converts depth values to and from the sidecar video representation. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/data_pipeline/depth_video/depth_utils.py#L24-L35) · [Depth quantization documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.data_pipeline.depth_video.rst) | +| OnlineDataEngine | Online data engine managing workers and their lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/data_pipeline/engine/data.py#L17-L17) · [Online data streaming guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/online_data.md) | +| OnlineDataset | Exposes live engine samples as an infinite PyTorch IterableDataset. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/data_pipeline/datasets/online_data.py#L39-L39) · [OnlineDataset documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.data_pipeline.datasets.rst) | +| Chunk samplers | Selects fixed-shape time windows using configurable chunk-length distributions. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/data_pipeline/datasets/sampler.py#L17-L17) · [Chunk samplers documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.data_pipeline.datasets.rst) | +| Data assets | Resolves asset paths and coordinates registered dataset downloads and caches. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/data/dataset.py#L17-L17) · [Data assets documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/resources/assets.md) · [Asset catalog and task role contracts](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/asset_generalization.rst) | +| Scene generation | Generates scene state from input images for scene asset workflows. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/scene_engine/pipeline/generate.py#L17-L17) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/generative_sim/scene_engine.md) | +| Scene understanding | Interprets an image, segments objects, and initializes a validated support graph. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/scene_engine/pipeline/generation/scene_understanding.py#L260-L260) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/generative_sim/scene_engine.md) | +| Asset generation & refinement | Generates geometry and articulated assets, then refines scene placement. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/scene_engine/pipeline/generation/scene_generation.py#L96-L96) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/generative_sim/scene_engine.md) | +| Scene asset preparation | Prepares generated Scene Engine geometry for layout and export. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/scene_engine/pipeline/utils/simready_processor.py#L90-L90) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/generative_sim/scene_engine.md) | +| Scene editing | Edits an existing portable scene using a typed edit plan. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/scene_engine/pipeline/edit.py#L17-L17) · [Scene editing documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/generative_sim/scene_engine.md) | +| Scene edit layout | Reconstructs scene placement against an edited goal graph. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/scene_engine/pipeline/editing/scene_edit_layout_generation.py#L32-L32) · [Scene editing documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/generative_sim/scene_engine.md) | +| Scene import | Reconstructs an editable scene and graph from a portable scene export. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/scene_engine/pipeline/utils/scene_importer.py#L50-L50) · [Scene export documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/generative_sim/scene_engine.md) | +| Scene export | Writes portable scene metadata and assets for downstream simulation preparation. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/scene_engine/pipeline/utils/scene_exporter.py#L53-L53) · [Scene export documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/generative_sim/scene_engine.md) | +| Asset Engine | Prepares an existing mesh through source conversion, semantic/physics inference, and mesh/USD export. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/asset_engine/pipeline/ingest.py#L172-L178) · [Asset Engine guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/generative_sim/asset_engine.md) | +| Asset parser stages | Dispatches inspector, geometry, physics, USD, and internal asset parsers. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/asset_engine/parser/base.py#L58-L64) · [Asset Engine guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/generative_sim/asset_engine.md) | +| GenSim UI sessions | Tracks one replaceable subprocess per Gradio session. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/gradio_ui/app_processes.py#L109-L109) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/generative_sim/scene_engine.md) | +| Training assembly | Builds environments, policies, algorithms, and the compatible trainer from configuration. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/train.py#L17-L17) · [Training assembly documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/rl/train_script.md) | +| RL algorithms | Registers PPO, GRPO, and APG with their rollout contracts. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/algo/__init__.py#L17-L17) · [RL algorithms documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/rl/algorithm.md) | +| PPO | Optimizes an actor-critic policy from standard TensorDict rollouts. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/algo/ppo.py#L52-L52) · [PPO documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst) | +| GRPO | Optimizes policies with group-relative advantages from standard rollouts. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/algo/grpo.py#L48-L48) · [GRPO documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst) | +| APG | Optimizes policy parameters through differentiable rollout rewards. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/algo/apg.py#L119-L119) · [APG documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.learning.rl.algo.rst) | +| Policy models | Defines policy action inference and model interfaces. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/policies/policy.py#L35-L35) · [Policy models documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/rl/models.md) | +| ActorCritic | Provides Gaussian actions and value estimates with separate actor and critic inputs. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/policies/actor_critic.py#L33-L33) · [ActorCritic documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.learning.rl.policies.rst) | +| ActorOnly | Provides Gaussian actions without a learned value network. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/policies/actor_only.py#L31-L31) · [ActorOnly documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.learning.rl.policies.rst) | +| Learning environments | Defines lightweight vector-environment protocols and factory registration. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/env.py#L17-L17) · [Learning environments documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.learning.rl.rst) | +| Standard trainer | Coordinates standard rollout collection, algorithm updates, evaluation, and checkpoints. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/utils/trainer.py#L17-L17) · [Reinforcement learning overview](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/rl/index.rst) | +| SyncCollector | Collects standard vector-environment rollouts into the training buffer. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/collector/sync_collector.py#L33-L33) · [SyncCollector documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.learning.rl.collector.rst) | +| Rollout buffer | Stores standard rollout transitions and bootstrap state in TensorDict buffers. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/buffer/standard_buffer.py#L27-L27) · [Rollout buffer documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/rl/buffer.md) | +| DifferentiableTrainer | Coordinates graph-connected rollouts and analytic policy-gradient updates. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/differentiable_trainer.py#L77-L77) · [DifferentiableTrainer documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.learning.rl.rst) | +| Differentiable collector | Collects differentiable trajectories while preserving their autograd graph. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/collector/differentiable.py#L145-L145) · [Differentiable collector documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.learning.rl.collector.rst) | +| Running observation normalization | Normalizes continuous observation fields with explicit running-statistic updates. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/normalization.py#L29-L29) · [Running observation normalization documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.learning.rl.rst) | +| Policy evaluation | Runs trained policy evaluation in headless or viewer workflows. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/policy_evaluation/cli.py#L17-L17) · [Policy evaluation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.learning.rl.policy_evaluation.rst) | +| Devices | Defines start, stop, and controller-state interfaces for robot input devices. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/devices/device.py#L17-L17) · [Laboratory and device API reference](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.rst) | +| Device controller | Maps input-device state to robot joint commands with joint-limit handling. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/devices/device_controller.py#L29-L29) · [Device controller documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.rst) | +| GraspKit | Defines parallel-jaw gripper models and grasp-pose generation interfaces. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/graspkit/pose_generator.py#L17-L17) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/grasp_generator.rst) | +| URDF assembly | Assembles URDF components, connections, sensors, and mesh resources. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/urdf_assembly/urdf_assembly_manager.py#L81-L81) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/urdf_assembly.md) | +| Convex decomposition | Prepares convex collision meshes for URDF assets. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/acd/cli.py#L17-L17) · [Convex decomposition documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/convex_decomposition.md) | +| Shared scene services | Composes explicit configurations, proposes layouts, and operates on caller-owned physical state. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/scene/__init__.py#L17-L22) · [Scene services architecture and capabilities](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/scene/index.md) · [Scene construction and preparation workflows](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/scene/workflows.md) · [Production scene service integrations](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/scene/integrations.md) · [Scene composition, layout, state and validation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.rst) · [Shared numerical geometry and layout](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.compute.scene_geometry.rst) | +| NeRD model extension | Bundles the causal NeRD network, physical codec, trajectory windows and supervised recipe in the main wheel. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/extensions/embodichain_models/embodichain_models/dynamics/nerd/model.py#L201-L201) · [Official model library](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/model_catalogs.rst) · [NeRD data and training workflow](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/neural_dynamics/nerd.md) | +| Supervised learning (SL) | Shares structured batches, objective-owned losses, optimizer updates and offline recovery across policy and dynamics fitting. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/sl/learner.py#L52-L52) · [SL batches, updates and recovery](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.learning.sl.rst) | +| NeRD showcase | Prepares pinned external resources and routes default NeRD training through shared SL. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/examples/learning/nerd/__main__.py#L81-L81) · [NeRD showcase commands](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/neural_dynamics/nerd.md) | +| Robot world model (RWM) | Owns action-conditioned robot-state prediction, differentiable rollout objectives and bounded transition replay. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/extensions/embodichain_models/embodichain_models/dynamics/rwm/model.py#L77-L77) · [RWM state, objectives, prediction, replay and SL recipe](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/rwm.rst) · [RWM principles and usage](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/neural_dynamics/rwm.md) | +| TaskSpec | Defines simulator-independent task requirements, milestones, semantic identity and evidence evaluation. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/task_spec/spec.py#L335-L335) · [TaskSpec protocol API](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/public_api.rst) | +| Task Engine | Interprets instructions and image evidence into candidates, TaskSpec requirements and canonical Semantic Call graphs. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/task_engine/agent.py#L72-L72) · [Task Engine workflow and artifact guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/generative_sim/task_engine.md) · [Task Engine semantic contracts](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.gen_sim.task_engine.rst) | ### Additional modules in specialist views | Module | Responsibility | Evidence and documentation | | --- | --- | --- | -| Antipodal grasp generator | Generates candidate grasp poses through a reusable parallel-jaw interface. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/graspkit/pg_grasp/pose_generator.py#L284-L284) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/grasp_generator.rst) | -| Newton autograd bridge | Connects Warp tape derivatives to PyTorch action and functional-state gradients. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/diff/bridge.py#L81-L81) · [Newton autograd bridge documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst) | -| BatchEntity | Defines shared configuration and instance operations for batched simulation entities. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/common.py#L55-L55) · [BatchEntity documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| URDF collision processor | Replaces mesh collisions with convex pieces and optionally recomputes inertia. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/acd/urdf_modifider.py#L17-L17) · [Convex decomposition documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/convex_decomposition.md) | -| BufferedGymCommandSink | Validates and buffers control commands for incremental Gym demonstration consumption. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/task_program/bridge.py#L864-L864) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [BufferedGymCommandSink documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.task_program.rst) | -| CompiledTaskProgram | Immutable compilation result with materialized task segments and analysis views. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/compiler/program.py#L1343-L1343) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [CompiledTaskProgram documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst) | -| Effect evidence | Collects typed evidence from registered providers for semantic effects. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/semantics/evidence.py#L438-L438) · [Effect evidence documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.task_program.semantics.rst) | -| ExecutionRunner | Advances atomic skill sessions through observation, command, and clock interfaces. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/atomic_skills/runner.py#L367-L367) · [Atomic Skills execution guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/atomic_skills/index.md) · [ExecutionRunner documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_skills.rst) | -| Interactive grasp annotation | Offers browser selection of a graspable mesh region. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/scripts/annotate_grasp.py#L42-L42) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/grasp_generator.rst) | -| Mesh grasp backend | Coordinates sampling, annotation, collision checks, and caches for one mesh. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/graspkit/pg_grasp/_antipodal_backend.py#L71-L71) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/grasp_generator.rst) | -| Gripper collision checks | Checks gripper geometry against the target mesh. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/graspkit/pg_grasp/gripper_collision_checker.py#L39-L39) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/grasp_generator.rst) | -| Antipodal sampling | Samples opposing surface contacts using mesh ray casting. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/graspkit/pg_grasp/antipodal_sampler.py#L62-L62) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/grasp_generator.rst) | -| Parallel-jaw gripper configuration | Owns native hand physics, calibration and declared grasp-generator support. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/cfg/gripper.py#L53-L53) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/grasp_generator.rst) | -| Integration catalog | Describes one Task Program integration and validates compilation against its fingerprint. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/integrations/catalog.py#L611-L611) · [Integration catalog documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.rst) | -| Object backend views | Adapts rigid and articulated object state to backend-neutral Scene batches. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/backends/__init__.py#L19-L19) · [Object backend views documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Parallel execution | Coordinates parallel semantic branches through command and safety interfaces. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/runtime/parallel_executor.py#L629-L629) · [Parallel execution documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.task_program.runtime.rst) | -| Program loading & validation | Loads bounded JSON or YAML into the canonical Task Program configuration. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/language/loader.py#L17-L17) · [Program loading & validation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.task_program.language.rst) | -| SceneRegistry | Binds semantic entity identifiers to scene metadata and runtime providers. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/semantics/scene.py#L1113-L1113) · [SceneRegistry documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/scene_registry.md) | -| SimulationTaskProgramFactory | Binds simulated robots, planners, and services to construct the atomic skill engine. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/integrations/simulation/environment.py#L731-L731) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [Simulation integration API](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.simulation.rst) | -| RobotSkillProfile | Declares robot resources, skill bindings, and effect-assurance capabilities. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/semantics/profiles.py#L1495-L1495) · [RobotSkillProfile documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/robot_profiles.md) | -| EnvironmentStepClock | Maintains execution time on the environment step grid. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/task_program/bridge.py#L351-L351) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [Gym Task Program API](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.task_program.rst) | -| Surface deformables | Specializes deformable objects for cloth particle sets. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/deformable/surface.py#L28-L28) · [Surface deformables documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| TaskProgramCfg | Declarative task configuration describing intent and execution structure. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/language/schema.py#L1067-L1067) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [TaskProgramCfg documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.task_program.language.rst) | -| Toolkit directory | Standalone grasp sampling, collision-mesh preparation, URDF composition and precision assembly assets. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/__init__.py#L17-L21) · [Toolkits documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/index.rst) · [Precision Assembly guide and native demos](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/precision_assembly.md) | -| URDF components | Processes component links, joints, naming, and mesh references. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/urdf_assembly/component.py#L87-L87) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/urdf_assembly.md) | -| URDF connections | Applies component connection rules and sensor attachment joints. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/urdf_assembly/connection.py#L30-L30) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/urdf_assembly.md) | -| URDF mesh resources | Copies mesh resources and rewrites references for the assembled robot. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/urdf_assembly/mesh.py#L33-L33) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/urdf_assembly.md) | -| URDF sensor attachments | Processes sensor descriptions and attachment metadata for assembly. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/urdf_assembly/sensor.py#L75-L75) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/urdf_assembly.md) | -| URDF file output | Formats and persists the assembled URDF document. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/urdf_assembly/file_writer.py#L28-L28) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/urdf_assembly.md) | -| Volume deformables | Specializes deformable objects for volumetric soft-body particle sets. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/deformable/volume.py#L36-L36) · [Volume deformables documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | -| Workspace analyzer | Analyzes robot reachability and metrics for reusable workspace caches. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/motion/workspace/analyzer.py#L182-L182) · [Workspace analyzer documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/workspace_analyzer/workspace_analyzer.md) | +| Antipodal grasp generator | Generates candidate grasp poses through a reusable parallel-jaw interface. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/graspkit/pg_grasp/pose_generator.py#L284-L284) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/grasp_generator.rst) | +| Newton autograd bridge | Connects Warp tape derivatives to PyTorch action and functional-state gradients. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/diff/bridge.py#L81-L81) · [Newton autograd bridge documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.rst) | +| BatchEntity | Defines shared configuration and instance operations for batched simulation entities. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/common.py#L55-L55) · [BatchEntity documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| URDF collision processor | Replaces mesh collisions with convex pieces and optionally recomputes inertia. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/acd/urdf_modifider.py#L17-L17) · [Convex decomposition documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/convex_decomposition.md) | +| BufferedGymCommandSink | Validates and buffers control commands for incremental Gym demonstration consumption. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/task_program/bridge.py#L892-L892) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [BufferedGymCommandSink documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.task_program.rst) | +| CompiledTaskProgram | Immutable compilation result with materialized task segments and analysis views. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/compiler/program.py#L1343-L1343) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [CompiledTaskProgram documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.task_program.compiler.rst) | +| Effect evidence | Collects typed evidence from registered providers for semantic effects. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/semantics/evidence.py#L438-L438) · [Effect evidence documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.task_program.semantics.rst) | +| ExecutionRunner | Advances atomic skill sessions through observation, command, and clock interfaces. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/atomic_skills/runner.py#L367-L367) · [Atomic Skills execution guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/atomic_skills/index.md) · [ExecutionRunner documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_skills.rst) | +| Interactive grasp annotation | Offers browser selection of a graspable mesh region. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/scripts/annotate_grasp.py#L42-L42) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/grasp_generator.rst) | +| Mesh grasp backend | Coordinates sampling, annotation, collision checks, and caches for one mesh. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/graspkit/pg_grasp/_antipodal_backend.py#L71-L71) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/grasp_generator.rst) | +| Gripper collision checks | Checks gripper geometry against the target mesh. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/graspkit/pg_grasp/gripper_collision_checker.py#L96-L96) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/grasp_generator.rst) | +| Antipodal sampling | Samples opposing surface contacts using mesh ray casting. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/graspkit/pg_grasp/antipodal_sampler.py#L62-L62) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/grasp_generator.rst) | +| Parallel-jaw gripper configuration | Owns native hand physics, calibration and declared grasp-generator support. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/cfg/gripper.py#L53-L53) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/grasp_generator.rst) | +| Integration catalog | Describes one Task Program integration and validates compilation against its fingerprint. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/integrations/catalog.py#L617-L617) · [Integration catalog documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.rst) | +| Object backend views | Adapts rigid and articulated object state to backend-neutral Scene batches. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/backends/__init__.py#L19-L19) · [Object backend views documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Parallel execution | Coordinates parallel semantic branches through command and safety interfaces. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/runtime/parallel_executor.py#L629-L629) · [Parallel execution documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.task_program.runtime.rst) | +| Program loading & validation | Loads bounded JSON or YAML into the canonical Task Program configuration. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/language/loader.py#L17-L17) · [Program loading & validation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.task_program.language.rst) | +| SceneRegistry | Binds semantic entity identifiers to scene metadata and runtime providers. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/semantics/scene.py#L1113-L1113) · [SceneRegistry documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/scene_registry.md) | +| SimulationTaskProgramFactory | Binds simulated robots, planners, and services to construct the atomic skill engine. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/integrations/simulation/environment.py#L731-L731) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [Simulation integration API](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.task_program.integrations.simulation.rst) | +| RobotSkillProfile | Declares robot resources, skill bindings, and effect-assurance capabilities. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/semantics/profiles.py#L1495-L1495) · [RobotSkillProfile documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/robot_profiles.md) | +| EnvironmentStepClock | Maintains execution time on the environment step grid. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/task_program/bridge.py#L351-L351) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [Gym Task Program API](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.task_program.rst) | +| Surface deformables | Specializes deformable objects for cloth particle sets. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/deformable/surface.py#L28-L28) · [Surface deformables documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| TaskProgramCfg | Declarative task configuration describing intent and execution structure. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/language/schema.py#L1067-L1067) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [TaskProgramCfg documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.task_program.language.rst) | +| Toolkit directory | Standalone grasp sampling, collision-mesh preparation, URDF composition and precision assembly assets. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/__init__.py#L17-L21) · [Toolkits documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/index.rst) · [Precision Assembly guide and native demos](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/precision_assembly.md) | +| URDF components | Processes component links, joints, naming, and mesh references. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/urdf_assembly/component.py#L87-L87) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/urdf_assembly.md) | +| URDF connections | Applies component connection rules and sensor attachment joints. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/urdf_assembly/connection.py#L30-L30) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/urdf_assembly.md) | +| URDF mesh resources | Copies mesh resources and rewrites references for the assembled robot. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/urdf_assembly/mesh.py#L33-L33) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/urdf_assembly.md) | +| URDF sensor attachments | Processes sensor descriptions and attachment metadata for assembly. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/urdf_assembly/sensor.py#L75-L75) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/urdf_assembly.md) | +| URDF file output | Formats and persists the assembled URDF document. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/urdf_assembly/file_writer.py#L28-L28) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/urdf_assembly.md) | +| Volume deformables | Specializes deformable objects for volumetric soft-body particle sets. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/deformable/volume.py#L36-L36) · [Volume deformables documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst) | +| Workspace analyzer | Analyzes robot reachability and metrics for reusable workspace caches. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/motion/workspace/analyzer.py#L182-L182) · [Workspace analyzer documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/workspace_analyzer/workspace_analyzer.md) | ## Coverage and interpretation From 0557cdcdf251fb98d652c9ca658bd4414f96f3ff Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 18 Nov 2026 04:46:55 +0000 Subject: [PATCH 38/45] feat(expansion): support rolling layouts and measured phase coverage --- .../topics/expansion-engine/workloads.md | 12 +- .../topics/scene-expansion/scene-expansion.md | 6 +- docs/source/api_reference/public_api.rst | 12 ++ embodichain/lab/expansion/__init__.py | 2 + embodichain/lab/expansion/_distribution.py | 1 + embodichain/lab/expansion/_pose_cells.py | 30 +++++ embodichain/lab/expansion/_pose_sampling.py | 16 ++- .../lab/expansion/_pose_sampling_cfg.py | 17 ++- .../lab/expansion/_scene_first_sampling.py | 9 +- embodichain/lab/expansion/distribution.md | 6 + embodichain/lab/expansion/distribution.py | 75 ++++++++++++ .../lab/expansion/task_pose_coverage.py | 103 ++++++++++++++--- .../lab/sim/scene/feasibility/planning.py | 43 +++++-- .../benchmark/expansion/README.scene_first.md | 6 +- .../lab/expansion/test_layout_sampling_sim.py | 97 ++++++++++++++++ .../expansion/test_scene_first_sampling.py | 8 +- .../lab/expansion/test_task_pose_coverage.py | 107 ++++++++++++++++++ 17 files changed, 502 insertions(+), 48 deletions(-) create mode 100644 embodichain/lab/expansion/distribution.py create mode 100644 tests/lab/expansion/test_layout_sampling_sim.py diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index a3a245238c..52e7c65d8e 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -192,6 +192,12 @@ or `workspace_joint`) independently of `scheduling.policy` (`fifo` or `_pose_sampling.py` owns bounded candidate snapshots, source feedback and budgets; `task_pose_coverage.TaskPoseCoverage` implements the session coverage protocol. Measured rigid initial/terminal poses contribute only after native sink receipts. +An optional `pose_coverage.measurements` mapping selects entity states at initial, +terminal or an explicitly named successful segment end. Its grids use the same +names. This supports held-object and multi-stage tasks without creating fake +place-reference entities. `fixed: true` contributes one category inside its +declared grid; tracking noise does not create placement novelty. Proposal ranking +predicts only initial cells; later boundaries require measured T+1 evidence. Separate SceneCase identities aggregate within a declared configuration/grid fingerprint; installation changes remain separate domains. FIFO can omit coverage. @@ -212,7 +218,11 @@ preparation/conditional generation and `RobotFeasibilityBackend`/`plan_task_mode it creates no physics World and owns only the isolated collision-query resources. `layout` and `task_mode` are explicit, validated task declarations. Root layout variables currently use XY only; selected nested members retain declared XY/yaw. -B=1 is checked before runtime creation. A `bounded_pose` job may opt into the +Homogeneous replicas support rolling W times B collection, including eight rows. +Preparation snapshots the common prototype geometry, installation and nominal +full-joint planning start before any row executes. The feasibility adapter uses +an explicit solver row and never treats an active peer's live posture as its +planning start. It creates no additional physical World. A `bounded_pose` job may opt into the same layout/task-mode screens for a controlled comparison without changing ordinary simple-sampling defaults. Screening budgets are bounded and separate from fixed physical quality thresholds. Prepared candidates stay immutable by diff --git a/agent_context/topics/scene-expansion/scene-expansion.md b/agent_context/topics/scene-expansion/scene-expansion.md index 0086ea2467..688c46e261 100644 --- a/agent_context/topics/scene-expansion/scene-expansion.md +++ b/agent_context/topics/scene-expansion/scene-expansion.md @@ -44,8 +44,10 @@ root XY conditions before generating the rest of the layout. See the [conditional layout source contract](../expansion-engine/workloads.md#conditional-layout-proposals-and-task-screening) for selectors and direction scheduling. Explicit `layout` and `task_mode` declarations bind support parents, permissions and -synchronized task stages. Its current path screening requires B=1, including -for a bounded-sampling baseline that opts into the same screens. Declared +synchronized task stages. Homogeneous replicas can share the prepared layout +and frozen nominal planning start within one rolling W times B World; the +feasibility adapter explicitly selects its solver row. A bounded-sampling +baseline can opt into the same screens. Declared workspace caches supply seeds; no cache hit certifies a pose or task. Pure layout queries do not advance the owning World; topology changes invalidate the prepared geometry. This is distinct from offline atlas region validation diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 0fb8621648..ea8aeca076 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -1744,6 +1744,18 @@ and durable episode-boundary recovery. See the offline expansion example. MeasuredTrajectory record_reference_trajectory preview_expansion_comparison + report_expansion_distribution + +Offline distribution reporting validates confirmed native receipts and emits a +complete HTML/JSON/JSONL bundle without opening a physics World. Comparisons +require matching task, robot installation and configured scene domains. + +embodichain.lab.expansion.distribution +------------------------------------- + +.. currentmodule:: embodichain.lab.expansion.distribution + +.. autofunction:: report_expansion_distribution embodichain.lab.expansion.acceptance ------------------------------------ diff --git a/embodichain/lab/expansion/__init__.py b/embodichain/lab/expansion/__init__.py index 97a515b7ed..468fa04777 100644 --- a/embodichain/lab/expansion/__init__.py +++ b/embodichain/lab/expansion/__init__.py @@ -83,6 +83,7 @@ "MeasuredTrajectory", "record_reference_trajectory", "preview_expansion_comparison", + "report_expansion_distribution", ] @@ -144,6 +145,7 @@ def __getattr__(name: str) -> object: "MeasuredTrajectory": "comparison", "record_reference_trajectory": "comparison", "preview_expansion_comparison": "comparison", + "report_expansion_distribution": "distribution", }.get(name, "engine") module = f"{__name__}.{owner}" value = getattr(import_module(module), name) diff --git a/embodichain/lab/expansion/_distribution.py b/embodichain/lab/expansion/_distribution.py index c2c25cc99b..239c514fc9 100644 --- a/embodichain/lab/expansion/_distribution.py +++ b/embodichain/lab/expansion/_distribution.py @@ -269,6 +269,7 @@ def _run( "XY bins cover observed domain only; no feasible-volume denominator.", "TCP is recorded-URDF FK from measured qpos/root plus explicit gripper TCP calibration.", "Orientation bins: canonical SO(3) rotation vectors, 30-degree Cartesian bins, not equal-volume; no symmetry folding.", + "Raw terminal pose bins include tracking variation; receipt-confirmed fixed task categories are a separate coverage metric.", "Joint path length mixes joint coordinates; compare only matching domains.", "Geometry buckets reuse existing 1e-3 descriptor quantization, not clustering.", ] diff --git a/embodichain/lab/expansion/_pose_cells.py b/embodichain/lab/expansion/_pose_cells.py index 2340e7cbc8..c62014829c 100644 --- a/embodichain/lab/expansion/_pose_cells.py +++ b/embodichain/lab/expansion/_pose_cells.py @@ -24,6 +24,36 @@ __all__: list[str] = [] +def _measurements(value: Mapping, grids: Mapping) -> dict: + from copy import deepcopy + + if not isinstance(value, Mapping) or not value or set(value) != set(grids): + raise ValueError("Named coverage measurements require matching grids") + for name, declaration in value.items(): + if ( + not isinstance(name, str) + or not name + or not isinstance(declaration, Mapping) + or set(declaration) - {"entity", "boundary", "segment", "fixed"} + or not {"entity", "boundary"} <= declaration.keys() + or not isinstance(declaration["entity"], str) + or not declaration["entity"] + or declaration["boundary"] not in ("initial", "terminal", "segment_end") + ): + raise ValueError("Invalid named coverage measurement") + if (declaration["boundary"] == "segment_end") != ("segment" in declaration) or ( + "segment" in declaration + and ( + not isinstance(declaration["segment"], str) + or not declaration["segment"] + ) + ): + raise ValueError("Segment coverage requires an explicit segment name") + if "fixed" in declaration and type(declaration["fixed"]) is not bool: + raise ValueError("Coverage fixed flag must be boolean") + return deepcopy(dict(value)) + + def _grid(value: Mapping) -> dict: if not isinstance(value, Mapping) or set(value) != { "lower", diff --git a/embodichain/lab/expansion/_pose_sampling.py b/embodichain/lab/expansion/_pose_sampling.py index 85c41306fe..f4c0090353 100644 --- a/embodichain/lab/expansion/_pose_sampling.py +++ b/embodichain/lab/expansion/_pose_sampling.py @@ -164,7 +164,11 @@ def __init__(self, env, options, *, seed, revision): None if self.coverage_cfg is None else TaskPoseCoverage( - self.domain_id, self.coverage_cfg["entity"], self.coverage_cfg["grids"] + self.domain_id, + self.coverage_cfg.get("entity") + or next(iter(self.coverage_cfg["measurements"].values()))["entity"], + self.coverage_cfg["grids"], + measurements=self.coverage_cfg.get("measurements"), ) ) self.budget = _PoseBudget( @@ -443,9 +447,13 @@ def candidate(self, ordinal): cells = ( () if self.coverage is None - else self.coverage.cells( - poses[self.coverage_cfg["entity"]], - poses[self.coverage_cfg["place_reference"]], + else ( + self.coverage.proposal_cells(poses) + if self.coverage_cfg.get("measurements") is not None + else self.coverage.cells( + poses[self.coverage_cfg["entity"]], + poses[self.coverage_cfg["place_reference"]], + ) ) ) self._events.append( diff --git a/embodichain/lab/expansion/_pose_sampling_cfg.py b/embodichain/lab/expansion/_pose_sampling_cfg.py index c0ae0999ba..b454e134b2 100644 --- a/embodichain/lab/expansion/_pose_sampling_cfg.py +++ b/embodichain/lab/expansion/_pose_sampling_cfg.py @@ -151,12 +151,6 @@ def validate_sampling(value: dict) -> None: "workspace_joint does not consume layout conditions; choose scene_first or a screened bounded_pose baseline" ) if sampling["strategy"] in _LAYOUT_STRATEGIES or "layout" in sampling: - if ( - value["engine"].get("batch_size", 1) - * value["engine"].get("workload_count", 1) - != 1 - ): - raise ValueError("Layout task-path screening currently requires B=1") layout = sampling.get("layout") if ( not isinstance(layout, dict) @@ -303,6 +297,17 @@ def validate_sampling(value: dict) -> None: "task_pose_coverage scheduling requires pose_coverage grids" ) return + if isinstance(coverage, dict) and set(coverage) == {"measurements", "grids"}: + from ._pose_cells import _measurements + + measurements, grids = coverage["measurements"], coverage["grids"] + if not isinstance(grids, dict): + raise ValueError("Named coverage measurements require matching grids") + coverage["measurements"] = _measurements(measurements, grids) + if any(d["entity"] not in value["scene"] for d in measurements.values()): + raise ValueError("Coverage requires declared scene entities") + coverage["grids"] = {k: _grid(v) for k, v in grids.items()} + return if not isinstance(coverage, dict) or set(coverage) != { "entity", "place_reference", diff --git a/embodichain/lab/expansion/_scene_first_sampling.py b/embodichain/lab/expansion/_scene_first_sampling.py index f9a7c20838..b6935d1a83 100644 --- a/embodichain/lab/expansion/_scene_first_sampling.py +++ b/embodichain/lab/expansion/_scene_first_sampling.py @@ -47,8 +47,6 @@ class _LayoutPoseSource(_AdaptivePoseSource): """ def __init__(self, env, options, *, seed, revision): - if env.num_envs != 1: - raise ValueError("Layout task-path screening currently requires B=1") before = self._world_clock(env.sim) self._collision = None super().__init__(env, options, seed=seed, revision=revision) @@ -119,6 +117,7 @@ def __init__(self, env, options, *, seed, revision): f"Task phase names unavailable control part {target.control_part!r}" ) self._entities = {k: np.array(v) for k, v in entities.items()} + self._initial_qpos = None self._anchor_pools = () self._workspace_seconds = self._screening_seconds = 0.0 if self.cfg["strategy"] in _ROBOT_LAYOUT_STRATEGIES: @@ -170,6 +169,11 @@ def bind_program(self, adapter, program): """Keep the declared phase graph and live runtime independent authorities.""" if not tuple(program.iter_segments()): raise ValueError("Layout sampling requires a nonempty task program") + # The owner binds every group before rolling admission. Retain its + # nominal planning start even while another row subsequently executes. + self._initial_qpos = ( + self.env.robot.get_qpos()[0].detach().cpu().double().numpy().copy() + ) @staticmethod def _world_clock(sim): @@ -331,6 +335,7 @@ def _offer_layout(self, candidate, raw, origin): self.budget.cfg, poses, seed=self.seed + raw, + initial_qpos=self._initial_qpos, ) plan = plan_task_mode(self._mode, backend, poses, self.budget) self._task_checks[raw] = [ diff --git a/embodichain/lab/expansion/distribution.md b/embodichain/lab/expansion/distribution.md index d73bfb3343..7cde799567 100644 --- a/embodichain/lab/expansion/distribution.md +++ b/embodichain/lab/expansion/distribution.md @@ -1,5 +1,11 @@ # Task distribution report +Integrators call the lazy public `embodichain.lab.expansion.report_expansion_distribution` +with before/after native jobs, expansion aggregates or workflow directories. It +uses the same receipt reader and report writer as the command below, returns the +HTML path and snapshot identity, and creates no simulation World. Preserve the +complete output directory, including detail pages and JSONL records. + Run from an EmbodiChain environment with the existing artifact reader dependencies: ```bash diff --git a/embodichain/lab/expansion/distribution.py b/embodichain/lab/expansion/distribution.py new file mode 100644 index 0000000000..128d608fe2 --- /dev/null +++ b/embodichain/lab/expansion/distribution.py @@ -0,0 +1,75 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Offline distribution reports from confirmed native collection artifacts.""" + +from __future__ import annotations + +from collections.abc import Sequence +import json +from pathlib import Path + +__all__ = ["report_expansion_distribution"] + + +def report_expansion_distribution( + before: Sequence[str | Path], + after: Sequence[str | Path], + output: str | Path, + *, + entity: str, + cell_size_m: float = 0.01, + minimum: int = 1, +) -> dict: + """Compare matching-domain receipts without creating a simulation World. + + Args: + before: Native jobs, expansion aggregates or workflow run directories. + after: Independent or cumulative matching-domain collection directories. + output: New destination for the complete HTML/JSON/JSONL report bundle. + entity: Recorded rigid entity whose measured states are compared. + cell_size_m: World-XY bin width, selected before examining results. + minimum: Required episode count for observed-bin deficit diagnostics. + + Returns: + Report directory, HTML entry, snapshot identity and confirmed counts. + + Raises: + ValueError: For invalid inputs, mismatched domains or corrupt evidence. + FileExistsError: If the destination already exists. + """ + from ._distribution import _run + from .preview import _inputs + + def jobs(paths): + return _inputs( + [ + Path(p) / "expand" if (Path(p) / "run.json").is_file() else Path(p) + for p in paths + ] + ) + + output = Path(output).expanduser().resolve() + _run(jobs(before), jobs(after), output, entity, cell_size_m, minimum) + report = json.loads((output / "summary.json").read_text()) + return { + "directory": str(output), + "html": str(output / "report.html"), + "snapshot_id": report["snapshot_id"], + "comparison_kind": report["comparison_kind"], + "before": report["before"]["count"], + "after": report["after"]["count"], + } diff --git a/embodichain/lab/expansion/task_pose_coverage.py b/embodichain/lab/expansion/task_pose_coverage.py index 106495c268..a095c3fcba 100644 --- a/embodichain/lab/expansion/task_pose_coverage.py +++ b/embodichain/lab/expansion/task_pose_coverage.py @@ -32,7 +32,7 @@ __all__ = ["TaskPoseCoverage"] -from ._pose_cells import _grid, _pose_cell +from ._pose_cells import _grid, _pose_cell, _measurements class TaskPoseCoverage: @@ -43,20 +43,37 @@ class TaskPoseCoverage: installation and grid, excluding sampled scene poses and RNG seed. entity_uid: Manipulated rigid object whose measured states are counted. grids: Explicit ``pick`` and ``place`` XY/yaw grids in arena coordinates. + measurements: Optional named entity/boundary declarations. Initial, + terminal and named segment-end states are measured independently. + Fixed categories never turn tracking noise into pose novelty. Scene cases keep their existing identities. This adapter aggregates them within one declared experiment, never across separate instances/domains. """ - def __init__(self, domain_id: str, entity_uid: str, grids: Mapping) -> None: + def __init__( + self, + domain_id: str, + entity_uid: str, + grids: Mapping, + *, + measurements: Mapping | None = None, + ) -> None: if not all( type(s) is str and s.strip() == s and s for s in (domain_id, entity_uid) ): raise ValueError("Coverage requires nonempty domain and entity identities") - if not isinstance(grids, Mapping) or set(grids) != {"pick", "place"}: - raise ValueError("Coverage requires pick and place grids") + if not isinstance(grids, Mapping) or not grids: + raise ValueError("Coverage requires explicit grids") + if measurements is None and set(grids) != {"pick", "place"}: + raise ValueError("Legacy coverage requires pick and place grids") + if measurements is not None and set(measurements) != set(grids): + raise ValueError("Coverage measurements must match grid names") self.domain_id, self.entity_uid = domain_id, entity_uid self.grids = {k: _grid(v) for k, v in grids.items()} + self.measurements = ( + None if measurements is None else _measurements(measurements, grids) + ) self._cases: dict[str, tuple[str, str, str]] = {} self._population = None self._pending: dict[str, tuple[str, tuple]] = {} @@ -117,17 +134,43 @@ def reserve(self, episode: ExpertEpisode) -> bool: raise ValueError("Episode scene is outside this coverage domain") if not episode.validation.accepted: return False - states = episode.observations.get(f"rigid/{self.entity_uid}/state") - if ( - states is None - or states.ndim != 2 - or states.shape[0] != len(episode.timestamps) - or states.shape[1] < 7 - ): - raise ValueError("Task coverage requires T+1 measured rigid states") - cells = self.cells( - states[0].detach().cpu().numpy(), states[-1].detach().cpu().numpy() - ) + if self.measurements is None: + states = self._states(episode, self.entity_uid) + cells = self.cells( + states[0].detach().cpu().numpy(), states[-1].detach().cpu().numpy() + ) + else: + measured = [] + for phase, declaration in self.measurements.items(): + states = self._states(episode, declaration["entity"]) + boundary = declaration["boundary"] + index = 0 if boundary == "initial" else len(states) - 1 + if boundary == "segment_end": + segments = [ + s + for s in episode.metadata.get("program_segments", ()) + if s["name"] == declaration["segment"] + ] + if len(segments) != 1 or not segments[0]["success"]: + raise ValueError( + "Coverage requires one successful named segment" + ) + index = int(segments[0]["end_step"]) + if not 0 <= index < len(states): + raise ValueError( + "Coverage segment boundary is outside T+1 states" + ) + cell = _pose_cell( + states[index].detach().cpu().numpy(), self.grids[phase] + ) + if cell is not None: + measured.append( + ( + phase, + *((0, 0, 0) if declaration.get("fixed", False) else cell), + ) + ) + cells = tuple(measured) value = (case_id, cells) previous = self._confirmed.get( episode.commit_id, self._pending.get(episode.commit_id) @@ -138,6 +181,33 @@ def reserve(self, episode: ExpertEpisode) -> bool: self._pending[episode.commit_id] = value return True # Duplicates are valid data; they cannot increase unique coverage. + @staticmethod + def _states(episode: ExpertEpisode, entity: str) -> torch.Tensor: + states = episode.observations.get(f"rigid/{entity}/state") + if ( + states is None + or states.ndim != 2 + or states.shape[0] != len(episode.timestamps) + or states.shape[1] < 7 + or not bool(torch.isfinite(states).all()) + ): + raise ValueError("Task coverage requires finite T+1 measured rigid states") + return states + + def proposal_cells(self, poses: Mapping) -> tuple: + """Predict only declared initial cells; later phases require measurements.""" + if self.measurements is None: + raise ValueError( + "Legacy proposal cells require explicit pick/place references" + ) + return tuple( + (phase, *((0, 0, 0) if declaration.get("fixed", False) else cell)) + for phase, declaration in self.measurements.items() + if declaration["boundary"] == "initial" + and (cell := _pose_cell(poses[declaration["entity"]], self.grids[phase])) + is not None + ) + def confirm(self, case_id: str, commit_id: str) -> None: """Idempotently count the reservation qualified by the session's receipt.""" value = self._confirmed.get(commit_id, self._pending.get(commit_id)) @@ -186,6 +256,7 @@ def state_dict(self) -> dict: "domain_id": self.domain_id, "entity_uid": self.entity_uid, "grids": self.grids, + "measurements": self.measurements, "population": self._population, "cases": self._cases, "pending": self._pending, @@ -198,7 +269,7 @@ def load_state_dict(self, state: Mapping) -> None: data = deepcopy(dict(state)) if data.get("version") != 1 or any( data.get(k) != getattr(self, k) - for k in ("domain_id", "entity_uid", "grids") + for k in ("domain_id", "entity_uid", "grids", "measurements") ): raise ValueError("Coverage checkpoint belongs to another domain/grid") if self._pending or self._confirmed or self._cases: diff --git a/embodichain/lab/sim/scene/feasibility/planning.py b/embodichain/lab/sim/scene/feasibility/planning.py index c871153968..d07d7dec51 100644 --- a/embodichain/lab/sim/scene/feasibility/planning.py +++ b/embodichain/lab/sim/scene/feasibility/planning.py @@ -227,7 +227,7 @@ def plan_task_mode( class RobotFeasibilityBackend: - """Live B=1 Robot adapter with multi-seed IK and full-joint RRT fallback. + """Selected-row Robot adapter with multi-seed IK and full-joint RRT fallback. Args: robot: Prepared robot at the candidate's actual installation. @@ -235,6 +235,9 @@ class RobotFeasibilityBackend: cfg: Bounded branch and discretization settings. entity_poses: Settled arena poses for static and movable scene entities. seed: Candidate-local planner randomness. + env_id: Explicit prototype row for numeric solver and geometry bindings. + initial_qpos: Optional owned nominal full-joint planning start. This avoids + using another rollout's current state during rolling admission. The fallback plans in full joint coordinates, with the same collision checks for straight connections and sampled detours. It does not replace runtime @@ -249,19 +252,37 @@ def __init__( entity_poses: Mapping[str, object], *, seed: int = 0, + env_id: int = 0, + initial_qpos: np.ndarray | None = None, ) -> None: - if robot.get_qpos().shape[0] != 1 or robot.pk_chain is None: + if ( + type(env_id) is not int + or not 0 <= env_id < robot.get_qpos().shape[0] + or robot.pk_chain is None + ): raise UnsupportedFeasibilityError( - "Robot planning requires B=1 and full-link kinematics" + "Robot planning requires a valid selected row and full-link kinematics" ) self.robot, self.collision, self.cfg = robot, collision, cfg + self.env_id = env_id self.joint_names = tuple(robot.joint_names) - self.initial_qpos = robot.get_qpos()[0].detach().cpu().double().numpy().copy() + self.initial_qpos = ( + robot.get_qpos()[env_id].detach().cpu().double().numpy().copy() + if initial_qpos is None + else np.array(initial_qpos, dtype=float, copy=True) + ) + if ( + self.initial_qpos.shape != (len(self.joint_names),) + or not np.isfinite(self.initial_qpos).all() + ): + raise ValueError( + "Planning start must contain finite full-joint coordinates" + ) self.joint_limits = ( - robot.get_qpos_limits()[0].detach().cpu().double().numpy().copy() + robot.get_qpos_limits()[env_id].detach().cpu().double().numpy().copy() ) self._root = ( - robot.get_local_pose(to_matrix=True)[0].detach().cpu().double().numpy() + robot.get_local_pose(to_matrix=True)[env_id].detach().cpu().double().numpy() ) self._entities = {k: np.asarray(v) for k, v in entity_poses.items()} self._rng = np.random.default_rng(seed) @@ -380,7 +401,9 @@ def solutions( solver = self.robot.get_solver(name=target.control_part) live_root = ( self.robot.get_link_pose( - link_name=solver.root_link_name, env_ids=[0], to_matrix=True + link_name=solver.root_link_name, + env_ids=[self.env_id], + to_matrix=True, )[0] .detach() .cpu() @@ -397,7 +420,7 @@ def solutions( budget.check() samples = self.robot.sample_reachable_pose( name=target.control_part, - env_ids=[0], + env_ids=[self.env_id], num_samples=self.cfg.ik_seeds, max_attempts=self.cfg.ik_seeds, generator=torch.Generator(device=self.robot.device).manual_seed( @@ -434,7 +457,7 @@ def solutions( device=self.robot.device, ), name=target.control_part, - env_ids=[0], + env_ids=[self.env_id], return_all_solutions=True, ) budget.check() @@ -484,7 +507,7 @@ def solutions( q[None], dtype=torch.float32, device=self.robot.device ), name=target.control_part, - env_ids=[0], + env_ids=[self.env_id], to_matrix=True, )[0] .detach() diff --git a/scripts/benchmark/expansion/README.scene_first.md b/scripts/benchmark/expansion/README.scene_first.md index 655dcc7f5a..02651c1bf1 100644 --- a/scripts/benchmark/expansion/README.scene_first.md +++ b/scripts/benchmark/expansion/README.scene_first.md @@ -68,8 +68,10 @@ and plans again from the measured prepared scene. it changes preflight effort, not acceptance thresholds. All raw proposals, discarded candidates and IK/planning calls consume the declared budgets. Failed layout or task screening remains preparation rejection and never earns physical -success or a sink receipt. The B=1 path is explicit; batch settings reject before -World construction. Collision queries own no physical World and are released on +success or a sink receipt. Homogeneous W times B replicas share one prepared +prototype and retain a frozen nominal planning start; eight-row collection is +covered by `tests/lab/expansion/test_layout_sampling_sim.py`. Each proposal still +uses the complete task-path screen before normal measured execution. Collision queries own no physical World and are released on normal completion and startup failures. A topology revision invalidates the prepared source instead of silently retaining stale geometry. diff --git a/tests/lab/expansion/test_layout_sampling_sim.py b/tests/lab/expansion/test_layout_sampling_sim.py new file mode 100644 index 0000000000..1baf5423d0 --- /dev/null +++ b/tests/lab/expansion/test_layout_sampling_sim.py @@ -0,0 +1,97 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Real eight-row conditional layout collection and immutable readback.""" + +from __future__ import annotations + +import os +from pathlib import Path +import subprocess +import sys +import pytest + + +def _probe(output: Path, strategy: str) -> None: + import json + import yaml + import torch + from embodichain.lab.expansion import ( + ArtifactEpisodeSink, + load_expansion_episode, + run_task_program_expansion, + report_expansion_distribution, + ) + + torch.set_num_threads(1) + root = Path(__file__).resolve().parents[3] + fixture = ( + root + / "extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first" + ) + cfg = yaml.safe_load((fixture / "expansion.yaml").read_text()) + cfg["engine"].update( + workload_count=1, batch_size=8, episodes_per_operator=8, max_attempts=32 + ) + cfg["scene_sampling"].update(strategy=strategy, max_rollouts=32) + path = output / "expansion.yaml" + path.write_text(yaml.safe_dump(cfg)) + result = run_task_program_expansion( + fixture / "task.yaml", path, output / "collection", seed=7 + ) + execution = json.loads((output / "collection/workload-execution.json").read_text()) + assert execution["num_envs"] == 8 and execution["world"]["world_count"] == 1 + group = result["operators"]["object_pose"] + receipts = ArtifactEpisodeSink(group["directory"]).inspect() + assert len(receipts) == group["committed_episodes"] == 8 + selected = set() + for receipt in receipts: + episode = load_expansion_episode(receipt.storage_id) + assert episode.validation.accepted + assert len(episode.timestamps) == len(episode.actions) + 1 + selected.add(episode.metadata["source_env_id"]) + assert selected == set(range(8)) + source = Path(group["directory"]) / "scene-first-checks.json" + checks = json.loads(source.read_text())["phase_checks"] + assert checks and all( + any(c["stage"] == "screening.clock" and c["status"] == "passed" for c in rows) + for rows in checks.values() + ) + report = report_expansion_distribution( + [output / "collection"], + [output / "collection"], + output / "distribution", + entity="cube", + cell_size_m=0.01, + ) + assert report["before"] == report["after"] == 8 + assert report["comparison_kind"] == "cumulative" + + +@pytest.mark.parametrize("strategy", ["scene_first", "robot_first", "bidirectional"]) +def test_conditional_layout_uses_one_world_and_eight_real_rows(tmp_path, strategy): + result = subprocess.run( + [sys.executable, __file__, str(tmp_path), strategy], + env={**os.environ, "EMBODICHAIN_SIM_EXIT_PROCESS": "0"}, + capture_output=True, + text=True, + timeout=600, + ) + assert result.returncode == 0, result.stdout + result.stderr + + +if __name__ == "__main__": + _probe(Path(sys.argv[1]), sys.argv[2]) diff --git a/tests/lab/expansion/test_scene_first_sampling.py b/tests/lab/expansion/test_scene_first_sampling.py index 6564b3eb9d..76c4ce805f 100644 --- a/tests/lab/expansion/test_scene_first_sampling.py +++ b/tests/lab/expansion/test_scene_first_sampling.py @@ -320,7 +320,7 @@ def test_source_obeys_hard_raw_candidate_budget(monkeypatch): assert source.budget.counts["candidates"] == 64 -def test_scene_first_config_rejects_batch_and_unknown_phase_fields_before_runtime(): +def test_scene_first_config_accepts_batch_and_rejects_unknown_phase_fields(): # Reuse a valid source's owned config, without constructing a new World. import yaml from pathlib import Path @@ -331,10 +331,8 @@ def test_scene_first_config_rejects_batch_and_unknown_phase_fields_before_runtim / "extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/acceptance/scene_first/expansion.yaml" ) cfg = yaml.safe_load(path.read_text()) - cfg["engine"]["batch_size"] = 4 - with pytest.raises(ValueError, match="B=1"): - validate_sampling(cfg) - cfg["engine"]["batch_size"] = 1 + cfg["engine"]["batch_size"] = 8 + validate_sampling(cfg) cfg["scene_sampling"]["task_mode"]["phases"][0]["unknown"] = True with pytest.raises(ValueError, match="Unknown TaskPhase"): validate_sampling(cfg) diff --git a/tests/lab/expansion/test_task_pose_coverage.py b/tests/lab/expansion/test_task_pose_coverage.py index 2ffb4b10e1..9c008eb475 100644 --- a/tests/lab/expansion/test_task_pose_coverage.py +++ b/tests/lab/expansion/test_task_pose_coverage.py @@ -131,6 +131,113 @@ def test_cells_boundaries_periodicity_and_measured_not_requested_pose(): assert cov.gain((("pick", 0, 2, 1),)) == 0.5 +def test_named_boundaries_use_real_artifacts_and_fixed_categories(tmp_path): + from embodichain.lab.expansion import ArtifactEpisodeSink, load_expansion_episode + from embodichain.lab.expansion.contracts import ( + CandidateIdentity, + ExpertEpisode, + ValidationCheck, + ValidationResult, + ) + + measurements = { + "initial": {"entity": "object", "boundary": "initial"}, + "handover": { + "entity": "object", + "boundary": "segment_end", + "segment": "handover", + "fixed": True, + }, + "handover_pose": { + "entity": "object", + "boundary": "segment_end", + "segment": "handover", + }, + "held": {"entity": "object", "boundary": "terminal", "fixed": True}, + } + grid = grids()["pick"] + cov = TaskPoseCoverage( + "domain", + "object", + {name: grid for name in measurements}, + measurements=measurements, + ) + cov.register_case( + SceneCase("case", "initial", "signature", "task", "robot"), + torch.tensor([[-1.0, 1.0]]), + ("joint",), + None, + ) + sink = ArtifactEpisodeSink(tmp_path / "episodes") + for ordinal, x in enumerate((-0.7, 0.7)): + state = torch.tensor( + [ + [x, 0.1, 0.1, 0, 0, 0, 1], + [0.1, 0.1, 0.1, 0, 0, 0, 1], + [0.1 + ordinal * 0.6, 0.1, 0.1, 0, 0, 0, 1], + ] + ) + episode = ExpertEpisode( + CandidateIdentity( + "case", + "initial", + f"candidate{ordinal}", + "family", + "source", + "v1", + "plan", + ), + {"rigid/object/state": state}, + torch.zeros(2, 1), + torch.tensor([0.0, 0.1, 0.2]), + "qpos", + ValidationResult((ValidationCheck("task", "passed"),)), + f"episode{ordinal}", + f"commit{ordinal}", + metadata={ + "program_segments": [ + { + "name": "handover", + "start_step": 0, + "end_step": 1, + "success": True, + } + ] + }, + ) + assert cov.reserve(episode) + assert cov.phase_counts()["initial"] == ordinal + receipt = sink.submit(episode, submission_id=ordinal) + restored = load_expansion_episode(receipt.storage_id) + assert cov.reserve(restored) + cov.confirm("case", episode.commit_id) + cov.confirm("case", episode.commit_id) + assert cov.phase_counts() == { + "initial": 2, + "handover": 1, + "handover_pose": 1, + "held": 1, + } + assert ("handover_pose", 2, 2, 1) in cov.state_dict()["confirmed"]["commit1"][1] + assert cov.proposal_cells({"object": np.eye(4)}) == (("initial", 2, 2, 1),) + snapshot = cov.state_dict() + restored = TaskPoseCoverage( + "domain", + "object", + {name: grid for name in measurements}, + measurements=measurements, + ) + restored.load_state_dict(snapshot) + assert restored.phase_counts() == cov.phase_counts() + with pytest.raises(ValueError, match="segment name"): + TaskPoseCoverage( + "domain", + "object", + {"bad": grid}, + measurements={"bad": {"entity": "object", "boundary": "segment_end"}}, + ) + + class _Robot: control_parts = {"arm": [0, 1, 2]} device = torch.device("cpu") From 88c2e18503a9beeb3403d6623aba55389b5d09ac Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 18 Nov 2026 05:15:21 +0000 Subject: [PATCH 39/45] fix(packaging): include expansion distribution report template --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index a301c9939e..fde2fea84d 100644 --- a/setup.py +++ b/setup.py @@ -153,7 +153,7 @@ def get_package_data() -> dict[str, list[str]]: ] return { "embodichain": ["VERSION"], - "embodichain.lab.expansion": ["_preview.html"], + "embodichain.lab.expansion": ["_preview.html", "_distribution.html"], "embodichain.data": ["default_materials.json"], "embodichain.compute.kinematics._warp": [ "ssik_LICENSE.txt", From 4132d860105147c1090bfa71ec7cdcae2f259aff Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 18 Nov 2026 05:29:07 +0000 Subject: [PATCH 40/45] docs(expansion): correct distribution API heading --- docs/source/api_reference/public_api.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index ea8aeca076..39bea20b8c 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -1751,7 +1751,7 @@ complete HTML/JSON/JSONL bundle without opening a physics World. Comparisons require matching task, robot installation and configured scene domains. embodichain.lab.expansion.distribution -------------------------------------- +-------------------------------------- .. currentmodule:: embodichain.lab.expansion.distribution From 65f7d5d63bb36d93de4714c33318c6cb30ec003d Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 18 Nov 2026 05:55:34 +0000 Subject: [PATCH 41/45] feat(expansion): separate nominal scene and proposal seeds --- .../topics/expansion-engine/workloads.md | 6 ++++++ .../lab/expansion/_task_program_config.py | 5 +++++ embodichain/lab/expansion/gym_workloads.py | 20 ++++++++++++++++--- .../lab/expansion/task_program_jobs.py | 1 + embodichain/lab/expansion/workloads.py | 14 ++++++++++++- tests/lab/expansion/test_configured.py | 9 +++++++++ .../lab/expansion/test_layout_sampling_sim.py | 10 ++++++++-- 7 files changed, 59 insertions(+), 6 deletions(-) diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index 52e7c65d8e..1ef5208038 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -189,6 +189,12 @@ switch devices. Task-facing expansion configs select `scene_sampling.strategy` (`bounded_pose` or `workspace_joint`) independently of `scheduling.policy` (`fifo` or `task_pose_coverage`). Absent settings preserve the legacy bounded sampler. +`engine.nominal_seed` optionally fixes the authored scene reset independently of +the expansion seed. Without it, both retain the historical shared seed. Runtime +candidate streams and proposal streams still use the expansion seed/ordinal; +the reference manifest records both seeds. This prevents a sampler comparison +from silently changing the nominal scene's reset randomization and moving its +declared local domain. Rolling resets use the same explicit nominal reset seed. `_pose_sampling.py` owns bounded candidate snapshots, source feedback and budgets; `task_pose_coverage.TaskPoseCoverage` implements the session coverage protocol. Measured rigid initial/terminal poses contribute only after native sink receipts. diff --git a/embodichain/lab/expansion/_task_program_config.py b/embodichain/lab/expansion/_task_program_config.py index 73c464e8f2..40ff525a1a 100644 --- a/embodichain/lab/expansion/_task_program_config.py +++ b/embodichain/lab/expansion/_task_program_config.py @@ -104,6 +104,7 @@ def load_task_program_expansion(path: str | Path) -> dict[str, Any]: "collection_renderer", "collision_backend", "reuse_idle_rows", + "nominal_seed", }: raise ValueError("Unknown or missing expansion engine settings") engine.setdefault("cpu_threads", 1) @@ -122,6 +123,10 @@ def load_task_program_expansion(path: str | Path) -> dict[str, Any]: engine.setdefault("collision_backend", "newton") if "reuse_idle_rows" in engine and type(engine["reuse_idle_rows"]) is not bool: raise ValueError("engine.reuse_idle_rows must be a boolean") + if "nominal_seed" in engine and ( + type(engine["nominal_seed"]) is not int or engine["nominal_seed"] < 0 + ): + raise ValueError("engine.nominal_seed must be a nonnegative integer") if engine["collection_renderer"] not in ("auto", "rt", "fast-rt", "hybrid"): raise ValueError( "engine.collection_renderer must be auto, rt, fast-rt or hybrid" diff --git a/embodichain/lab/expansion/gym_workloads.py b/embodichain/lab/expansion/gym_workloads.py index c8e32ac457..20a58221d2 100644 --- a/embodichain/lab/expansion/gym_workloads.py +++ b/embodichain/lab/expansion/gym_workloads.py @@ -759,7 +759,15 @@ def physical_key(env_id: str, cfg: Any) -> str: writer = ThreadPoolExecutor(max_workers=1, thread_name_prefix="expansion-export") try: cfg = ExpansionRunCfg( - **{**workloads[0].cfg.to_dict(), "batch_size": execution.num_envs} + **{ + **workloads[0].cfg.to_dict(), + "batch_size": execution.num_envs, + "seed": ( + workloads[0].cfg.seed + if execution.nominal_seed is None + else execution.nominal_seed + ), + } ) owner = GymExpansionHost( cfg, @@ -786,7 +794,9 @@ def physical_key(env_id: str, cfg: Any) -> str: # No trajectories are live during this one nominal reset. event.params["allow_partial_envs"] = True events.set_functor_cfg(name, event) - env.reset(seed=execution.seed, options={"save_data": False, "reset_ids": [0]}) + env.reset( + seed=execution.reset_seed, options={"save_data": False, "reset_ids": [0]} + ) # The reference now owns all reset-time scene randomization/settling. # Re-running those functors during row refill could advance live peers # outside the executor and lose measured control boundaries. @@ -810,7 +820,11 @@ def physical_key(env_id: str, cfg: Any) -> str: } _atomic_json( root / "reference-scene.json", - {"seed": execution.seed, "poses_xyzw": nominal}, + { + "seed": execution.reset_seed, + "sampling_seed": execution.seed, + "poses_xyzw": nominal, + }, ) for workload, (raw, environment_cfg) in zip(workloads, configs, strict=True): path = Path(workload.cfg.output_dir) diff --git a/embodichain/lab/expansion/task_program_jobs.py b/embodichain/lab/expansion/task_program_jobs.py index 55af8d08d0..d62af0d1c1 100644 --- a/embodichain/lab/expansion/task_program_jobs.py +++ b/embodichain/lab/expansion/task_program_jobs.py @@ -361,6 +361,7 @@ def _run_operators( settling_steps=settings["settling_steps"], seed=seed, reuse_idle_rows=settings.get("reuse_idle_rows", False), + nominal_seed=settings.get("nominal_seed"), ) chunks = settings["grasp_nms_chunk_size"] if torch.cuda.is_available() else 2048 with pose_nms_scope(chunk_size=chunks): diff --git a/embodichain/lab/expansion/workloads.py b/embodichain/lab/expansion/workloads.py index bbb27180bc..edbc884b2c 100644 --- a/embodichain/lab/expansion/workloads.py +++ b/embodichain/lab/expansion/workloads.py @@ -92,6 +92,8 @@ class WorkloadExecutionCfg: max_steps: Maximum ordinary controller commands per candidate. settling_steps: Minimum physical substeps before a row becomes active. seed: Base seed; each candidate owns a stream keyed by local ordinal. + nominal_seed: Optional independent reset seed for the authored nominal + scene. Omission preserves the historical coupling to ``seed``. batch_planning: Cooperatively merge compatible native motion/solver calls. reuse_idle_rows: Let unfinished groups borrow otherwise idle rows. A rejected attempt permits one additional competing candidate, bounded @@ -106,6 +108,7 @@ class WorkloadExecutionCfg: seed: int = 0 batch_planning: bool = True reuse_idle_rows: bool = False + nominal_seed: int | None = None def __post_init__(self) -> None: for name in ("workload_count", "batch_size", "max_steps"): @@ -118,6 +121,15 @@ def __post_init__(self) -> None: raise TypeError("batch_planning must be a bool") if type(self.reuse_idle_rows) is not bool: raise TypeError("reuse_idle_rows must be a bool") + if self.nominal_seed is not None and ( + type(self.nominal_seed) is not int or self.nominal_seed < 0 + ): + raise ValueError("nominal_seed must be a nonnegative integer or None") + + @property + def reset_seed(self) -> int: + """Return the declared nominal scene seed, with legacy coupling by default.""" + return self.seed if self.nominal_seed is None else self.nominal_seed @property def num_envs(self) -> int: @@ -400,7 +412,7 @@ def _fill(self) -> None: assignments = tuple(added) before = self.env.sim.physics_step_index result = self.env.reset( - seed=self.cfg.seed, + seed=self.cfg.reset_seed, options={ "save_data": False, "reset_ids": [a.env_id for a in added], diff --git a/tests/lab/expansion/test_configured.py b/tests/lab/expansion/test_configured.py index 94bb78c69f..2f018fb898 100644 --- a/tests/lab/expansion/test_configured.py +++ b/tests/lab/expansion/test_configured.py @@ -42,6 +42,15 @@ def test_task_program_declarations_are_pure_and_protect_contact_phases(tmp_path) value = json.loads(process.stdout) assert (value["engine"]["workload_count"], value["engine"]["batch_size"]) == (2, 1) assert value["trajectory"]["operators"] == ["object_pose", "joint_residual"] + value["engine"]["nominal_seed"] = 7 + nominal = tmp_path / "nominal.yaml" + nominal.write_text(yaml.safe_dump(value)) + assert load_task_program_expansion(nominal)["engine"]["nominal_seed"] == 7 + value["engine"]["nominal_seed"] = True + nominal.write_text(yaml.safe_dump(value)) + with pytest.raises(ValueError, match="nominal_seed"): + load_task_program_expansion(nominal) + value["engine"]["nominal_seed"] = 7 value["trajectory"]["free_phases"]["pick_up"] = ["approach", "lift"] invalid = tmp_path / "protected.yaml" invalid.write_text(yaml.safe_dump(value)) diff --git a/tests/lab/expansion/test_layout_sampling_sim.py b/tests/lab/expansion/test_layout_sampling_sim.py index 1baf5423d0..1c92261536 100644 --- a/tests/lab/expansion/test_layout_sampling_sim.py +++ b/tests/lab/expansion/test_layout_sampling_sim.py @@ -44,15 +44,21 @@ def _probe(output: Path, strategy: str) -> None: ) cfg = yaml.safe_load((fixture / "expansion.yaml").read_text()) cfg["engine"].update( - workload_count=1, batch_size=8, episodes_per_operator=8, max_attempts=32 + workload_count=1, + batch_size=8, + episodes_per_operator=8, + max_attempts=32, + nominal_seed=7, ) cfg["scene_sampling"].update(strategy=strategy, max_rollouts=32) path = output / "expansion.yaml" path.write_text(yaml.safe_dump(cfg)) result = run_task_program_expansion( - fixture / "task.yaml", path, output / "collection", seed=7 + fixture / "task.yaml", path, output / "collection", seed=11 ) execution = json.loads((output / "collection/workload-execution.json").read_text()) + reference = json.loads((output / "collection/reference-scene.json").read_text()) + assert (reference["seed"], reference["sampling_seed"]) == (7, 11) assert execution["num_envs"] == 8 and execution["world"]["world_count"] == 1 group = result["operators"]["object_pose"] receipts = ArtifactEpisodeSink(group["directory"]).inspect() From 657daec6c21a9d8f296dd0f4b5083472e3117845 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 18 Nov 2026 06:00:37 +0000 Subject: [PATCH 42/45] fix(expansion): stabilize installation identity in offline reports --- embodichain/lab/expansion/_distribution.py | 24 +++++++++++++++++++++- embodichain/lab/expansion/distribution.md | 4 ++++ tests/lab/expansion/test_distribution.py | 15 ++++++++++++++ 3 files changed, 42 insertions(+), 1 deletion(-) diff --git a/embodichain/lab/expansion/_distribution.py b/embodichain/lab/expansion/_distribution.py index 239c514fc9..421751f942 100644 --- a/embodichain/lab/expansion/_distribution.py +++ b/embodichain/lab/expansion/_distribution.py @@ -43,6 +43,23 @@ def _cell(pose: list[float], cell_size: float) -> str: return ",".join(str(int(x)) for x in np.floor(np.asarray(pose[:2]) / cell_size)) +def _installation_pose(pose: list[float]) -> list[float]: + """Group installation readout jitter only; never alter recorded/FK evidence.""" + value = np.array(pose, dtype=float, copy=True) + if value.shape != (7,) or not np.isfinite(value).all(): + raise ValueError("Installation requires a finite XYZ/xyzw pose") + norm = np.linalg.norm(value[3:]) + if norm < 1e-12: + raise ValueError("Installation quaternion must be nonzero") + value[3:] /= norm + value = np.round(value, 6) + first = next(v for v in value[[6, 3, 4, 5]] if v != 0) + if first < 0: + value[3:] *= -1 + value[value == 0] = 0.0 + return value.tolist() + + def _motion(points: np.ndarray, times: np.ndarray) -> dict: if points.ndim != 2 or points.shape[0] != len(times) or len(times) < 2: raise ValueError("Motion requires aligned T+1 points and timestamps") @@ -149,7 +166,11 @@ def _read(paths: list[Path], entity: str) -> tuple[list[dict], list[str]]: meta = episode.metadata # Fixed topology, installation and task identity; sampled object poses are features. robot = [ - dict(uid=s["uid"], joints=list(s["joint_names"]), pose=list(s["pose"])) + dict( + uid=s["uid"], + joints=list(s["joint_names"]), + pose=_installation_pose(s["pose"]), + ) for s in meta.get("initial_state", ()) if s["kind"] == "robot" ] @@ -270,6 +291,7 @@ def _run( "TCP is recorded-URDF FK from measured qpos/root plus explicit gripper TCP calibration.", "Orientation bins: canonical SO(3) rotation vectors, 30-degree Cartesian bins, not equal-volume; no symmetry folding.", "Raw terminal pose bins include tracking variation; receipt-confirmed fixed task categories are a separate coverage metric.", + "Installation grouping rounds measured XYZ and canonical normalized xyzw to 1e-6; declared calibration is exact and raw states/FK remain unrounded.", "Joint path length mixes joint coordinates; compare only matching domains.", "Geometry buckets reuse existing 1e-3 descriptor quantization, not clustering.", ] diff --git a/embodichain/lab/expansion/distribution.md b/embodichain/lab/expansion/distribution.md index 7cde799567..65351f0b7d 100644 --- a/embodichain/lab/expansion/distribution.md +++ b/embodichain/lab/expansion/distribution.md @@ -20,6 +20,10 @@ Inputs must be job directories containing `job.json` and confirmed artifact rece Repeated input paths/commit identities do not multiply samples. Conflicting commit identities fail. Output must be new; inputs are never overwritten. Both snapshots must have the same task, robot installation, topology and configured scene domain. +For installation identity only, measured XYZ and sign-canonical normalized xyzw +components are rounded to 1e-6 to avoid splitting one fixed base by readout noise. +Declared robot calibration remains exact in the domain fingerprint. Raw episode +poses, TCP FK and every physical acceptance threshold remain unchanged. Open `report.html`. Select a spatial region or joint-distribution cell to inspect recorded object paths. The evidence panel identifies the full artifact and the diff --git a/tests/lab/expansion/test_distribution.py b/tests/lab/expansion/test_distribution.py index 3e6d07a7a3..7fdcf87c1e 100644 --- a/tests/lab/expansion/test_distribution.py +++ b/tests/lab/expansion/test_distribution.py @@ -20,6 +20,21 @@ from embodichain.lab.expansion._distribution import _motion, _summarize +def test_installation_noise_does_not_split_domain_or_modify_raw_poses(): + from embodichain.lab.expansion._distribution import _installation_pose + + measured = [2.3283064365386963e-10, -3.725290298461914e-9, 0, 0, 0, 0, -1] + raw = list(measured) + assert _installation_pose(measured) == _installation_pose([0, 0, 0, 0, 0, 0, 1]) + assert measured == raw + assert _installation_pose([0.01, 0, 0, 0, 0, 0, 1]) != _installation_pose(measured) + assert _installation_pose([0, 0, 0, 0, 0, -1, -1e-10]) == _installation_pose( + [0, 0, 0, 0, 0, 1, 1e-10] + ) + with pytest.raises(ValueError, match="nonzero"): + _installation_pose([0] * 7) + + def test_motion_density_and_time_are_separate(): a = _motion(np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]]), np.array([0.0, 2.0])) b = _motion( From f18d7f7f4788a987650b3f4bfa74ce369e430e4e Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 18 Nov 2026 06:09:24 +0000 Subject: [PATCH 43/45] fix(expansion): enforce task identity and layout admission contracts Share simulator-free layout and feasibility declaration validation. Preserve final parent follower poses, distinguish sampled rejection from invalid authored anchors, retire exhausted warming rows, and record event transport history. --- agent_context/MAP.yaml | 1 + .../topics/expansion-engine/workloads.md | 14 +- .../topics/scene-expansion/scene-expansion.md | 1 + embodichain/lab/_scene_config/__init__.py | 20 + embodichain/lab/_scene_config/feasibility.py | 294 ++++++++++++ embodichain/lab/_scene_config/policy.py | 430 ++++++++++++++++++ embodichain/lab/_scene_config/scene.py | 157 +++++++ embodichain/lab/_scene_config/values.py | 195 ++++++++ embodichain/lab/expansion/_distribution.py | 16 +- embodichain/lab/expansion/_pose_sampling.py | 21 +- .../lab/expansion/_pose_sampling_cfg.py | 73 ++- .../lab/expansion/_robot_first_sampling.py | 67 +-- .../lab/expansion/_scene_first_sampling.py | 34 +- embodichain/lab/expansion/distribution.md | 3 + embodichain/lab/expansion/gym_workloads.py | 17 +- embodichain/lab/sim/scene/_support_grid.py | 39 +- embodichain/lab/sim/scene/_validation.py | 65 +-- .../lab/sim/scene/expansion/_policy.py | 325 +------------ .../lab/sim/scene/expansion/conditions.py | 30 +- .../lab/sim/scene/expansion/contracts.py | 148 +----- embodichain/lab/sim/scene/expansion/layout.py | 65 +-- .../lab/sim/scene/feasibility/_values.py | 133 +----- .../lab/sim/scene/feasibility/contracts.py | 239 +--------- .../lab/sim/scene/feasibility/persistence.py | 48 +- .../task_program/integrations/feasibility.py | 13 +- .../gym/envs/task_program/test_feasibility.py | 38 ++ tests/lab/expansion/test_configured.py | 34 ++ tests/lab/expansion/test_distribution.py | 49 ++ tests/lab/expansion/test_gym_workloads.py | 69 ++- .../expansion/test_scene_first_sampling.py | 47 ++ 30 files changed, 1563 insertions(+), 1122 deletions(-) create mode 100644 embodichain/lab/_scene_config/__init__.py create mode 100644 embodichain/lab/_scene_config/feasibility.py create mode 100644 embodichain/lab/_scene_config/policy.py create mode 100644 embodichain/lab/_scene_config/scene.py create mode 100644 embodichain/lab/_scene_config/values.py diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 90d9f63338..8f2c36270a 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -303,6 +303,7 @@ topics: SimulationSceneExpansionHost, SimulationSceneInitialState, episode_prepare] paths: [topics/scene-expansion/scene-expansion.md] source_of_truth: + - embodichain/lab/_scene_config/ - embodichain/lab/sim/scene/feasibility/ - embodichain/lab/sim/scene/expansion/ - embodichain/lab/task_program/integrations/feasibility.py diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index 52e7c65d8e..cfad989c0c 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -88,11 +88,11 @@ After at least the declared physical settling substeps, `activate` captures the initial measured boundary, performs the source's required preparation checks, registers a native candidate and uses `ExpansionEngine.admit_row`. It returns a fresh bridge bound exactly to that row and the environment's control cadence. -Rejected preparation returns None and never earns quota. A warming row whose -admission time budget expires is retired without proposal or receipt; its group -can finish incomplete while already-admitted peers continue and export normally. -The native source also handles expiry during initial-state validation before -atomic session proposal. +Rejected preparation returns None and never earns quota. The native source retires +warming rows when the admission time or independent rollout budget is exhausted. +It records the stop reason before proposal, including exhaustion detected during +initial-state validation. The group can finish incomplete while already-admitted +peers continue and export normally. Settling rows remain controller-active so Gym consumes their explicit reset hold targets. They are not live trajectories: only cursor rows receive source @@ -227,6 +227,10 @@ same layout/task-mode screens for a controlled comparison without changing ordinary simple-sampling defaults. Screening budgets are bounded and separate from fixed physical quality thresholds. Prepared candidates stay immutable by ordinal; failed screening cannot turn into an accepted simple fallback. +Static policy and task-mode decoding use shared simulator-free declarations. +Authored anchors must satisfy the prepared layout domain. An out-of-domain +bounded draw rejects only that proposal. Followers outside the support tree +inherit the final parent's displacement before task screening. `robot_first` anchors one movable support-tree root per proposal from the workspace of a declared `robot_anchors` phase/part target. It uses the target's diff --git a/agent_context/topics/scene-expansion/scene-expansion.md b/agent_context/topics/scene-expansion/scene-expansion.md index 688c46e261..ae6adb910c 100644 --- a/agent_context/topics/scene-expansion/scene-expansion.md +++ b/agent_context/topics/scene-expansion/scene-expansion.md @@ -57,6 +57,7 @@ and does not certify continuous reachable volume. | Request | Owner | |---|---| +| Simulator-free layout policy and feasibility declarations | `embodichain/lab/_scene_config/` | | Shared proposal contracts and fixed source | `embodichain/lab/sim/scene/expansion/` | | Candidate limits and movable entity permissions | `cfg.py` in that package | | Arena poses and proposal identity | `contracts.py` in that package | diff --git a/embodichain/lab/_scene_config/__init__.py b/embodichain/lab/_scene_config/__init__.py new file mode 100644 index 0000000000..b5ff999488 --- /dev/null +++ b/embodichain/lab/_scene_config/__init__.py @@ -0,0 +1,20 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Scene layout and feasibility declarations with no simulator imports.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/embodichain/lab/_scene_config/feasibility.py b/embodichain/lab/_scene_config/feasibility.py new file mode 100644 index 0000000000..d69e076d71 --- /dev/null +++ b/embodichain/lab/_scene_config/feasibility.py @@ -0,0 +1,294 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Closed, immutable scene and task feasibility request declarations.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass, fields +from functools import cached_property +import math +from typing import Literal + +from .scene import ScenePoseChange +from .values import _identifier, _rigid_pose, digest, finite, names, poses + +__all__: list[str] = [] + + +_IDENTITY = ( + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.0), + (0.0, 0.0, 0.0, 1.0), +) + + +@dataclass(frozen=True) +class PoseSearchVariable: + """One XY/yaw displacement applied to a scene group or robot installation. + + Args: + name: Unique variable name. + kind: ``scene`` or ``robot``; robot members are physical robot UIDs. + reference_pose: Group-to-arena Z-up transform, including mounting height. + lower: Lower XY metre / yaw radian displacement bounds. + upper: Upper displacement bounds; equal endpoints fix a parameter. + members: Member UID to member-to-group transforms. All members move + together, preserving dual-arm mounting and support-group relations. + """ + + name: str + kind: Literal["scene", "robot"] + reference_pose: tuple + lower: tuple[float, float, float] + upper: tuple[float, float, float] + members: Mapping[str, tuple] + + def __post_init__(self) -> None: + _identifier(self.name, "variable name") + if self.kind not in ("scene", "robot"): + raise ValueError("variable kind must be scene or robot") + object.__setattr__(self, "reference_pose", _rigid_pose(self.reference_pose)) + lo, hi = finite(self.lower, "lower"), finite(self.upper, "upper") + if len(lo) != 3 or len(hi) != 3 or any(a > b for a, b in zip(lo, hi)): + raise ValueError("bounds require three ordered XY/yaw coordinates") + if hi[2] - lo[2] > 2 * math.pi + 1e-10: + raise ValueError("yaw interval must span at most one turn") + object.__setattr__(self, "lower", lo) + object.__setattr__(self, "upper", hi) + object.__setattr__(self, "members", poses(self.members)) + if not self.members: + raise ValueError("variables require explicit member UIDs") + + +@dataclass(frozen=True) +class PhaseTarget: + """One simultaneous TCP target in an explicit arena or entity frame. + + Args: + control_part: Explicit solver name, preserving custom names. + pose: TCP-to-frame rigid transform. + frame_uid: Entity frame UID, or ``None`` for the arena frame. + """ + + control_part: str + pose: tuple + frame_uid: str | None = None + + def __post_init__(self) -> None: + _identifier(self.control_part, "control_part") + object.__setattr__(self, "pose", _rigid_pose(self.pose)) + if self.frame_uid is not None: + _identifier(self.frame_uid, "frame_uid") + + +@dataclass(frozen=True) +class HeldObject: + """Rigid attachment during travel; never a physical-success assertion. + + Args: + entity_uid: Held object UID. + control_part: Owner TCP/control part. + tcp_from_object: Object-to-TCP transform used by collision FK. + """ + + entity_uid: str + control_part: str + tcp_from_object: tuple = _IDENTITY + + def __post_init__(self) -> None: + _identifier(self.entity_uid, "held entity") + _identifier(self.control_part, "held owner") + object.__setattr__(self, "tcp_from_object", _rigid_pose(self.tcp_from_object)) + + +@dataclass(frozen=True) +class TaskPhase: + """A synchronized stage and its explicit planning-state transition. + + Args: + phase_id: Stable stage identity. + targets: Simultaneous targets, at most one per control part. + dependencies: Predecessors; input order must be a topological order. + held_objects: Attachments during incoming motion, one owner per object. + allowed_contacts: Physical collision IDs allowed to touch in this stage. + resulting_poses: Projected object states after this stage, used only for + subsequent planning; runtime must still read fresh physical state. + joint_targets: Explicit joint-name commands, e.g. open/closed fingers. + """ + + phase_id: str + targets: tuple[PhaseTarget, ...] = () + dependencies: tuple[str, ...] = () + held_objects: tuple[HeldObject, ...] = () + allowed_contacts: tuple[tuple[str, str], ...] = () + resulting_poses: tuple[ScenePoseChange, ...] = () + joint_targets: tuple[tuple[str, float], ...] = () + + def __post_init__(self) -> None: + _identifier(self.phase_id, "phase_id") + for attr, cls in ( + ("targets", PhaseTarget), + ("held_objects", HeldObject), + ("resulting_poses", ScenePoseChange), + ): + values = tuple(getattr(self, attr)) + if any(not isinstance(v, cls) for v in values): + raise TypeError(f"{attr} contains an invalid contract") + object.__setattr__(self, attr, values) + names([v.control_part for v in self.targets], "phase target parts") + names([v.entity_uid for v in self.held_objects], "held objects") + names([v.entity_uid for v in self.resulting_poses], "resulting poses") + object.__setattr__( + self, "dependencies", names(self.dependencies, "dependencies") + ) + pairs = tuple( + tuple(sorted(names(pair, "contact pair"))) for pair in self.allowed_contacts + ) + if any(len(p) != 2 for p in pairs) or len(set(pairs)) != len(pairs): + raise ValueError("allowed contacts require unique distinct pairs") + object.__setattr__(self, "allowed_contacts", pairs) + joints = tuple( + (_identifier(k, "joint target"), finite((v,), "joint target")[0]) + for k, v in self.joint_targets + ) + names([k for k, _ in joints], "joint targets") + object.__setattr__(self, "joint_targets", joints) + + +@dataclass(frozen=True) +class TaskOperationMode: + """One permitted grasp/assignment/handover branch of the original task.""" + + mode_id: str + phases: tuple[TaskPhase, ...] + + def __post_init__(self) -> None: + _identifier(self.mode_id, "mode_id") + values = tuple(self.phases) + if not values or any(not isinstance(v, TaskPhase) for v in values): + raise ValueError("modes require explicit task phases") + names([v.phase_id for v in values], "phase IDs") + seen = set() + for phase in values: + if not set(phase.dependencies) <= seen: + raise ValueError("phases must be in dependency order without cycles") + seen.add(phase.phase_id) + object.__setattr__(self, "phases", values) + + +@dataclass(frozen=True) +class TaskFeasibilityRequest: + """Versioned fixed semantics, robot, scene, and allowed joint search domain. + + Args: + task_id: Semantic identity including original roles/goals/invariants. + robot_profile_id: Robot, tool and calibrated configuration identity. + policy_id: Planner/runtime policy version. + validator_id: Measured acceptance version. + parent_scene_id: Base scene revision/content identity. + entity_poses: Authored arena poses, including every variable member. + variables: Permitted scene and mounting changes. + modes: Allowed operation modes, preserving resource constraints. + """ + + task_id: str + robot_profile_id: str + policy_id: str + validator_id: str + parent_scene_id: str + entity_poses: Mapping[str, tuple] + variables: tuple[PoseSearchVariable, ...] + modes: tuple[TaskOperationMode, ...] + + def __post_init__(self) -> None: + for key in ( + "task_id", + "robot_profile_id", + "policy_id", + "validator_id", + "parent_scene_id", + ): + _identifier(getattr(self, key), key) + object.__setattr__(self, "entity_poses", poses(self.entity_poses)) + variables, modes = tuple(self.variables), tuple(self.modes) + if any(not isinstance(v, PoseSearchVariable) for v in variables): + raise TypeError("variables require PoseSearchVariable values") + if not modes or any(not isinstance(v, TaskOperationMode) for v in modes): + raise ValueError("request requires explicit operation modes") + names([v.name for v in variables], "variable names") + members = [k for v in variables for k in v.members] + names(members, "variable members") + if not set(members) <= self.entity_poses.keys(): + raise ValueError("variable members must exist in the authored scene") + names([v.mode_id for v in modes], "mode IDs") + for mode in modes: + for phase in mode.phases: + referenced = { + t.frame_uid for t in phase.targets if t.frame_uid is not None + } + referenced |= {h.entity_uid for h in phase.held_objects} + referenced |= {p.entity_uid for p in phase.resulting_poses} + if not referenced <= self.entity_poses.keys(): + raise ValueError("phase refers to an undeclared entity") + object.__setattr__(self, "variables", variables) + object.__setattr__(self, "modes", modes) + + @cached_property + def request_id(self) -> str: + """Return the content identity of all fixed conditions and permissions.""" + return digest(self) + + +def _closed(cls: type, value: Mapping[str, object]) -> dict: + if not isinstance(value, Mapping): + raise ValueError(f"{cls.__name__} requires a mapping") + unknown = set(value) - {f.name for f in fields(cls)} + if unknown: + raise ValueError(f"Unknown {cls.__name__} fields: {sorted(unknown)}") + return dict(value) + + +def request_from_mapping(value: Mapping[str, object]) -> TaskFeasibilityRequest: + """Decode a provider-independent request through closed production contracts. + + Args: + value: JSON/YAML task, scene, mounting and phase declarations. + Returns: + Validated immutable request. No dotted import or model client is loaded. + """ + data = _closed(TaskFeasibilityRequest, value) + data["variables"] = tuple( + PoseSearchVariable(**_closed(PoseSearchVariable, v)) for v in data["variables"] + ) + modes = [] + for mode in data["modes"]: + m = _closed(TaskOperationMode, mode) + phases = [] + for phase in m["phases"]: + p = _closed(TaskPhase, phase) + for key, cls in ( + ("targets", PhaseTarget), + ("held_objects", HeldObject), + ("resulting_poses", ScenePoseChange), + ): + p[key] = tuple(cls(**_closed(cls, v)) for v in p.get(key, ())) + phases.append(TaskPhase(**p)) + m["phases"] = tuple(phases) + modes.append(TaskOperationMode(**m)) + data["modes"] = tuple(modes) + return TaskFeasibilityRequest(**data) diff --git a/embodichain/lab/_scene_config/policy.py b/embodichain/lab/_scene_config/policy.py new file mode 100644 index 0000000000..acbb6342da --- /dev/null +++ b/embodichain/lab/_scene_config/policy.py @@ -0,0 +1,430 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Source-independent policies for bounded scene layout sampling.""" + +from __future__ import annotations +from collections.abc import Mapping +from copy import deepcopy +from dataclasses import MISSING +import math +from typing import Any, TYPE_CHECKING +from embodichain.utils import configclass + +if TYPE_CHECKING: + from shapely.geometry import Polygon, MultiPolygon + +__all__: list[str] = [] + + +@configclass +class MemberRandomizationCfg: + """One non-root member's displacement bounds in its parent support frame. + + ``xy_offset_bounds_m`` is ``[[min_x, min_y], [max_x, max_y]]`` relative + to the original entity origin; ``None`` samples the complete support area. + Yaw is a continuous increment around the support normal, retaining tilt. + """ + + xy_offset_bounds_m: list[list[float]] | None = None + yaw_delta_range_deg: list[float] = [-180.0, 180.0] + + +@configclass +class SupportOverrideCfg: + """Explicit parent-local support frame and GeoJSON Polygon/MultiPolygon. + + The proper rigid ``parent_from_support`` matrix locates the support plane + at support Z=0 in the parent local Z-up geometry after asset scaling. Its polygon must be covered by actual coplanar mesh faces. + """ + + parent_from_support: list[list[float]] = MISSING + polygon: dict = MISSING + + +@configclass +class IntraGroupRandomizationCfg: + """Explicit member selection and conservative support geometry settings. + + Fixed members follow their parent. Clearances and grid resolution are in + metres. Overrides are keyed by parent UID, independently of target order. + """ + + targets: dict[str, MemberRandomizationCfg] = {} + boundary_margin_m: float = 0.002 + sibling_margin_m: float = 0.002 + support_gap_m: float = 0.001 + grid_resolution_m: float = 0.001 + support_overrides: dict[str, SupportOverrideCfg] = {} + + +def _validate_backend(*, backend: str, device: str, batch_size: int) -> None: + """Validate execution controls without initializing CUDA or reading assets.""" + if backend not in ("slsqp", "batched"): + raise ValueError("backend must be slsqp or batched.") + if not isinstance(device, str) or not ( + device == "cpu" + or device == "cuda" + or ( + device.startswith("cuda:") and device[5:].isascii() and device[5:].isdigit() + ) + ): + raise ValueError("device must be cpu, cuda, or cuda:N.") + if backend == "slsqp" and device != "cpu": + raise ValueError( + "The slsqp backend runs on CPU; use backend: batched for CUDA." + ) + if type(batch_size) is not int or batch_size < 1: + raise ValueError("batch_size must be a positive integer.") + + +def _validate_generation_parameters( + *, seed: int, candidate_count: int, max_attempts: int, collision_margin_m: float +) -> None: + """Validate YAML values and Python overrides before accessing scene files.""" + for name, value, minimum in ( + ("seed", seed, 0), + ("candidate_count", candidate_count, 1), + ("max_attempts", max_attempts, 1), + ): + if type(value) is not int or value < minimum: + raise ValueError(f"{name} must be an integer >= {minimum}.") + if candidate_count > max_attempts: + raise ValueError("max_attempts must be at least candidate_count.") + if type(collision_margin_m) not in (int, float) or collision_margin_m < 0: + raise ValueError("collision_margin_m must be finite and nonnegative.") + try: + finite = math.isfinite(collision_margin_m) + except OverflowError: + finite = False + if not finite: + raise ValueError("collision_margin_m must be finite and nonnegative.") + + +def _mapping(value: Any, *, name: str, allowed: set[str]) -> dict[str, Any]: + if not isinstance(value, dict): + raise ValueError(f"{name} must be a mapping.") + unknown = set(value) - allowed + if unknown: + raise ValueError(f"Unknown {name} fields: {', '.join(sorted(unknown))}.") + return value + + +def _decode_intra(value: Any) -> IntraGroupRandomizationCfg | None: + """Strictly decode nested YAML mappings, also used for persisted policies.""" + if value is None: + return None + root = _mapping( + value, + name="intra_group", + allowed={ + "targets", + "boundary_margin_m", + "sibling_margin_m", + "support_gap_m", + "grid_resolution_m", + "support_overrides", + }, + ) + targets = root.get("targets", {}) + if not isinstance(targets, dict): + raise ValueError("targets must be a mapping.") + decoded = {} + for uid, target in targets.items(): + if not isinstance(uid, str) or not uid.strip(): + raise ValueError("Target UIDs must be nonempty strings.") + item = _mapping( + target, + name=f"target {uid}", + allowed={"xy_offset_bounds_m", "yaw_delta_range_deg"}, + ) + decoded[uid] = MemberRandomizationCfg(**item) + overrides = root.get("support_overrides", {}) + if not isinstance(overrides, dict): + raise ValueError("support_overrides must be a mapping.") + override_cfg = {} + for uid, override in overrides.items(): + item = _mapping( + override, + name=f"support override {uid}", + allowed={"parent_from_support", "polygon"}, + ) + if set(item) != {"parent_from_support", "polygon"}: + raise ValueError("Support overrides require a transform and polygon.") + override_cfg[uid] = SupportOverrideCfg(**item) + cfg = IntraGroupRandomizationCfg( + **{k: v for k, v in root.items() if k not in ("targets", "support_overrides")}, + targets=decoded, + support_overrides=override_cfg, + ) + _validate_intra(cfg) + return cfg + + +def _read_support_polygon(value: object) -> Polygon | MultiPolygon: + """Decode finite XY GeoJSON before accessing any scene assets.""" + import numpy as np + import shapely + from shapely.geometry import Polygon, MultiPolygon, shape + + if not isinstance(value, dict) or value.get("type") not in ( + "Polygon", + "MultiPolygon", + ): + raise ValueError( + "Support polygon must be a GeoJSON Polygon or MultiPolygon mapping." + ) + try: + polygon = shape(value) + except ( + AttributeError, + KeyError, + IndexError, + TypeError, + ValueError, + OverflowError, + shapely.errors.GEOSException, + ) as exc: + raise ValueError("Support polygon has invalid GeoJSON coordinates.") from exc + if ( + not isinstance(polygon, (Polygon, MultiPolygon)) + or polygon.is_empty + or polygon.has_z + or not np.isfinite(shapely.get_coordinates(polygon)).all() + or not polygon.is_valid + or polygon.area <= 0 + ): + raise ValueError( + "Support polygon must contain a valid nonempty finite XY area." + ) + return polygon + + +def _validate_intra(cfg: IntraGroupRandomizationCfg) -> None: + import numpy as np + + if not isinstance(cfg, IntraGroupRandomizationCfg): + raise ValueError("intra_group must be an IntraGroupRandomizationCfg.") + cfg.validate() + if not isinstance(cfg.targets, dict) or not isinstance(cfg.support_overrides, dict): + raise ValueError("targets and support_overrides must be mappings.") + for field in ( + "boundary_margin_m", + "sibling_margin_m", + "support_gap_m", + "grid_resolution_m", + ): + value = getattr(cfg, field) + try: + finite = type(value) in (int, float) and math.isfinite(value) + except OverflowError: + finite = False + if not finite or value < 0 or (field == "grid_resolution_m" and value == 0): + raise ValueError( + f"{field} must be finite and {'positive' if field == 'grid_resolution_m' else 'nonnegative'}." + ) + for uid, item in cfg.targets.items(): + if ( + not isinstance(uid, str) + or not uid + or not isinstance(item, MemberRandomizationCfg) + ): + raise ValueError("targets must map UIDs to MemberRandomizationCfg.") + yaw = np.asarray(item.yaw_delta_range_deg) + if ( + yaw.shape != (2,) + or yaw.dtype.kind not in "fi" + or not np.isfinite(yaw).all() + or yaw[0] > yaw[1] + or not np.isfinite(yaw[1] - yaw[0]) + ): + raise ValueError( + "yaw_delta_range_deg requires finite ordered bounds and width." + ) + if item.xy_offset_bounds_m is not None: + xy = np.asarray(item.xy_offset_bounds_m) + if ( + xy.shape != (2, 2) + or xy.dtype.kind not in "fi" + or not np.isfinite(xy).all() + or np.any(xy[0] > xy[1]) + ): + raise ValueError( + "xy_offset_bounds_m requires finite ordered lower/upper XY corners." + ) + for uid, override in cfg.support_overrides.items(): + if ( + not isinstance(uid, str) + or not uid + or not isinstance(override, SupportOverrideCfg) + ): + raise ValueError("support_overrides must map UIDs to SupportOverrideCfg.") + override.validate() + try: + _read_support_polygon(override.polygon) + except ValueError as exc: + raise ValueError(f"Support override {uid!r}: {exc}") from exc + try: + matrix = np.asarray(override.parent_from_support, dtype=float) + except (TypeError, ValueError, OverflowError) as exc: + raise ValueError( + "parent_from_support must contain finite numeric values." + ) from exc + if ( + matrix.shape != (4, 4) + or not np.isfinite(matrix).all() + or not np.array_equal(matrix[3], [0, 0, 0, 1]) + or not np.allclose( + matrix[:3, :3].T @ matrix[:3, :3], np.eye(3), atol=1e-8, rtol=0 + ) + or not np.isclose(np.linalg.det(matrix[:3, :3]), 1, atol=1e-8, rtol=0) + ): + raise ValueError("parent_from_support must be a proper rigid transform.") + + +def _intra_payload(cfg: IntraGroupRandomizationCfg) -> dict: + """Serialize only documented settings, independent of configclass internals.""" + return deepcopy( + { + "targets": { + uid: { + "xy_offset_bounds_m": item.xy_offset_bounds_m, + "yaw_delta_range_deg": list(item.yaw_delta_range_deg), + } + for uid, item in sorted(cfg.targets.items()) + }, + **{ + key: float(getattr(cfg, key)) + for key in ( + "boundary_margin_m", + "sibling_margin_m", + "support_gap_m", + "grid_resolution_m", + ) + }, + "support_overrides": { + uid: { + "parent_from_support": item.parent_from_support, + "polygon": item.polygon, + } + for uid, item in sorted(cfg.support_overrides.items()) + }, + } + ) + + +@configclass +class SceneRandomizationPolicyCfg: + """Geometric policy independent of scene files and runtime lifecycles. + + Args: + backend: Explicit numeric backend; the shared interface uses batched sampling. + device: CPU or CUDA tensor device. + batch_size: Maximum logical proposals evaluated together. + collision_margin_m: Minimum group AABB separation in metres. + randomize_groups: Sample permitted group-root XY translations. + group_targets: Root UIDs to move; None selects all permitted roots. + group_xy_offset_bounds_m: Optional per-root arena XY displacement bounds. + intra_group: Selected non-root XY/yaw changes and support settings. + """ + + backend: str = "batched" + device: str = "cpu" + batch_size: int = 4096 + collision_margin_m: float = 0.02 + randomize_groups: bool = True + group_targets: tuple[str, ...] | None = None + group_xy_offset_bounds_m: dict[str, list[list[float]]] = {} + intra_group: IntraGroupRandomizationCfg | None = None + + +@configclass +class SceneRandomizationJobCfg: + """Bounded caller-owned generation settings and output representation. + + Args: + policy: Source-independent geometric sampling policy. + seed: Addressed generation seed. + candidate_count: Requested valid layouts. + max_attempts: Whole-layout proposal budget. + layout_storage: JSON or packed candidate storage. + """ + + policy: SceneRandomizationPolicyCfg = SceneRandomizationPolicyCfg() + seed: int = 0 + candidate_count: int = 10 + max_attempts: int = 1000 + layout_storage: str = "packed" + + +def _policy_data(cfg: SceneRandomizationPolicyCfg | Mapping) -> dict: + if isinstance(cfg, Mapping): + raw = dict(cfg) + cfg = SceneRandomizationPolicyCfg(**raw) + if not isinstance(cfg, SceneRandomizationPolicyCfg): + raise TypeError("policy must be a SceneRandomizationPolicyCfg or mapping") + cfg = cfg.copy() + cfg.validate() + _validate_backend(backend=cfg.backend, device=cfg.device, batch_size=cfg.batch_size) + if cfg.backend != "batched": + raise ValueError( + "The source-independent layout API supports backend='batched'." + ) + _validate_generation_parameters( + seed=0, + candidate_count=1, + max_attempts=1, + collision_margin_m=cfg.collision_margin_m, + ) + if type(cfg.randomize_groups) is not bool: + raise ValueError("randomize_groups must be a boolean.") + intra = cfg.intra_group + if isinstance(intra, dict) or intra is None: + intra = _decode_intra(intra) + if intra is None: + intra = IntraGroupRandomizationCfg() + _validate_intra(intra) + if not isinstance(cfg.group_xy_offset_bounds_m, dict): + raise ValueError("group_xy_offset_bounds_m must map root UIDs to XY bounds.") + for uid, bounds in cfg.group_xy_offset_bounds_m.items(): + if bounds is None: + raise ValueError( + "Root XY bounds must be explicit lower/upper corners; omit unbounded roots." + ) + _decode_intra( + { + "targets": { + uid: {"xy_offset_bounds_m": bounds, "yaw_delta_range_deg": [0, 0]} + } + } + ) + if cfg.group_xy_offset_bounds_m and not cfg.randomize_groups: + raise ValueError("Root XY bounds require randomize_groups=True.") + if cfg.group_targets is not None and ( + isinstance(cfg.group_targets, str) + or len(set(cfg.group_targets)) != len(cfg.group_targets) + ): + raise ValueError("group_targets must contain unique root UIDs.") + return { + "backend": cfg.backend, + "device": cfg.device, + "batch_size": cfg.batch_size, + "collision_margin_m": float(cfg.collision_margin_m), + "randomize_groups": cfg.randomize_groups, + "group_targets": None if cfg.group_targets is None else list(cfg.group_targets), + "group_xy_offset_bounds_m": deepcopy(cfg.group_xy_offset_bounds_m), + "intra_group": _intra_payload(intra), + } diff --git a/embodichain/lab/_scene_config/scene.py b/embodichain/lab/_scene_config/scene.py new file mode 100644 index 0000000000..e22e38fecc --- /dev/null +++ b/embodichain/lab/_scene_config/scene.py @@ -0,0 +1,157 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Immutable scene proposals, independent of task and execution lifecycles.""" + +from __future__ import annotations + +from dataclasses import dataclass +import hashlib +import json + +__all__: list[str] = [] + + +from .values import _identifier, _nonnegative_integer, _rigid_pose + + +def _variant_id( + parent_scene_id: str, + seed: int, + ordinal: int, + pose_changes: tuple, + metadata: tuple, +) -> str: + """Hash already validated, canonically ordered proposal values.""" + payload = { + "schema": "scene-variant/v1", + "parent_scene_id": parent_scene_id, + "seed": seed, + "ordinal": ordinal, + "pose_changes": pose_changes, + "metadata": metadata, + } + encoded = json.dumps( + payload, sort_keys=True, separators=(",", ":"), allow_nan=False + ).encode("utf-8") + return "scene-variant:" + hashlib.sha256(encoded).hexdigest() + + +@dataclass(frozen=True) +class ScenePoseChange: + """A proposed pose for one existing physical entity. + + Args: + entity_uid: Stable physical entity UID, independent of environment slots. + arena_pose: Homogeneous 4x4 transform from the entity frame into its + local Z-up arena frame. Inputs are copied into immutable tuples. + + .. attention:: + An arena pose excludes replication offsets. The host converts it to + backend coordinates when applying the candidate. This contract does + not establish support, collision freedom, or manipulation feasibility. + """ + + entity_uid: str + arena_pose: tuple[tuple[float, ...], ...] + + def __post_init__(self) -> None: + _identifier(self.entity_uid, "entity_uid") + object.__setattr__(self, "arena_pose", _rigid_pose(self.arena_pose)) + + +@dataclass(frozen=True) +class SceneVariant: + """An immutable scene proposal with reproducible proposal identity. + + Args: + parent_scene_id: Content/revision identity of the original scene. + seed: Nonnegative proposal-generation seed. + ordinal: Nonnegative position in the candidate source. + pose_changes: Unique existing entities and their proposed arena poses. + metadata: Unique string key/value provenance entries. Entries and pose + changes are sorted so equivalent input ordering has one identity. + + .. attention:: + ``variant_id`` identifies a proposal, not a settled initial state or a + successful execution. Hosts record those identities and evidence + separately. Metadata describes provenance, not executable operations. + Since ``ordinal`` participates in the hash, consuming the same layout + at a different source position changes ``variant_id``. File-backed + consumers should retain the original layout identity and proposal + ordinal in metadata for provenance across selection orders; this ID + is not a geometry-only deduplication key. + """ + + parent_scene_id: str + seed: int + ordinal: int + pose_changes: tuple[ScenePoseChange, ...] + metadata: tuple[tuple[str, str], ...] = () + + def __post_init__(self) -> None: + _identifier(self.parent_scene_id, "parent_scene_id") + _nonnegative_integer(self.seed, "seed") + _nonnegative_integer(self.ordinal, "ordinal") + try: + changes = tuple(self.pose_changes) + except TypeError as exc: + raise ValueError( + "pose_changes must contain ScenePoseChange values" + ) from exc + if any(not isinstance(change, ScenePoseChange) for change in changes): + raise ValueError("pose_changes must contain ScenePoseChange values") + if len({change.entity_uid for change in changes}) != len(changes): + raise ValueError("pose_changes must contain unique entity UIDs") + object.__setattr__( + self, + "pose_changes", + tuple(sorted(changes, key=lambda change: change.entity_uid)), + ) + try: + entries = tuple(self.metadata) + if any(isinstance(entry, str) for entry in entries): + raise ValueError("metadata must contain string key/value pairs") + metadata = tuple(tuple(entry) for entry in entries) + except TypeError as exc: + raise ValueError("metadata must contain string key/value pairs") from exc + if any(len(entry) != 2 for entry in metadata): + raise ValueError("metadata must contain string key/value pairs") + for key, value in metadata: + _identifier(key, "metadata key") + if not isinstance(value, str): + raise ValueError("metadata values must be strings") + if len({key for key, _ in metadata}) != len(metadata): + raise ValueError("metadata must contain unique keys") + object.__setattr__(self, "metadata", tuple(sorted(metadata))) + + @property + def variant_id(self) -> str: + """Return a stable identity including geometry, source, seed, and ordinal. + + Returns: + SHA-256 identity independent of Python hash randomization and + temporary physical environment assignments. + """ + return _variant_id( + self.parent_scene_id, + self.seed, + self.ordinal, + tuple( + (change.entity_uid, change.arena_pose) for change in self.pose_changes + ), + self.metadata, + ) diff --git a/embodichain/lab/_scene_config/values.py b/embodichain/lab/_scene_config/values.py new file mode 100644 index 0000000000..53efdf7eb2 --- /dev/null +++ b/embodichain/lab/_scene_config/values.py @@ -0,0 +1,195 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Private canonical value validation for feasibility artifacts.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import fields, is_dataclass +import hashlib +import json +import math +from numbers import Real +from enum import Enum +import numpy as np +import sys +from pathlib import Path +from types import MappingProxyType + +__all__ = [] + + +def _identifier(value: str, name: str) -> str: + if not isinstance(value, str) or not value or value.strip() != value: + raise ValueError(f"{name} must be a nonempty stripped string") + return value + + +def _nonnegative_integer(value: int, name: str) -> int: + if type(value) is not int or value < 0: + raise ValueError(f"{name} must be a nonnegative integer") + return value + + +def _rigid_pose(value: object) -> tuple[tuple[float, ...], ...]: + try: + rows = tuple(tuple(row) for row in value) # type: ignore[union-attr] + except TypeError as exc: + raise ValueError("arena_pose must be a 4x4 rigid transform") from exc + if len(rows) != 4 or any(len(row) != 4 for row in rows): + raise ValueError("arena_pose must be a 4x4 rigid transform") + if any( + isinstance(item, bool) or not isinstance(item, Real) or not math.isfinite(item) + for row in rows + for item in row + ): + raise ValueError("arena_pose must contain finite real numbers") + pose = tuple( + tuple(float(item) if item != 0 else 0.0 for item in row) for row in rows + ) + tolerance = 1e-6 + if any( + abs(pose[3][index] - target) > tolerance + for index, target in enumerate((0, 0, 0, 1)) + ): + raise ValueError("arena_pose must have homogeneous last row [0, 0, 0, 1]") + rotation = tuple(row[:3] for row in pose[:3]) + if any( + abs(sum(rotation[k][i] * rotation[k][j] for k in range(3)) - (i == j)) + > tolerance + for i in range(3) + for j in range(3) + ): + raise ValueError("arena_pose rotation must be orthonormal") + determinant = ( + rotation[0][0] + * (rotation[1][1] * rotation[2][2] - rotation[1][2] * rotation[2][1]) + - rotation[0][1] + * (rotation[1][0] * rotation[2][2] - rotation[1][2] * rotation[2][0]) + + rotation[0][2] + * (rotation[1][0] * rotation[2][1] - rotation[1][1] * rotation[2][0]) + ) + if abs(determinant - 1.0) > tolerance: + raise ValueError("arena_pose rotation must be proper (determinant +1)") + return (*pose[:3], (0.0, 0.0, 0.0, 1.0)) + + +def plain(value: object) -> object: + if isinstance(value, np.ndarray): + return plain(value.tolist()) + if isinstance(value, np.generic): + return plain(value.item()) + # Tensor values can only exist when torch is already loaded by the caller. + torch = sys.modules.get("torch") + if torch is not None and isinstance(value, torch.Tensor): + return plain(value.detach().cpu().tolist()) + if isinstance(value, Path) or ( + torch is not None and isinstance(value, torch.device) + ): + return str(value) + if isinstance(value, slice): + return {"slice": [value.start, value.stop, value.step]} + if isinstance(value, Enum): + return plain(value.value) + if isinstance(getattr(value, "name", None), str) and isinstance( + getattr(value, "value", None), Real + ): + return { + "enum_type": type(value).__module__ + "." + type(value).__qualname__, + "name": value.name, + "value": plain(value.value), + } + if isinstance(value, (set, frozenset)): + return sorted( + (plain(v) for v in value), key=lambda v: json.dumps(v, sort_keys=True) + ) + if is_dataclass(value) and not isinstance(value, type): + return {f.name: plain(getattr(value, f.name)) for f in fields(value)} + if isinstance(value, Mapping): + return {str(k): plain(v) for k, v in value.items()} + if isinstance(value, (tuple, list)): + return [plain(v) for v in value] + return value + + +def digest(value: object) -> str: + return hashlib.sha256( + json.dumps( + plain(value), sort_keys=True, separators=(",", ":"), allow_nan=False + ).encode() + ).hexdigest() + + +def owned_json(value: Mapping[str, object]) -> Mapping[str, object]: + copied = json.loads(json.dumps(plain(value), allow_nan=False)) + + def freeze(item: object) -> object: + if isinstance(item, dict): + return MappingProxyType({k: freeze(v) for k, v in item.items()}) + if isinstance(item, list): + return tuple(freeze(v) for v in item) + return item + + return freeze(copied) + + +def finite(values: object, name: str) -> tuple[float, ...]: + result = tuple(values) + if any( + isinstance(v, bool) or not isinstance(v, Real) or not math.isfinite(v) + for v in result + ): + raise ValueError(f"{name} requires finite real values") + return tuple(float(v) for v in result) + + +def count(value: int, name: str, minimum: int = 1) -> None: + if type(value) is not int or value < minimum: + raise ValueError(f"{name} must be an integer >= {minimum}") + + +def names(values: object, name: str) -> tuple[str, ...]: + if isinstance(values, str): + raise ValueError(f"{name} requires a sequence") + result = tuple(values) + for value in result: + _identifier(value, name) + if len(set(result)) != len(result): + raise ValueError(f"{name} must be unique") + return result + + +def poses(values: Mapping[str, object]) -> Mapping[str, tuple]: + return MappingProxyType( + { + _identifier(k, "entity UID"): _rigid_pose(v) + for k, v in sorted(values.items()) + } + ) + + +def atomic_json(path: Path, value: object) -> None: + import os + import tempfile + + path.parent.mkdir(parents=True, exist_ok=True) + fd, temporary = tempfile.mkstemp(prefix=path.name + ".", dir=path.parent) + try: + with os.fdopen(fd, "w", encoding="utf-8") as stream: + json.dump(plain(value), stream, allow_nan=False, sort_keys=True, indent=2) + os.replace(temporary, path) + finally: + Path(temporary).unlink(missing_ok=True) diff --git a/embodichain/lab/expansion/_distribution.py b/embodichain/lab/expansion/_distribution.py index 239c514fc9..d5d660aca3 100644 --- a/embodichain/lab/expansion/_distribution.py +++ b/embodichain/lab/expansion/_distribution.py @@ -23,6 +23,7 @@ import argparse from collections import Counter +from collections.abc import Mapping from copy import deepcopy import hashlib import json @@ -147,6 +148,17 @@ def _read(paths: list[Path], entity: str) -> tuple[list[dict], list[str]]: if not episode.validation.accepted: raise ValueError("Confirmed episode lacks acceptance") meta = episode.metadata + case = meta.get("scene_case") + task_id = case.get("task_id") if isinstance(case, Mapping) else None + source_uuid = meta.get("source_episode_uuid") + if isinstance(task_id, str) and task_id.strip(): + task_identity = ("task_id", task_id) + elif isinstance(source_uuid, str) and source_uuid.strip(): + task_identity = ("legacy_source_episode_uuid", source_uuid) + else: + raise ValueError( + f"Episode {episode.episode_id} lacks a recorded task identity" + ) # Fixed topology, installation and task identity; sampled object poses are features. robot = [ dict(uid=s["uid"], joints=list(s["joint_names"]), pose=list(s["pose"])) @@ -156,6 +168,7 @@ def _read(paths: list[Path], entity: str) -> tuple[list[dict], list[str]]: domain = _digest( { "robot": robot, + "task_identity": task_identity, "instruction": meta.get("instruction"), "backend": meta.get("physics_backend"), "deployment_robot": deployment.get("robot"), @@ -196,7 +209,8 @@ def _read(paths: list[Path], entity: str) -> tuple[list[dict], list[str]]: "artifact": receipt.storage_id, "job": str(directory), "domain_id": domain, - "source_episode_uuid": meta.get("source_episode_uuid"), + "task_id": task_id, + "source_episode_uuid": source_uuid, "initial_pose": None if state is None else state[0, :7].tolist(), "final_pose": None if state is None else state[-1, :7].tolist(), "orientation": ( diff --git a/embodichain/lab/expansion/_pose_sampling.py b/embodichain/lab/expansion/_pose_sampling.py index f4c0090353..42458748e7 100644 --- a/embodichain/lab/expansion/_pose_sampling.py +++ b/embodichain/lab/expansion/_pose_sampling.py @@ -311,16 +311,7 @@ def _guided(self, candidate, rng): for uid, opts in self.configuration.items(): if "follow" in opts: parent = opts["follow"] - poses[uid][:3, 3] = ( - self.poses[uid][:3, 3] - + poses[parent][:3, 3] - - self.poses[parent][:3, 3] - ) - poses[uid][:3, :3] = ( - poses[parent][:3, :3] - @ self.poses[parent][:3, :3].T - @ self.poses[uid][:3, :3] - ) + poses[uid] = self._follower_pose(uid, parent, poses[parent]) changes = tuple( ScenePoseChange(uid, tuple(map(tuple, pose))) for uid, pose in poses.items() ) @@ -379,6 +370,16 @@ def _check(self, candidate): return False, "full_pose_fk_residual" return True, "passed" + def _follower_pose( + self, uid: str, parent: str, parent_pose: np.ndarray + ) -> np.ndarray: + """Apply the parent's arena displacement and rotation to the nominal follower.""" + nominal_parent = self.poses[parent] + pose = self.poses[uid].astype(parent_pose.dtype, copy=True) + pose[:3, 3] = pose[:3, 3] + parent_pose[:3, 3] - nominal_parent[:3, 3] + pose[:3, :3] = parent_pose[:3, :3] @ nominal_parent[:3, :3].T @ pose[:3, :3] + return pose + def _offer(self, candidate, rng, raw): if self.cfg["strategy"] == "workspace_joint": candidate, origin = self._guided(candidate, rng) diff --git a/embodichain/lab/expansion/_pose_sampling_cfg.py b/embodichain/lab/expansion/_pose_sampling_cfg.py index b454e134b2..e8fc9bfe65 100644 --- a/embodichain/lab/expansion/_pose_sampling_cfg.py +++ b/embodichain/lab/expansion/_pose_sampling_cfg.py @@ -17,15 +17,82 @@ """Strict optional scene sampling and pose coverage declarations.""" from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass import math +from typing import TYPE_CHECKING + import numpy as np +if TYPE_CHECKING: + from embodichain.lab._scene_config.feasibility import PhaseTarget, TaskOperationMode + __all__: list[str] = [] _ROBOT_LAYOUT_STRATEGIES = ("robot_first", "bidirectional") _LAYOUT_STRATEGIES = ("scene_first", *_ROBOT_LAYOUT_STRATEGIES) +@dataclass(frozen=True) +class _AnchorTarget: + phase_id: str + target: PhaseTarget + root_uid: str + + +def _anchor_targets( + mode: TaskOperationMode, layout: Mapping, policy: Mapping, selections: object +) -> tuple[_AnchorTarget, ...]: + """Resolve each named target to a rigid frame in the initial support tree.""" + if not isinstance(selections, list) or not selections: + raise ValueError( + "robot_anchors requires a nonempty list of phase/part selectors" + ) + targets = {} + moved = set() + support, parents = layout["support_uid"], layout["parent_by_uid"] + variable_members = policy["intra_group"]["targets"] + for phase in mode.phases: + for target in phase.targets: + targets[phase.phase_id, target.control_part] = (target, moved.copy()) + moved.update(held.entity_uid for held in phase.held_objects) + moved.update(change.entity_uid for change in phase.resulting_poses) + result, seen = [], set() + for sel in selections: + if ( + not isinstance(sel, Mapping) + or set(sel) != {"phase_id", "control_part"} + or any(type(value) is not str for value in sel.values()) + ): + raise ValueError("robot_anchors selects explicit phase_id and control_part") + key = (sel["phase_id"], sel["control_part"]) + if key not in targets or key in seen: + raise ValueError("robot_anchors must select unique declared phase targets") + seen.add(key) + target, earlier_moves = targets[key] + root = target.frame_uid + if root not in parents: + raise ValueError("A robot anchor target needs a frame in the support tree") + while True: + if root in variable_members or root in earlier_moves: + raise ValueError( + "A robot anchor needs an initial frame rigidly attached to its layout root" + ) + if parents[root] == support: + break + root = parents[root] + groups = policy["group_targets"] + if not policy["randomize_groups"] or ( + groups is not None and root not in groups + ): + raise ValueError("A robot anchor requires a movable layout root") + if root in layout.get("conditions", {}): + raise ValueError("A robot anchor cannot replace an authored root condition") + result.append(_AnchorTarget(key[0], target, root)) + return tuple(result) + + def validate_sampling(value: dict) -> None: """Normalize optional settings without changing legacy configurations.""" from ._pose_cells import _grid @@ -210,8 +277,8 @@ def validate_sampling(value: dict) -> None: or not 0 < screening["planning_timeout_s"] <= 60 ): raise ValueError("Screening timeout must be positive and bounded") - from embodichain.lab.sim.scene.expansion.layout import _policy_data - from embodichain.lab.sim.scene.feasibility import request_from_mapping + from embodichain.lab._scene_config.feasibility import request_from_mapping + from embodichain.lab._scene_config.policy import _policy_data normalized_policy = _policy_data(layout["policy"]) if any( @@ -270,8 +337,6 @@ def validate_sampling(value: dict) -> None: "robot_anchors" in sampling or sampling["strategy"] in _ROBOT_LAYOUT_STRATEGIES ): - from ._robot_first_sampling import _anchor_targets - anchors = _anchor_targets( request.modes[0], layout, diff --git a/embodichain/lab/expansion/_robot_first_sampling.py b/embodichain/lab/expansion/_robot_first_sampling.py index 1cbd0cdabe..34076263ee 100644 --- a/embodichain/lab/expansion/_robot_first_sampling.py +++ b/embodichain/lab/expansion/_robot_first_sampling.py @@ -27,77 +27,16 @@ from embodichain.lab.sim.scene.expansion import SceneLayoutCondition from embodichain.lab.sim.scene.feasibility.proposals import _workspace_fk_samples +from ._pose_sampling_cfg import _AnchorTarget + if TYPE_CHECKING: from embodichain.lab.sim.objects import Robot from embodichain.lab.sim.scene.expansion import PreparedSceneRandomization - from embodichain.lab.sim.scene.feasibility import ( - FeasibilityBudget, - PhaseTarget, - TaskOperationMode, - ) + from embodichain.lab.sim.scene.feasibility import FeasibilityBudget __all__: list[str] = [] -@dataclass(frozen=True) -class _AnchorTarget: - phase_id: str - target: PhaseTarget - root_uid: str - - -def _anchor_targets( - mode: TaskOperationMode, layout: Mapping, policy: Mapping, selections: object -) -> tuple[_AnchorTarget, ...]: - """Resolve each named target to a rigid frame in the initial support tree.""" - if not isinstance(selections, list) or not selections: - raise ValueError( - "robot_anchors requires a nonempty list of phase/part selectors" - ) - targets = {} - moved = set() - support, parents = layout["support_uid"], layout["parent_by_uid"] - variable_members = policy["intra_group"]["targets"] - for phase in mode.phases: - for target in phase.targets: - targets[phase.phase_id, target.control_part] = (target, moved.copy()) - moved.update(held.entity_uid for held in phase.held_objects) - moved.update(change.entity_uid for change in phase.resulting_poses) - result, seen = [], set() - for sel in selections: - if ( - not isinstance(sel, Mapping) - or set(sel) != {"phase_id", "control_part"} - or any(type(value) is not str for value in sel.values()) - ): - raise ValueError("robot_anchors selects explicit phase_id and control_part") - key = (sel["phase_id"], sel["control_part"]) - if key not in targets or key in seen: - raise ValueError("robot_anchors must select unique declared phase targets") - seen.add(key) - target, earlier_moves = targets[key] - root = target.frame_uid - if root not in parents: - raise ValueError("A robot anchor target needs a frame in the support tree") - while True: - if root in variable_members or root in earlier_moves: - raise ValueError( - "A robot anchor needs an initial frame rigidly attached to its layout root" - ) - if parents[root] == support: - break - root = parents[root] - groups = policy["group_targets"] - if not policy["randomize_groups"] or ( - groups is not None and root not in groups - ): - raise ValueError("A robot anchor requires a movable layout root") - if root in layout.get("conditions", {}): - raise ValueError("A robot anchor cannot replace an authored root condition") - result.append(_AnchorTarget(key[0], target, root)) - return tuple(result) - - @dataclass(frozen=True) class _AnchorPool: selection: _AnchorTarget diff --git a/embodichain/lab/expansion/_scene_first_sampling.py b/embodichain/lab/expansion/_scene_first_sampling.py index b6935d1a83..ff94f5d5a2 100644 --- a/embodichain/lab/expansion/_scene_first_sampling.py +++ b/embodichain/lab/expansion/_scene_first_sampling.py @@ -24,8 +24,8 @@ import numpy as np from ._pose_sampling import _AdaptivePoseSource -from ._pose_sampling_cfg import _ROBOT_LAYOUT_STRATEGIES -from ._robot_first_sampling import _anchor_targets, _prepare_anchor_pools +from ._pose_sampling_cfg import _ROBOT_LAYOUT_STRATEGIES, _anchor_targets +from ._robot_first_sampling import _prepare_anchor_pools from embodichain.lab.sim.scene.expansion import ( SceneLayoutCondition, scene_layout_input_from_sim, @@ -34,6 +34,7 @@ ScenePoseChange, SceneVariant, ) +from embodichain.lab.sim.scene.expansion.conditions import _LayoutConditionDomainError __all__: list[str] = [] @@ -254,6 +255,16 @@ def _offer_layout(self, candidate, raw, origin): for c in candidate.scene.pose_changes } changes.update({c.entity_uid: c for c in generated.pose_changes}) + for uid, options in self.configuration.items(): + if ( + uid not in self._prepared_layout.source.poses + and "follow" in options + ): + parent = options["follow"] + pose = self._follower_pose( + uid, parent, np.asarray(changes[parent].arena_pose) + ) + changes[uid] = ScenePoseChange(uid, pose) candidate = replace( candidate, scene=SceneVariant( @@ -281,14 +292,17 @@ def _offer_layout(self, candidate, raw, origin): actual = anchors.get(uid, self._prepared_layout.source.poses[uid]) if not np.allclose(actual, pose, rtol=0, atol=1e-9): return candidate, "bounded_pose", False, "condition_mismatch" - batch = generate_scene_variants( - self._prepared_layout, - seed=self.seed, - candidate_count=1, - max_attempts=1, - start_ordinal=raw, - conditions=SceneLayoutCondition(anchors), - ) + try: + batch = generate_scene_variants( + self._prepared_layout, + seed=self.seed, + candidate_count=1, + max_attempts=1, + start_ordinal=raw, + conditions=SceneLayoutCondition(anchors), + ) + except _LayoutConditionDomainError: + return candidate, origin, False, "layout_outside_sampled_domain" if not batch.variants: return candidate, "bounded_pose", False, "layout_budget_exhausted" poses = { diff --git a/embodichain/lab/expansion/distribution.md b/embodichain/lab/expansion/distribution.md index 7cde799567..92d1a2c5e7 100644 --- a/embodichain/lab/expansion/distribution.md +++ b/embodichain/lab/expansion/distribution.md @@ -20,6 +20,9 @@ Inputs must be job directories containing `job.json` and confirmed artifact rece Repeated input paths/commit identities do not multiply samples. Conflicting commit identities fail. Output must be new; inputs are never overwritten. Both snapshots must have the same task, robot installation, topology and configured scene domain. +The recorded scene-case task ID identifies the task across seeds and source +recordings. Older artifacts can use their recorded source UUID when the task ID +is absent. Artifacts without either identity cannot be compared. Open `report.html`. Select a spatial region or joint-distribution cell to inspect recorded object paths. The evidence panel identifies the full artifact and the diff --git a/embodichain/lab/expansion/gym_workloads.py b/embodichain/lab/expansion/gym_workloads.py index c8e32ac457..356075f06a 100644 --- a/embodichain/lab/expansion/gym_workloads.py +++ b/embodichain/lab/expansion/gym_workloads.py @@ -215,10 +215,11 @@ def activate(self, assignment: WorkloadAssignment, observation: object) -> Any: host, engine = group["host"], group["engine"] ctx = host._context request = ctx.requests[row] - if group["session"].stop_reason == "wall_time_exhausted": - ctx.journal.update( - request, status="initial_rejected", reason="wall_time_exhausted" - ) + session = group["session"] + budget_stops = ("wall_time_exhausted", "rollout_budget_exhausted") + stop_reason = session.stop_reason + if stop_reason in budget_stops: + ctx.journal.update(request, status="initial_rejected", reason=stop_reason) return None if self.activation_snapshot is None: self.activation_snapshot = ( @@ -270,7 +271,6 @@ def activate(self, assignment: WorkloadAssignment, observation: object) -> Any: ctx.program.program_id, self.env.robot.uid, ) - session = group["session"] session.register_case(case, ctx.limits, joint_names=ctx.names) try: identity, _ = session.propose( @@ -285,11 +285,10 @@ def activate(self, assignment: WorkloadAssignment, observation: object) -> Any: except RuntimeError: # The budget can expire during initial-state validation, including # between the early check and the session's atomic proposal guard. - if session.stop_reason != "wall_time_exhausted": + stop_reason = session.stop_reason + if stop_reason not in budget_stops: raise - ctx.journal.update( - request, status="initial_rejected", reason="wall_time_exhausted" - ) + ctx.journal.update(request, status="initial_rejected", reason=stop_reason) return None episode_id, commit_id = session.episode_ids(identity) record = ctx.journal.update( diff --git a/embodichain/lab/sim/scene/_support_grid.py b/embodichain/lab/sim/scene/_support_grid.py index 96aaeaeb39..97dfd4d0c9 100644 --- a/embodichain/lab/sim/scene/_support_grid.py +++ b/embodichain/lab/sim/scene/_support_grid.py @@ -24,45 +24,12 @@ import numpy as np import shapely -from shapely.geometry import Polygon, MultiPolygon, mapping, shape +from shapely.geometry import Polygon, MultiPolygon, mapping from shapely.ops import unary_union -__all__: list[str] = [] - +from embodichain.lab._scene_config.policy import _read_support_polygon -def _read_support_polygon(value: object) -> Polygon | MultiPolygon: - """Decode finite XY GeoJSON before accessing any scene assets.""" - if not isinstance(value, dict) or value.get("type") not in ( - "Polygon", - "MultiPolygon", - ): - raise ValueError( - "Support polygon must be a GeoJSON Polygon or MultiPolygon mapping." - ) - try: - polygon = shape(value) - except ( - AttributeError, - KeyError, - IndexError, - TypeError, - ValueError, - OverflowError, - shapely.errors.GEOSException, - ) as exc: - raise ValueError("Support polygon has invalid GeoJSON coordinates.") from exc - if ( - not isinstance(polygon, (Polygon, MultiPolygon)) - or polygon.is_empty - or polygon.has_z - or not np.isfinite(shapely.get_coordinates(polygon)).all() - or not polygon.is_valid - or polygon.area <= 0 - ): - raise ValueError( - "Support polygon must contain a valid nonempty finite XY area." - ) - return polygon +__all__: list[str] = [] def _plane_region( diff --git a/embodichain/lab/sim/scene/_validation.py b/embodichain/lab/sim/scene/_validation.py index 52898f649f..a867003193 100644 --- a/embodichain/lab/sim/scene/_validation.py +++ b/embodichain/lab/sim/scene/_validation.py @@ -18,69 +18,18 @@ from __future__ import annotations -import math -from numbers import Real from typing import TYPE_CHECKING +from embodichain.lab._scene_config.values import ( + _identifier, + _nonnegative_integer, + _rigid_pose, +) + if TYPE_CHECKING: import torch -__all__ = [] - - -def _identifier(value: str, name: str) -> str: - if not isinstance(value, str) or not value or value.strip() != value: - raise ValueError(f"{name} must be a nonempty stripped string") - return value - - -def _nonnegative_integer(value: int, name: str) -> int: - if type(value) is not int or value < 0: - raise ValueError(f"{name} must be a nonnegative integer") - return value - - -def _rigid_pose(value: object) -> tuple[tuple[float, ...], ...]: - try: - rows = tuple(tuple(row) for row in value) # type: ignore[union-attr] - except TypeError as exc: - raise ValueError("arena_pose must be a 4x4 rigid transform") from exc - if len(rows) != 4 or any(len(row) != 4 for row in rows): - raise ValueError("arena_pose must be a 4x4 rigid transform") - if any( - isinstance(item, bool) or not isinstance(item, Real) or not math.isfinite(item) - for row in rows - for item in row - ): - raise ValueError("arena_pose must contain finite real numbers") - pose = tuple( - tuple(float(item) if item != 0 else 0.0 for item in row) for row in rows - ) - tolerance = 1e-6 - if any( - abs(pose[3][index] - target) > tolerance - for index, target in enumerate((0, 0, 0, 1)) - ): - raise ValueError("arena_pose must have homogeneous last row [0, 0, 0, 1]") - rotation = tuple(row[:3] for row in pose[:3]) - if any( - abs(sum(rotation[k][i] * rotation[k][j] for k in range(3)) - (i == j)) - > tolerance - for i in range(3) - for j in range(3) - ): - raise ValueError("arena_pose rotation must be orthonormal") - determinant = ( - rotation[0][0] - * (rotation[1][1] * rotation[2][2] - rotation[1][2] * rotation[2][1]) - - rotation[0][1] - * (rotation[1][0] * rotation[2][2] - rotation[1][2] * rotation[2][0]) - + rotation[0][2] - * (rotation[1][0] * rotation[2][1] - rotation[1][1] * rotation[2][0]) - ) - if abs(determinant - 1.0) > tolerance: - raise ValueError("arena_pose rotation must be proper (determinant +1)") - return (*pose[:3], (0.0, 0.0, 0.0, 1.0)) +__all__: list[str] = [] def _owned_batch_pose(value: torch.Tensor, name: str) -> torch.Tensor: diff --git a/embodichain/lab/sim/scene/expansion/_policy.py b/embodichain/lab/sim/scene/expansion/_policy.py index 52567481ca..03a4c4ffee 100644 --- a/embodichain/lab/sim/scene/expansion/_policy.py +++ b/embodichain/lab/sim/scene/expansion/_policy.py @@ -13,15 +13,23 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- - -"""Source-independent policies for bounded scene layout sampling.""" +"""Public scene policy configuration imports.""" from __future__ import annotations -from copy import deepcopy -from dataclasses import MISSING -import math -from typing import Any -from embodichain.utils import configclass + +from embodichain.lab._scene_config.policy import ( + IntraGroupRandomizationCfg, + MemberRandomizationCfg, + SceneRandomizationJobCfg, + SceneRandomizationPolicyCfg, + SupportOverrideCfg, + _decode_intra, + _intra_payload, + _policy_data, + _validate_backend, + _validate_generation_parameters, + _validate_intra, +) __all__ = [ "MemberRandomizationCfg", @@ -30,306 +38,3 @@ "SceneRandomizationPolicyCfg", "SceneRandomizationJobCfg", ] - - -@configclass -class MemberRandomizationCfg: - """One non-root member's displacement bounds in its parent support frame. - - ``xy_offset_bounds_m`` is ``[[min_x, min_y], [max_x, max_y]]`` relative - to the original entity origin; ``None`` samples the complete support area. - Yaw is a continuous increment around the support normal, retaining tilt. - """ - - xy_offset_bounds_m: list[list[float]] | None = None - yaw_delta_range_deg: list[float] = [-180.0, 180.0] - - -@configclass -class SupportOverrideCfg: - """Explicit parent-local support frame and GeoJSON Polygon/MultiPolygon. - - The proper rigid ``parent_from_support`` matrix locates the support plane - at support Z=0 in the parent local Z-up geometry after asset scaling. Its polygon must be covered by actual coplanar mesh faces. - """ - - parent_from_support: list[list[float]] = MISSING - polygon: dict = MISSING - - -@configclass -class IntraGroupRandomizationCfg: - """Explicit member selection and conservative support geometry settings. - - Fixed members follow their parent. Clearances and grid resolution are in - metres. Overrides are keyed by parent UID, independently of target order. - """ - - targets: dict[str, MemberRandomizationCfg] = {} - boundary_margin_m: float = 0.002 - sibling_margin_m: float = 0.002 - support_gap_m: float = 0.001 - grid_resolution_m: float = 0.001 - support_overrides: dict[str, SupportOverrideCfg] = {} - - -def _validate_backend(*, backend: str, device: str, batch_size: int) -> None: - """Validate execution controls without initializing CUDA or reading assets.""" - if backend not in ("slsqp", "batched"): - raise ValueError("backend must be slsqp or batched.") - if not isinstance(device, str) or not ( - device == "cpu" - or device == "cuda" - or ( - device.startswith("cuda:") and device[5:].isascii() and device[5:].isdigit() - ) - ): - raise ValueError("device must be cpu, cuda, or cuda:N.") - if backend == "slsqp" and device != "cpu": - raise ValueError( - "The slsqp backend runs on CPU; use backend: batched for CUDA." - ) - if type(batch_size) is not int or batch_size < 1: - raise ValueError("batch_size must be a positive integer.") - - -def _validate_generation_parameters( - *, seed: int, candidate_count: int, max_attempts: int, collision_margin_m: float -) -> None: - """Validate YAML values and Python overrides before accessing scene files.""" - for name, value, minimum in ( - ("seed", seed, 0), - ("candidate_count", candidate_count, 1), - ("max_attempts", max_attempts, 1), - ): - if type(value) is not int or value < minimum: - raise ValueError(f"{name} must be an integer >= {minimum}.") - if candidate_count > max_attempts: - raise ValueError("max_attempts must be at least candidate_count.") - if type(collision_margin_m) not in (int, float) or collision_margin_m < 0: - raise ValueError("collision_margin_m must be finite and nonnegative.") - try: - finite = math.isfinite(collision_margin_m) - except OverflowError: - finite = False - if not finite: - raise ValueError("collision_margin_m must be finite and nonnegative.") - - -def _mapping(value: Any, *, name: str, allowed: set[str]) -> dict[str, Any]: - if not isinstance(value, dict): - raise ValueError(f"{name} must be a mapping.") - unknown = set(value) - allowed - if unknown: - raise ValueError(f"Unknown {name} fields: {', '.join(sorted(unknown))}.") - return value - - -def _decode_intra(value: Any) -> IntraGroupRandomizationCfg | None: - """Strictly decode nested YAML mappings, also used for persisted policies.""" - if value is None: - return None - root = _mapping( - value, - name="intra_group", - allowed={ - "targets", - "boundary_margin_m", - "sibling_margin_m", - "support_gap_m", - "grid_resolution_m", - "support_overrides", - }, - ) - targets = root.get("targets", {}) - if not isinstance(targets, dict): - raise ValueError("targets must be a mapping.") - decoded = {} - for uid, target in targets.items(): - if not isinstance(uid, str) or not uid.strip(): - raise ValueError("Target UIDs must be nonempty strings.") - item = _mapping( - target, - name=f"target {uid}", - allowed={"xy_offset_bounds_m", "yaw_delta_range_deg"}, - ) - decoded[uid] = MemberRandomizationCfg(**item) - overrides = root.get("support_overrides", {}) - if not isinstance(overrides, dict): - raise ValueError("support_overrides must be a mapping.") - override_cfg = {} - for uid, override in overrides.items(): - item = _mapping( - override, - name=f"support override {uid}", - allowed={"parent_from_support", "polygon"}, - ) - if set(item) != {"parent_from_support", "polygon"}: - raise ValueError("Support overrides require a transform and polygon.") - override_cfg[uid] = SupportOverrideCfg(**item) - cfg = IntraGroupRandomizationCfg( - **{k: v for k, v in root.items() if k not in ("targets", "support_overrides")}, - targets=decoded, - support_overrides=override_cfg, - ) - _validate_intra(cfg) - return cfg - - -def _validate_intra(cfg: IntraGroupRandomizationCfg) -> None: - import numpy as np - - if not isinstance(cfg, IntraGroupRandomizationCfg): - raise ValueError("intra_group must be an IntraGroupRandomizationCfg.") - cfg.validate() - if not isinstance(cfg.targets, dict) or not isinstance(cfg.support_overrides, dict): - raise ValueError("targets and support_overrides must be mappings.") - for field in ( - "boundary_margin_m", - "sibling_margin_m", - "support_gap_m", - "grid_resolution_m", - ): - value = getattr(cfg, field) - try: - finite = type(value) in (int, float) and math.isfinite(value) - except OverflowError: - finite = False - if not finite or value < 0 or (field == "grid_resolution_m" and value == 0): - raise ValueError( - f"{field} must be finite and {'positive' if field == 'grid_resolution_m' else 'nonnegative'}." - ) - for uid, item in cfg.targets.items(): - if ( - not isinstance(uid, str) - or not uid - or not isinstance(item, MemberRandomizationCfg) - ): - raise ValueError("targets must map UIDs to MemberRandomizationCfg.") - yaw = np.asarray(item.yaw_delta_range_deg) - if ( - yaw.shape != (2,) - or yaw.dtype.kind not in "fi" - or not np.isfinite(yaw).all() - or yaw[0] > yaw[1] - or not np.isfinite(yaw[1] - yaw[0]) - ): - raise ValueError( - "yaw_delta_range_deg requires finite ordered bounds and width." - ) - if item.xy_offset_bounds_m is not None: - xy = np.asarray(item.xy_offset_bounds_m) - if ( - xy.shape != (2, 2) - or xy.dtype.kind not in "fi" - or not np.isfinite(xy).all() - or np.any(xy[0] > xy[1]) - ): - raise ValueError( - "xy_offset_bounds_m requires finite ordered lower/upper XY corners." - ) - for uid, override in cfg.support_overrides.items(): - if ( - not isinstance(uid, str) - or not uid - or not isinstance(override, SupportOverrideCfg) - ): - raise ValueError("support_overrides must map UIDs to SupportOverrideCfg.") - override.validate() - from embodichain.lab.sim.scene._support_grid import _read_support_polygon - - try: - _read_support_polygon(override.polygon) - except ValueError as exc: - raise ValueError(f"Support override {uid!r}: {exc}") from exc - try: - matrix = np.asarray(override.parent_from_support, dtype=float) - except (TypeError, ValueError, OverflowError) as exc: - raise ValueError( - "parent_from_support must contain finite numeric values." - ) from exc - if ( - matrix.shape != (4, 4) - or not np.isfinite(matrix).all() - or not np.array_equal(matrix[3], [0, 0, 0, 1]) - or not np.allclose( - matrix[:3, :3].T @ matrix[:3, :3], np.eye(3), atol=1e-8, rtol=0 - ) - or not np.isclose(np.linalg.det(matrix[:3, :3]), 1, atol=1e-8, rtol=0) - ): - raise ValueError("parent_from_support must be a proper rigid transform.") - - -def _intra_payload(cfg: IntraGroupRandomizationCfg) -> dict: - """Serialize only documented settings, independent of configclass internals.""" - return deepcopy( - { - "targets": { - uid: { - "xy_offset_bounds_m": item.xy_offset_bounds_m, - "yaw_delta_range_deg": list(item.yaw_delta_range_deg), - } - for uid, item in sorted(cfg.targets.items()) - }, - **{ - key: float(getattr(cfg, key)) - for key in ( - "boundary_margin_m", - "sibling_margin_m", - "support_gap_m", - "grid_resolution_m", - ) - }, - "support_overrides": { - uid: { - "parent_from_support": item.parent_from_support, - "polygon": item.polygon, - } - for uid, item in sorted(cfg.support_overrides.items()) - }, - } - ) - - -@configclass -class SceneRandomizationPolicyCfg: - """Geometric policy independent of scene files and runtime lifecycles. - - Args: - backend: Explicit numeric backend; the shared interface uses batched sampling. - device: CPU or CUDA tensor device. - batch_size: Maximum logical proposals evaluated together. - collision_margin_m: Minimum group AABB separation in metres. - randomize_groups: Sample permitted group-root XY translations. - group_targets: Root UIDs to move; None selects all permitted roots. - group_xy_offset_bounds_m: Optional per-root arena XY displacement bounds. - intra_group: Selected non-root XY/yaw changes and support settings. - """ - - backend: str = "batched" - device: str = "cpu" - batch_size: int = 4096 - collision_margin_m: float = 0.02 - randomize_groups: bool = True - group_targets: tuple[str, ...] | None = None - group_xy_offset_bounds_m: dict[str, list[list[float]]] = {} - intra_group: IntraGroupRandomizationCfg | None = None - - -@configclass -class SceneRandomizationJobCfg: - """Bounded caller-owned generation settings and output representation. - - Args: - policy: Source-independent geometric sampling policy. - seed: Addressed generation seed. - candidate_count: Requested valid layouts. - max_attempts: Whole-layout proposal budget. - layout_storage: JSON or packed candidate storage. - """ - - policy: SceneRandomizationPolicyCfg = SceneRandomizationPolicyCfg() - seed: int = 0 - candidate_count: int = 10 - max_attempts: int = 1000 - layout_storage: str = "packed" diff --git a/embodichain/lab/sim/scene/expansion/conditions.py b/embodichain/lab/sim/scene/expansion/conditions.py index 2b70be330d..8d9390a6f0 100644 --- a/embodichain/lab/sim/scene/expansion/conditions.py +++ b/embodichain/lab/sim/scene/expansion/conditions.py @@ -35,6 +35,10 @@ __all__ = ["SceneLayoutCondition"] +class _LayoutConditionDomainError(ValueError): + """A well-formed anchor conflicts with its prepared layout domain.""" + + @dataclass(frozen=True) class SceneLayoutCondition: """Fix declared entities to arena poses without enlarging their motion domain. @@ -94,7 +98,7 @@ def _condition_prepared( parent = parent_uid while parent != source.support_uid: if parent in variable and parent not in condition.poses: - raise ValueError( + raise _LayoutConditionDomainError( f"Anchor {uid!r} requires its moving ancestor {parent!r} to be anchored" ) parent = source.parent_by_uid[parent] @@ -108,20 +112,24 @@ def _condition_prepared( not np.allclose(wanted[:3, :3], original[:3, :3], rtol=0, atol=1e-9) or abs(wanted[2, 3] - original[2, 3]) > 1e-9 ): - raise ValueError( + raise _LayoutConditionDomainError( "Root anchors support declared XY translation only; rotation/height must remain authored" ) delta = wanted[:2, 3] - original[:2, 3] if uid not in roots: if not np.allclose(delta, 0, rtol=0, atol=1e-9): - raise ValueError(f"Condition cannot move fixed group {uid!r}") + raise _LayoutConditionDomainError( + f"Condition cannot move fixed group {uid!r}" + ) else: bounds = policy["group_xy_offset_bounds_m"].get(uid) if bounds is not None and ( np.any(delta < np.asarray(bounds)[0] - 1e-9) or np.any(delta > np.asarray(bounds)[1] + 1e-9) ): - raise ValueError("Anchor lies outside declared root XY bounds") + raise _LayoutConditionDomainError( + "Anchor lies outside declared root XY bounds" + ) policy["group_xy_offset_bounds_m"][uid] = [ delta.tolist(), delta.tolist(), @@ -135,7 +143,9 @@ def _condition_prepared( wanted = np.asarray(condition.poses[uid]) if uid not in targets: if not np.allclose(wanted, parent_world @ relative, rtol=0, atol=1e-9): - raise ValueError(f"Condition cannot move undeclared member {uid!r}") + raise _LayoutConditionDomainError( + f"Condition cannot move undeclared member {uid!r}" + ) world[uid] = wanted continue spec = targets[uid] @@ -158,18 +168,22 @@ def _condition_prepared( not np.allclose(rotation, yaw_rotation, rtol=0, atol=1e-9) or abs(local[2, 3] - height) > 1e-9 ): - raise ValueError( + raise _LayoutConditionDomainError( "Member anchor violates its authored tilt or support height" ) if angle < low - 1e-8 or angle > high + 1e-8: - raise ValueError("Member anchor lies outside declared yaw bounds") + raise _LayoutConditionDomainError( + "Member anchor lies outside declared yaw bounds" + ) offset = local[:2, 3] - baseline[:2, 3] bounds = spec["xy_offset_bounds_m"] if bounds is not None and ( np.any(offset < np.asarray(bounds)[0] - 1e-9) or np.any(offset > np.asarray(bounds)[1] + 1e-9) ): - raise ValueError("Member anchor lies outside declared XY bounds") + raise _LayoutConditionDomainError( + "Member anchor lies outside declared XY bounds" + ) spec["xy_offset_bounds_m"] = [offset.tolist(), offset.tolist()] spec["yaw_delta_range_deg"] = [angle, angle] world[uid] = wanted diff --git a/embodichain/lab/sim/scene/expansion/contracts.py b/embodichain/lab/sim/scene/expansion/contracts.py index 2b103efa11..a1bc4fb78a 100644 --- a/embodichain/lab/sim/scene/expansion/contracts.py +++ b/embodichain/lab/sim/scene/expansion/contracts.py @@ -13,145 +13,19 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- - -"""Immutable scene proposals, independent of task and execution lifecycles.""" +"""Public scene proposal contracts shared with simulator-free configuration.""" from __future__ import annotations -from dataclasses import dataclass -import hashlib -import json +from embodichain.lab._scene_config.scene import ( + ScenePoseChange, + SceneVariant, + _variant_id, +) +from embodichain.lab._scene_config.values import ( + _identifier, + _nonnegative_integer, + _rigid_pose, +) __all__ = ["ScenePoseChange", "SceneVariant"] - - -from .._validation import _identifier, _nonnegative_integer, _rigid_pose - - -def _variant_id( - parent_scene_id: str, - seed: int, - ordinal: int, - pose_changes: tuple, - metadata: tuple, -) -> str: - """Hash already validated, canonically ordered proposal values.""" - payload = { - "schema": "scene-variant/v1", - "parent_scene_id": parent_scene_id, - "seed": seed, - "ordinal": ordinal, - "pose_changes": pose_changes, - "metadata": metadata, - } - encoded = json.dumps( - payload, sort_keys=True, separators=(",", ":"), allow_nan=False - ).encode("utf-8") - return "scene-variant:" + hashlib.sha256(encoded).hexdigest() - - -@dataclass(frozen=True) -class ScenePoseChange: - """A proposed pose for one existing physical entity. - - Args: - entity_uid: Stable physical entity UID, independent of environment slots. - arena_pose: Homogeneous 4x4 transform from the entity frame into its - local Z-up arena frame. Inputs are copied into immutable tuples. - - .. attention:: - An arena pose excludes replication offsets. The host converts it to - backend coordinates when applying the candidate. This contract does - not establish support, collision freedom, or manipulation feasibility. - """ - - entity_uid: str - arena_pose: tuple[tuple[float, ...], ...] - - def __post_init__(self) -> None: - _identifier(self.entity_uid, "entity_uid") - object.__setattr__(self, "arena_pose", _rigid_pose(self.arena_pose)) - - -@dataclass(frozen=True) -class SceneVariant: - """An immutable scene proposal with reproducible proposal identity. - - Args: - parent_scene_id: Content/revision identity of the original scene. - seed: Nonnegative proposal-generation seed. - ordinal: Nonnegative position in the candidate source. - pose_changes: Unique existing entities and their proposed arena poses. - metadata: Unique string key/value provenance entries. Entries and pose - changes are sorted so equivalent input ordering has one identity. - - .. attention:: - ``variant_id`` identifies a proposal, not a settled initial state or a - successful execution. Hosts record those identities and evidence - separately. Metadata describes provenance, not executable operations. - Since ``ordinal`` participates in the hash, consuming the same layout - at a different source position changes ``variant_id``. File-backed - consumers should retain the original layout identity and proposal - ordinal in metadata for provenance across selection orders; this ID - is not a geometry-only deduplication key. - """ - - parent_scene_id: str - seed: int - ordinal: int - pose_changes: tuple[ScenePoseChange, ...] - metadata: tuple[tuple[str, str], ...] = () - - def __post_init__(self) -> None: - _identifier(self.parent_scene_id, "parent_scene_id") - _nonnegative_integer(self.seed, "seed") - _nonnegative_integer(self.ordinal, "ordinal") - try: - changes = tuple(self.pose_changes) - except TypeError as exc: - raise ValueError( - "pose_changes must contain ScenePoseChange values" - ) from exc - if any(not isinstance(change, ScenePoseChange) for change in changes): - raise ValueError("pose_changes must contain ScenePoseChange values") - if len({change.entity_uid for change in changes}) != len(changes): - raise ValueError("pose_changes must contain unique entity UIDs") - object.__setattr__( - self, - "pose_changes", - tuple(sorted(changes, key=lambda change: change.entity_uid)), - ) - try: - entries = tuple(self.metadata) - if any(isinstance(entry, str) for entry in entries): - raise ValueError("metadata must contain string key/value pairs") - metadata = tuple(tuple(entry) for entry in entries) - except TypeError as exc: - raise ValueError("metadata must contain string key/value pairs") from exc - if any(len(entry) != 2 for entry in metadata): - raise ValueError("metadata must contain string key/value pairs") - for key, value in metadata: - _identifier(key, "metadata key") - if not isinstance(value, str): - raise ValueError("metadata values must be strings") - if len({key for key, _ in metadata}) != len(metadata): - raise ValueError("metadata must contain unique keys") - object.__setattr__(self, "metadata", tuple(sorted(metadata))) - - @property - def variant_id(self) -> str: - """Return a stable identity including geometry, source, seed, and ordinal. - - Returns: - SHA-256 identity independent of Python hash randomization and - temporary physical environment assignments. - """ - return _variant_id( - self.parent_scene_id, - self.seed, - self.ordinal, - tuple( - (change.entity_uid, change.arena_pose) for change in self.pose_changes - ), - self.metadata, - ) diff --git a/embodichain/lab/sim/scene/expansion/layout.py b/embodichain/lab/sim/scene/expansion/layout.py index 14ec5af156..17763a96f3 100644 --- a/embodichain/lab/sim/scene/expansion/layout.py +++ b/embodichain/lab/sim/scene/expansion/layout.py @@ -45,13 +45,10 @@ _specification, ) from ._policy import ( - IntraGroupRandomizationCfg, SceneRandomizationPolicyCfg, - _decode_intra, - _intra_payload, + _policy_data, _validate_backend, _validate_generation_parameters, - _validate_intra, ) if TYPE_CHECKING: @@ -253,66 +250,6 @@ def scene_layout_input_from_sim( ) -def _policy_data(cfg: SceneRandomizationPolicyCfg | Mapping) -> dict: - if isinstance(cfg, Mapping): - raw = dict(cfg) - cfg = SceneRandomizationPolicyCfg(**raw) - if not isinstance(cfg, SceneRandomizationPolicyCfg): - raise TypeError("policy must be a SceneRandomizationPolicyCfg or mapping") - cfg = cfg.copy() - cfg.validate() - _validate_backend(backend=cfg.backend, device=cfg.device, batch_size=cfg.batch_size) - if cfg.backend != "batched": - raise ValueError( - "The source-independent layout API supports backend='batched'." - ) - _validate_generation_parameters( - seed=0, - candidate_count=1, - max_attempts=1, - collision_margin_m=cfg.collision_margin_m, - ) - if type(cfg.randomize_groups) is not bool: - raise ValueError("randomize_groups must be a boolean.") - intra = cfg.intra_group - if isinstance(intra, dict) or intra is None: - intra = _decode_intra(intra) - if intra is None: - intra = IntraGroupRandomizationCfg() - _validate_intra(intra) - if not isinstance(cfg.group_xy_offset_bounds_m, dict): - raise ValueError("group_xy_offset_bounds_m must map root UIDs to XY bounds.") - for uid, bounds in cfg.group_xy_offset_bounds_m.items(): - if bounds is None: - raise ValueError( - "Root XY bounds must be explicit lower/upper corners; omit unbounded roots." - ) - _decode_intra( - { - "targets": { - uid: {"xy_offset_bounds_m": bounds, "yaw_delta_range_deg": [0, 0]} - } - } - ) - if cfg.group_xy_offset_bounds_m and not cfg.randomize_groups: - raise ValueError("Root XY bounds require randomize_groups=True.") - if cfg.group_targets is not None and ( - isinstance(cfg.group_targets, str) - or len(set(cfg.group_targets)) != len(cfg.group_targets) - ): - raise ValueError("group_targets must contain unique root UIDs.") - return { - "backend": cfg.backend, - "device": cfg.device, - "batch_size": cfg.batch_size, - "collision_margin_m": float(cfg.collision_margin_m), - "randomize_groups": cfg.randomize_groups, - "group_targets": None if cfg.group_targets is None else list(cfg.group_targets), - "group_xy_offset_bounds_m": deepcopy(cfg.group_xy_offset_bounds_m), - "intra_group": _intra_payload(intra), - } - - @dataclass(frozen=True) class PreparedSceneRandomization: """Own reusable numeric geometry, policy, permissions and source identity.""" diff --git a/embodichain/lab/sim/scene/feasibility/_values.py b/embodichain/lab/sim/scene/feasibility/_values.py index 1d3f6a1ea2..0c86962f9d 100644 --- a/embodichain/lab/sim/scene/feasibility/_values.py +++ b/embodichain/lab/sim/scene/feasibility/_values.py @@ -13,126 +13,19 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- -"""Private canonical value validation for feasibility artifacts.""" +"""Internal compatibility imports for canonical feasibility value validation.""" from __future__ import annotations -from collections.abc import Mapping -from dataclasses import fields, is_dataclass -import hashlib -import json -import math -from numbers import Real -from enum import Enum -import numpy as np -import torch -from pathlib import Path -from types import MappingProxyType - -from .._validation import _identifier, _rigid_pose - -__all__ = [] - - -def plain(value: object) -> object: - if isinstance(value, np.ndarray): - return plain(value.tolist()) - if isinstance(value, np.generic): - return plain(value.item()) - if isinstance(value, torch.Tensor): - return plain(value.detach().cpu().tolist()) - if isinstance(value, (Path, torch.device)): - return str(value) - if isinstance(value, slice): - return {"slice": [value.start, value.stop, value.step]} - if isinstance(value, Enum): - return plain(value.value) - if isinstance(getattr(value, "name", None), str) and isinstance( - getattr(value, "value", None), Real - ): - return { - "enum_type": type(value).__module__ + "." + type(value).__qualname__, - "name": value.name, - "value": plain(value.value), - } - if isinstance(value, (set, frozenset)): - return sorted( - (plain(v) for v in value), key=lambda v: json.dumps(v, sort_keys=True) - ) - if is_dataclass(value) and not isinstance(value, type): - return {f.name: plain(getattr(value, f.name)) for f in fields(value)} - if isinstance(value, Mapping): - return {str(k): plain(v) for k, v in value.items()} - if isinstance(value, (tuple, list)): - return [plain(v) for v in value] - return value - - -def digest(value: object) -> str: - return hashlib.sha256( - json.dumps( - plain(value), sort_keys=True, separators=(",", ":"), allow_nan=False - ).encode() - ).hexdigest() - - -def owned_json(value: Mapping[str, object]) -> Mapping[str, object]: - copied = json.loads(json.dumps(plain(value), allow_nan=False)) - - def freeze(item: object) -> object: - if isinstance(item, dict): - return MappingProxyType({k: freeze(v) for k, v in item.items()}) - if isinstance(item, list): - return tuple(freeze(v) for v in item) - return item - - return freeze(copied) - - -def finite(values: object, name: str) -> tuple[float, ...]: - result = tuple(values) - if any( - isinstance(v, bool) or not isinstance(v, Real) or not math.isfinite(v) - for v in result - ): - raise ValueError(f"{name} requires finite real values") - return tuple(float(v) for v in result) - - -def count(value: int, name: str, minimum: int = 1) -> None: - if type(value) is not int or value < minimum: - raise ValueError(f"{name} must be an integer >= {minimum}") - - -def names(values: object, name: str) -> tuple[str, ...]: - if isinstance(values, str): - raise ValueError(f"{name} requires a sequence") - result = tuple(values) - for value in result: - _identifier(value, name) - if len(set(result)) != len(result): - raise ValueError(f"{name} must be unique") - return result - - -def poses(values: Mapping[str, object]) -> Mapping[str, tuple]: - return MappingProxyType( - { - _identifier(k, "entity UID"): _rigid_pose(v) - for k, v in sorted(values.items()) - } - ) - - -def atomic_json(path: Path, value: object) -> None: - import os - import tempfile - - path.parent.mkdir(parents=True, exist_ok=True) - fd, temporary = tempfile.mkstemp(prefix=path.name + ".", dir=path.parent) - try: - with os.fdopen(fd, "w", encoding="utf-8") as stream: - json.dump(plain(value), stream, allow_nan=False, sort_keys=True, indent=2) - os.replace(temporary, path) - finally: - Path(temporary).unlink(missing_ok=True) +from embodichain.lab._scene_config.values import ( + atomic_json, + count, + digest, + finite, + names, + owned_json, + plain, + poses, +) + +__all__: list[str] = [] diff --git a/embodichain/lab/sim/scene/feasibility/contracts.py b/embodichain/lab/sim/scene/feasibility/contracts.py index c38fdcfa09..51069c3e43 100644 --- a/embodichain/lab/sim/scene/feasibility/contracts.py +++ b/embodichain/lab/sim/scene/feasibility/contracts.py @@ -22,12 +22,19 @@ from dataclasses import dataclass, field from functools import cached_property from types import MappingProxyType -import math from typing import Literal -from .._validation import _identifier, _rigid_pose -from ..expansion import ScenePoseChange, SceneVariant -from ._values import count, digest, finite, names, owned_json, poses +from embodichain.lab._scene_config.feasibility import ( + HeldObject, + PhaseTarget, + PoseSearchVariable, + TaskFeasibilityRequest, + TaskOperationMode, + TaskPhase, +) +from .._validation import _identifier +from ..expansion import SceneVariant +from ._values import digest, finite, names, owned_json, poses __all__ = [ "PoseSearchVariable", @@ -42,230 +49,6 @@ "CandidateEvaluation", ] -_IDENTITY = ( - (1.0, 0.0, 0.0, 0.0), - (0.0, 1.0, 0.0, 0.0), - (0.0, 0.0, 1.0, 0.0), - (0.0, 0.0, 0.0, 1.0), -) - - -@dataclass(frozen=True) -class PoseSearchVariable: - """One XY/yaw displacement applied to a scene group or robot installation. - - Args: - name: Unique variable name. - kind: ``scene`` or ``robot``; robot members are physical robot UIDs. - reference_pose: Group-to-arena Z-up transform, including mounting height. - lower: Lower XY metre / yaw radian displacement bounds. - upper: Upper displacement bounds; equal endpoints fix a parameter. - members: Member UID to member-to-group transforms. All members move - together, preserving dual-arm mounting and support-group relations. - """ - - name: str - kind: Literal["scene", "robot"] - reference_pose: tuple - lower: tuple[float, float, float] - upper: tuple[float, float, float] - members: Mapping[str, tuple] - - def __post_init__(self) -> None: - _identifier(self.name, "variable name") - if self.kind not in ("scene", "robot"): - raise ValueError("variable kind must be scene or robot") - object.__setattr__(self, "reference_pose", _rigid_pose(self.reference_pose)) - lo, hi = finite(self.lower, "lower"), finite(self.upper, "upper") - if len(lo) != 3 or len(hi) != 3 or any(a > b for a, b in zip(lo, hi)): - raise ValueError("bounds require three ordered XY/yaw coordinates") - if hi[2] - lo[2] > 2 * math.pi + 1e-10: - raise ValueError("yaw interval must span at most one turn") - object.__setattr__(self, "lower", lo) - object.__setattr__(self, "upper", hi) - object.__setattr__(self, "members", poses(self.members)) - if not self.members: - raise ValueError("variables require explicit member UIDs") - - -@dataclass(frozen=True) -class PhaseTarget: - """One simultaneous TCP target in an explicit arena or entity frame. - - Args: - control_part: Explicit solver name, preserving custom names. - pose: TCP-to-frame rigid transform. - frame_uid: Entity frame UID, or ``None`` for the arena frame. - """ - - control_part: str - pose: tuple - frame_uid: str | None = None - - def __post_init__(self) -> None: - _identifier(self.control_part, "control_part") - object.__setattr__(self, "pose", _rigid_pose(self.pose)) - if self.frame_uid is not None: - _identifier(self.frame_uid, "frame_uid") - - -@dataclass(frozen=True) -class HeldObject: - """Rigid attachment during travel; never a physical-success assertion. - - Args: - entity_uid: Held object UID. - control_part: Owner TCP/control part. - tcp_from_object: Object-to-TCP transform used by collision FK. - """ - - entity_uid: str - control_part: str - tcp_from_object: tuple = _IDENTITY - - def __post_init__(self) -> None: - _identifier(self.entity_uid, "held entity") - _identifier(self.control_part, "held owner") - object.__setattr__(self, "tcp_from_object", _rigid_pose(self.tcp_from_object)) - - -@dataclass(frozen=True) -class TaskPhase: - """A synchronized stage and its explicit planning-state transition. - - Args: - phase_id: Stable stage identity. - targets: Simultaneous targets, at most one per control part. - dependencies: Predecessors; input order must be a topological order. - held_objects: Attachments during incoming motion, one owner per object. - allowed_contacts: Physical collision IDs allowed to touch in this stage. - resulting_poses: Projected object states after this stage, used only for - subsequent planning; runtime must still read fresh physical state. - joint_targets: Explicit joint-name commands, e.g. open/closed fingers. - """ - - phase_id: str - targets: tuple[PhaseTarget, ...] = () - dependencies: tuple[str, ...] = () - held_objects: tuple[HeldObject, ...] = () - allowed_contacts: tuple[tuple[str, str], ...] = () - resulting_poses: tuple[ScenePoseChange, ...] = () - joint_targets: tuple[tuple[str, float], ...] = () - - def __post_init__(self) -> None: - _identifier(self.phase_id, "phase_id") - for attr, cls in ( - ("targets", PhaseTarget), - ("held_objects", HeldObject), - ("resulting_poses", ScenePoseChange), - ): - values = tuple(getattr(self, attr)) - if any(not isinstance(v, cls) for v in values): - raise TypeError(f"{attr} contains an invalid contract") - object.__setattr__(self, attr, values) - names([v.control_part for v in self.targets], "phase target parts") - names([v.entity_uid for v in self.held_objects], "held objects") - names([v.entity_uid for v in self.resulting_poses], "resulting poses") - object.__setattr__( - self, "dependencies", names(self.dependencies, "dependencies") - ) - pairs = tuple( - tuple(sorted(names(pair, "contact pair"))) for pair in self.allowed_contacts - ) - if any(len(p) != 2 for p in pairs) or len(set(pairs)) != len(pairs): - raise ValueError("allowed contacts require unique distinct pairs") - object.__setattr__(self, "allowed_contacts", pairs) - joints = tuple( - (_identifier(k, "joint target"), finite((v,), "joint target")[0]) - for k, v in self.joint_targets - ) - names([k for k, _ in joints], "joint targets") - object.__setattr__(self, "joint_targets", joints) - - -@dataclass(frozen=True) -class TaskOperationMode: - """One permitted grasp/assignment/handover branch of the original task.""" - - mode_id: str - phases: tuple[TaskPhase, ...] - - def __post_init__(self) -> None: - _identifier(self.mode_id, "mode_id") - values = tuple(self.phases) - if not values or any(not isinstance(v, TaskPhase) for v in values): - raise ValueError("modes require explicit task phases") - names([v.phase_id for v in values], "phase IDs") - seen = set() - for phase in values: - if not set(phase.dependencies) <= seen: - raise ValueError("phases must be in dependency order without cycles") - seen.add(phase.phase_id) - object.__setattr__(self, "phases", values) - - -@dataclass(frozen=True) -class TaskFeasibilityRequest: - """Versioned fixed semantics, robot, scene, and allowed joint search domain. - - Args: - task_id: Semantic identity including original roles/goals/invariants. - robot_profile_id: Robot, tool and calibrated configuration identity. - policy_id: Planner/runtime policy version. - validator_id: Measured acceptance version. - parent_scene_id: Base scene revision/content identity. - entity_poses: Authored arena poses, including every variable member. - variables: Permitted scene and mounting changes. - modes: Allowed operation modes, preserving resource constraints. - """ - - task_id: str - robot_profile_id: str - policy_id: str - validator_id: str - parent_scene_id: str - entity_poses: Mapping[str, tuple] - variables: tuple[PoseSearchVariable, ...] - modes: tuple[TaskOperationMode, ...] - - def __post_init__(self) -> None: - for key in ( - "task_id", - "robot_profile_id", - "policy_id", - "validator_id", - "parent_scene_id", - ): - _identifier(getattr(self, key), key) - object.__setattr__(self, "entity_poses", poses(self.entity_poses)) - variables, modes = tuple(self.variables), tuple(self.modes) - if any(not isinstance(v, PoseSearchVariable) for v in variables): - raise TypeError("variables require PoseSearchVariable values") - if not modes or any(not isinstance(v, TaskOperationMode) for v in modes): - raise ValueError("request requires explicit operation modes") - names([v.name for v in variables], "variable names") - members = [k for v in variables for k in v.members] - names(members, "variable members") - if not set(members) <= self.entity_poses.keys(): - raise ValueError("variable members must exist in the authored scene") - names([v.mode_id for v in modes], "mode IDs") - for mode in modes: - for phase in mode.phases: - referenced = { - t.frame_uid for t in phase.targets if t.frame_uid is not None - } - referenced |= {h.entity_uid for h in phase.held_objects} - referenced |= {p.entity_uid for p in phase.resulting_poses} - if not referenced <= self.entity_poses.keys(): - raise ValueError("phase refers to an undeclared entity") - object.__setattr__(self, "variables", variables) - object.__setattr__(self, "modes", modes) - - @cached_property - def request_id(self) -> str: - """Return the content identity of all fixed conditions and permissions.""" - return digest(self) - @dataclass(frozen=True) class RobotDeploymentCandidate: diff --git a/embodichain/lab/sim/scene/feasibility/persistence.py b/embodichain/lab/sim/scene/feasibility/persistence.py index b8d3e5b135..1e47b696e8 100644 --- a/embodichain/lab/sim/scene/feasibility/persistence.py +++ b/embodichain/lab/sim/scene/feasibility/persistence.py @@ -19,21 +19,15 @@ from __future__ import annotations from collections.abc import Mapping -from dataclasses import fields from pathlib import Path import json import numpy as np +from embodichain.lab._scene_config.feasibility import _closed, request_from_mapping from ..expansion import ScenePoseChange, SceneVariant from ._values import atomic_json, digest, plain from .cfg import SceneRobotFeasibilityCfg from .contracts import ( - PoseSearchVariable, - PhaseTarget, - HeldObject, - TaskPhase, - TaskOperationMode, - TaskFeasibilityRequest, RobotDeploymentCandidate, JointSceneCandidate, CandidateEvaluation, @@ -49,46 +43,6 @@ __all__ = ["request_from_mapping", "save_feasibility_atlas", "load_feasibility_atlas"] -def _closed(cls: type, value: Mapping[str, object]) -> dict: - if not isinstance(value, Mapping): - raise ValueError(f"{cls.__name__} requires a mapping") - unknown = set(value) - {f.name for f in fields(cls)} - if unknown: - raise ValueError(f"Unknown {cls.__name__} fields: {sorted(unknown)}") - return dict(value) - - -def request_from_mapping(value: Mapping[str, object]) -> TaskFeasibilityRequest: - """Decode a provider-independent request through closed production contracts. - - Args: - value: JSON/YAML task, scene, mounting and phase declarations. - Returns: - Validated immutable request. No dotted import or model client is loaded. - """ - data = _closed(TaskFeasibilityRequest, value) - data["variables"] = tuple( - PoseSearchVariable(**_closed(PoseSearchVariable, v)) for v in data["variables"] - ) - modes = [] - for mode in data["modes"]: - m = _closed(TaskOperationMode, mode) - phases = [] - for phase in m["phases"]: - p = _closed(TaskPhase, phase) - for key, cls in ( - ("targets", PhaseTarget), - ("held_objects", HeldObject), - ("resulting_poses", ScenePoseChange), - ): - p[key] = tuple(cls(**_closed(cls, v)) for v in p.get(key, ())) - phases.append(TaskPhase(**p)) - m["phases"] = tuple(phases) - modes.append(TaskOperationMode(**m)) - data["modes"] = tuple(modes) - return TaskFeasibilityRequest(**data) - - def save_feasibility_atlas(atlas: FeasibilityAtlas, path: str | Path) -> None: """Atomically publish a frozen, content-addressed evidence snapshot. diff --git a/embodichain/lab/task_program/integrations/feasibility.py b/embodichain/lab/task_program/integrations/feasibility.py index f422c48727..279b2e5f3b 100644 --- a/embodichain/lab/task_program/integrations/feasibility.py +++ b/embodichain/lab/task_program/integrations/feasibility.py @@ -93,6 +93,14 @@ def __init__( self.events.append((data["event_id"], conditions, steps)) if len({e[0] for e in self.events}) != len(self.events): raise ValueError("event IDs must be unique") + event_conditions = ( + condition for _, conditions, _ in self.events for condition in conditions + ) + self._transport_guards = { + json.dumps(condition, sort_keys=True): condition + for condition in (*self.final_checks, *event_conditions) + if condition["kind"] == "transport_guard" + } self.initial_heights = {} self._index = 0 self._steps = 0 @@ -434,10 +442,7 @@ def batch_scope( def observe(self, env: Any, action: Any) -> None: """Record consecutive event predicates at actual control boundaries.""" self._steps += 1 - for condition in self.final_checks: - if condition["kind"] != "transport_guard": - continue - key = json.dumps(dict(condition), sort_keys=True) + for key, condition in self._transport_guards.items(): lifted, dropped = self._transport.get(key, (False, False)) pose = env.sim.get_rigid_object(condition["entity"]).get_local_pose()[ self.env_id, :3 diff --git a/tests/gym/envs/task_program/test_feasibility.py b/tests/gym/envs/task_program/test_feasibility.py index 9a62f0a910..e61087f2d2 100644 --- a/tests/gym/envs/task_program/test_feasibility.py +++ b/tests/gym/envs/task_program/test_feasibility.py @@ -99,6 +99,44 @@ def test_monitor_requires_measured_lift_then_stable_terminal_and_rejects_reverse assert not monitor.validation(env).accepted +@pytest.mark.parametrize("drop", [False, True]) +def test_event_only_transport_guard_uses_earlier_measured_boundaries(drop): + env, cube = environment() + goal = Cube() + goal.position[0, 0] = 0.5 + objects = {"cube": cube, "goal": goal} + env.sim.get_rigid_object_uid_list = lambda: tuple(objects) + env.sim.get_rigid_object = objects.get + arrived = { + "kind": "pose_near", + "entity": "cube", + "position": [0.5, 0.0, 0.025], + "tolerance_m": 0.01, + } + transport = { + "kind": "transport_guard", + "entity": "cube", + "other": "goal", + "lift_height_m": 0.05, + "minimum_transit_height_m": 0.025, + "goal_tolerance_m": 0.03, + } + monitor = MeasuredTaskMonitor( + (arrived,), + ({"event_id": "transported", "conditions": [arrived, transport]},), + ) + monitor.reset(env) + cube.position[0, 2] = 0.1 + monitor.observe(env, None) + assert not monitor.receipts + if drop: + cube.position[0, 2] = 0.025 + monitor.observe(env, None) + cube.position[:] = goal.position + monitor.observe(env, None) + assert monitor.validation(env).accepted is not drop + + def test_monitor_reads_physical_velocity_and_closed_schema(): env, cube = environment() monitor = MeasuredTaskMonitor( diff --git a/tests/lab/expansion/test_configured.py b/tests/lab/expansion/test_configured.py index 94bb78c69f..efb844a7a7 100644 --- a/tests/lab/expansion/test_configured.py +++ b/tests/lab/expansion/test_configured.py @@ -49,6 +49,40 @@ def test_task_program_declarations_are_pure_and_protect_contact_phases(tmp_path) load_task_program_expansion(invalid) +def test_official_layout_jobs_load_without_simulation_imports(): + import subprocess + import sys + + task_root = ( + Path(__file__).resolve().parents[3] + / "extensions/embodichain_tasks/configs/tasks/manipulation" + ) + paths = sorted(task_root.glob("**/acceptance/scene_first/expansion.yaml")) + assert paths + code = """ +import importlib.abc +import sys + +class NoSimulator(importlib.abc.MetaPathFinder): + def find_spec(self, fullname, path=None, target=None): + if fullname.startswith(("dexsim", "embodichain.lab.sim", "embodichain.lab.gym")): + raise AssertionError(f"configuration loader imported {fullname}") + +sys.meta_path.insert(0, NoSimulator()) +from embodichain.lab.expansion import load_task_program_expansion +for path in sys.argv[1:]: + sampling = load_task_program_expansion(path)["scene_sampling"] + assert sampling["layout"]["policy"] and sampling["task_mode"]["phases"] + assert sampling["robot_anchors"] +""" + result = subprocess.run( + [sys.executable, "-c", code, *map(str, paths)], + capture_output=True, + text=True, + ) + assert result.returncode == 0, result.stderr + + @pytest.mark.parametrize("name", ["default", "newton", "rgbd", "motion"]) def test_example_preflight_resolves_owned_backend_and_budget(name, tmp_path): path = ( diff --git a/tests/lab/expansion/test_distribution.py b/tests/lab/expansion/test_distribution.py index 3e6d07a7a3..0a04e75923 100644 --- a/tests/lab/expansion/test_distribution.py +++ b/tests/lab/expansion/test_distribution.py @@ -60,6 +60,55 @@ def test_missing_layout_is_not_zero(): assert r["joint"] == {} +@pytest.mark.parametrize( + ("tasks", "source_uuids", "matches"), + [ + (("program-a", "program-b"), ("source-a", "source-b"), False), + (("program-a", "program-a"), ("source-a", "source-b"), True), + ((None, None), ("source-a", "source-b"), False), + ((None, None), ("source-a", "source-a"), True), + ], +) +def test_report_requires_matching_recorded_task_identity( + tmp_path, monkeypatch, tasks, source_uuids, matches +): + from types import SimpleNamespace + import torch + from embodichain.lab.expansion import report_expansion_distribution + from embodichain.lab.expansion import persistence + + jobs = [tmp_path / name for name in ("before", "after")] + episodes = {} + for job, task_id, source_uuid in zip(jobs, tasks, source_uuids, strict=True): + job.mkdir() + (job / "job.json").write_text("{}") + metadata = {"instruction": "Pick and place", "source_episode_uuid": source_uuid} + if task_id is not None: + metadata["scene_case"] = {"task_id": task_id} + episodes[job.name] = SimpleNamespace( + episode_id=f"episode-{job.name}", + commit_id=f"commit-{job.name}", + metadata=metadata, + timestamps=torch.tensor([0.0, 1.0]), + observations={}, + validation=SimpleNamespace(accepted=True), + ) + monkeypatch.setattr( + persistence.ArtifactEpisodeSink, + "inspect", + lambda self: (SimpleNamespace(confirmed=True, storage_id=self.directory.name),), + ) + monkeypatch.setattr(persistence, "load_expansion_episode", episodes.__getitem__) + output = tmp_path / "report" + if matches: + report = report_expansion_distribution(jobs[:1], jobs[1:], output, entity="mug") + assert (report["before"], report["after"]) == (1, 1) + else: + with pytest.raises(ValueError, match="matching domain"): + report_expansion_distribution(jobs[:1], jobs[1:], output, entity="mug") + assert not output.exists() + + def test_orientation_sign_and_out_of_plane_rotation(): from embodichain.lab.expansion._distribution_pose import orientation diff --git a/tests/lab/expansion/test_gym_workloads.py b/tests/lab/expansion/test_gym_workloads.py index 0c7737e906..820bdd5ee2 100644 --- a/tests/lab/expansion/test_gym_workloads.py +++ b/tests/lab/expansion/test_gym_workloads.py @@ -96,16 +96,69 @@ def test_expired_warming_row_finishes_group_without_disturbing_live_peer( assert executor.live[1] is peer -def test_native_source_retires_expired_attempt_before_reading_world(): +@pytest.mark.parametrize("stop", ["wall_time_exhausted", "rollout_budget_exhausted"]) +def test_native_source_retires_exhausted_attempt_before_reading_world(stop): + from embodichain.lab.expansion.contracts import ( + EpisodeCandidate, + SceneCase, + ValidationCheck, + ValidationResult, + ) from embodichain.lab.expansion.session import ExpansionSession from embodichain.lab.expansion.session_cfg import ExpansionSessionCfg clock = [0.0] session = ExpansionSession( - ExpansionSessionCfg.from_mapping({"collection": {"max_wall_time_s": 0.01}}), + ExpansionSessionCfg.from_mapping( + { + "collection": { + "max_wall_time_s": 0.01, + "target_committed_episodes": 2, + "max_proposals": 3, + "max_rollout_attempts": 2, + }, + "execution": {"ready_high_watermark": 3, "ready_low_watermark": 0}, + "persistence": {"pending_episode_limit": 2, "pending_max_bytes": 4096}, + } + ), clock=lambda: clock[0], ) - clock[0] = 1.0 + if stop == "wall_time_exhausted": + clock[0] = 1.0 + else: + session.register_case( + SceneCase("case", "initial", "scene", "task", "robot"), + torch.tensor([[-1.0, 1.0]]), + joint_names=("joint",), + ) + passed = ValidationResult( + ( + ValidationCheck("path_collision", "passed"), + ValidationCheck("task_success", "passed"), + ) + ) + identities = [] + for index in range(2): + identity, _ = session.propose( + "case", + "initial", + source_id="source", + source_revision="revision", + template_id=f"recipe-{index}", + operator_id="episode", + ) + session.add_prepared_episode(EpisodeCandidate(identity, "{}"), passed) + assert ( + session.take_ready("case", "initial", episode_byte_budget=1024) + is not None + ) + session.mark_rollout_started(identity) + identities.append(identity) + session.release(identities[0], reason="rejected") + assert session.stop_reason is None + session.release(identities[1], reason="rejected") + assert session.stop_reason == stop + proposed = session.snapshot()["counts"]["proposed"] request = object() journal = Mock() ctx = SimpleNamespace(requests={0: request}, journal=journal) @@ -119,12 +172,14 @@ def test_native_source_retires_expired_attempt_before_reading_world(): } assert source.activate(WorkloadAssignment("expired", 0, 0, 0), None) is None journal.update.assert_called_once_with( - request, status="initial_rejected", reason="wall_time_exhausted" + request, status="initial_rejected", reason=stop ) - assert session.snapshot()["counts"]["proposed"] == 0 + assert session.snapshot()["counts"]["proposed"] == proposed -@pytest.mark.parametrize("stop", ["wall_time_exhausted", "unexpected"]) +@pytest.mark.parametrize( + "stop", ["wall_time_exhausted", "rollout_budget_exhausted", "unexpected"] +) def test_admission_expiry_race_only_suppresses_budget_exhaustion(stop): session = SimpleNamespace(stop_reason=None, register_case=Mock()) @@ -162,7 +217,7 @@ def propose(*args, **kwargs): source.activation_snapshot = (SimpleNamespace(batches={0: ()}), {}) source.env = SimpleNamespace(robot=SimpleNamespace(uid="robot")) assignment = WorkloadAssignment("group", 0, 0, 0) - if stop == "wall_time_exhausted": + if stop != "unexpected": assert source.activate(assignment, None) is None assert ctx.journal.update.call_args.kwargs["reason"] == stop else: diff --git a/tests/lab/expansion/test_scene_first_sampling.py b/tests/lab/expansion/test_scene_first_sampling.py index 76c4ce805f..85f8b6ba5b 100644 --- a/tests/lab/expansion/test_scene_first_sampling.py +++ b/tests/lab/expansion/test_scene_first_sampling.py @@ -37,6 +37,7 @@ def setup_source( member_yaw=False, workspace=None, bounded_fixed_root=False, + configure=None, ): import embodichain.lab.expansion._scene_first_sampling as module @@ -187,6 +188,8 @@ def screen(mode, backend, poses, budget): opts["scene_sampling"]["layout"]["policy"]["intra_group"] = { "targets": {"mug": {"yaw_delta_range_deg": [30, 30]}} } + if configure is not None: + configure(opts, objects) validate_sampling(opts) return ( _LayoutPoseSource(env, opts, seed=7, revision="test-source"), @@ -388,6 +391,50 @@ def test_fixed_layout_geometry_cannot_be_moved_by_a_bounded_draw(monkeypatch): assert source._events[0]["reason"] == "layout_moves_fixed_entity" +def test_external_follower_uses_final_layout_parent_pose(monkeypatch): + def configure(opts, objects): + opts["scene"]["reference"] = {"follow": "tray"} + objects["reference"] = SimpleNamespace( + cfg=SimpleNamespace(to_dict=lambda: {"uid": "reference"}), + get_local_pose=lambda to_matrix=True: torch.eye(4)[None], + ) + opts["scene_sampling"]["layout"]["conditions"]["tray"][0][3] += 0.05 + + source, checked, _ = setup_source(monkeypatch, configure=configure) + candidate = source.candidate(0) + assert source.validate(0).accepted + poses = { + c.entity_uid: np.asarray(c.arena_pose) for c in candidate.scene.pose_changes + } + np.testing.assert_allclose(poses["reference"][:3, 3], [0.05, 0, 0], atol=1e-9) + np.testing.assert_allclose(checked[0]["reference"], poses["reference"]) + + +@pytest.mark.parametrize("authored", [False, True]) +def test_outside_layout_bounds_rejects_sample_but_invalidates_authored_anchor( + monkeypatch, authored +): + def configure(opts, objects): + opts["scene"]["tray"]["translation_m"][0] = [0.075, 0.075] + layout = opts["scene_sampling"]["layout"] + layout["policy"]["group_xy_offset_bounds_m"]["tray"][1][0] = 0.06 + if authored: + layout["conditions"]["tray"][0][3] += 0.075 + else: + layout["conditions"] = {} + + if authored: + with pytest.raises(ValueError, match="outside declared root XY bounds"): + setup_source(monkeypatch, strategy="bounded_pose", configure=configure) + else: + source, checked, _ = setup_source( + monkeypatch, strategy="bounded_pose", configure=configure + ) + source.candidate(0) + assert not source.validate(0).accepted and not checked + assert source._events[0]["reason"] == "layout_outside_sampled_domain" + + def test_member_layout_yaw_cannot_expand_the_task_domain(monkeypatch): source, checked, _ = setup_source(monkeypatch, member_yaw=True) source.candidate(0) From f38aacba9d82e956ea277a01dad9faffe3c08715 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 18 Nov 2026 06:31:09 +0000 Subject: [PATCH 44/45] fix(expansion): preserve admission deadlines for active rollouts --- .../topics/expansion-engine/workloads.md | 4 +++ embodichain/lab/expansion/_pose_sampling.py | 26 ++++++++++++++++ embodichain/lab/expansion/gym_workloads.py | 6 +++- .../lab/expansion/test_task_pose_coverage.py | 31 +++++++++++++++++++ 4 files changed, 66 insertions(+), 1 deletion(-) diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index 1ef5208038..3121786f51 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -195,6 +195,10 @@ candidate streams and proposal streams still use the expansion seed/ordinal; the reference manifest records both seeds. This prevents a sampler comparison from silently changing the nominal scene's reset randomization and moving its declared local domain. Rolling resets use the same explicit nominal reset seed. +The raw pose budget enforces the original admission deadline. An admitted +trajectory's planning view retains shared IK/planning call limits and explicit +cancellation without treating that deadline as a new mid-trajectory hard stop. +The original max_steps, physical checks and safe-stop handshake still apply. `_pose_sampling.py` owns bounded candidate snapshots, source feedback and budgets; `task_pose_coverage.TaskPoseCoverage` implements the session coverage protocol. Measured rigid initial/terminal poses contribute only after native sink receipts. diff --git a/embodichain/lab/expansion/_pose_sampling.py b/embodichain/lab/expansion/_pose_sampling.py index f4c0090353..63694b1263 100644 --- a/embodichain/lab/expansion/_pose_sampling.py +++ b/embodichain/lab/expansion/_pose_sampling.py @@ -109,6 +109,9 @@ class _PoseBudget(FeasibilityBudget): def charge(self, kind: str, amount: int = 1) -> None: self.check() + self._reserve(kind, amount) + + def _reserve(self, kind: str, amount: int = 1) -> None: if type(amount) is not int or amount < 1: raise ValueError("Budget charges must be positive integers") limits = { @@ -123,6 +126,28 @@ def charge(self, kind: str, amount: int = 1) -> None: self.counts[kind] += amount +class _AdmittedPoseBudget: + """Retain shared call limits/cancellation after the admission deadline. + + Already admitted trajectories still have the host's original max_steps and + bridge safe-stop rules. The source's raw budget alone admits new work. + """ + + def __init__(self, budget: _PoseBudget) -> None: + self.budget = budget + + def check(self) -> None: + budget = self.budget + if budget._blocked is not None and budget._blocked[0] == budget.validation: + raise FeasibilityBudgetExhausted(budget._blocked[1]) + if budget.cancelled is not None and budget.cancelled.is_set(): + raise FeasibilityBudgetExhausted("cancelled") + + def charge(self, kind: str, amount: int = 1) -> None: + self.check() + self.budget._reserve(kind, amount) + + class _AdaptivePoseSource(_BoundedPoseSource): """One bounded proposal stream; candidates never change after allocation.""" @@ -183,6 +208,7 @@ def __init__(self, env, options, *, seed, revision): self.budget.validation = ( True # Use the full host allocation, not search/held-out splitting. ) + self.planning_budget = _AdmittedPoseBudget(self.budget) self._candidates = {} self._checks = {} self._predicted = {} diff --git a/embodichain/lab/expansion/gym_workloads.py b/embodichain/lab/expansion/gym_workloads.py index 20a58221d2..8327074e25 100644 --- a/embodichain/lab/expansion/gym_workloads.py +++ b/embodichain/lab/expansion/gym_workloads.py @@ -127,7 +127,11 @@ def __init__( def planning_budget(self, assignment): feedback = self.groups[assignment.workload_id]["host"]._context.feedback - return None if feedback is None else feedback.budget + return ( + None + if feedback is None + else getattr(feedback, "planning_budget", feedback.budget) + ) def can_admit(self, name): feedback = self.groups[name]["host"]._context.feedback diff --git a/tests/lab/expansion/test_task_pose_coverage.py b/tests/lab/expansion/test_task_pose_coverage.py index 9c008eb475..23a8adf3ae 100644 --- a/tests/lab/expansion/test_task_pose_coverage.py +++ b/tests/lab/expansion/test_task_pose_coverage.py @@ -34,6 +34,37 @@ from embodichain.lab.sim.scene.feasibility import FeasibilityBudgetExhausted +def test_admitted_planning_retains_count_and_cancel_limits_after_admission_time(): + from threading import Event + import time + from embodichain.lab.expansion._pose_sampling import ( + _PoseBudget, + _AdmittedPoseBudget, + ) + from embodichain.lab.sim.scene.feasibility import SceneRobotFeasibilityCfg + + cancellation = Event() + budget = _PoseBudget( + SceneRobotFeasibilityCfg(max_wall_time_s=0.01, max_ik_calls=2), + cancelled=cancellation, + ) + budget.validation = True + admitted = _AdmittedPoseBudget(budget) + time.sleep(0.03) + with pytest.raises(FeasibilityBudgetExhausted, match="wall_time"): + budget.charge("candidates") + admitted.check() + admitted.charge("ik") + assert budget.counts["ik"] == 1 and budget.counts["candidates"] == 0 + cancellation.set() + with pytest.raises(FeasibilityBudgetExhausted, match="cancelled"): + admitted.check() + cancellation.clear() + with pytest.raises(FeasibilityBudgetExhausted, match="ik_budget"): + admitted.charge("ik", 2) + assert budget.counts["ik"] == 1 + + def grids(): return { phase: { From c4d9caebaa1cb21230e581f85a7dfede28b28266 Mon Sep 17 00:00:00 2001 From: han-xudong <42986190+han-xudong@users.noreply.github.com> Date: Thu, 19 Nov 2026 10:34:36 +0800 Subject: [PATCH 45/45] feat(task-program): compile portable fitted Pickup deployments --- .../atomic-actions/trajectory-fitting.md | 14 +- docs/source/api_reference/skill_fitting.rst | 12 + embodichain/lab/expansion/gym_workloads.py | 64 +- embodichain/lab/expansion/host_extensions.py | 19 +- .../lab/expansion/task_program_jobs.py | 96 ++- .../lab/sim/atomic_skills/demo_skill.py | 37 +- embodichain/lab/sim/atomic_skills/fitting.py | 45 +- .../primitives/move_held_object.py | 27 +- .../sim/atomic_skills/primitives/pick_up.py | 29 + .../integrations/_configured_composition.py | 30 +- .../integrations/_native_plan_expansion.py | 2 + .../task_program/integrations/configured.py | 46 +- .../task_program/integrations/demo_skill.py | 31 +- .../task_program/integrations/environment.py | 3 + .../task_program/integrations/expansion.py | 1 + .../lab/task_program/integrations/nominal.py | 13 +- .../lab/task_program/runtime/executor.py | 10 + embodichain/toolkits/skill_fitting/_bundle.py | 13 +- .../toolkits/skill_fitting/_pipeline.py | 6 + embodichain/toolkits/skill_fitting/_robot.py | 24 + embodichain/toolkits/skill_fitting/_scene.py | 78 ++- embodichain/toolkits/skill_fitting/cli.py | 41 +- .../toolkits/skill_fitting/task_program.py | 614 ++++++++++++++++++ tests/data_pipeline/test_demo_skill.py | 56 +- tests/toolkits/test_scene_skill_fitting.py | 66 +- 25 files changed, 1301 insertions(+), 76 deletions(-) create mode 100644 embodichain/toolkits/skill_fitting/task_program.py diff --git a/agent_context/topics/atomic-actions/trajectory-fitting.md b/agent_context/topics/atomic-actions/trajectory-fitting.md index bcaebf9ed3..07f6c358d3 100644 --- a/agent_context/topics/atomic-actions/trajectory-fitting.md +++ b/agent_context/topics/atomic-actions/trajectory-fitting.md @@ -56,6 +56,17 @@ Optional `frame_source` adds a content-bound local video for same inventory and persists mutable `annotations.edited.json` separately from the sealed annotation seed. `--package-video` creates video-enabled resources; the web fit and exported native runtime need no original recording directory. +A v2 bundle additionally pins a host-owned execution context and declares the +qualified design reference as its fitting trajectory. CLI fitting accepts saved +annotation edits for the same frame source via `--annotations`. +`export_task_program` uses that context to bind a scene-bound Pickup to one +named invocation. It separates the fitted core from the surrounding annotation +samples. Each retained interval needs an explicit named DemoSkill and measured +boundary contract. A retained slice on the same post-step clock can be built +from the pinned source asset with its original commands, shifted phases, events +and scoped evidence. The output links recorded event ticks and asset hashes to +compiled calls. The Expansion Engine owns physical qualification and writes a +new receipt before dataset conversion. `_collision.py` prepares content-addressed scene convex parts; web import and packaging supply precomputed files for decomposition-free runtime startup. Scene-owned Z compensation precedes fitting; the existing pickup interpolation @@ -88,7 +99,8 @@ exports the native invocation deployment described above. selects a unique closing hand; ambiguous evidence requires an explicit choice. Names do not establish capabilities. A profile must supply explicit runtime TCP calibration and actual finger collision assets; automatic scene geometry supports -opposing prismatic fingers. Without a scene, the effect-free world-TCP motion +opposing prismatic fingers, including two URDF followers of one aperture driver. +Without a scene, the effect-free world-TCP motion fitter remains available for PickUp, Slide, Twist and explicitly bound HandOver. Scene HandOver needs time-resolved object/attachment bindings and is not inferred from an initial object pose. Both paths retain explicit source identity, inclusive diff --git a/docs/source/api_reference/skill_fitting.rst b/docs/source/api_reference/skill_fitting.rst index eafb0aabe5..38424c41e0 100644 --- a/docs/source/api_reference/skill_fitting.rst +++ b/docs/source/api_reference/skill_fitting.rst @@ -34,6 +34,18 @@ Native skill fitting .. automodule:: embodichain.lab.sim.atomic_skills.fitting :members: +Task Program execution bindings +------------------------------- + +Use ``export_task_program`` to bind an accepted Pickup fit to a supplied +execution context and its explicitly assigned demonstration spans. The export +preserves the execution cadence and records how timed source events map to +compiled invocations. Use ``load_invocation_bindings`` to verify the sealed files +and validate the invocation policies against a fresh compile before execution. + +.. automodule:: embodichain.toolkits.skill_fitting.task_program + :members: + Numerical primitives -------------------- diff --git a/embodichain/lab/expansion/gym_workloads.py b/embodichain/lab/expansion/gym_workloads.py index 44d2cd8d77..b5afd08f5b 100644 --- a/embodichain/lab/expansion/gym_workloads.py +++ b/embodichain/lab/expansion/gym_workloads.py @@ -335,26 +335,43 @@ def activate(self, assignment: WorkloadAssignment, observation: object) -> Any: if native is None: ctx.journal.update(request, status="initial_rejected") return None + row_context = ExpansionRowContext( + row, + identity, + assignment.ordinal, + request.attempt_id, + request.directory, + case, + request.recipe, + ) factory = None if ctx.extensions.plan_transform_factory is not None: - factory = ctx.extensions.plan_transform_factory( - self.env, - ExpansionRowContext( - row, - identity, - assignment.ordinal, - request.attempt_id, - request.directory, - case, - request.recipe, - ), - ) + factory = ctx.extensions.plan_transform_factory(self.env, row_context) if factory is None: raise ValueError("plan_transform_factory returned no factory") - adapter = group["adapter"].for_env_ids((row,)) - bridge = adapter.create_bridge( - ctx.program, plan_transform_factory=factory, isolate_policy_sessions=True + if group["adapter"].configured_nominal_sources and not getattr( + factory, "reject_nominal_source_overlap", False + ): + raise ValueError( + "A workload transform must declare call-local nominal-source exclusion." + ) + nominal = ( + None + if ctx.extensions.nominal_source_factory is None + else ctx.extensions.nominal_source_factory(self.env, row_context) ) + adapter = group["adapter"].for_env_ids((row,)) + bridge_options = { + "plan_transform_factory": factory, + "isolate_policy_sessions": True, + } + if nominal is not None: + bridge_options["nominal_source_factory"] = nominal + if ctx.extensions.invocation_policy_bindings: + bridge_options["invocation_policy_bindings"] = ( + ctx.extensions.invocation_policy_bindings + ) + bridge = adapter.create_bridge(ctx.program, **bridge_options) # The frame owner is shared and observes all active rows once per step. self.owner._context.initial, self.owner._context.initial_obs = ( initial, @@ -629,6 +646,8 @@ def run_gym_expansion_workloads( or common.feedback_factory or common.recipe_transform or common.plan_transform_factory + or common.nominal_source_factory + or common.invocation_policy_bindings ): raise ValueError( "common extensions may only select rendering and measured validation" @@ -895,17 +914,18 @@ def physical_key(env_id: str, cfg: Any) -> str: ) adapters[id(factory)] = adapter ctx.program = adapter.compile(environment_cfg.task_program) + if workload.extensions.invocation_policy_bindings: + from embodichain.lab.task_program.integrations.invocation_policy import ( + BoundInvocationPolicies, + ) + + BoundInvocationPolicies( + ctx.program, workload.extensions.invocation_policy_bindings + ) if ctx.feedback is not None: bind = getattr(ctx.feedback, "bind_program", None) if bind is not None: bind(adapter, ctx.program) - if ( - workload.extensions.plan_transform_factory is not None - and adapter.configured_nominal_sources - ): - raise ValueError( - "a workload transform cannot replace configured nominal sources" - ) if ( groups and ctx.program.program_id diff --git a/embodichain/lab/expansion/host_extensions.py b/embodichain/lab/expansion/host_extensions.py index 911e1e67b4..b326efe219 100644 --- a/embodichain/lab/expansion/host_extensions.py +++ b/embodichain/lab/expansion/host_extensions.py @@ -21,10 +21,15 @@ from dataclasses import dataclass, field from collections.abc import Callable, Mapping from pathlib import Path -from typing import Any, Protocol +from typing import Any, Protocol, TYPE_CHECKING from .contracts import CandidateIdentity, SceneCase, ValidationResult from .recipes import ExpansionRecipe +if TYPE_CHECKING: + from embodichain.lab.task_program.integrations.invocation_policy import ( + InvocationPolicyBinding, + ) + __all__ = ["ExpansionValidator", "ExpansionRowContext", "GymExpansionHostExtensions"] @@ -83,6 +88,9 @@ class GymExpansionHostExtensions: plan_transform_factory: Receives environment and immutable row identity; returns an ordinary Task Program transform factory. Its record metadata and hashed plan attachments are archived by the native episode owner. + nominal_source_factory: Receives environment and immutable row identity; + returns an attempt-local Task Program nominal source factory. + invocation_policy_bindings: Presets pinned to exact compiled invocations. feedback_factory: Optional rolling-collection feedback owner receiving env, scene source and artifact directory; exposes coverage, budget, validate, finish and checkpoint without advancing physics or writing episodes. @@ -94,12 +102,20 @@ class GymExpansionHostExtensions: validator_factory: Callable[[Any, Path], ExpansionValidator] | None = None recipe_transform: Callable[[ExpansionRecipe], ExpansionRecipe] | None = None plan_transform_factory: Callable[[Any, ExpansionRowContext], Any] | None = None + nominal_source_factory: Callable[[Any, ExpansionRowContext], Any] | None = field( + default=None, kw_only=True + ) + invocation_policy_bindings: tuple[InvocationPolicyBinding, ...] = field( + default=(), kw_only=True + ) feedback_factory: Callable[[Any, Any, Path], Any] | None = field( default=None, kw_only=True ) require_plan_transform: bool = False def __post_init__(self) -> None: + if type(self.invocation_policy_bindings) is not tuple: + raise TypeError("invocation_policy_bindings must be a tuple") if self.collection_renderer is not None and ( type(self.collection_renderer) is not str or not self.collection_renderer.strip() @@ -111,6 +127,7 @@ def __post_init__(self) -> None: "validator_factory", "recipe_transform", "plan_transform_factory", + "nominal_source_factory", ): value = getattr(self, name) if value is not None and not callable(value): diff --git a/embodichain/lab/expansion/task_program_jobs.py b/embodichain/lab/expansion/task_program_jobs.py index d62af0d1c1..64b9be2fcb 100644 --- a/embodichain/lab/expansion/task_program_jobs.py +++ b/embodichain/lab/expansion/task_program_jobs.py @@ -17,7 +17,7 @@ """Declarative scene/motion jobs composed through the native workload engine.""" from __future__ import annotations -from collections.abc import Mapping, Sequence +from collections.abc import Callable, Mapping, Sequence from copy import deepcopy from dataclasses import replace from pathlib import Path @@ -34,6 +34,13 @@ if TYPE_CHECKING: from embodichain.lab.sim.scene.feasibility import JointSceneCandidate + from embodichain.lab.task_program.runtime.expansion import ( + TaskProgramNominalSourceFactory, + ) + from embodichain.lab.task_program.integrations.invocation_policy import ( + InvocationPolicyBinding, + ) + from .host_extensions import ExpansionRowContext, ExpansionValidator __all__ = [ "load_task_program_expansion", @@ -58,7 +65,15 @@ def _load_deployment(path: Path) -> None: from ._pose_sampling import _BoundedPoseSource, _AdaptivePoseSource -def _task_expansion_extensions(*, options, operator, seed, report): +def _task_expansion_extensions( + *, + options, + operator, + seed, + report, + nominal_source_factory=None, + invocation_policy_bindings=(), +): """Declare task proposals; upstream owns collection and persistence.""" from .host_extensions import GymExpansionHostExtensions @@ -121,7 +136,9 @@ def plans(env, row): scene_source_factory=source, feedback_factory=feedback if "scene_sampling" in options else None, recipe_transform=recipe_transform, - plan_transform_factory=plans, + plan_transform_factory=plans if operator != "object_pose" else None, + nominal_source_factory=nominal_source_factory, + invocation_policy_bindings=invocation_policy_bindings, require_plan_transform=operator != "object_pose", ) @@ -256,7 +273,18 @@ def derive_grasp_expansion_deployment( def _run_operators( - deployment, output, options, selected, *, seed, sources, viewer, runtime_versions + deployment, + output, + options, + selected, + *, + seed, + sources, + viewer, + runtime_versions, + validator_factory=None, + nominal_source_factory=None, + invocation_policy_bindings=(), ): """Call the public upstream collector without operator processes.""" from embodichain.lab.expansion import ( @@ -333,7 +361,12 @@ def _run_operators( report, artifacts, _task_expansion_extensions( - options=options, operator=name, seed=seed, report=report + options=options, + operator=name, + seed=seed, + report=report, + nominal_source_factory=nominal_source_factory, + invocation_policy_bindings=invocation_policy_bindings, ), ) ) @@ -342,16 +375,19 @@ def _run_operators( renderer = viewer["renderer"] if settings["capture_media"] else None common = GymExpansionHostExtensions( collection_renderer=renderer, - validator_factory=lambda env, path: MeasuredExpansionValidator( - env, - options["validation"], - path, - backend=( - "native_contacts" - if settings["collision_backend"] == "native_contacts" - else "geometry" - ), - device=settings["collision_device"], + validator_factory=validator_factory + or ( + lambda env, path: MeasuredExpansionValidator( + env, + options["validation"], + path, + backend=( + "native_contacts" + if settings["collision_backend"] == "native_contacts" + else "geometry" + ), + device=settings["collision_device"], + ) ), ) execution = WorkloadExecutionCfg( @@ -383,6 +419,11 @@ def run_task_program_expansion( runtime_versions: Mapping[str, object] | None = None, viewer: Mapping[str, object] | None = None, preview_output: str | Path | None = None, + validator_factory: Callable[[Any, Path], ExpansionValidator] | None = None, + nominal_source_factory: ( + Callable[[Any, ExpansionRowContext], TaskProgramNominalSourceFactory] | None + ) = None, + invocation_policy_bindings: tuple[InvocationPolicyBinding, ...] = (), ) -> Mapping[str, object]: """Collect declared scene/motion operators through the single-World engine. @@ -397,6 +438,9 @@ def run_task_program_expansion( viewer: Headless/renderer declarations. A non-headless job opens the native read-only preview after collection, qualification and export. preview_output: Optional service status/log directory outside saved data. + validator_factory: Optional task-owned measured validation port. + nominal_source_factory: Optional independent factory for each attempted row. + invocation_policy_bindings: Named presets bound to the exact program calls. Returns: Owned operator quotas, confirmed readback, native dataset paths and preview @@ -445,6 +489,25 @@ def run_task_program_expansion( inputs = output / "inputs" inputs.mkdir(exist_ok=True) sources = dict(source_artifacts or {}) + if (deployment.parent / "invocation_bindings.json").is_file(): + from embodichain.toolkits.skill_fitting.task_program import ( + load_invocation_bindings, + ) + + sealed = load_invocation_bindings(deployment) + if invocation_policy_bindings and invocation_policy_bindings != sealed: + raise ValueError( + "Explicit invocation policies differ from the sealed deployment." + ) + invocation_policy_bindings = sealed + for name in ( + "manifest.json", + "compile_report.json", + "source_map.json", + "invocation_bindings.json", + ): + path = deployment.parent / name + sources[f"fitted_{name}"] = {"path": str(path), "sha256": file_digest(path)} derived = _prepare_deployment(deployment, inputs) atomic_json(inputs / "expansion.json", options) for path in (derived, inputs / "environment.yaml", inputs / "expansion.json"): @@ -461,6 +524,9 @@ def run_task_program_expansion( sources=sources, viewer=view, runtime_versions=runtime_versions, + validator_factory=validator_factory, + nominal_source_factory=nominal_source_factory, + invocation_policy_bindings=invocation_policy_bindings, ) result = { "complete": all(value.get("complete", False) for value in groups.values()), diff --git a/embodichain/lab/sim/atomic_skills/demo_skill.py b/embodichain/lab/sim/atomic_skills/demo_skill.py index fcca25fd87..c66ac54029 100644 --- a/embodichain/lab/sim/atomic_skills/demo_skill.py +++ b/embodichain/lab/sim/atomic_skills/demo_skill.py @@ -322,8 +322,19 @@ def instantiate( device=context.robot.qpos.device, dtype=context.robot.qpos.dtype, ).unsqueeze(0) + entry_observed = context.robot.qpos + if spec.initial_held_role is not None: + # A verified squeeze can deflect passive follower joints in contact. + # Those joints obey mimic relationships on commanded trajectory samples. + robot = self.motion_generator.robot + passive = set(getattr(robot, "mimic_ids", ())) + independent = [ + index for index in range(initial.shape[-1]) if index not in passive + ] + initial = initial[:, independent] + entry_observed = entry_observed[:, independent] entry_matches = torch.allclose( - initial, context.robot.qpos, atol=spec.entry_tolerance, rtol=0 + initial, entry_observed, atol=spec.entry_tolerance, rtol=0 ) if not entry_matches and not spec.allow_entry_connection: raise ValueError("DemoSkill entry posture differs from the recorded state") @@ -466,6 +477,30 @@ def instantiate( ) self.monitor.spec = spec self.monitor.sample_count = path.shape[1] + if entry_matches and spec.initial_held_role is not None: + hand = request.binding.endpoint("primary", "grasp").require_target( + JointPositionTarget + ) + controlled = set(arm.joint_ids) | set(hand.joint_ids) + for child, parent in zip( + getattr(generator.robot, "mimic_ids", ()), + getattr(generator.robot, "mimic_parents", ()), + strict=True, + ): + if parent in controlled: + controlled.add(child) + unowned = sorted(set(range(path.shape[-1])) - controlled) + if unowned and torch.allclose( + path[:, :, unowned], + path[:, :1, unowned].expand(-1, path.shape[1], -1), + atol=1e-6, + rtol=0, + ): + path[:, :, unowned] = context.robot.qpos[:, None, unowned] + # The held-entry guard first qualified the measured posture. Use that + # exact posture for the zero-time arm sample while keeping gripper load. + ids = list(arm.joint_ids) + path[:, 0, ids] = context.robot.qpos[:, ids] self._nominal_spec = spec timed = TimedTrajectory.from_uniform_step( path, env_ids=context.env_ids, step_dt=context.require_control_dt() diff --git a/embodichain/lab/sim/atomic_skills/fitting.py b/embodichain/lab/sim/atomic_skills/fitting.py index 281fac14ca..2a2f6016e6 100644 --- a/embodichain/lab/sim/atomic_skills/fitting.py +++ b/embodichain/lab/sim/atomic_skills/fitting.py @@ -715,19 +715,42 @@ def partition(boundaries: list[tuple[str, int]]) -> list[dict[str, Any]]: if scene_grasps is not None and skill_id == "pick_up" and phase_hints is None: # Only the outer clock/boundaries are fixed for scene reconstruction. # A changed grasp may need more time to connect to the recorded end. - # Search a bounded set of free-phase allocations while preserving the - # observed closing duration; explicit phase hints remain authoritative. + # The fitted command may open wider than the observed grasp. Allocate + # enough ticks to close at the same velocity and acceleration limits. + # Explicit phase hints remain authoritative. closing = lift_start - close_start + hand = endpoints["primary"]["grasp"] + hand_ids = participant_data["primary"][4] + hand_profile = engine.control_profiles[hand] + travel = ( + torch.as_tensor(hand_profile.commands[OPEN_COMMAND].positions).cpu().numpy() + - torch.as_tensor(hand_profile.commands[GRASP_COMMAND].positions) + .cpu() + .numpy() + ) + source_dt = float(np.min(np.diff(times))) + max_step = np.minimum( + velocity[hand_ids] * source_dt, + acceleration[hand_ids] * source_dt**2, + ) + closing = max(closing, int(np.ceil(np.max(np.abs(travel) / max_step))) + 1) free = len(q) - closing - for fraction in (0.5, 0.45, 0.4, 0.55, 0.6): - before = int(round(free * fraction)) - alternative = [ - dict(name="approach", start=0, stop=before), - dict(name="close", start=before, stop=before + closing), - dict(name="lift", start=before + closing, stop=len(q)), - ] - if alternative not in phase_options: - phase_options.append(alternative) + if free >= 4: + native_max_step = np.minimum( + velocity[hand_ids] * cfg.control_dt, + acceleration[hand_ids] * cfg.control_dt**2, + ) + native_closing = int(np.ceil(np.max(np.abs(travel) / native_max_step))) + 1 + options = replace(options, hand_interp_steps=max(closing, native_closing)) + for fraction in (0.5, 0.45, 0.4, 0.55, 0.6): + before = int(round(free * fraction)) + alternative = [ + dict(name="approach", start=0, stop=before), + dict(name="close", start=before, stop=before + closing), + dict(name="lift", start=before + closing, stop=len(q)), + ] + if alternative not in phase_options: + phase_options.append(alternative) def connect_scene( values: np.ndarray, fit_phases: list[dict[str, Any]] diff --git a/embodichain/lab/sim/atomic_skills/primitives/move_held_object.py b/embodichain/lab/sim/atomic_skills/primitives/move_held_object.py index c567926f82..08772c5086 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_skills/primitives/move_held_object.py @@ -18,7 +18,7 @@ from __future__ import annotations -from dataclasses import dataclass, field +from dataclasses import dataclass, field, replace import math from typing import ClassVar @@ -110,6 +110,19 @@ def __post_init__(self) -> None: class MoveHeldObjectOptions(SkillOptions): """Per-invocation held-object transport behavior.""" + verified_terminal_object_to_eef: torch.Tensor | None = None + """Optional measured terminal relation for a demonstrated in-hand pivot.""" + + def __post_init__(self) -> None: + pose = self.verified_terminal_object_to_eef + if pose is not None: + validate_pose_goal( + pose, "verified_terminal_object_to_eef", allow_waypoints=False + ) + if not isinstance(pose, torch.Tensor) or pose.shape != (4, 4): + raise ValueError("verified terminal relation must be one SE(3) tensor") + object.__setattr__(self, "verified_terminal_object_to_eef", pose.clone()) + class MoveHeldObject(AtomicSkill[HeldObjectPoseGoal, MoveHeldObjectOptions]): """Move the held object through exact target poses with a closed hand. @@ -335,13 +348,23 @@ def plan_target(poses: torch.Tensor) -> PlanResult: control_dt=context.require_control_dt(), ) + terminal_held = held_object + terminal_relation = options.verified_terminal_object_to_eef + if terminal_relation is not None: + terminal_held = replace( + held_object, + object_to_eef=terminal_relation.to(start_arm_qpos) + .unsqueeze(0) + .expand(context.batch_size, -1, -1) + .clone(), + ) return self.build_plan( request, context, success=success, trajectory=timed, expected_effects=( - StateDelta(held_object_updates={task_state_key: held_object}) + StateDelta(held_object_updates={task_state_key: terminal_held}) if target.object_id is not None else None ), diff --git a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py index 16ea210a81..aa4692b551 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py @@ -240,6 +240,12 @@ class PickUpOptions(SkillOptions): observed rotation instead of a shared world vector. """ + verified_object_to_eef: torch.Tensor | None = None + """Expected measured attachment relation when the command is servo-compensated.""" + + grasp_relative_approach: tuple[torch.Tensor, ...] = () + """Optional TCP waypoints relative to the resolved grasp, ordered before close.""" + cartesian_lift: bool = False """Preserve a straight world-Z lift with IK interpolation and native limits. @@ -303,6 +309,20 @@ def __post_init__(self) -> None: or not torch.isfinite(self.obj_upright_direction).all() ): raise ValueError("obj_upright_direction must be a finite (3,) tensor.") + for index, pose in enumerate(self.grasp_relative_approach): + _validate_single_se3(pose, f"grasp_relative_approach[{index}]") + object.__setattr__( + self, + "grasp_relative_approach", + tuple(pose.clone() for pose in self.grasp_relative_approach), + ) + if self.verified_object_to_eef is not None: + _validate_single_se3(self.verified_object_to_eef, "verified_object_to_eef") + object.__setattr__( + self, + "verified_object_to_eef", + self.verified_object_to_eef.clone(), + ) _validate_single_se3(self.grasp_frame_to_eef, "grasp_frame_to_eef") if type(self.cartesian_lift) is not bool: raise TypeError("cartesian_lift must be boolean.") @@ -437,6 +457,12 @@ def _get_full_pickup_trajectory( motion_options.sample_count = None waypoints = [pre_grasp_xpos, grasp_xpos, lift_xpos] phase_names = ["pre_grasp", "grasp", "lift"] + if options.grasp_relative_approach: + waypoints[:1] = [ + grasp_xpos @ waypoint.to(grasp_xpos) + for waypoint in options.grasp_relative_approach + ] + phase_names[:1] = ["pre_grasp"] * len(options.grasp_relative_approach) if options.initial_lift_height > 0.0: start_xpos = self.robot.compute_fk( qpos=start_arm_qpos, @@ -951,6 +977,9 @@ def _assemble_pick_plan( sem = target.semantics state = context object_to_eef = torch.bmm(pose_inv(object_pose), grasp_xpos) + if options.verified_object_to_eef is not None: + object_to_eef = options.verified_object_to_eef.to(object_pose) + object_to_eef = object_to_eef.unsqueeze(0).expand_as(object_pose).clone() held = HeldObjectState( semantics=sem, object_to_eef=object_to_eef, grasp_xpos=grasp_xpos ) diff --git a/embodichain/lab/task_program/integrations/_configured_composition.py b/embodichain/lab/task_program/integrations/_configured_composition.py index 55647ff708..df96780357 100644 --- a/embodichain/lab/task_program/integrations/_configured_composition.py +++ b/embodichain/lab/task_program/integrations/_configured_composition.py @@ -713,7 +713,9 @@ def _compose_integration_payload( _normalize_skill_options_key(task["profile"], path="task integration.profile"), path="task integration.profile", required=frozenset({"defaults", "skill_options", "effect_monitors"}), - optional=frozenset({"skill_presets", "grounding_providers"}), + optional=frozenset( + {"skill_presets", "grounding_providers", "invocation_presets"} + ), ) preset = { "preset_id": deepcopy(policy["preset_id"]), @@ -730,6 +732,30 @@ def _compose_integration_payload( preset["required_planner"] = deepcopy(policy["planner"]["type"]) elif "required_planner" in policy: preset["required_planner"] = deepcopy(policy["required_planner"]) + named = _owned_mapping( + profile.get("invocation_presets", {}), path="profile.invocation_presets" + ) + extra_presets = [] + for key, item in named.items(): + path = f"profile.invocation_presets.{key}" + name = _identifier(key, path=path) + if name == preset["preset_id"]: + raise ValueError("An invocation preset cannot replace the default preset.") + override = _mapping( + item, path=path, required=frozenset({"skill_options", "motion"}) + ) + options = _owned_mapping( + override["skill_options"], path=f"{path}.skill_options" + ) + motion = _owned_mapping(override["motion"], path=f"{path}.motion") + extra_presets.append( + { + **deepcopy(preset), + "preset_id": name, + "skill_options": {**preset["skill_options"], **options}, + "motion": {**preset["motion"], **motion}, + } + ) scene_payload = { key: deepcopy(value) for key, value in scene.items() if key != "contract_id" @@ -739,7 +765,7 @@ def _compose_integration_payload( "resources": deepcopy(skill_profile["resources"]), "command_presets": deepcopy(skill_profile["command_presets"]), "defaults": deepcopy(profile["defaults"]), - "presets": [preset], + "presets": [preset, *extra_presets], "default_preset": None, "skill_presets": deepcopy(profile.get("skill_presets", {})), "grounding_providers": deepcopy(profile.get("grounding_providers", {})), diff --git a/embodichain/lab/task_program/integrations/_native_plan_expansion.py b/embodichain/lab/task_program/integrations/_native_plan_expansion.py index 66e3a72e27..ccf6074412 100644 --- a/embodichain/lab/task_program/integrations/_native_plan_expansion.py +++ b/embodichain/lab/task_program/integrations/_native_plan_expansion.py @@ -101,6 +101,8 @@ def record(resolved, context, plan): calls = self.options.get("call_indices") if calls is not None and request.analysis_call_index not in calls: return None + if not self.options["free_phases"].get(request.invocation.skill_id): + return None def transform(resolved, context, plan): return self._transform(request, engine, resolved, context, plan) diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index c5afef5008..8eef2dec5e 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -1115,6 +1115,9 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: "translation_distance", "preshape_fraction", "approach_along_grasp_axis", + "grasp_relative_approach", + "verified_object_to_eef", + "verified_terminal_object_to_eef", "cartesian_lift", "release_retreat_distance", "push_approach_offset", @@ -1161,6 +1164,8 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: *( ( "approach_along_grasp_axis", + "grasp_relative_approach", + "verified_object_to_eef", "initial_lift_height", "cartesian_lift", ) @@ -1172,6 +1177,28 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: ), ) kwargs: dict[str, object] = {} + if "verified_object_to_eef" in config: + import torch + + kwargs["verified_object_to_eef"] = torch.tensor( + _pose_matrix( + config["verified_object_to_eef"], + path=f"{path}.verified_object_to_eef", + ), + dtype=torch.float32, + ).reshape(4, 4) + if "grasp_relative_approach" in config: + import torch + + key = f"{path}.grasp_relative_approach" + kwargs["grasp_relative_approach"] = tuple( + torch.tensor( + _pose_matrix(item, path=f"{key}[{index}]"), dtype=torch.float32 + ).reshape(4, 4) + for index, item in enumerate( + _sequence(config["grasp_relative_approach"], path=key) + ) + ) if "cartesian_lift" in config: kwargs["cartesian_lift"] = _boolean( config["cartesian_lift"], path=f"{path}.cartesian_lift" @@ -1345,8 +1372,23 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: ) return PlaceOptions(**kwargs) if kind == "move_held_object": - _mapping(value, path=path, required=frozenset({"kind"})) - return MoveHeldObjectOptions() + config = _mapping( + value, + path=path, + required=frozenset({"kind"}), + optional=frozenset({"verified_terminal_object_to_eef"}), + ) + key = "verified_terminal_object_to_eef" + if key not in config: + return MoveHeldObjectOptions() + import torch + + return MoveHeldObjectOptions( + verified_terminal_object_to_eef=torch.tensor( + _pose_matrix(config[key], path=f"{path}.{key}"), + dtype=torch.float32, + ).reshape(4, 4) + ) if kind == "move_joints": _mapping( value, diff --git a/embodichain/lab/task_program/integrations/demo_skill.py b/embodichain/lab/task_program/integrations/demo_skill.py index ea3598cbd7..e4111ecb92 100644 --- a/embodichain/lab/task_program/integrations/demo_skill.py +++ b/embodichain/lab/task_program/integrations/demo_skill.py @@ -153,6 +153,31 @@ def lower(self, call, context, *, bound, option_template): "DemoSkill roles must belong to the registered simulation" ) roles[name], uids[name] = ref.entity_id, entity.uid + initial_role = getattr(owner, "initial_held_role", None) + if initial_role is not None: + primary = bound.binding.resources.get("primary") + motion = ( + None if primary is None else primary.endpoints.get("motion") + ) + grasp = None if primary is None else primary.endpoints.get("grasp") + if ( + motion is None + or grasp is None + or motion.task_state_key != grasp.task_state_key + ): + raise ValueError( + "DemoSkill entry requires a shared motion and grasp state key" + ) + held = context.task.get_held_object(motion.task_state_key) + if ( + held is None + or held.semantics.entity_id != roles[initial_role] + or held.env_mask is None + or not bool(held.env_mask.all()) + ): + raise ValueError( + "DemoSkill entry requires verified held-object evidence" + ) semantics = None registered_effect = None if owner.terminal_held_role is not None or owner.release_on_exit: @@ -167,7 +192,7 @@ def lower(self, call, context, *, bound, option_template): role = owner.terminal_held_role if role is None: - role = owner.released_role + role = initial_role or owner.released_role if role is None: raise ValueError( "release_on_exit requires an explicit terminal object role" @@ -203,7 +228,9 @@ def lower(self, call, context, *, bound, option_template): ), object_id=roles[role], slot_id="primary", - allow_missing_detached_baseline=owner.release_on_exit, + allow_missing_detached_baseline=( + owner.release_on_exit and initial_role is None + ), ), ), ) diff --git a/embodichain/lab/task_program/integrations/environment.py b/embodichain/lab/task_program/integrations/environment.py index 5e592f3744..33d6ee1bd8 100644 --- a/embodichain/lab/task_program/integrations/environment.py +++ b/embodichain/lab/task_program/integrations/environment.py @@ -900,6 +900,8 @@ def create_traced_configured_sources( self, program: CompiledTaskProgram, collision_guard: TabletopCollisionGuardFactory | None = None, + *, + decorate_demo_source: Callable | None = None, ) -> TracingTaskProgramNominalSources: """Bind configured prototypes to a paired host collision guard. @@ -924,6 +926,7 @@ def create_traced_configured_sources( program, self._configured_nominal_sources, host_collision_guard=collision_guard, + decorate_demo_source=decorate_demo_source, ) return TracingTaskProgramNominalSources(program, delegate) diff --git a/embodichain/lab/task_program/integrations/expansion.py b/embodichain/lab/task_program/integrations/expansion.py index 7c62629306..e0b4100f95 100644 --- a/embodichain/lab/task_program/integrations/expansion.py +++ b/embodichain/lab/task_program/integrations/expansion.py @@ -227,6 +227,7 @@ def from_native_plan( from ._native_plan_expansion import _NativePlanExpansion instance = object.__new__(cls) + instance.reject_nominal_source_overlap = True instance._native_delegate = _NativePlanExpansion( operator, options, diff --git a/embodichain/lab/task_program/integrations/nominal.py b/embodichain/lab/task_program/integrations/nominal.py index 152726a502..175882b034 100644 --- a/embodichain/lab/task_program/integrations/nominal.py +++ b/embodichain/lab/task_program/integrations/nominal.py @@ -154,6 +154,7 @@ def __init__( ], *, host_collision_guard: object | None = None, + decorate_demo_source=None, ) -> None: if host_collision_guard is not None: from .simulation.real2sim_collision import TabletopCollisionGuardFactory @@ -166,6 +167,9 @@ def __init__( self._fingerprint = _fingerprint(program) self._declarations = declarations self._host_collision_guard = host_collision_guard + if decorate_demo_source is not None and not callable(decorate_demo_source): + raise TypeError("decorate_demo_source must be callable.") + self._decorate_demo_source = decorate_demo_source self._sources = {item.call_index: item for item in declarations} if len(self._sources) != len(declarations): raise ValueError("configured nominal source call indices must be unique") @@ -247,7 +251,14 @@ def create_nominal_source( if isinstance(declaration, _ConfiguredDemoSkillSource): from .simulation.demo_skill import _create_demo_skill_source - return _create_demo_skill_source(declaration, request, engine) + source = _create_demo_skill_source(declaration, request, engine) + if self._decorate_demo_source is not None: + source = self._decorate_demo_source(source) + if not isinstance(source, NominalMotionSource): + raise TypeError( + "A DemoSkill decorator must return a nominal source." + ) + return source prototype = load_prototype(declaration.prototype_manifest) return PrototypeMotionProvider( prototype, diff --git a/embodichain/lab/task_program/runtime/executor.py b/embodichain/lab/task_program/runtime/executor.py index c0e0686cda..c21a7d4d00 100644 --- a/embodichain/lab/task_program/runtime/executor.py +++ b/embodichain/lab/task_program/runtime/executor.py @@ -1203,6 +1203,16 @@ def _prepare_grounded_call( nominal_source = self._nominal_source_factory.create_nominal_source( plan_request, engine=self._engine ) + if ( + nominal_source is not None + and plan_transform is not None + and getattr( + self._plan_transform_factory, "reject_nominal_source_overlap", False + ) + ): + raise ValueError( + "A native expansion and a declared nominal source select the same call." + ) if effect_guards: external_plan_transform = plan_transform diff --git a/embodichain/toolkits/skill_fitting/_bundle.py b/embodichain/toolkits/skill_fitting/_bundle.py index 3f443dc5bb..7f941fb2de 100644 --- a/embodichain/toolkits/skill_fitting/_bundle.py +++ b/embodichain/toolkits/skill_fitting/_bundle.py @@ -239,10 +239,14 @@ def load_inputs( root = directory.resolve(strict=True) manifest = json.loads((root / "fitting.json").read_text()) required = {"schema", "inputs", "annotations", "frame_clock", "files"} + schema = manifest.get("schema") + if schema == "scene-skill-fitting-inputs/v2": + required.add("execution_context") if ( not required <= manifest.keys() or manifest.keys() - (required | {"frame_source"}) - or manifest["schema"] != "scene-skill-fitting-inputs/v1" + or schema + not in {"scene-skill-fitting-inputs/v1", "scene-skill-fitting-inputs/v2"} ): raise ValueError("Unsupported fitting input bundle.") for name, digest in manifest["files"].items(): @@ -255,6 +259,11 @@ def load_inputs( settings[key] = str(_contained(root, settings[key])) inputs = FitInputs(**settings) inputs.check() + if ( + schema == "scene-skill-fitting-inputs/v2" + and inputs.qpos_semantics != "design_reference" + ): + raise ValueError("Version 2 fitting requires an explicit design reference.") # Robot/scene dependencies are also confined to the sealed file inventory. profile = RobotProfile.load(inputs.robot_profile) from ._scene import load_scene @@ -299,6 +308,8 @@ def load_inputs( visual = _video_source(root, manifest) if visual is not None: dependencies.add(str(visual[0])) + if schema == "scene-skill-fitting-inputs/v2": + dependencies.add(str(_contained(root, manifest["execution_context"]))) inventory = {str(_contained(root, p)) for p in manifest["files"]} if dependencies - inventory: raise ValueError("Bundle has unlisted or external robot/scene dependencies.") diff --git a/embodichain/toolkits/skill_fitting/_pipeline.py b/embodichain/toolkits/skill_fitting/_pipeline.py index 2b50af2139..bfa61b427a 100644 --- a/embodichain/toolkits/skill_fitting/_pipeline.py +++ b/embodichain/toolkits/skill_fitting/_pipeline.py @@ -49,6 +49,7 @@ class FitInputs: env_index: int = 0 device: str = "cpu" phase_hints: list[dict[str, Any]] | None = None + observed_object_to_tcp: list[list[float]] | None = None fit_config: dict[str, Any] = {} def check(self) -> None: @@ -141,6 +142,10 @@ def fit_recording( if set(inputs.fit_config) - set(SkillFitCfg.__dataclass_fields__): raise ValueError("Unknown fit configuration fields.") + if inputs.observed_object_to_tcp is not None and ( + not inputs.scene or not inputs.object_id or skill_id != "pick_up" + ): + raise ValueError("A measured grasp requires a scene-bound Pickup.") cfg = SkillFitCfg(**inputs.fit_config) cfg.validate() paths = [ @@ -220,6 +225,7 @@ def fit_recording( recording.qpos[start:stop], cfg, inputs.phase_hints, + observed_object_to_tcp=inputs.observed_object_to_tcp, **options, ) if any(scene_files.get(p) != d for p, d in defaults_files.items()): diff --git a/embodichain/toolkits/skill_fitting/_robot.py b/embodichain/toolkits/skill_fitting/_robot.py index 72e658ea34..d7ab925881 100644 --- a/embodichain/toolkits/skill_fitting/_robot.py +++ b/embodichain/toolkits/skill_fitting/_robot.py @@ -21,6 +21,7 @@ import json from pathlib import Path from typing import Any +import xml.etree.ElementTree as ET import numpy as np import torch @@ -161,6 +162,29 @@ def __init__( self.mimic_ids: list[int] = [] self.mimic_parents: list[int] = [] self.mimic_multipliers: list[float] = [] + self.mimic_offsets: list[float] = [] + mimic_joints: dict[str, tuple[str, float, float]] = {} + for model in {mapping["urdf_path"] for mapping in profile.solvers.values()}: + for joint in ET.parse(model).findall("./joint"): + mimic = joint.find("mimic") + name = joint.get("name") + if mimic is None or name not in self.joint_names: + continue + relation = ( + mimic.attrib["joint"], + float(mimic.get("multiplier", "1")), + float(mimic.get("offset", "0")), + ) + if name in mimic_joints and mimic_joints[name] != relation: + raise ValueError(f"Conflicting mimic declarations for {name}.") + mimic_joints[name] = relation + for name, (driver, multiplier, offset) in mimic_joints.items(): + if driver not in self.joint_names: + raise ValueError(f"Robot profile is missing the driver of {name}.") + self.mimic_ids.append(self.joint_names.index(name)) + self.mimic_parents.append(self.joint_names.index(driver)) + self.mimic_multipliers.append(multiplier) + self.mimic_offsets.append(offset) self.solvers = {} self.bases = {} for part, declaration in profile.solvers.items(): diff --git a/embodichain/toolkits/skill_fitting/_scene.py b/embodichain/toolkits/skill_fitting/_scene.py index 06dbf1daa9..5303bb9aaf 100644 --- a/embodichain/toolkits/skill_fitting/_scene.py +++ b/embodichain/toolkits/skill_fitting/_scene.py @@ -226,7 +226,21 @@ def ancestors(link: str) -> list[str]: result.append(link) return result - names = profile.control_parts[hand] + drivers = profile.control_parts[hand] + names = drivers + followers = {} + if len(drivers) == 1: + # Some parallel grippers expose only an aperture driver to the + # controller. Use its physical URDF followers for geometry. + for name, joint in joints.items(): + mimic = joint.find("mimic") + if mimic is not None and mimic.get("joint") == drivers[0]: + followers[name] = ( + float(mimic.get("multiplier", "1")), + float(mimic.get("offset", "0")), + ) + if followers: + names = list(followers) if any(n not in joints or joints[n].attrib["type"] != "prismatic" for n in names): raise ValueError( "Automatic grasp geometry needs configured parallel prismatic fingers and their collision meshes." @@ -244,8 +258,15 @@ def ancestors(link: str) -> list[str]: def frames(command: str) -> dict[str, np.ndarray]: state = { **values, - **dict(zip(names, profile.hand_commands[hand][command], strict=True)), + **dict(zip(drivers, profile.hand_commands[hand][command], strict=True)), } + if followers: + state.update( + { + name: state[drivers[0]] * gain + offset + for name, (gain, offset) in followers.items() + } + ) fk = chain.forward_kinematics( torch.tensor([[state[n] for n in parameters]], dtype=torch.float64) ) @@ -298,7 +319,7 @@ def frames(command: str) -> dict[str, np.ndarray]: tcp_link=solver["end_link_name"], collision_links=links, joint_positions=dict( - zip(names, profile.hand_commands[hand]["open"], strict=True) + zip(drivers, profile.hand_commands[hand]["open"], strict=True) ), link_from_tcp=tcp_offset, grasp_from_tcp=np.linalg.inv(canonical) @ root_from_tcp, @@ -411,6 +432,7 @@ def prepare_scene_grasps( phase_hints: list[dict[str, Any]] | None, *, grasp_world_z_offset_m: float = 0.0, + observed_object_to_tcp: list[list[float]] | None = None, ) -> tuple[list[GraspGoal], dict[str, Any], dict[str, str]]: """Sample one bounded affordance pool, then rank against the observed grasp.""" from embodichain.lab.sim.atomic_skills import ObjectSemantics @@ -473,10 +495,6 @@ def distance(obj: _Object) -> float: ) target = min(objects, key=distance) - geometry, gripper_inputs = _hand_geometry( - profile, pair, qpos[0], cfg.grasp_contact_inset_m - ) - files.update(geometry.provenance["files_sha256"]) affordance = AntipodalAffordance( mesh_vertices=torch.as_tensor( target.vertices, dtype=torch.float32, device=robot.device @@ -485,6 +503,52 @@ def distance(obj: _Object) -> float: target.triangles, dtype=torch.long, device=robot.device ), ) + if observed_object_to_tcp is not None: + relation = np.asarray(observed_object_to_tcp, dtype=float) + if ( + relation.shape != (4, 4) + or not np.isfinite(relation).all() + or not np.allclose(relation[3], (0, 0, 0, 1), atol=1e-7) + or not np.allclose( + relation[:3, :3].T @ relation[:3, :3], np.eye(3), atol=1e-5 + ) + or not np.isclose(np.linalg.det(relation[:3, :3]), 1.0, atol=1e-5) + or grasp_world_z_offset_m != 0 + ): + raise ValueError( + "The measured grasp must be a rigid object-to-TCP transform." + ) + pose = target.pose @ relation + if np.linalg.norm(pose[:3, 3] - reference[:3, 3]) > cfg.max_grasp_distance_m: + raise ValueError("The measured grasp is outside the fitted encounter.") + object_pose = torch.as_tensor( + target.pose, dtype=torch.float32, device=robot.device + ) + goal = GraspGoal( + semantics=ObjectSemantics( + affordance=affordance, geometry={}, entity_id=target.identifier + ), + grasp_xpos=object_pose.new_tensor(pose), + object_pose=object_pose, + ) + return ( + [goal], + { + "object_id": target.identifier, + "world_from_object": target.pose.tolist(), + "object_to_tcp": relation.tolist(), + "scene_path": str(scene_path.resolve()), + "mesh_path": str(target.mesh_path), + "observed_grasp_xpos": pose.tolist(), + "candidate_count": 1, + "grasp_validity_scope": "measured_relative_grasp_requires_runtime_validation", + }, + files, + ) + geometry, gripper_inputs = _hand_geometry( + profile, pair, qpos[0], cfg.grasp_contact_inset_m + ) + files.update(geometry.provenance["files_sha256"]) rows, costs, roll_variants = [], [], [] for half_turn in (False, True): # Keep the actual TCP position/approach axis fixed, even for a virtual diff --git a/embodichain/toolkits/skill_fitting/cli.py b/embodichain/toolkits/skill_fitting/cli.py index 8e17588c44..226330360f 100644 --- a/embodichain/toolkits/skill_fitting/cli.py +++ b/embodichain/toolkits/skill_fitting/cli.py @@ -38,10 +38,18 @@ def _bundle_main(argv: Sequence[str]) -> None: parser.add_argument("--bundle", type=Path, required=True) parser.add_argument("--output", type=Path) parser.add_argument("--run-task-output", type=Path) + parser.add_argument( + "--annotations", type=Path, help="Saved edits for the bundle's source video." + ) parser.add_argument("--package-output", type=Path) parser.add_argument("--package-video", type=Path) parser.add_argument("--video-stream", type=int) parser.add_argument("--device", default="cpu") + parser.add_argument( + "--observed-grasp", + type=Path, + help="Measured object-to-TCP JSON matrix for a scene-bound Pickup.", + ) args = parser.parse_args(argv) if args.output is None and args.package_output is None: parser.error("Choose --output for fitting or --package-output for repackaging.") @@ -54,10 +62,35 @@ def _bundle_main(argv: Sequence[str]) -> None: from ._bundle import load_inputs, pack_inputs, _video_source from ._episode import fit_episode from ._export import export_run_task - from embodichain.data_pipeline.skill_fitting import save_skill_fit + from embodichain.data_pipeline.skill_fitting import file_digest, save_skill_fit inputs, document, clock, durations = load_inputs(args.bundle) inputs.device = args.device + grasp_digest = None + if args.observed_grasp is not None: + if args.output is None or args.package_output is not None: + parser.error("--observed-grasp requires a fit output.") + if args.output.resolve() == args.observed_grasp.resolve(): + parser.error("The fit output cannot replace its measured grasp input.") + grasp_digest = file_digest(args.observed_grasp) + inputs.observed_object_to_tcp = json.loads(args.observed_grasp.read_text()) + edited_digest = None + if args.annotations is not None: + edited_digest = file_digest(args.annotations) + edited = json.loads(args.annotations.read_text()) + actual, expected = edited.get("source", {}), document["source"] + if ( + edited.get("frame_index_base") != 0 + or edited.get("interval") != "closed" + or any( + actual.get(k) != expected[k] + for k in ("identity", "frame_count", "timing") + ) + ): + raise ValueError( + "Edited annotations do not belong to this bundle's frame source." + ) + document = edited if args.package_output is not None: root = args.bundle.expanduser().resolve() visual = _video_source(root, json.loads((root / "fitting.json").read_text())) @@ -85,6 +118,12 @@ def _bundle_main(argv: Sequence[str]) -> None: arrays, report = fit_episode( inputs, document, lambda a, b: (clock[a : b + 1], durations[a : b + 1]) ) + if args.annotations is not None and file_digest(args.annotations) != edited_digest: + raise ValueError("Annotations changed during fitting.") + if grasp_digest is not None: + if file_digest(args.observed_grasp) != grasp_digest: + raise ValueError("Measured grasp changed during fitting.") + report["observed_grasp_sha256"] = grasp_digest save_skill_fit(args.output, arrays, report) archive = None if args.run_task_output and report["accepted"]: diff --git a/embodichain/toolkits/skill_fitting/task_program.py b/embodichain/toolkits/skill_fitting/task_program.py new file mode 100644 index 0000000000..32addb359d --- /dev/null +++ b/embodichain/toolkits/skill_fitting/task_program.py @@ -0,0 +1,614 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Compile checked scene-bound Pickup fits into a sealed Task Program deployment.""" + +from __future__ import annotations + +from dataclasses import asdict, replace +import json +from pathlib import Path +import shutil +from tempfile import TemporaryDirectory +from typing import TYPE_CHECKING, Any, Mapping + +import numpy as np +import yaml + +from embodichain.data_pipeline.skill_fitting import file_digest + +if TYPE_CHECKING: + from embodichain.data_pipeline.real2sim import DemoSkillArtifact + from embodichain.lab.task_program.integrations.invocation_policy import ( + InvocationPolicyBinding, + ) + +__all__ = ["export_task_program", "load_invocation_bindings"] + +_BINDING_SCHEMA = "fitted-task-program/v1" + + +def _write(path: Path, value: object) -> None: + path.write_text(json.dumps(value, indent=2, allow_nan=False) + "\n") + + +def _read_yaml(path: Path) -> dict: + value = yaml.safe_load(path.read_text()) + if not isinstance(value, dict): + raise ValueError(f"Expected a mapping in {path.name}.") + return value + + +def _compile(root: Path): + from embodichain.lab.task_program.integrations.composition import ( + load_configured_task_program_deployment, + ) + from embodichain.lab.task_program.semantics.integration import ( + SemanticIntegrationManifest, + ) + + deployment = _read_yaml(root / "deployment.yaml") + embodiment = _read_yaml(root / deployment["embodiment"]["component"]) + cfg = load_configured_task_program_deployment( + task_program=deployment["task_program"], + skill_profile=embodiment["skill_profile"], + base_dir=root, + ) + catalog = cfg.integration.registration.catalog + program = catalog.preflight(cfg.load_program()) + semantic = SemanticIntegrationManifest( + scene=catalog.scene, + robot_profile=catalog.robot_profile, + call_catalog=catalog.call_catalog, + runtime_preset=cfg.selection.runtime_preset, + ) + return program, semantic, cfg.integration.integration_fingerprint + + +def _execution_timeline(timeline: list[dict], size: int) -> list[dict]: + """Keep the sample ownership of annotation shoulders outside the fit core.""" + result = [] + cursor = 0 + for span in timeline: + start, stop = span["start"], span["stop"] + if start != cursor or not cursor < stop <= size: + raise ValueError("Fit does not partition all original control samples.") + cursor = stop + if span["kind"] == "demo_skill": + result.append(dict(span)) + continue + if span["kind"] != "atomic_skill": + raise ValueError(f"Unknown fitted timeline span kind: {span['kind']}.") + left, right = span["fit_start"], span["fit_stop"] + if not start <= left < right <= stop: + raise ValueError("Fit core lies outside its source annotation.") + for begin, end, fitted in ( + (start, left, False), + (left, right, True), + (right, stop, False), + ): + if begin == end: + continue + if fitted: + result.append({**span, "start": begin, "stop": end}) + else: + result.append( + { + "kind": "demo_skill", + "label": "DemoSkill", + "skill_id": "execute_demo_skill", + "start": begin, + "stop": end, + "source": "annotation_transition", + "annotation": span["annotation"], + } + ) + if cursor != size: + raise ValueError("Fit dropped source samples.") + return result + + +def _bound_demos( + root: Path, + authored: dict, + integration: dict, + spans: list[dict], + pickup: dict, + bindings: Mapping[str, Any], + arrays: Mapping[str, np.ndarray], + dt: float, +) -> list[dict]: + """Verify every retained interval against a registered, measured DemoSkill.""" + from embodichain.data_pipeline.real2sim import load_demo_skill + + if "retained_demos" in bindings: + declarations = bindings["retained_demos"] + elif len(spans) == 1 and spans[0]["start"] == bindings["continuation_sample"]: + declarations = [ + { + **bindings["continuation"], + "start": spans[0]["start"], + "stop": spans[0]["stop"], + } + ] + else: + raise ValueError( + "Every retained source span needs its own qualified DemoSkill binding." + ) + expected = [(span["start"], span["stop"]) for span in spans] + if [(item["start"], item["stop"]) for item in declarations] != expected: + raise ValueError( + "Retained DemoSkill bindings must cover every retained interval in source order." + ) + locations = { + row["segment"]: (i, row["steps"]) for i, row in enumerate(authored["program"]) + } + pickup_order = locations[bindings["segment"]][0] + previous = -1 + result = [] + for item in declarations: + order, steps = locations[item["program_segment"]] + start, stop = item["start"], item["stop"] + if order <= previous or (stop <= pickup["start"]) != (order < pickup_order): + raise ValueError( + "Retained DemoSkill calls do not follow the source execution order." + ) + previous = order + declaration = integration["runtime_services"]["demo_skills"][ + steps["demo_skill"]["skill"] + ] + if "source_slice" in item: + path, asset = _build_source_slice( + root, start, stop, item["source_slice"], dt + ) + declaration["asset_manifest"] = str(path.relative_to(root)) + declaration["asset_sha256"] = asset.content_sha256 + item = {**item, "asset_sha256": asset.content_sha256} + else: + path = (root / declaration["asset_manifest"]).resolve(strict=True) + if not path.is_relative_to(root): + raise ValueError( + "Retained DemoSkill asset leaves the execution bundle." + ) + asset = load_demo_skill(path) + if ( + asset.content_sha256 != item["asset_sha256"] + or declaration["asset_sha256"] != item["asset_sha256"] + or len(asset.execution_qpos) != stop - start + or not np.allclose( + np.diff(asset.execution_timestamps), dt, atol=1e-8, rtol=0 + ) + ): + raise ValueError( + "Retained DemoSkill asset disagrees with its source interval or clock." + ) + if "retained_demos" in bindings and not np.array_equal( + asset.execution_qpos, np.asarray(arrays["original_qpos"])[start:stop] + ): + raise ValueError( + "Retained DemoSkill must preserve the qualified commands for its interval." + ) + result.append( + { + **item, + "asset_manifest": str(path.relative_to(root)), + "source_sample_offset": asset.demonstration.source_sample_offset, + } + ) + return result + + +def _build_source_slice( + root: Path, start: int, stop: int, binding: dict, dt: float +) -> tuple[Path, DemoSkillArtifact]: + """Build a DemoSkill on an authoritative measured post-step control clock.""" + from embodichain.data_pipeline.real2sim import ( + DemoSkillConstraint, + build_demo_skill, + load_demo_skill, + save_demo_skill, + ) + from embodichain.data_pipeline.real2sim.demo_skill import decode_demo_skill_spec + + source, contract = ( + (root / binding[key]).resolve(strict=True) + for key in ("reference_asset", "contract") + ) + if not source.is_relative_to(root) or not contract.is_relative_to(root): + raise ValueError( + "Retained source and its semantic contract must be bundled resources." + ) + reference = load_demo_skill(source) + recording = reference.demonstration + if ( + reference.content_sha256 != binding["reference_asset_sha256"] + or reference.reference is not None + or len(reference.execution_qpos) != len(recording.qpos) + or recording.command_alignment != "post_step" + or recording.time_unit != "s" + or not np.allclose(np.diff(recording.timestamps), dt, atol=1e-9, rtol=0) + or (start == 0 and recording.initial_observed_qpos is None) + or not 0 <= start < stop <= len(recording.timestamps) + ): + raise ValueError( + "Source slices require pinned commands and initial state on the control grid." + ) + payload = _read_yaml(contract) + original = reference.execution_spec + phases = tuple( + replace( + phase, + start=max(phase.start, start) - start, + stop=min(phase.stop, stop) - start, + ) + for phase in original.phases + if phase.start < stop and phase.stop > start + ) + events = tuple( + replace(event, sample_index=event.sample_index - start) + for event in original.events + if start <= event.sample_index < stop + ) + event_ids = {event.event_id for event in events} + phase_names = {phase.name for phase in phases} + requested = tuple(DemoSkillConstraint(**item) for item in payload["constraints"]) + checks = {check.check_id: check for check in requested} + if len(checks) != len(requested): + raise ValueError("Retained source contract has duplicate evidence checks.") + for check in original.constraints: + if ( + check.scope == "entry" + and start != 0 + or check.scope == "terminal" + and stop != len(recording.qpos) + ): + continue + if check.scope == "event" and check.event_id not in event_ids: + continue + if check.phase_names: + overlap = tuple(name for name in check.phase_names if name in phase_names) + if not overlap: + continue + check = replace(check, phase_names=overlap) + if check.check_id in checks and checks[check.check_id] != check: + raise ValueError( + f"Retained source contract changes evidence check {check.check_id}." + ) + checks[check.check_id] = check + payload["phases"] = [asdict(phase) for phase in phases] + payload["events"] = [asdict(event) for event in events] + payload["constraints"] = [asdict(check) for check in checks.values()] + spec = decode_demo_skill_spec(payload) + if {(r.role_id, r.geometry_sha256, r.reference_kind) for r in spec.roles} != { + (r.role_id, r.geometry_sha256, r.reference_kind) for r in original.roles + }: + raise ValueError( + "A retained source slice must keep its declared physical role geometry." + ) + clip = replace( + recording, + qpos=recording.qpos[start:stop], + commands=recording.commands[start:stop], + timestamps=recording.timestamps[start:stop], + qvel=None if recording.qvel is None else recording.qvel[start:stop], + eef_poses=( + None if recording.eef_poses is None else recording.eef_poses[start:stop] + ), + initial_observed_qpos=( + recording.qpos[start - 1] if start else recording.initial_observed_qpos + ), + source_sample_offset=recording.source_sample_offset + start, + ) + asset = build_demo_skill(clip, spec, joints=reference.joint_specs, control_dt=dt) + path = save_demo_skill(asset, root / "demos" / asset.content_sha256) + return path, asset + + +def export_task_program( + arrays: Mapping[str, np.ndarray], + report: Mapping[str, Any], + *, + execution_context: Path, + segment_bindings: Mapping[str, Any], + output_dir: Path, +) -> Path: + """Bind one checked Pickup to an explicitly authored hybrid task context. + + The caller owns the source-reference hash, qualified continuation, robot + endpoints, and explicit approval of deviations from fitted native options. + The copied context retains deployment components and measured effect + contracts. Only the named Pickup occurrence receives the fitted preset. + + Args: + arrays: Fitting output arrays on the qualified control clock. + report: Accepted fitting report with the source digest and timeline. + execution_context: Directory containing the qualified deployment. + segment_bindings: Explicit object, resource, and demonstration bindings. + output_dir: New directory for the compiled and sealed deployment. + + Returns: + Path to the compiled deployment directory. + """ + from embodichain.lab.task_program.integrations.invocation_policy import ( + BoundInvocationPolicies, + InvocationPolicyBinding, + ) + from embodichain.lab.task_program.integrations._program_identity import ( + task_program_fingerprint, + ) + + base = execution_context.resolve(strict=True) + output = output_dir.resolve() + if output.exists(): + raise FileExistsError(output) + if output.is_relative_to(base) or base.is_relative_to(output): + raise ValueError("Execution context and compiled output must be disjoint.") + if ( + not report["accepted"] + or report["source_joint_sha256"] != segment_bindings["reference_sha256"] + ): + raise ValueError("Fit does not belong to the qualified control reference.") + ticks = np.asarray(arrays["timestamps"]) + if ticks.ndim != 1 or len(ticks) < 2 or not np.isfinite(ticks).all(): + raise ValueError("Fitted timeline has no usable control clock.") + dt = float(segment_bindings["control_dt_s"]) + if dt <= 0 or not np.allclose(np.diff(ticks), dt, atol=1e-8, rtol=0): + raise ValueError("Fitted timeline changed the execution cadence.") + timeline = _execution_timeline(report["timeline"], len(ticks)) + fitted = [span for span in timeline if span["kind"] == "atomic_skill"] + retained = [span for span in timeline if span["kind"] == "demo_skill"] + if len(fitted) != 1 or not retained: + raise ValueError( + "This compiler requires one fitted Pickup and qualified retained motion." + ) + (pickup,) = fitted + if pickup["skill_id"] != "pick_up" or not pickup["accepted"]: + raise ValueError("The fitted core must be an accepted Pickup.") + fit = report["fits"][pickup["fit_index"]] + if fit["selected_participants"] != {"primary": segment_bindings["participants"]}: + raise ValueError( + "The fitted motion and grasp endpoints differ from the execution binding." + ) + goal = fit["native_goal"] + if goal["object_id"] != segment_bindings["object"]: + raise ValueError("The fitted grasp is bound to a different scene object.") + relation = np.linalg.solve(goal["object_pose"], goal["grasp_xpos"]) + measured = fit.get("scene", {}).get("object_to_tcp") + if measured is not None: + measured = np.asarray(measured, dtype=float) + if measured.shape != (4, 4) or not np.allclose( + measured, relation, atol=1e-6, rtol=0 + ): + raise ValueError( + "The measured grasp disagrees with the fitted native goal." + ) + # The solver serializes its world goal in float32. Preserve the + # original relative transform rather than reconstructing it twice. + relation = measured + if ( + not np.isfinite(relation).all() + or not np.allclose(relation[3], (0, 0, 0, 1), atol=1e-6) + or not np.allclose(relation[:3, :3].T @ relation[:3, :3], np.eye(3), atol=1e-5) + or np.linalg.det(relation[:3, :3]) < 0 + ): + raise ValueError("Fitted grasp must be a proper object-to-TCP transform.") + if fit.get("replay_interpolation_factor", 1) != 1: + raise ValueError( + "A duration scaling policy must be explicitly compiled before use." + ) + motion = dict(fit["native_motion_policy"]) + if not np.isclose(motion.pop("control_dt"), dt, atol=1e-8, rtol=0): + raise ValueError("Pickup fit does not use the authoritative control cadence.") + options = { + key: value + for key, value in fit["native_options"].items() + if value is not None and value != [] + } + if options.pop("grasp_variant", "closest") != "closest": + raise ValueError( + "The standard Pick requires the default closest grasp selector." + ) + if "grasp_frame_to_eef" in options: + options["grasp_frame_to_eef"] = np.ravel(options["grasp_frame_to_eef"]).tolist() + options.update(segment_bindings["option_overrides"]) + options["fixed_object_to_eef"] = relation.reshape(-1).tolist() + motion.update(segment_bindings["motion_overrides"]) + + output.parent.mkdir(parents=True, exist_ok=True) + with TemporaryDirectory(prefix=".task-program-", dir=output.parent) as temporary: + root = Path(temporary) / "bundle" + shutil.copytree(base, root) + deploy = _read_yaml(root / "deployment.yaml") + integration_path = root / deploy["task_program"]["integration"] + integration = _read_yaml(integration_path) + program_path = root / deploy["task_program"]["program"] + authored = _read_yaml(program_path) + selected = [ + segment + for segment in authored["program"] + if segment["segment"] == segment_bindings["segment"] + ] + if len(selected) != 1 or "pick" not in selected[0]["steps"]: + raise ValueError("Expected one authored standard Pick for the fit.") + call = selected[0]["steps"]["pick"] + if call["object"] != goal["object_id"]: + raise ValueError("The authored Pick binds a different entity.") + demos = _bound_demos( + root, authored, integration, retained, pickup, segment_bindings, arrays, dt + ) + alias = pickup["annotation"]["id"] + if not alias or "/" in alias or ":" in alias: + raise ValueError("Pickup annotation must have a portable identifier.") + preset_id = f"fit-{alias}" + presets = integration["profile"].setdefault("invocation_presets", {}) + preset = { + "skill_options": {"pick": {"kind": "pick_up", **options}}, + "motion": motion, + } + if preset_id in presets and presets[preset_id] != preset: + raise ValueError("Fit preset identifier already has a different policy.") + presets[preset_id] = preset + integration_path.write_text(yaml.safe_dump(integration, sort_keys=False)) + program_path.write_text(yaml.safe_dump(authored, sort_keys=False)) + program, manifest, integration_hash = _compile(root) + positions = {row["segment"]: i for i, row in enumerate(authored["program"])} + compiled_segments = { + row.source_path: row for row in program.iter_segments() if not row.implicit + } + + def compiled_call(name): + owner = compiled_segments[("program", "items", positions[name])] + (entry,) = owner.calls + identity = ( + f"{program.program_id}/{owner.segment_id}:{entry.segment_call_index}" + ) + return entry, identity + + entry, invocation = compiled_call(segment_bindings["segment"]) + if ( + entry.call.semantic_id != "pick" + or entry.call.object.entity_id != goal["object_id"] + ): + raise ValueError( + "The compiled Pickup occurrence differs from the fitted object binding." + ) + for demo in demos: + _, demo["invocation_id"] = compiled_call(demo["program_segment"]) + owners = { + (demo["start"], demo["stop"]): demo["invocation_id"] for demo in demos + } + owners[(pickup["start"], pickup["stop"])] = invocation + source_events = [] + for event in segment_bindings.get("source_events", ()): + tick = event["control_tick"] + matching = [ + owner for (start, stop), owner in owners.items() if start <= tick < stop + ] + if len(matching) != 1: + raise ValueError( + f"Source event {event['event_id']} has no unique control interval." + ) + source_events.append({**event, "invocation_id": matching[0]}) + fingerprint = task_program_fingerprint(program) + bindings = [InvocationPolicyBinding(fingerprint, invocation, preset_id)] + for authored_name, name in segment_bindings.get( + "companion_presets", {} + ).items(): + _, identity = compiled_call(authored_name) + bindings.append(InvocationPolicyBinding(fingerprint, identity, name)) + BoundInvocationPolicies(program, bindings).validate_manifest(program, manifest) + _write( + root / "source_map.json", + { + "timeline": timeline, + "pickup_invocation": invocation, + "retained_demos": demos, + "source_events": source_events, + **( + {"continuation": segment_bindings["continuation"]} + if "continuation" in segment_bindings + else {} + ), + }, + ) + _write( + root / "compile_report.json", + { + "accepted": True, + "source_joint_sha256": report["source_joint_sha256"], + "annotation_sha256": report["annotation_sha256"], + "observed_grasp_sha256": report.get("observed_grasp_sha256"), + "fit_archive_sha256": segment_bindings["fit_archive_sha256"], + "object_to_tcp": relation.tolist(), + "control_dt_s": dt, + "approved_option_overrides": segment_bindings["option_overrides"], + "approved_motion_overrides": segment_bindings["motion_overrides"], + "physical_validation": "pending_native_execution", + }, + ) + _write( + root / "invocation_bindings.json", + { + "schema": _BINDING_SCHEMA, + "integration_fingerprint": integration_hash, + "bindings": [asdict(binding) for binding in bindings], + }, + ) + _write( + root / "manifest.json", + { + "schema": _BINDING_SCHEMA, + "files": { + item.relative_to(root).as_posix(): file_digest(item) + for item in sorted(root.rglob("*")) + if item.is_file() and item != root / "manifest.json" + }, + }, + ) + load_invocation_bindings(root / "deployment.yaml") + root.rename(output) + return output + + +def load_invocation_bindings(deployment: Path) -> tuple[InvocationPolicyBinding, ...]: + """Verify ownership of all files and validate policies against a fresh compile. + + Args: + deployment: Path to the Task Program deployment. + + Returns: + Validated bindings in declaration order, or an empty tuple when the + deployment has no fitted invocation bindings. + """ + from embodichain.lab.task_program.integrations.invocation_policy import ( + BoundInvocationPolicies, + InvocationPolicyBinding, + ) + + root = deployment.resolve(strict=True).parent + path = root / "invocation_bindings.json" + if not path.is_file(): + return () + if deployment.resolve(strict=True) != root / "deployment.yaml": + raise ValueError( + "Fitted invocation bindings require their sealed deployment.yaml." + ) + seal = json.loads((root / "manifest.json").read_text()) + declaration = json.loads(path.read_text()) + if seal["schema"] != _BINDING_SCHEMA or declaration["schema"] != _BINDING_SCHEMA: + raise ValueError("Unsupported fitted Task Program binding.") + for required in ( + "deployment.yaml", + "invocation_bindings.json", + "source_map.json", + "compile_report.json", + ): + if required not in seal["files"]: + raise ValueError( + f"Missing sealed fitted Task Program artifact: {required}." + ) + for name, digest in seal["files"].items(): + candidate = (root / name).resolve(strict=True) + if not candidate.is_relative_to(root) or file_digest(candidate) != digest: + raise ValueError(f"Fitted Task Program artifact has changed: {name}.") + program, manifest, integration_hash = _compile(root) + if declaration["integration_fingerprint"] != integration_hash: + raise ValueError("Fitted Task Program integration has changed.") + bindings = tuple( + InvocationPolicyBinding(**item) for item in declaration["bindings"] + ) + BoundInvocationPolicies(program, bindings).validate_manifest(program, manifest) + return bindings diff --git a/tests/data_pipeline/test_demo_skill.py b/tests/data_pipeline/test_demo_skill.py index 3407941283..9b2134d748 100644 --- a/tests/data_pipeline/test_demo_skill.py +++ b/tests/data_pipeline/test_demo_skill.py @@ -18,7 +18,7 @@ from __future__ import annotations -from dataclasses import replace +from dataclasses import asdict, replace import json import shutil @@ -113,6 +113,60 @@ def _build(source=None, spec=None, **kwargs): ) +def test_fitted_source_slice_keeps_controls_and_measured_events(tmp_path): + from embodichain.toolkits.skill_fitting.task_program import _build_source_slice + + raw = _source() + raw = replace( + raw, + commands=raw.qpos + 0.125, + command_alignment="post_step", + initial_observed_qpos=np.array([-1.0, 10.0]), + ) + spec = _spec() + reference = build_demo_skill( + raw, + spec, + joints=(JointSpec("arm", "revolute"), JointSpec("finger", "prismatic")), + control_dt=0.02, + ) + manifest = save_demo_skill(reference, tmp_path / "reference") + entry = np.eye(4) + entry[0, 3] = 0.012 + spec.roles = (replace(spec.roles[0], source_pose=tuple(entry.flat)),) + spec.constraints = (spec.constraints[-1],) + (tmp_path / "contract.json").write_text(json.dumps(asdict(spec))) + _, clip = _build_source_slice( + tmp_path, + 2, + 8, + { + "reference_asset": manifest.relative_to(tmp_path).as_posix(), + "reference_asset_sha256": reference.content_sha256, + "contract": "contract.json", + }, + 0.02, + ) + assert clip.demonstration.source_sample_offset == 2 + np.testing.assert_array_equal(clip.demonstration.qpos, raw.qpos[2:8]) + np.testing.assert_array_equal(clip.demonstration.commands, raw.commands[2:8]) + np.testing.assert_array_equal(clip.demonstration.initial_observed_qpos, raw.qpos[1]) + assert [(phase.name, phase.start, phase.stop) for phase in clip.spec.phases] == [ + ("enter", 0, 2), + ("trace", 2, 6), + ] + assert [(event.event_id, event.sample_index) for event in clip.spec.events] == [ + ("touch", 2), + ("mid_trace", 4), + ] + assert {constraint.check_id for constraint in clip.spec.constraints} == { + "touch", + "maintain", + "end", + } + assert clip.spec.roles[0].source_pose == tuple(entry.flat) + + def test_unlabeled_demo_preserves_observed_commands_and_internal_hard_events(): artifact = _build() np.testing.assert_allclose(artifact.qpos[2], [np.pi / 180, 0.01]) diff --git a/tests/toolkits/test_scene_skill_fitting.py b/tests/toolkits/test_scene_skill_fitting.py index 771a142edb..559b26e68a 100644 --- a/tests/toolkits/test_scene_skill_fitting.py +++ b/tests/toolkits/test_scene_skill_fitting.py @@ -38,6 +38,34 @@ ) +def test_task_program_keeps_annotation_shoulders_and_unmarked_spans(): + from embodichain.toolkits.skill_fitting.task_program import _execution_timeline + + spans = [ + {"kind": "demo_skill", "start": 0, "stop": 3}, + { + "kind": "atomic_skill", + "start": 3, + "stop": 13, + "fit_start": 5, + "fit_stop": 11, + "annotation": {"id": "acquire"}, + }, + {"kind": "demo_skill", "start": 13, "stop": 20}, + ] + assert [ + (s["kind"], s["start"], s["stop"]) for s in _execution_timeline(spans, 20) + ] == [ + ("demo_skill", 0, 3), + ("demo_skill", 3, 5), + ("atomic_skill", 5, 11), + ("demo_skill", 11, 13), + ("demo_skill", 13, 20), + ] + with pytest.raises(ValueError, match="outside its source annotation"): + _execution_timeline([spans[0], {**spans[1], "fit_start": 2}, spans[2]], 20) + + def test_scene_pickup_defaults_are_explicit_validated_and_digest_bound( tmp_path, ) -> None: @@ -364,26 +392,43 @@ def test_half_turn_preserves_runtime_tcp_origin_and_qualifies_its_actual_surface assert rank_grasps(candidates, reference, 0.05) == [1, 0] -def test_contact_grounding_preserves_a_virtual_runtime_tcp(tmp_path: Path) -> None: +@pytest.mark.parametrize( + "coupled", [False, True], ids=["direct_fingers", "mimic_driver"] +) +def test_contact_grounding_preserves_a_virtual_runtime_tcp( + tmp_path: Path, coupled: bool +) -> None: """The configured TCP is 56 mm behind the geometry's contact centre.""" parts = [ '', '', ] + if coupled: + parts.append( + '' + '' + '' + ) + follower = '' if coupled else "" for name, x, axis in (("one", -0.04, 1), ("two", 0.04, -1)): parts.append( f'' ) parts.append( - f'' + f'{follower}' ) path = tmp_path / "hand.urdf" path.write_text('' + "".join(parts) + "") tcp = np.eye(4) tcp[2, 3] = 0.10 profile = RobotProfile( - joint_names=("pivot", "one", "two"), - control_parts={"drive": ["pivot"], "clamp": ["one", "two"]}, + joint_names=( + ("pivot", "one", "two", "driver") if coupled else ("pivot", "one", "two") + ), + control_parts={ + "drive": ["pivot"], + "clamp": ["driver"] if coupled else ["one", "two"], + }, solvers={ "drive": { "urdf_path": str(path), @@ -391,10 +436,19 @@ def test_contact_grounding_preserves_a_virtual_runtime_tcp(tmp_path: Path) -> No "tcp": tcp.tolist(), } }, - hand_commands={"clamp": {"open": [0.0, 0.0], "grasp": [0.03, 0.03]}}, + hand_commands={ + "clamp": ( + {"open": [0.0], "grasp": [0.06]} + if coupled + else {"open": [0.0, 0.0], "grasp": [0.03, 0.03]} + ) + }, ) geometry, model = _hand_geometry( - profile, {"motion": "drive", "grasp": "clamp"}, np.zeros(3), 0.004 + profile, + {"motion": "drive", "grasp": "clamp"}, + np.zeros(len(profile.joint_names)), + 0.004, ) contact = torch.eye(4) contact[:3, 3] = torch.tensor([1.0, 2.0, 3.0])