From 9d526a31f776bff1f9b2e06fbc43687bb885e1cf Mon Sep 17 00:00:00 2001 From: Matthew Taylor Date: Mon, 21 Sep 2026 21:20:37 +0000 Subject: [PATCH] Add a self-contained Franka smoothie demonstration --- .../source/_static/css/environment-browser.js | 1 + docs/source/_static/tasks/franka_smoothie.png | Bin 0 -> 68226 bytes scripts/environments/run_franka_smoothie.py | 134 +++ .../changelog.d/franka-smoothie.minor.rst | 9 + .../contrib/franka_smoothie/.gitattributes | 2 + .../contrib/franka_smoothie/.gitignore | 4 + .../contrib/franka_smoothie/README.md | 80 ++ .../contrib/franka_smoothie/__init__.py | 18 + .../contrib/franka_smoothie/agents.py | 25 + .../franka_smoothie/assembly_controller.py | 827 ++++++++++++++++++ .../franka_smoothie/assets/blackberry.usdc | 3 + .../franka_smoothie/assets/blade_cap.usda | 3 + .../franka_smoothie/assets/blueberry.usdc | 3 + .../contrib/franka_smoothie/assets/cup.usda | 3 + .../franka_smoothie/assets/franka.usdc | 3 + .../franka_smoothie/assets/fruit_basket.usda | 3 + .../contrib/franka_smoothie/assets/mango.usda | 3 + .../contrib/franka_smoothie/assets/motor.usda | 3 + .../assets/overrides/strawberry.usda | 3 + .../assets/overrides/workstation.usda | 3 + .../franka_smoothie/assets/strawberry.usdc | 3 + .../contrib/franka_smoothie/assets/tap.usda | 3 + .../franka_smoothie/assets/workstation.usda | 3 + .../franka_smoothie/basket_controller.py | 285 ++++++ .../franka_smoothie/basket_geometry.py | 9 + .../basket_grasp_continuation.py | 257 ++++++ .../franka_smoothie/basket_pose_controller.py | 63 ++ .../franka_smoothie/basket_reference.py | 113 +++ .../contrib/franka_smoothie/basket_stage.py | 141 +++ .../franka_smoothie/bounded_return_pose.py | 243 +++++ .../franka_smoothie/controlled_fruit_pour.py | 461 ++++++++++ .../franka_smoothie/cup_contact_spacing.py | 162 ++++ .../franka_smoothie/fruit_receipt_geometry.py | 187 ++++ .../contrib/franka_smoothie/grasp_feedback.py | 81 ++ .../franka_smoothie/pad_torsional_contact.py | 219 +++++ .../contrib/franka_smoothie/pose_control.py | 81 ++ .../franka_smoothie/recorded_controller.py | 63 ++ .../contrib/franka_smoothie/return_path.py | 86 ++ .../contrib/franka_smoothie/scene_cfg.py | 150 ++++ .../smoothie_assembly_geometry.py | 505 +++++++++++ .../contrib/franka_smoothie/smoothie_asset.py | 276 ++++++ .../franka_smoothie/smoothie_controller.py | 508 +++++++++++ .../contrib/franka_smoothie/smoothie_env.py | 472 ++++++++++ .../franka_smoothie/smoothie_env_cfg.py | 88 ++ .../franka_smoothie/smoothie_gripper.py | 57 ++ .../contrib/franka_smoothie/smoothie_task.py | 232 +++++ .../franka_smoothie/smoothie_visuals.py | 140 +++ .../test/contrib/test_franka_smoothie.py | 437 +++++++++ 48 files changed, 6455 insertions(+) create mode 100644 docs/source/_static/tasks/franka_smoothie.png create mode 100644 scripts/environments/run_franka_smoothie.py create mode 100644 source/isaaclab_tasks/changelog.d/franka-smoothie.minor.rst create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/.gitattributes create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/.gitignore create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/README.md create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/__init__.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/agents.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assembly_controller.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/blackberry.usdc create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/blade_cap.usda create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/blueberry.usdc create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/cup.usda create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/franka.usdc create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/fruit_basket.usda create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/mango.usda create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/motor.usda create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/overrides/strawberry.usda create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/overrides/workstation.usda create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/strawberry.usdc create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/tap.usda create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/workstation.usda create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_controller.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_geometry.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_grasp_continuation.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_pose_controller.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_reference.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_stage.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/bounded_return_pose.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/controlled_fruit_pour.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/cup_contact_spacing.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/fruit_receipt_geometry.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/grasp_feedback.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/pad_torsional_contact.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/pose_control.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/recorded_controller.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/return_path.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/scene_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_assembly_geometry.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_asset.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_controller.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_env.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_env_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_gripper.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_task.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_visuals.py create mode 100644 source/isaaclab_tasks/test/contrib/test_franka_smoothie.py diff --git a/docs/source/_static/css/environment-browser.js b/docs/source/_static/css/environment-browser.js index dadfd2dca455..672a2b2d79ff 100644 --- a/docs/source/_static/css/environment-browser.js +++ b/docs/source/_static/css/environment-browser.js @@ -84,6 +84,7 @@ ["IsaacContrib-Forge-NutThread-Direct", "rl_games", "", "", "", "tasks/factory/nut_thread.jpg"], ["IsaacContrib-Forge-PegInsert-Direct", "rl_games", "", "", "", "tasks/factory/peg_insert.jpg"], ["IsaacContrib-Franka-Pour", "rsl_rl", "", "", "", "tasks/manipulation/franka_pour.jpg"], + ["IsaacContrib-Franka-Smoothie", "rsl_rl", "", "", "", "tasks/franka_smoothie.png"], ["IsaacContrib-Humanoid-AMP-Dance-Direct", "skrl", "", "", "", "tasks/others/humanoid_amp.jpg"], ["IsaacContrib-Humanoid-AMP-Run-Direct", "skrl", "", "", "", "tasks/others/humanoid_amp.jpg"], ["IsaacContrib-Humanoid-AMP-Walk-Direct", "skrl", "", "", "", "tasks/others/humanoid_amp.jpg"], diff --git a/docs/source/_static/tasks/franka_smoothie.png b/docs/source/_static/tasks/franka_smoothie.png new file mode 100644 index 0000000000000000000000000000000000000000..950421111091adfa380bf0a78366b472d21fe522 GIT binary patch literal 68226 zcmeFYX*iVc8$UdyqEMtXvPB{LzHf!d2sPI18e7I#vJ+B~W$?A{Bu%o6ea60&kZn*H zjC~v1*mvQ%$M41e?Q=YDpZ^>OZZr2>_jR4uxqQy^{9NJBbu?(N-Mt0^foL_KszE`Z z%fKbYJ*tbqpY0>#BG3g8NK@^xzE8&LxNiyz-k0rYJwwl6{O{k2izz>7L_k-l!!tp= zYbFoFzpA~AwRv&N*1jgTgf#5tobl8m?l;z%Q&z{RW8Z=&@KvkME7Mpm(@6B-O(V@) zym2$vFFv^N%F@o#A6-x}rmSP&>Dg;_|4Ite&awZ&BgF>8!AGEpL)@X#p6MBB#UoKP zG62+b=?*h6H4x(+(^U!wp{{>#OaMTK{Q=|U!8D77I`rV0DHn_c5$H-aIPw60wH$x5 zPyZ~Vd0nPDLUQmd+1hPNnyDEY0-`B>Ch(&P68m4Vqt=Eu3#i$5N>>OZ!)|}!>~phn z=!LYdKWE-oPW3N>jOnz_ZoCCKTu(mD>sK*+C;H?HC@HCM%hKtf;)Yd6E#F!91&~bk z{O+gh?a=E!n*WZefn}!kZyjk}0oGJpy>Q(A^GU`gb0J6?MmfPBgbN!3-UorY%(q)# zm~`?q)Tdbqc6ypsveYg%!T)Op6aqB0T)KExY4M=PZ^FK+{^G_LhvI=uZ1tJ<%bUsP zUlyXh38tSG6JVuk*7|$%FzBf}TDFNUY##WqR6w972qMU4b|dT_s6|#FR+%^H`LY?O zkxem|M-*^Fv6t4~B!w1DF@wMps^-!i!A)hwWVJoB8T{f{(y!Z>K=1gSU!KileodKN zG<$f->YY}l*=gs?T2c2JvmYVl9!(6O((h@c8!cQnCh6W4GF%jS6kqLl%4xZGxLI(h z0|E`Su)BatX=M7@4ZBXW6F_|rHf|KTH?z-!jINyC14LRpb8X}1zj&6j|0qD!GFlrk zZ-8kzIBTtD^Jw{8-MLu@m3+EzzRuFG0wX(>zXYa1^)`Yhlcr&d=WkOxENOtMMR0%q z8FKzGk3$`5D^k8~B1&OMZgK_uy>3x2X*{y$?)Lj<>vwyz%2kao2_K) zA-~@7U$_AN-(}jxiz5HKlDK@y>VH=b*MPy!Uwyae-jSCMe4SUu$d3Dxx-i+hfI!V7 zW~nxQAqybb&dlx5Y?<>n&Ezhn7Ixph*6bQGefGWUZeSqrUDv_prcO}OEnw1IltFqI z1s-b=dGrqM1LI85x-%{a|3Pi91OL410+lg@VISbWcVNp^@;;DX)i?jo)XDdd zUyt+v^F6>f1c zKK)6)=e8=~u&dL0vXp+y`V&o^YiEa;!@w}ln7gWK$loGuL+CZOMUtGT*zPYa4V`Em z;m&>_mVnF*$q~%xL-2$UrwN{r*yz460Dn6_RW})M1Lj`7&Vr<;H*Kngo3HQ2G(Hw?Rc_ge=|Ts z*IHJBJV`-vgi7LM_CrKTCz|J)s>pPdNYU_I9TICB@UAWAO?GH}vpFKRQ9C$J(U2K* z(ziW)lN_{+D9MxL`X%M4!`GlT@E?qLQ-$t@+=(M%S+7{LsaLk(hr3uQ$E$tv3KReAy2m(5e;1|5t{OF~N*FCh zOSQ4EXB}+a7tUJ80ONwR=*gn7HDZ-tj$xn8;OS4ktLnIu7+$de47FB0i_oM|vZNn& zKzvQuBf-~Af<6fGQfN@xo$IS~OucI({#=1%$tz8M@%mtQ>o*f{LO^olz<`8mg51Fgp#`9F0wF6!^j%WIPt6OL_E&|6Fg+-H~U_Io*tGVD%R$J z#tiMrF0ua1N`Bc;Ta}kbJlJH&)5(-lXI9OqXlL_ZEo*1f+H$|?_cvtZAgDL?wXh@w z&y$7SfbpueG1q=0vgujHl4GhseLe#vc=qpKaR!F?`8PFYLU>__l$$OuwA0Rt?aV&g zPKL;D=wUCG`ZrHECMt@b>%%>LNYL4_^5^fR@EO(`OlOIxIy^p5!r+q5ry1YbwR&${ zlbl>fa~0{X+Ld#GT?7!bRd8aSdW(pH+sxd~&Vaf1_{eCQ3jz^vvfI-7D*rV6#WMp4 zFPC1hR0Mj&sq33|dU2ie{?gw(%n+mln*SZ+Hy5ih%jufnNjk7hcZqjEJd?Ylk)48T zcB8GC{%>(?a4J;06CfGhJK|YRI%_huvEVOoytwYsb{&CxHu)596 zoh^bF?Oj#fHRVQ7GB6j=RQcn}fH*jAbBBgTH=VAZo$#zTohflbN*+^N8X=vRg+v^@5vRO|DK%ky#}zs8}ySaEyx-V7$v`ub~lg1iWi zD1d8}=TC9ZNm$5eXLqa|T7s3veDza`7rj;WK_^WiuTC)=v%Q@vo~ve~)wmrlr_#RO z4FsAW;1Az`CxS``K3}8(l{Uv+F>;rGf#NS)_rs5)p#4fOAt($MU0OMmAK`W@IE1bZ zjQ320X1Gu8$4mC+KN;XY3CYsCrNc^t`swQ@=z+fO#8sHtn-vI$dRloMJ4>89U@pqr zfFfN^vT$dagxM2s4n{QZOy8K?&s_i4FkAiNMarhn#ZqhJ$Qx%fJcj>^Cl~_jjGj&C zy507aSMXAv14)9bBQ?y;BC;=EwIXEEMxazPlmd?r?!Zy9-Iep6mX1QDYZ0Uy$dNvj z0??o4oA0jyMx)|riQ#tEMa3(1OMOanuyZN5OkS%Yjz6XwE@BQ1CI#3c!&jPLe8YfY z(f6J79Ppl%hR6HsMtpa9ShN`o<ry<(+&HEqVB@IKkXK`YkQ0Q1stC>^qVZI zrUHn$bi6o8c zu>>zFL5%9He@uedL_uNrQ{LO5)7{<32J5OzgorZax65#p{vX3pXFeSjx05fcUvHTa zvNIHHh(~+7PeLZ$Z>X1R)%^v-9cZ(%X$;8yoz^FyvQgU17PQd1-*gs`iX>6DGQl$3#qCSb+S#U6&Waw3bo3*{ zVLG_`oAPK`Urlgm8yKa+`3-aZ{r6i=$%@Rdgg+VLFeIG{ul8K)g_PFFxZE2tlNCw9 zpFXv39cHj@qzjR_>8o;v<-L11SUNU%I9~yQ@&Tuf6)*u~H8KLCE`klFu`3<^W33vW zlKBZP4DZJ(xFsiK#gOS@?$J<7<8-WY z|6I>sJtl9z)r!}c#G9?bd&Aml8O$7)U|w=URq#-z!pLlEaGQHf%E_OFzaEzS=}Luz z1#~*~RY{j0)2(wAOV)8la8@-KQQ=Y=KE3Xdt%{6CFLI%vC}V*Viv~yMPVFQYjWcoG zolIrgUUOCi{>fyczw76!Z6JrxiPEt=;3DTX#&qX0SfA?aUUe!%qX#|B>+X$seJT&> z8{l)`6!Y2@Yn6P3N*I5zdfb>0;qI*UL+XKjh2Y~UE#CM*6CiyEBuQAcISnSShD||G zd}@^pDehvSc}j`}D|b~J8u)o7ONI>#h)y8q-?c@jUgKl4z9TgC#Z~andD|uIffvrP zaxqXT1nO$RiU(5^`(xW|u-|UgL!s{KU!8dPlK3t#t2VMfAeF==Yw>=#S6Y}E{@bhB z1mkE6ho5$*igy%QnX*Oau|LtyBgr||8K?c$=QF6gkt+HPQx;GYmt~xr%OG{q_91o^ zYXy;mwZc<~ZR54{5Ko?&ErH4tXWbcy*6f;4k8$+b(rQH)JD zFJ)*{Q6AvX=vfhN#olmc2T_h{x)=1rYt-0mc`2#Ezc6^yQi7@0&knXJA7*hb-LPzT zHzRILW7;^dOXe;_OXYt*nH#!+Edt%4bO*qxP^724v)^J4<u|lji-Fd;eL3 zN96%fQV?*wmk8mKKQ@B1#%xGJGzr)E*C-Gy?VjdU#sZfgm%7a^z)Qv*O>LjJe3c+7 z#BYugwh!M5W%t@vyu^DZK&>AJDAT-u&YPbg*2WkSC7RHM`K9+RzW*2ZYk9>ych5g% zU4f4`bEAlX6GAUF#g~I8)?&`O36y?v!YA*^K|kXTUo((SboFok`EWzYm6+*0656Zc zN}%g<{w>*deO3}$U=#gf%fysUXL45Nm7Sg4+qbU>SVZ>0uk~MApJ`)6?X)T}Y(|2D zBhdWS;|s+8ygH%3X@u5c7)-p-(6fElf)HD4cnLdR>NG9wSNDAaj#p=RQEpvbcI#~; zS6{z-T>Ac`?@32&e)~RaYq1qDr|KfIXBSEa1gBJg#_PBE!@F@?2$wpf-s#X*(vzBf zJQkFmk+Bt^yvxa~pSg*qZ$S^;FVK8G%C_&Vj?Ck zei`;fK=7lh`KK?Q-SyB-Q_eJfbE*IiCK+FD zdfFYYfUi|6;Q<<1!``c}iba9xH9u*-=>Vv`!+3Oh&`vz@I;&jDEvMO#p52IRg+WL=d&5;O$rY8N)4B zyyA*$d2Yxz2;i#(NdB9ev>w0w-g0Vmt{Y_ONxwez9CrsXUk@ooW7+LG=S%TnFIHRi|w0Kgj6Q@-UV*j(olnnC~)iYEPKoR5_2Ix}5g# z%=}_j6lFVuBm1o%;?XOAM~lqsj^;XY90#k8-;=8E!;nxvk8kz0g_7yJA6*rC5s04F z^iQdDUOO)Yuka88!o#*hnV#yi2gKuymquL?+pb$Sbx&j^FD(}#a{!Y&=_j7zYgwhM zJc?@j3dw-qRp-2Z@Y`GIOrxdUvUlEO)ye`Qsfs;|7f|?i^w5`tF%3GwpJk5kGvD#Y z49O+KU&G-BkSZ*;$ZW(G>BeIJ=8d1-w|gI-l2&a^;JkzTdzm8x@lDvKzn+BtwpDYcqv8DTGbRg3IV21A0Tf&6^jL(tC(8>y129ZaPn<+LRLS`zt7RfbxS*9QKA(3ZhM_-Q%A>@g3tL63c-gyM z|Mhvd@qotYQ1C2FOU30VR1<%IoH<^{(F z%s(-M1Cx6vOE%Zy%IVopSwX9HfcUC7p0|JSCeCSY_s5)$^`4Ar%nIjHrFCHjvaBD{ z>50Oit(_t`!OpD$ocdZW9)3mmS+9p7OmUc5r}1F>l9-j@;8 zw8=P6o$zaF6p7N`(p7PVBAjV{dU@{JD{X%QQhG;AckT93^quV`U_KyEktbh_g+9c3vDW!b3Ej_a z1pp<4{vIamai9`JqZ2w)YG<}q?uVe^m*__p;YfOw;TIYidI~!ZY;76-Kp8B8x1sQ* z-iiL#xvTv}(>@C#Fq?HP7g=on0X~p4n|-os4`jx_z%G{VJvGAT#xQbTQAj(b#lPLi zn_vvJ#+cB)ckRtm>`>jjZUM6=al6HZT@HSBQ3Uo&*4sR*7(c2pYutsrDr2-H`=p(_ zGh-VKmkUsy$`D_0#U=j9a7TB5Gu8SY(D3w2>!3T_(suqrqdqeyyXSpyJCRqjx1UTE zEq6yUCxr$5n`J*+wO96E=ziSx9nF3o#V4q`F93i>0&e6<7t0W-L77>XpTO}2njiNT zA-(CBMU*LC8TGyyeV?TrSiS%1%*;+53ll#ZOWNkiXOrEQ=b_dP$RU%Z9h#Z2Op&5^ z)TmclSt1R+&Kj}_IedrDHrooi&wsp<5fiCwkpAEaLGn-29-Q5>!cKd-J0~2UWj3`= z2J{l=RmUgpf3X)#_jVLYFp@->_cIeln^B>J4n=M%mE(}{#x7Zg_wLS=XIpb@?QZ2G z&!o~b2{y~Su_{3RPj}_(5?m^)LSwc8ulc=X)#oO7;es<74RJ3#>f`NfDmW=YJzL2ZO0%;G_uW@{wN`1zC z+xu^Jf<@&3`9X`RZ}BAbL*5s zcoRR)JQ(^`P^S*JMZSQvJuyGdj(|7ug*UoYIv26!>3JXJ_>6lZ6g@M7@s~n6riqOV zx47@D)sd~hS(DsMJ9}fs{hBotKHu{K>qZ z88NEOMO)@{#{ohj(!TH8R%oAIl9#PA|C}mnLwI?WW6ycb|8#e{rrp)1=oh#S+tdZN zglns*xN=$pG8l;|&er}l6PESQQQj(5rV`#}T)UDu)Yf=`hZ@{iyJ_rOP)jW8eTs`a0V9I7) zTtZS_q^I}IVh*+Tdt2NPi)+po=A}{X-JW^p=(P3qA#zlhme}p~QFLOt_}_-U+nO~C zo1oXi*aXgSQktA=EB1m_p?o-m1v-5U9$zygmkN8f`No;^Ppx4^v^ZL^&H;h#Y3=|& zNDqvP?YQaOwi;;nqlR9E&Q1P7__oItJ91EsW|5VX@UUCn&8x+ZtVv!5#4<1$j^KAx9?iRUSu0}gp-HPDFpBI2q( zFYFFTT0u7dNa`)qrJPxr3_{3XIT}@5YQhT;&Q4r2akmW(&-Nwlj9gzBnbmU{?L#18 zNb8~@YyPXvd*7c0oMO7tNmuO9+5rmRY|Ja6X@dr_E6yWhaHOtU@yc+LRq;x7G5nbz z1w(94^DV(o+?;!5FFB4t+O(o=VocH&9T^`xY+ks{ zChujvsez)89@B+de@?ijO6Y>zg>}wD=srSTbI!`((u>XAu^&=}J}Jw}(O=3*z{sj& z5s3+%Qk~aSQ@?zDRuzhkDu4DS=0t38MQ-WH49=_` zn~{o!ez1Fdiw`i7r)^PE3c-*Bwq9&No2f>+rf`IMl#vvy@ol1w(6xKc+hf z{G7o3g}JI&v3;{!f3Bkao4S3c*Mk>vp7HNp%UivjXE2*}&e9tSKV){_%i7yOiQK4i zNs>A1?=JoA4#WPh$hDSNEU@m^eREX!b+3RSxsIP%D-C%{0C z0*)qGao|ESnZIJNh56wR*HT&&ppEOFa_ZeyMjO}u$zFdwWF(xC!IZ_v)fK0gFCPBG zE2N_MMkVHwyvtGMx`3l-^{Ld0%E@HS{GUL`Ig;_Hm2%ci5eeT7}^>W2MpXjNoP~lREvROo2R2WvTG5}5VRdM~|fTbh8 z@L_yU{Ud~~c;(lJkeB7&-k+;h>NPOy&!%@{Uz&c#GRbKEq=o17non^R@NeL*b}wmk z3=_Q-2!rsh2w`Jf{Bl{2Gv(d9-ZY`41F3U&1 z&G9#Oyezh=t^=Br#N-hSgk75!J=AUhzf6_*CuC1yF&rAt9ju#K^v4gfS0JGTn2+3I`n%CY^j04kBoIW#%#o`D zi#xRVqA~+Z)5Ez8EncSC;uQzP7sf7ihFDhmuL%og4rrEkYh+Dv{DX;u7xFU6GQ_^IOYJK5>qZ1>R8`?Go(wSkyEjN_^^|%DCgfcr1R}kgeQqW+R(MsR#1?|ma zFzOcZy{Ji5?aHb5+7oi8#er=#KKr=s6*~n$^gI4-T7|8uswsx8R3T%5@(==fUG+6` z!R#q-_){wDu6nQeiXjs!9N3%1+5b7$WdkiZ>m?5Qk@5Nt$7!b_|Fyn8*|v+)4%9caq8j+<-v4fWG86qZp0P7ui~FID-*p^tK!;Dk93n4Om$zA ziEiit`)~aC1ct>ZY%Pv^PhxDll0;~vTqYFu1>!7TLHRgjoaM&bDGTEP6$1?_E#nbsh+#Jh!H(F zEIw;!v^atBJ&(UFtHv)vXw8tv(auw)?DSM#FYhYio0rT~lW)h{-Uc4j%Lg;xqR>{W zXyp8op}Kc(+gY$?6y!pUMhn4^PwZQL)7xcYUb#c-F;($Y+M8*PoSd&9OnB|RD}?>L z2ja>b%@i?8M~grrunpje*T?q;(aB18j$s!~DHk$=4l@1EHnb+2zH5;|Sl9LQj&}OP zqdfIIN_0kYZmm{aXI%3T|82fTwAzrGQRQj=Odb<%i?3mmSx>+nHRmDMe zc3$JPfpsh|fsWo+PpDy{)UJ6|S+aXz*^?d?d|){79;>&ep2f%#f*1%%6m6Lqb{^+j*`Z)cI~JEboC+y*Fx()KDi4XMK;n^`d+RzaZQ0_Rs_@AVb7Xbb&Kv+(O+0 zXMUwdNACp(2XfA2jA$TAc-tJAokQrNXCEB8?pv+znjYPmb|UK0^muT+ou2EiXy?|j z@3aOV1=5A)h_;0q*IoZd!S2%p?KddD(Mrlh2Y)(a`SJRMqbfTgm+BesIyCoDDPr>gFyX1H@-in07% zIg}AOr!i%0#eoL@yHoI!@IunR#e?)zelHLX?HHv0amhy}+-!HW#Bq9(kUheUslEP2tB&iwq; z)b|NuG`1}>_K@2_f9EF$ZZ1+6P*8KkW#1N2U-7GR9uE&8cjb(j$&%)pYwqX}p-KEB zweZ?p%R@8O+v;AyP}>ijSLGKQG1s`6u&^>4dQS#xLt&Q7zz6>{G4+IMO3 zJCx7jqB*};mW*4QkpH*pdl{pfT{C^m;|#i2A_6fCajW9-FYBW3BXb4{7qV|xssSlD zNf~JP{4p6^KSdp7Ht%IE8r;D!%DuddD7ivh9m-^8JW>EwWF>lzfdmH&*e^XcBR+t2 zcR#bX`z9jNCoR%vuj*vRWS5^=nDDF)Qzj+22I`4pM!In>)N*Et{PMc7w`yE9b1id@ zS}Iy)^d>2(CAW1}RS!$$gjDi$ zwDH0YPh{z4{Wmx4Ls`^N=jA^Eb%s7%$Z*HSHTJ1caro3H`)Hn_M3hXmW5Uyz-}V|& zHvX5?=ZP)_!>ZPP=|2F}ps+pe6aY<+-0!9e9R}V*rDY24?w6QUe5>MEp_lyU_t@!8 zw79Fpv*Gd>q0P+ZaEJAhA^om~r6rAo(W}pPJ*Rexrrg8p-jC!zWgfEPyGfQjnI~t} zAuFL^Dv(4S0)@k|O5;y(FQyeVDkG~d;qADuayaoCQ#s>LCS0}8E@pSXpUG%n!vCW9 z?{4Ie-Q&Q&mIDogH89tL`v5IvGJ^AbJI@E@^;=qW;GWI2Z54hI;GahwMDWRoBV81P z#>f8_`j`Y2oaZcEk=yKXpmrVqLOci#4e8SyQ81^B&NlPk-Z5Nm%YRx}S4IaAs;2nJ zEaK8ztmzN$M508BJQa$A>P4H(t>}mM+RJl4fMIQ5fSmMzBB3hkpbv?w2?b&Az64a>29o>fXnP^l=Bg{G;hJ zL8DL=lmc{8bQ_q_q@{Xf9wjhnGCMhH-t=_v!I~92pyS>p2UZRZEpbLR8^-yE(5X=2 zJ)PtixhyVWRgnnim|PbqMe*)?tgcd*!q-Fil75wO?OuH7J*xOBa(d7e7z&K(DtoTS z=Eu6vhQ0dSBb)cGx+kgQGsGzfD)tgf?N~jXC^eHWPo~nbw8MRD``^k7Kl55#Lqs+& z@efo#_F0Sa(~NOA_JcheFNWa`$tfaJFC8$jkodWCUFq9!LO~56-@Gp3J^dqGp10`W z-J1}8Il{09AMd@w-j|?v6Iy-xHAF5lq+BZP$*m`r5jwwyb*wclo7jGEY>$*Q<>< z6DkDH=BTP&+0K=znsikDp7?wB#BM#8IE#zg`UJ6_I7{iB60qFCSfbo(U|3_T^Ic0D zzv@(H-6MEWZ~WO*$Ts6@=O*dm#VE<~P6y=A|#(fpOv(6Jz&Co=+d}dhK zYUS(nGFH8A1^UMSW(h@#BGT+ z(z7-RTm|C$KKE+f1FgsqIumA25&(1LXt-b=<82nOZUCSN6FM>{ADNv-dGn^{YQ(U~ zdwH~k{tz^kqW;k}A3?H3h@*!>x90}p40^i3ueu$yk*FXGD`Ak9V4>Zv5t2R@YC>h* zRDZlyXJX`Xy4lq(be2zUTdJJ1Uanlf-s8l+xTnxN^cpcQmifo26LUdW^XHQOSTp$e zzp~pp=-g`sbY>9NWW^3}n=F9Q?CB{sMh#f87X^SGrsBVimKzEiQ<8Wy_nBW;dymUj zs+i(e&|Oq0<*QVrM!%;Na+A!FJufA^ zo4>5}8F1pCfB^RX-glQ(k7@bf+}iKaBSk6QIE-!u>9aZhwu(&jQ?HBeLTh6c)7LVI zEYj=7?XnCi3LDKInod`mHpkZkV?^cN%#{ibU*k~PPF1!-0=3$=P&rS>J#`QI)!Sis z4Tixzm5Mnp$EzL`jLMFt_7wm{cN1&=!DxXK*s|vL=Oha@Krl&N1}I!D!!Tt1lFnt7 zx_N`VxKd5$Jn!yN=_+^tYr7lEr!X1&0RB-%6vq%C5(>pk>+h#C!lzsElZVG&A2L@p zan@}eDxbDbG5`~gS4#G&XERH{nA$(HvKg$QG6}PFkaLQ*IzQx1jt>o z_NOO=#tTxg$!V0y!WRP zox?o4aI}-h6Z=Z1n=#7As}*}$HgC%mt^HS{X9K6HgA#kIMx6~_-@2h0(F-M1O)GR% zFD(*Ew}67%&=ujU90|}4@hg6{YyQWc$=hURZS(tc)`1kx^paHDL)6!zu1fctCD&DX zQP+SUnidfnTe?fw(%0KLw81JeQWWBCytrnga^9WMEZm*!)BdWall@JBFAp-+&k7Ub zpnRxGLseD#thZYoZyPa68+et#e+1_m0cF^2n|9v+)?Wmdg z{+l*vN#ZkJ(*YFwzbQ)T37=e|ZzDUEwULr|TP5i-!jex7J)Hiu_EGQa09)ZFq;Dr% z1Ni`F~*L_b-1;r(YOI_Yukm+X?Ab*9lIme9_x zc88-$I89>SffW4{L4dm>wRkfYn&#dHW>5jcSTvY{pW)1A>|G~H>Z=@;NnUUM-2P4x zxpe$WgXd0)#7z%7H(@0sG_LXIsK&apQ&m06Hb9 z^^RpZk2SLyfnp)!Y~xeRc;GSV@GU&&IZ_%z2fvvd??iKAQlhAFp-)Ai9uGVN3;}pHj7ROrZ>5iq)A6(FK`kKvmm(Z=J0s-pk z$Oa`T!;lG``z?-xH{Lg|t-@!~l*%Y*@G=C{Wmm=(zz+OpTEH&G~YIAfia;vH>?9+O^P~^ONPeppOZVJ zNzur}>$JVY(S}}oF8{JKBMaj#Nm5fgi&Hx^*37nuV)4+`qA?$b;>@coyOHIEXeL<9(O^4;b0Dw`QAt8kMNluDH;)NAoYf*0bZA%sVE2zL#_sLZoU8Zb?>8FH>4NDX^v zATST){1~F|u6TTyXruS`a^2qbJ&!q|r6MH2)mQZU*Lb`ZFXrU)artkls2y@cO)3^vQWXTx#7yCK>1jOXt7nt%>YT zZu*Dv>2#A=9NrI@+~-f+QoW(5rlvwwz)WjfIkfp@esL9`5?4BZZmP)bX_O}Z%!0K{ z%pQd5uzLC>L3eWB0|fa;=Ns?Xclj>J%2!_LWhURJl{!;7Q!^9|Y|QHcHhiGi!mfyM zI&M?hts9MaXN$aQoqVirY5~-Z2`@(*d3gSK;mZUs^V?QRW#AD#<8?J9K~D(OfWCf% z-0_z)Nncu85)N33Z0l=L_S!&49p z2p$kaKT-LtX}xwuhxt_^v_oa_V193$h8}=;2!yZyP3wo|crH%dlYl<7dF$btXukH( zewD=?ovE$g)-Yu9y4Uvj9siernpizPM>PHw20(|SOt9s6yO@r1{Ykn7@1Q`$3@73( zk9{}QkJ(ROig9i+53@>R+R)g_p|R5#D7L)Q>2V@dGffm*D}T1ORzGBCQ*bcT-%qXk z6QYUVtsH7(KV1qs2_lz4gDEI2eCHD;JBu$E(A$PUWPA!oVfKI^ z%X%pX{i&+b1|Tx5d_6G$xZ;dm09U*%4_|WCy{_udmBYS)K`AZ!gny7(PabYj^w}+M| z3oeM!>P_;IujpqDKU`#i%{6;KPB;sni%PWi(Ove9DIsmtzKIr`9-V6UJyO4g6ldyn zGs9~~G)dj9u(^2?s96K$=AS~vUgjE*`PyRbCdd=>TxOBB<5~hZ7l39h@vZAh?guOy zjbDOJHj}d@+Uie_RtQxtgr*>6`f>mDhLZZN<XEVgPxGKJt>R7HuQ5Y@kJE#Xx{+Y&7tCQ3>=|bks5;*8|prHP}xY zp%5xj8JUB~rUUA2R)GrfPk;9xoRlGRL=r+&+=#ew%i~e}&*!-A?pu+qo15W5$3a0y z6Z;dkceH{4ww~4v#ZTn6wyW)V2YIjvylB*3aI66dc|>qi+EeDJHYat z_c9h?%Ae?nSteu+pO=9z1w#v-KV*!Y&&pc!pb!B#(KEr6YzGtjjou60si~<<=-tiP zc6Pwk`o<(}*Q5t)!o|hK-RFL9CR+xQ`f^}AIvM$fAAm_r`V!?Q&rbIhx4ZTB8~24E zX||$AsddHdi=^7`F8qLI)Qgfqu-S=S;WFxP2rd7>x2*b}VyTf)y7UeopeNtj3lwd0 z9_NG1s(gomZj!=FA1Cj@QA{5|p-%I*cRJE(8&;Hd4m{n>xpO_H+rQF(@+b>}I!cmX zKkPJWRQc8TZ@y%8xJ=Pv z0*683LsWPJFE;Fr7PvG9-#g0%C>w&z?vg=uT5DN2 zK;gQK#+XwhsG9%_t1-i(xK@0N1uvRvvSGDz5QE9T89(xl-g|ayKXN9+-Bj`0K=t^} zu(`aObeXsNB#^6Yvk`y&IsoEGbwNXGw6Ij0!r8w_H5(rA6Boui?+}O5R6p-gkv8%~ zXh>_8sDbs0Aw6zbyR7NTc5_pMN9JWm!Kt%s!LlEH@H9 z+48x_K;Qq!1+TGIfBK~{2UxOe8QJfp{od^86xZNE{I%w&sTSWGVWu>f4~BX2Wo}y}jtS zM)q|iUW%?v{<2_~mJSJm+ym2Ok*!;F+4g z6gq4Fo{`JM_Q9-j?f!(nH^2%+07PO!6|j0sv9{H*8h%_JydsVR}7{`+fV^}ZtMJ;RaQmzw626oKZ- zuC41|x&Q_wJxrpHa2PAUZ}CSOXsLp5WYW{;rDusB@VM@WC}>v>E8bIv7(EL_HPT1h z`zZ7I^t+o~VJW2V&we3AGu+$4+=bKzEF#_4e6xtMUP#xmQpc>@{TbW7cT_C*le~kb zsVyHeY5_u6?-FjR2dK=zekb2xk#X&j#6LK6v$NYPYYzkn9LN5D^Z&4sD3yImMBA{( zoi=7bQuo@3Ul8@jB(;zjc*faH;j_n&!B=_}wrOAwPznKl zUrlzSL`xd|jCnmzb~SgR^!Wy1?K;`A;wA#z>A}PWi+H3qMCzt^8*sGSncz9}gPb|I z@}8@Ae{9)IaD2S=!-|n-rlzJ+>CqpyK}QB||PUNAjFR_|%J36}~0+ zzU88yLkQ2c)ij4IYVd1Qm@{&vroHyIdrYbS_+D?|-VG_l=uWi0-n`=+ACKAW!u@5s@?6)kz5JdM%343~0mUgxbj9ivFZ%`&v0M+$J;O7(AfPm=Dxw9;$3E*7cmz)s^ud+E6__80aZDYU?;$tQ_(1s^X6i z>j%{0gaWXcsd8i0emnk$leGz(3*k{MAuyxdSKYqPbyg}~9sdrsQil>OkWth^Yp6?qkv-rn9WGTA_wwRPIT&rc=IStlb9WC|jeY=xA~Bg{OCbg9bM z+uvU_X>DXXj&tb>XqYv5q{QY`a9)yIJw@{IWyUMd@Kk$GHHz~nuI$jTw>wNTe@^kU zz6xaQas`r1w8?O8*+V(QzPH<$(VjgjBYVtn|x#p6`K!r6Uv*TdQuem84)%5e4N5*i?DA~t4jPfi*f?C@+JeW|VW zdj6%r42OH#)jNy~dw@XF^Jf6jq#3SZ%gm1($WPw^mIQLoy3kNKM#kB!^yOrQL+_5% z@|=PC5n?9oaLQH*TLNpjN?~!Xahh!Inls4kVOv$Eb%i(nGi?Bpn@sFJNo3FB!7}FN zzMxXa#?g0^q7@%({30uT>shU;3b+{bd(J>F*W_9VT3NUE}W9n zwKvxMH1$@KNF)lrzES1bI`)oboZGp44nUWqJLVmd#N^vYCT1?Ah3*37?-VbFa85tgsIrRXu{4ayRosuG>?`5y^(_H@v@ z5Q8zb&CUGZKw~2sJD*Wi4xS1d z8#BLagE8AG7^pfl=58AQ3UrZ&DcAnypyhV+IDS ztEdSVNMXYC_3%bL#wc`wDW#N3LcUSc=M32Yz4``fS()uKGMn_Bp)l-^XT(@HtqL)F za_wR*O2K@<@7*={>Y4kaQ@W)4pX7V=l4MON*7*@WCJ5e;$ZPNcWPR8AWugX~a%0^;z~ zRX~ClrujPaPFVX7A&so>e|TA!)9LPlZukV~yg1k3oZ2_!Sp@Wln)hz)Xv%iDsS4zM z;T-v;(iqw{8~KgxE(bgYMi|M9sm=kG125ri32OtIJ1I&lFI9G4uPB|=uL%Av$Njo? zEy6h>O3wkSr+WZvft-2CV?yb11-1>vIdbw6oS{Iw&Kmwk?ODrXtzI&@IPfUZyH18# zzuZjZYU?(OBf2zgwgShb6y3}Y7rheXNpRs6u&ijBP0GR_w^=IhpS3s-HqQlGZI3(4 z2HQnSIODg~G71>puKoH#wtmbCnG;|`?bkahg?ILtid7w{7dcQTL2H;K$fF; zw%_D?s>l-ik(Dw^g)9A>fuOPa*Z*kl20nSCYkuQzMrUV|6*=Lq`^_7;#MFb3T_3!7 zgrqB3EL^OX=ov!b&MxVrUBYYq=l!$&x>Po|hY>~uF-&at+LRS#Jy%N6^t^{_tf{X zbSqHkFW`*gO48II8t-7;l;gwp4?4(W;V3CPt4#6xWS3Vg(qJWu|1{PD`?lQ!0rPKZ zZlqC;MDQ%Ch?$a(dI#0x`D|L#)=u#`$^lBwy|62W{mXE_?2 zo7+g;^8!4?srmy9r&|blmsgvTq;kwLkiomBdaB zZb}Z~*gun3`!%Aq6Y$N(GXB7Fs=$5Z2YZ4EsR-p7y{9oJW7zzj)c0F7#hW@ht&L|l z+Od_oTBefP8|$yLUdvo`&rk##Q^ff%l5Mt?Z7EgRdnOBc%8LE!MT~>=hwji(;iIgm z;l6Kgn6{srk-G*ZJmYxvMRIycbe>lky1<#1WB9(ITdi6Z#De9&R!$#oo_%)i%WpYL zPLFp_MK4pvY-;7mGb5YI8^fbzObYMH;)c5sJ>I!frrOo4Z>%bZ<3GiA++z~OOx*rq z=a?m>zbQUfkvv=jhocMItC6wc;+ea7K(tV;rN1wbVPhkCUll4rWSVpw@efQeQp^tG zdXAWZK;AgueP?%@j#;AV4rn-~4o&v?Yc&@a79)f1cQC!rr>g2=fg!b&+8eL!6bh#iu5-qd;Gj))>LRI_eX z!U$8^P^}!BZfEuuxg!S*8s^}brS#(l{lmL`a}LT>!o>8i zIdW(^y}FNgiHn=BYxnTG@f>i*KM)U0rN}=*Wa62F1)?z%_q*a*Uvp+SvxnC3klI+y zI-U)ehKDIA4C*G!ZET7OF$aLOap#ZtWBR7CI||PaTm+9m@}LqkS*1 za#Zw3CN&^hvs#SUsd3l^?8)w(`oh8sWE6!$S>NMg7Qq12ZZNu_c3cm9iGNZ5?MT>} z-ikgWf?NVCTTg5HOH!m|t^j4@mRpudbNu4}GN=Ap)Uv}YdOow#u3_jR*T}pKvqk#^ zeElxmn#x(_ZL%W?%38Za|(&Z3PqRtVf5Ghx@g{Z=;-K47GWXC;5|o;={p zbpBnLD>L*!NEUn@i5Qc-UHn{Mr~d0%Inkud$p#h!ZR@xWJqu;LhmO6Mm;FA;y(og> z`%-by_&hHLEo*N1A(So;|4}MAIXNLUH6`IT?Sj)akg7D`6t}Gu)ie|gC{w_D?$0|R zq|S05bC)^NquU!a~i3+6F?nG@Bp`rrBl%eJZ{~r#yONT~*P8(AUOXyDO>C3E z`y9^)qh zE1Mr~$zI)pE7bjjy*&?frb*9zQtkZ&DR0q|1UY}Vii)QJsp6dm=XA|#F&!Smzx!U~ zMiDeb8p`FI?;yi!OcucP@>ZO32ZWe!S=%>qva3xK^8X&u{b#j*_a-xTZY6!s*Oj(= z{`v3d3l(|~_(R)_E=KBxt?5M+yHse6f^GkMcDG z>V8^Iv4i9t_zEBEt10Z8bkun6ha$-Ij8Rzgo&`DG!)KwWYqN9y>qV(Dv>N5VFK3~k z;0p+8e0*+l_Q@!Cm?VpAFd_QmBC03i3u5?lcY+M5`r*CL!ld5j5Ua`?3ORqBdu6EA z@bKxy@m;{0Rg!!X3%gy{OQNnGF4X5$ZkYCMWwEIWarBP>+LKwon;(qFsecxc>6~MD zoipi0Gd_R**|?9>dMD3eZ#_zNzx;JJW7@S8k+&!0ed)P_2ua|^!?t+~gzrXhwFbz9 z9UBFVZ_*7#uT>*V)w9XG3uTN?++I&K<>7%WO%xX{TIQMf@m;a2y6mlt!ClO^8;+{N z=Ys)OytdX{pZTUSij{IXyl*`wO69VzFX~ zzHNMI>6v>kI29>sYoPtzcF+Zx{tyO!En_^BZIR|u{pC8XxLh~LF%*jpUwJF#0KIbV z8TM0W(yy`ou)N9HyiX!EIq{T%`ySD+L`2V1{4#`C5@0#WtxDvkth(*DqRw<| zB;MLbHC~eb`WX8p`4wV7?p@6}#_PRo`K?=cws<_m;A{NAUPCi!iV4qaVfHicg&B5T{AHL`6#uX zoxi*JqJ!V-!WX?IpkYqm#4VajzI45Iof;^wK zMUu_AoLx!~Od$v-zJ(G$zsoooOqgp>I(f4VKE_HoC(yK@Iz2`d@TeT{CR>1-f`M^x zaHx8(t`@?x&B(*$8`t=J4>A1N#4OGcl$*m$+o~{)lT&}2nm1bm{FJ;#J$A@7+tPf% z_v39$x9~rJCV5Nl4?8N)SXal{Tpb2n=FG@ff>C$aGfj+Vz3D%hm$kC@m>7#bIrt#? zOP-$(0NqaX)Z|9x5_DgvBs`6&mhnOzeg?YOx!Pu(yk>h>e=|+NWul#od^GwpZ|aR& z^G-nJxzAxhL0b4!Fyqy$npdxm8!Z*w4tEm_Ac3Rdgk#EJH2JFbG^|_=Y9Fdusi5dI zTy6!-a9X3PBv!Id1}y#Ok}Qv`_4GP6H}5igczPZWq#X~a9$+kQ^8Ka>%C9of_M@AJ zP5uKYdE4tj&R4@oejtTI@j^xaVbPc3`}|YFfy7$yQII50eT3jY-5{Vxl9#mmbf#mw znjMaIC6!NxiTP(46TwR+ANjzDpXFmr(PQDb)|!}Zr;j+JBMJHlNkA+U%F4Aoyoasc z=!Dkk%J606%njT?Zv$1!ap&rb(fL$Raqd*ZJEAOAJRRH>j>@ww_L_5yQ_GL{tKVL# zR#8?q$L^=3q-aKR`2cC!$XcWq?JWE2)(u);*>xyZG7`9rC@L++_l(2*c^NHi7fCw+D7tUZ5$BLk79-{&o3W(r&q9N z`wavfG`}u2udFC72HSZujrBv{U|%4>HH5N{P*wh;U0pZMm*SRE4?2JU&L7Lj28HDPzjI=&Y)(-Z;Nf zwSaRDSfGH;gU^ct(~;5bpZt;hzJ7l8_H@&3Intwn)N?0M{Fx1Jl)(l0UqKqD$VFOC zG{|3tFsa$*^%U-9qw3+>uSPp#D+ zpM-CNR`D(3I}6^P>D7cx7IUnHS03Bx@iQ8uzjhsM{RwVMiMX$)&op^&&r%_cxOuww2172d|K#GszUicb}^} zMqJDrBMl~!8-~9|3w>BnNJjD5p;hD+K!+T_(EJ9N`zjQHl~6=q|8ge~kjqnR54CHR zKxRL}t~gr`)IMzloR&^Q8V#-NV!P4wvDHA_I6_ehsN$H+5JY*hN#ljSn-N75&orSn z@ed)SCHk)~7z=N+qDV1C;zGn-z>C(JdzGr&$PAFbRMopmQV}B9einKqlggf;?o?j+%<;LFI^U*?q>3d2$$EInZlv~utS&eyRpbfMXu1(0d1hv#uY zd+D+T=Dme-)bbJr9oE!oa?QDArB$L{xTqQE@(t#EoA^NLVTpj)zO_uWnf>l z|N9lod)Ex$&b(t@iirx-5Tp~oe(hR<*`beG$8{xagdJM>XcMAqA2b?DXi(t;Z%P(T zF44&vsb^GS3VFys`M)nzg{CZk(`)YKz-SftpXjo#HD+zg1CUm*Xs&&?Vt_?b_l?qqiL_UvRZL)b4 zc2)TTO?Hq$qmjP2s@Vws*z}VBbFHiD5I(87I40LL`?$peTXdLr5o|EamZmm$<;tb# zJ_44e*67hH_|fR`>a-Zjff`x!vU(T^4Y>(k!_MUYSO;Be>mb#7^18bT!DZGy-9Bqz z#uEAVY_WHTgx)rz={+XFEgNvoqEhe1SXAatg8i=Kw|duHm+g80=hx`f_isbP&OfBJ z^>S%BrxO~9zEYYGOWtnb-e4RbO86m8Xc_~wex(JgCRsYwuhCsNC=;@L2t%P5Tpd`)}SF5+q)%(RP;ZB)=chbS?Ph9_V+19alB=CiN$B){`L#Sqml+7BgC%!!kX(CUy@QewY zEEx}f;DyMkLE~|Dv~4F5N?Vd!;1Iu&ssZ@OFf&{#v^0;3+QrXbxP%Xe*B9ri!S zwFJ?@zmhN35-O7X?-3`qv)OLO56EGU?5g?`;`RKYT z#AoXFth+q>(_Y{9AK_?fMI=jYxw&R_Rcr0We?e5}7gF7cXi%YTsBT{cl&Z&uv|5<^ zrcQ+wv;NWYaD`1`Oi<3%e;L>=G0Wp9yN+75o$PSC_tQQ6nZS^XH|(2qybj^m0?35K zPk*9`9<6Cs&0jbobmJo=p`qW5b|X@x_laKmRbvCVk28@}=8t`YpHcYQqBG;9ZGTPE zJQ+K})5OoIfVO5MEGc_ySvNqs@%ir%T8&IcTbpPp!I!&_Snm(K4^PoQ>p3CaF~}|z zDoK3wn?^ce&T_>8am*=Kf1YX;|^?3siu92_s~NcXe(FHG^R2qy#*iDe^G$=%6fIvCl(9@$II6fZZR?c z31Kh|JbM9KyZ&4(!OuON7cy``$y8VbYl<4^i_4!3%Sj04hqAu*Q8-V&5<(4(TT&YC zON0>J38h!WV$}-sOX6Vimp;xwXGA$;-DUo7TK+!v=R3zp>bK@<-^!OF{d|1&qAjo} zu>Perox7kAImY+!=fg{a#LwJbzvhK5WRHB1BtG>+AWZMrqeY(NLf-I6Hpr7TWa~%9 zpb8giy zm6t60{1!C%zH|Lac{v|*g#V(9y?hj-$;l?zXepQ(b?t?%oNQ=V1I1sFvZ3m8e5C_r zBjw8zGYj88MXnJcEeeA}H%g zjx#Agw7DL*A8M~RRNdc5`&9QotTJ&py1j~3s3Y`N;LEfWJA$e|y_uj*SUL!s|5GNxgpVgH20q$ViX|vKI zy-`H@SKOhlI3i{ZDHZw>NpEk6*x;I5o}Zm}RQ2(FVdLoQ;Lz7vCRU-az88n4`dAn; z;?9g4-g_znS9eo)ktUW*qXLqrk9H{K+6=WYIbf;27{3VHOEu^`wc2C;8?r= z=Qybf%?>}MYz_@=(mj=0N#~VnjDbjiH)70}!>>|iLHJT9Vjr~Omi4tj&={h0N?^2=N{tC$ECJWc%gL1I$wMNT za6!=vu3Nn!QPn0(<;wV=B=0-k4ktVJgDiu{;oCd`$B5ds%Hf^k*F9+0gLX;cP*&h- zJmZKetjdm=5ooQR>;KtQ1#;&|l?yxh%!O9WpyT(unom8H9^uV@usEAXij*O{rcz34 z1)`G@o!Y&fyuAEsq~qHGOSZ(O+DyAI?%xH~ysH5bl0^1K=1BgQ*xUfMau`FsL_C65 z)%PKL3xQz7=N9t8Hr936QHCGWO?-jm;&s${x{zn{t^3D_rT)C%rPj-nP$+uDoqub4 z+yC(7*oS=BYdSIyk$H4;kx>BMCHn2S+0w#z9=d<#uYc`m{B+=Q5Y!*o(=nFueMK#47y3M(l3nUGf1-8$pE~7Lpr8FqJc@Z)i2=iO9 zJ$l=(CCXEr+jVqxL7xZFd|CgQCy~94d$#SQ9&e%OC3Rg4q z@2EF!koIj9+rz@ZREBaKV#Q0@{wT5LHs@IqCy3wRhps^lON_fIVnO~=haFt?eEs9g z`gh_PFz(!qLT)9Oes;c(s=FGpWIq^c%I3DO-STR(gG+9^EwOG$&kHdlhyE-%)7+RB zbSPhp@(NunvWKI)OwOo!4;|F=9QsiQ`!p@q{8DnbO)Jz8e8{cE z5wfnP+sPO_MMhK0ag!C13vyZ_yr-EjknoxERL}c&srYE~mYV(kH2VCxenLuoTi%|| zaZ*9^mEatE&3euQC2YNe>%Q}`)^Tl6_f>3|43Ygx3|&<9@4e=NqoMHLgp;+PVV9NK zgZ*a51I3DsW(h0sq6xmDZGo@*#wEbK`~BHV1EDP@3+1mPN!`+-zKKjfKbMO;%<{K8 zitHxpER@n)IBrU7iD62B8M#gQM8{R2Iw_Jl`!?O(T!{QB7AArT`zDYikb$PUk(R<| z1r$wp^HdL`t7UQXvnt*_idtt*e$*cnD<`CMD_UzI$R#!Q72l4R_d0-aBgId3Rkn6D zN?MW>#+ezDe|)^68yFPiK~cVK(qf&y+jBCtqouBC1WZs(%Ga`0Hb>YP8JPc^b$2h& z;-A%1n}H-h*EALX^;!*Sw`1O8ig-@Dlfn|$l#;xAJ_Wmg zgUH+BPCT;N?qUN>a8d+Ylt3>sftJ4%eQ)->m7c>p){r!n^#;nywd0o3LwI9jA9k*K zdXSr3FhrDBR0wpiU2(TR2uwGMq7VPkJanR68n|4myd88{4-YVviOJGX*?cu%tXpze zKU%XCTJ7vRFXXUZC%(EqH9ykp=D)jgObu-8RNb>zRfqCNzX37eJ$nj}Z5V0wVtLiv zptP)6PeDvk^xJFJd7x5n|C}}0LY-v}ug`^bW!~eG;^aflGjK55DrMhJQ`Fpy;8cp1 z)Q`%0(OwKoH?$HKdjfbSY!_`+&mVSrLAHW?B>6JX3(OyR&hGVbF_)*RL#=M0$`^mP zIo97_t^c>h@Q(4jZ<)%@dcc>mWBRM1r&6I&D1~P6 zyMN3@-!|ul7sMZD7_7{$_MLlTwD*2Mr@ZS`Jj!af#o47R(Y7|wa_l>qCuO_DbGzy# z1@G)+TJBrrx2>4{-ijQ4C2jviF5WF>U!?LA}7N+fDp0q72{Xixhi zNH`b5;+L_6fs6(D2AKS$Z{X8BMoOuhvQT`xLL`%#H%?5B3WT6sLKi8KpMwn7ZMSxt zYuSg~cQ+ST*-m~a-resb;Bc1JD-|69I}b3I{Wq5(UZd+D#e+`tMm%PJC5qQB`f?Qd zQGxzp-ub08@r6ihU^xb}wf3_u^<>*pcSTMnFrR9?ahAs;)0DE$ztq-=)=uX0d?Ua;Qj}P!{S|-tM}8%L2#JvF0ge_f_OpA>YZ`qid%d%W>nB(P|Pm;U4n{-xWHXeV6 z(cG2;QQo5!@ruZLR7xKGq*kuwK2_Yjr*vaAXm{~k>eA{EJxefGykvK}jSV`+M5{i5 z-wZo7i(ed1t3KX28JJ9MQUGRXxITrLbeP+;%t?>skhGf{_4d(;%}71%zWZ<9g}DFG zX@`Sx5cZE$mHX#AV z6snaA$*kv;$seu=K(RM{)kTcwLr1ArPD`(Y z+w+I?1X;WH0%TV;5z`_L-n-2B$DwrBLwErkPm<4;yU;UDU!M%AYyWqnM{(43_nk9V z42*J8n=;D3l+~D}%RN+m9EYPG$Q(Caxt+|O8Fy>p+5Ld(Y@Q%%Bq272kNDNnW6+d6 z;YmSA^$FtX++(S9Oq}RVK4grcRjJp~tDbD9!QZ;EdyJ>@GXDnicxW9`7wJk5YAB!v ze2?~vkW7YP8SZ0~@RGkKpUH3Uv3;7k#>WG{MX%ntr9^HoC78K`)dwv6i(fbM?iHFP zq>6;zdzx!V*PwQin0lM+_&s*SOeU$-_6!@LB&ppdtO;eFbCX&A=Q!vtJa9?skcKB0 z-@L%`@$pa%;IhSp7qMzRoUVX<=jKm#t#&IP>`L|G(!Ujt?B3=wo|A~q;q>+2bNC5a z_Db^~{}Ea9Bjh!rbXsLik$ZICje_b>Z7bW|3qsn=a2!!P@aHuV4d|KF#+86HNSW1r zzr4M!o-Av07t}H1E+zIQuZFX{x7_5E|Ff&aX*41ebBP`P#M1`es(M{SNd{z%#J&TP ztJ0sGafJq7l{o-1X+2i)T(kqS>eoQONKLb2rs2LVgDi>H{4{QVyv08WIQlJB;A@|e zxC4;m#-gTM4BFEzo9?{lZ85Ohaqkg;L2c1HqoAuT6FsHJHxXPpC8x&34Ezb>wyMg{ zwXw5+UORu8U7N}?w`r#4$G(xAO%mtTz|^e~MPk41N%*!fd@i6X4;no{-eBrzm+ zZVb6$KDFEYV@hY&b|*WsXeU^3oD0dZD7-e0C!&D|+B1utp`hyVsK`9kxpmy$KG}lz zp?JK}$hGTRO%kN`#r(aN(dK2+YUhGQj&;m$EfMTpA$WgyHR#cYyZrStkQxCG1ey5pq@_}Q8pZxh}if^b+D=a z0BbH+2CK_g0ONBn?iL)F_d~%-BdtoUU$1XnMZkAq-QMo1GW5q(vd;-X6Ucl1?onK#I8sz}zEd z^QIpoH!q;zl?p8Ox6c1`6r7aDbyvW!m-VxkzD_G(VDdiL_WFBF-l@h}4(u(FgsRE& zl?DCFOhzX5QrGR#tOIbBQ%M10t}e^Kzw=wq=b#l5=yG z5WD`+(g0DPuSok#j?v$QHv=Zv-Ks@8I$KN8lp0m?rwzo zhs~g8_W@`IZ?P#<44OwFnL-GNl7L8^pUEiA)icdMES0x-8_RlMJ9GCR&(rC3bNijF z&CS1;wT!tl|OcexfB)vWP*p$cFhwL@rS!`XZh{?ojXTF=w__P|ubEw$p z`Zn_VIuNy+<$(bku}r~|I6Mpx)c-;VU|hIJ<&s}xrp)@cwzWbnvTS`FeoOfFaPQDi zoFDSEAxSQ=-@D?&Oo|iywPk7)bAAnOYBx{VUd3ajnkQcHRSNWvGk0b4QQb~`E)T0%!**of@v;RaT{OP+@<(b}h2j+5!!O$rI%3^CqFT1{b6h( z|5IAyTwrfkstXt2#eYUUl4r8oh)OvD)!EX+qk=@4N)kY^HL6M zOPSn7e8KHe0n>VuP9OZ;Y%SaV;uA3Dq?7Szo7A`bB*3$-IK|e6O-WI$U%DgyGcPsV z#D=3@o($f};si8o(CIAUoX@h?gH)!_?(06nwxCq2HrRXR$=%_BmmD&K-Vea(_)$`o zWmNF%c8~MO6B- zp&x3`8s8T7RptzF-+95*YeoCRX4 zzMd$5M1ZrNZ9P8LP4c`y5E1;n!2y31nhxt`*MX+fKMW;8pjjr^G85Zx4eXP~Eng$n z%rx>%L`tXDw~LwJ$Xgql{tS<`1-T9=Et*w4kZf zRM}Goxq_h2ss;rW?`Wcd7Uc}3c~?O8`rzi-p^BL_rQ^Yq!13O-mI%FVZ2yth$wuC} znsI!Iosq9t%bV4WH1EyY!=IF`w7^l=K#w4c0vy@;a_>MMa)4g-qwFDw>KK=iXzw z+KMSGEJP5T+=gF#E1;GW>We28f+cxRn?HS>_uf_y$_X#I6wAj~`FEKa?y`QlN%e>I zm4SZtHqf?L@|-V(|9B7xl?p1>fL=oD@S!T00aDNDWeDNz9pqg6iCo#S=+ z+WD0LN?X7vC&_i0X@i}LHZgbpW^$p~JCY)jd~8N(9?0PksLxjUHw3fq1q^MPN z;+_FBg!UcF&nYc-sK`lSj9pga$e{s`1;o2OR$deIXIlQar09avk3YjFv%vNdtE0}) zl2!xe0pKc_ov$nZ4)F_nS_McA9$Z*g2&^q z`QwUzz~Of3QKbhtfq*NdM(SYKcO08WgrAgS{mdCSkj{ekK8L%LlPV_Lf)8)DSNXH4>JH*^m!FD(N z!+~X_M$||`N=py}d+IXTt|Z{t_VbRcAg4TX+{=law`o1M**Rui4ih#ul}d<I z+p)Ck8vJX_w3dfM#p7FXii#UOJqD(w`IcS7lFAS`bkiw?8@c^liHy&5XH(jT<*sr9 zL)M>AfT4-R-afRj-q$IM1U5_1uO)8HynIM{)mE=5HFwH7xFuoynpwaN4u}{_`@Cd@P zLLpdBZjC%&s2~y6c}{x9E&V&fSBXaj$4hz|Wc@ECLrJDP*9_A0N&c9g85vrA4!=;TV_`BN~oQ!aDj=0Zd2fZTEgV(5P^4CY> zfZCtzc`@%tgEn7*P$m~2WchD7mv{c7FnXJ6&$n(=`;A4K07buhTFqOp^`s7ahd-8pj`(ocjuCsC- zIrKPxdSz-_l&*l7xb*XbWMB8E0Xez<0SKPC7^5_Pp0ciW9*1@f`Jx>bM$L-37~4G< z=kLK366^3W%~Jd_u2JhxMVuPE<&yZ*K`WvI$yE4PL7gkk$^+oLGGNXB+uJp#ew zxTWB*yY?{tBA3ox}DGi0f=K9RuyKr%e7-MI(S_oLIyvUWwWK`DVQr&=KA3L00yTYP@`}-%A zr!gkSTms`|;&Ht7VZCt{5I&;tWVw~_v#Npq?Jsudl5|pBenAX&?gA0r=l+t@mb2iM zB&&1FR{aM@z<{qDCT~xyE0sRYz17s{1Kj7zQrX{b!Kyf7#~)Q;a6Q0Zy^&;E zv)Ynw&vQhnZO1W=Zq?}csg5lk#O)D{EtZCg+$JkxAq5QR=|VN9WBMc(%T1iM zwj#3P`se=n>j%u8qY*eC{NViVhZggUmB?2Eqc&T7ZRx^8$Z6l$95O+AbS^++ms>7>KQT+0cJ%nS=9QB?Wy{J!0AKOG94|u$@U?soj3% zS88P6{(4loWMlAE4sSIM-hr`vOJ@@nLvN|P3d@nJ4y+IT*tLeHdX$vX@v>(o3_Jr~ z?4Tg)Qw!@+uTSA#YOt0kcV|Q}N4kWZ50;2-ex%a@rk>&ZLV7oBdb%5?GC;2uatxa09g91kTe&>S88fSCtgecsqy{`^CE0 z?gTXX(^l{e7cMLQ{Fv}Cv}Dk6yd^*hPQLXGdZgS4)A)Y&-Wc$BX`8XgD6jJzQCzB? z!Q_$QL}9<@(xV4=UlBsJ7rFou2z66)&u$`_kPXJG+m5p4bAvsNz@&7b7< zTIX16g&is}FWF8Gs;NxHByqNLtDK&lu9sbU{)lWP; zyq*{t8{LULxV+#c-+Bc4lNuTt%gi4O{YS*Vkp!eBk`nM!La9{X`L;58X`!1~wflr$ z@36-spt?D2uboY0yRjoG+1+w&cyuH8w?Sjj#_}OQoElG^qDlcrk-L5@X$DaZwL?8g z&F%Inr=qz1SC>Rk(3<_@9guD|1=N#Y-#O=>iz-G zacXHTAg4#tEmagki?TV=z%2WcKOX!?rH#ck)y+_6DScrQ>Qi!YdR-vdBlCe2F<65B z5M*IY4F9h2_5EzPCS&zDgEQE2@#331Su{HW`z1G$1H(t0YiGQqk%EXB;J(2dJ{2w) z9h=sp!OUWe;n1C@6aFjry0R>R*{4SAj1rrF?btcoB&+}ENPeVHV9xJ7E)A)KFijZI zI+a=hsx3iM!w3w6g;S)~6KYn1R1Vg9#C-y{-BzP4E0+?1+Ij46258I+mqv)Ja7BmJt*Z~soAm79#*Cdk{`O1B{ib z8*$DSPCgx6Y;2|aaJ1x z(hqLI0SlLu4gbm?G6{OvQVC`^1tjB|XV!gGnRiAGXJn^~%FFjKdwZCXkr;*_AV-cj zxav%oRoZe{JFP9^Bzup#8lvpcJ~-1rHux`&N(Bhdd!KFk?_DM<$kQcQD+jpy+-$+^ zL%Sk*zE7~wkNM7<*s1c!n4(gl`%o)ZpDCi5m*VS!mXvAF`_Ve-p3hv@&Jar^tyo`T zqb>4fnNRXgx;Ktax`Q@(_GgS|;spz`CT5y;EHuTH*MI(0Yss#A&3L-mWp<%sWaAuN z*5lpHT7Avyx`z`SfQetDQSllQY2_RcnUdKa+MCUsRTV zh7f(=+I6G8oBHa47f)e&e{|N_-_Z?PRX^E@I+{JW#LY#}v+5T;_8_O0$@%UX2OZV_ zSroPzyQ?#HRZo+{4*w5NZynZj`-lDC_iccHgw&7{P*Q<`;%J2lI8bC#A|Z^FX3`~H z10+UBNr{MnbPps(4?(572GTVeo{R7A_dI`i9R495$3CBNUGF&0^QDj;TEw&s-U->% zadFs^y8O{pS6_=L{<(qcbvhO?Qw+5Hd-b=9!#$&O-?NFvb9rp|QHb)*{cHdI^JwLw zKp5uqj0SGU@OGP{JKR7;&rKDX=DgZwf&sE_V+f;{d`fQ~GVF&R&b5AyJ(xePGp&}C zG%^1AsenPuld8%5oXDBaO8-`S(I!C?L@u|jTPR^d9T(k;;$uPN>YAg97~KfLBKrZ| z6M((PB0=y-R9X2zQ;hP$3)>KR&-A=_q#l{Zyi}BN4}NqnWz?LzdWI_czMD}NXG^TbzQ6KUq%xjQs>(+5j$cxK zX|e4yUHg1|ryQE|H1z7#>AH=Y?h_IACw{SF;aOYVi8u0p_nnQM9~5yO7)nzExvt{J z623k;W%!}Ki?JPVM&Drl!^RJddL8ur`wc7Y@`okS+NZpxq@Ol#vr;zuUG{8)gyxF9tI!Z8CSr);6I2^6~b}0 z%IpWPsB_5bfAfZB;U+b;jw&7()SrIw8n1GqzfqVHP+`1Zn&$VUCn~_un-!S1-DH@s zE<1KEd}D7~aweP^p616>xBT3{#NEYsVlcHv!lE3c(zv*PNRH5VAXo}>2Ns|vq#zZ+ z!eJ1Pj8WDhlXqeX0~Clis_~!tWHc+4583L&_epeuda3;(m>V{=f%vpaJ2( z*V44X4%@^QXPh=gpZ`weIC7HE`G9aD*f%Wio{aY#3@<4ySJ}MTY?}VL>3de(SVii^ zKOcxio*fmR9x-e1$ z@g(NA$FudwoN`Cm6cKn#xdk@hRuA*ZEpkeq7n_&H+6v>DQiXD(s3y2EzaFmc?zJyw zl4oML^u!UIa2_PPHXeuI&i0C@yngL%oM*aDzMH#_^BpsQ2culB9PEkj zkF)1R(`qz0+25Jk?~bs@)x+Ae2}?^RXpgZCPen6XzMk6migv&I$i zC6}12-^()s3Envs60Ki9zbSEIGKOPj9g9z>@eovxV-OKG8U+(h^O&GC=(1T530nDW z+q;nvZlpH!O7H1d=FcdeER|6oCqj*FICTY6YNExvk#BZHzmemxc3A-c9K=n-xNjp7 z*SL$8W^<@ql|pfJaQnPz>;}2pm9I{6bI(sQCf)sxRj-cb_hsA z_DH{;TyQNDb`!xWV8-yIvSq|c763PuZeuA<{>T}(JAR1|EU@_y457Q1TZW(~Vi*;_ zLxanqde*rN(8ZdfHRV5g%E&8^zhGs=#FD5LxGqoaT)qo3@%5AuS03|YOADg4#YXVG zxUZc%`qJc5bkTG%2AFwGhCd*PWaZOtPQXS;jRG_`#MrbQ;aRjI_58LOH%CNU{Q_2v znn|5Z|l&6_a^vV#2KEQep(deI_5HsHS+RiTX*YDsi6`z#8g3Srq1{1EvRfS5f zlg_&5;_`YHB7}hZtg{B&GxK9Id3PI2Btw#!+i~C$pZRY1{qW%~{df3ziG1-P@<9L3`dvwpF*%3xmfR(s4JBAl-H#^oCQ1vD;tGbowcEuSTZYwIKz{Sj1 z>8_H6M03hJ%`LB74GA6)>QW#+}=L$l-5Gdi904pmhkoG~<%|9Qv^}W2mi!eUbBIj1>p?2XOWACk5f#IX( z4343xj$H6@y0bg8ycyqbBixv%Jc1L&*vwOB1M$J@ zwu-gID4sxC!MlT=wsGjg>@E?vbXFw|$dg|>7Sz`{s}J!nQF*WmW^Zm)Hw{Qd~#(D?7f zozoF3-@ifmLu_>mCriJ(sf+DE3Z$}w9iRQ5T;9eodzm9$?gT0Cqur&YFN5J|f{DIB zZhUx_D3dBJ%Jec+Y*dA1q160wKC8Vqosfi;HMf&&F>Q&x{HgySvm|Qtw%TLLcC5xO zcn9Ps{@PuUZ4`&8@y654VnUQkEBoh5$CH}_tm*X*z@W=i1v3Da8TM-zA~3>7dhc$h z#}ITXEapP(*6tXZH1Q8xggYDq`i0poHwHkV0&$WdeO|Rye0%tZ4H|-sS7V4207eTt zcP}`3i5iPpAs?cfdxwi zP#Ye{cr1M0-7Ze`$3jSfO!%sHs#@i;>#0_xQL@{g?hW>(HAlNfGj{13&^YrCgI zK`x`O-*h_FbC*?Ty{2ZP*Oeq|SUhh{8)cu zVvgD8ji`k0gY|0HagX8gw`_#-c^(U+BfKJ554xsFyYAsH$=hYSb*=w&V&fjblXJiJ zvK?n8ju3?neJA!56R-vM~Y9^`=!>F(p z<}gH|GGd`hV|kZ4T;^EWgMA>E{W6%Fw{|3_`mXq-i_b8Z%9G;c&v@jIe7HSYR6WBi z&7nK$iq};bYuad=^YqfMe$oYfJvw z3b4%~Xx_kSQyWHCkqjS4Y%FiDdyg|uE@(lCwR+b5BL27TPC4yB^a-6SM#EQLUtfQE zr>v&_^v~p&(th&hxU_wpK&TxK=CJ^BZN2(b*E`K9FDZJDcxekbZ%%(l2Iw$p`)smT zK?zDI%I?Fq6c0lOLnp_x?RI6~Y*Fu_4l6l_ZNkHegY=9I&+gpTrskGx-v>v{=%Mk{ zo6Bn{y1FM_#JtH)2N$YL>2QwZ>)ck+t@N(s=`T5nU)iR1b2 zU}IAqS>NZz`(;FDiN?I6tWkT>(HUuU5L`*-#Kjhq_loW=`#Dq6DfoR$)vOI7e zS#q1S#>eWl2hBljO(C?XW=G6DG9ktwACR}Z(ZBaTEc;i&O8$W_noosjN9b-yAN#czKj7Xp4r zI8k3GsrTNqbqwnfR+@$19q`y#m>Sp<1v5AplmOA``m7eM&*hd`PRehr3JG0R%ip$n z>xl#-URi2c)$0P2X$WRTWuQjVzWq?u+&rhx?|w zZ?CkpY#f}`t$WNTH#P_oaBH4+26ylFEhHE^t>o{h-!^jPBxd}&-F6>lp=SN7 zn(ilj_xDSS$bJpTO|i1FG7HZ0ubM|4F)<*c!uOoh$N23wf;aAX*Wzsdkf@X`DvEdD zg3XC|^xH;zA3v?rj|k1;7gdrjYKQch|B9NUXrEEA%*_%F&1&i+h7ONSof#B+PNbZl z^qtT8p7qhhb=tbyUWnQzE;1h9B`(=6$>aVQve~@WdFRO_I^s zlehoi^Qz7By4JjLp>rgQ-nZT9wi8ta2ea~lTHNCZEEA77L6#6XI0{9qz-2ziyBP3= z=dfqq#%2w8Ey7dBlC?p*TUxyra4jm9MkN2}|n zHAp>paNnzmre*SMlH+f$sTe6i3|Z5bDZ|N}+W7a(I)gLCAl`0T$wENNZ4HGmN3TXO z3zL{iT^o<0e3z-e1)fIp6B{Gt&w~>U_Hh;#%7o~%HG~||JU!ZntSBaPfai(+z?GYW>u9HP;^vrH=wE}E(X}@vbKuBmgaq@Up zH*3GZ=#&Vu`OlLH02#2|yx+LvS>+pZ@$lKHE*`$c&8pv3QG=C=?KbD=YeIS2eP|&v zcp{dv4BL|Na3s)FzC)Oz$l1U8@1-PYe3`+|JwW2|cdTO#LqBe1;o;&gx=aKm%7c0r z-SX+wP))0?Xraq=S2S8BtHP?L$p8;qLqj_uL}I9a{A3m+OvEYEkhz~P?=WBI>vN`| zqS3+h@``Yv^0>ezG|ofx*)RiQ)#ltb<|#MtyX&RM z{HgTgiSGNQYvM{o^s?sEhcqoa;2;z3=`+H)oq@puDbJTg4AMVpY1|K)+8f-cU&dQA zCv4jvuWxX$t7+v(lNM z{sJS<`QrAx&qrh-JiTTRYHH5Owm;lJ*!7O%qGw)v8?p3S+b)FUJ>?wnD|4+5GM6h= z=*Y$ehDU&#uv%%z1@9%8q9ls1i*bjK(6>Dx@za%^Tfi|e3(5$g7P6~SE-Do@qgL4@dH!H`j0wYuM{ZEvK%>iM6`@X_o4&FJ(zpG=iL8_RqJRzqlj8yMd)QGhKV zH%C7&nXDUR_}F$1oxQcnmZaCJ(p&S9a1qr>t6KFs`w`N(Io(295Rp09+{=58J993s zKl}Y4cjcjigCTUX6OvuQ#jWgsPZA7jG9jzrMTtc3@W)2!rtug|?WTmcd%ofCMTYkyPYv=t`!9pQzEwujSV|URxI|FOUf|BEm`7H`}Q!KJRvWm-uO}QQk;us zyry;vrgQb&?LUi|VU|Cit~#6l<9gCmGN&%Ly_#pYcK*X6{bSwQJk8R;LyY}qnr$jE zy}^>Ilix{S=jl^Zuc?gW+Pi`DK^Fq9?CWu2M}t)y&q%wC8zH{az9*9zV)T;DvThEr zM4`*ZZ_067yZI$s`2l1!XDtWB>N)eWWcjl=dG8I+$wTYLv)5tpzLGgUFZtpML&J~K zqE0#Etc}Vc!5(23&h<})7v+o1=La-L>m7(68|tmu6>C2=80hE#&y4b6UfXeCe{d+1)3QqUeAt4z+-c@U z-LX#5T{-u2Tl+?6#kY8q)Wk)D2Nfq^0lSKDOrdkk&4Dspy^{UFHI<*D0;vEKqcCGw z*cpL;b@j1&(q-vGdVAjFXKH`mJg~hz>jnPK%xg|$W-7)+?_a07_H20<0qN|MJNRSc zF|u>xP=RZgQm-@ju)zCLi{0JFN<&*^U(}k_4XY9Gi<5cnzl-|3k~$P;MpAJwlP+J8 z5)xwJs-A;wxz(rA8VqTC#oK&!KJBwvqioy$xDu-Jca~KGCf*YYLxB^*+gsN2Fh`KG zab*PJb>6-1Gd4C^e|lVwHO>X(T!DL73jY@f#;32Qw(X77-@1K2Pf<^G*57D)^W>`e zZAGj~D;rmm!-WfiP(4SEx6x-z-AQUU^4M$-TPi)TSzbjo!)k7^F8?&{(NOl1$d5i%5I>byUqoN8$WhQY zq}b-^Eq=H$KbDH(5MD|oNfYkRmgP&`R=ap=_x@)w(&Gca>-qq%!M858;IedfhI6hm zDbbZl>BXBsJ*g`X24@c^w!2ptr@lt?R;qMxnb(# z;$dtzx-dh!^nES1qERX4Hn(!9WBW>m7AE|Pmw!Z8eckNqK z2)&-3WEsoH>FZ?dqXa9`))O$pgn`ml4Br9f6`}-1*7w;ePm}u=Y(%vYSA+a9+=Ugf z1N%E-6p{U6;mx98=6EO9s3XMq6OaRzmgtIY?!e=pib!O(v(5qN=;(!Wx#?>Z2506M z?-viAif+%G*R%KKir+!EZFW=TObN=OkvvN@a&Tu(Xx5_I?yYFA-_BV9>qnA3R zH;KL^os5IlJY{P3wPgd&=`aMyG4xs7ruJJ>dXPbup!Y90BS>H<-^}~-49s{;dhdEdXO*A=|uzs~$OJv^4am^|f3Zr3v{zAB{v>v7ikD=Ba(2OcrQN=x&v^2V&yDOt3 zh@xzI@U@9iImlHb{`e$-#;Eo_>35t3<>C0jaGRTqq|=h6*dLVoT{PErjg4F)n6p~% z4q*FLb#Op)qqS3_Ek8Cu6vl?u@KfT*Pse4)Q^D3zQs5p|+()Cj_CS-JUdfNpXCpb+xRgNCn>eGl4Th-bi`a>x+?9|8-oOi{O_ZBwYmj5kW5_oW*h4yHR!nzL4_ z9~z)fR!1yoIOR(o6n@YVrQw+;k z`-+`CMY-(yYxlvSNh~y2gODf7OnE_Vn5BX0@1w2d-@!*=B`2Rg*Z z8h>ydPYUN2<7HBXMg5W&y_@^3n>wdK5s2(9uKo%w0=0^*l-py{Yjp7#?kSws{+!lV0vmpxfI@9L)l0$I%mW$}~@Noq`k~ zfr^(-0nQ2Rg9;(Um9f+{jzt8TGJ-55B#>5uf;HC;xBjtygOc!T#9Kw*rMDF13FRPj zv%BH&{f2$ni$CPK1JL* zk_gBjzO0Y`QcBStYKsdd0C4?rzH28IU#+PsU6-T?({Ik@eUI}KC(xd} zM%7}H^y>H^+Tbt6d3+q`mKQZqy1IH$C2_?N8us5^4ke2(#qnx=_k`~ikGma7wbPj{ zO>h2N*ZInd-6E^F(f6H+;9@NwHK*4}e%Z71_hyaDl?5`fv7s;_{DI$?9jnLKzB#x1 z31KYy9Dgo-IR7h7J&oPxX!rLoqDjfY!2*#!-;m`AQVEwaq!*K^@&uuDH{m|mc@6H| zKy>m`tX>jexf9zU>mA^IN`~q0R;2{C*OEq_l*3o{FBioAmx^P_1lp)2ics9MmV@A# zGTpK}N;YM(MuX|I7Rs-2o|4pUPE;kY@U>WGK%a2wH*zJ2|#epIYs*$izs z{xhe-MPJ!^&GIo+yklR0~%Ajqgi<)zXHh$pa_I&hPJD>uoWSz^18iy2yQF39R z5==M6Tu_dYlw;HW;1HYHzJE6*)qv~2LR)rSN&nE)0fr(2i#M;2_w*lf?1X}*1b#d4 z#b`B1klf!RQ4mC;p50(>x#Z0gaOxj%i|Ts+4ztLU7%4Z)Byl;|PkCwS3^lx`rw2iH zXFQBv8D-b!+#c-_CH^}uocqCp#)Dyc^$gIt#h^kulT5{Y)|+!dwQD0>RVOsX4l)Vv zhQ~|bCFIG#MIQ^T?5}FZPk(C+*qYW-J7lg)8Kq4g5Erw(a#0}OBm@116m8aqoZcF& zkpDmH!ZngnE`yNWm%=pvvm=zRwb2VC6WBTM1o8v|s@*kXi(6JG$Q+1GAz}n_1HWwb zjcj(z%mjo~`7U6fgieye*hDj;n7VcJ?DU(?s@HSgRRhT=+rNvgkQib09h20(y?rKs z(O9QFQmki}CLZ*NfXpgBC!SmFj}>f74qRw0H$PQ$aBQ`i2~c zXv4-M5F$)8Esx0CcT5nWlQ=(p^EQd$8}z0RFR?z)y;0PElrUB`w)e3&N62U))k?95 zcJ#9`;SzUadByO~xGj@kch%iIBW3dZ_&IU`ENyt`s>NG~mJ%!_gp7 zC9LQhU0vwIVftKbY2248$(w}_F7nELU!+OBBZp5h(E(d(tyai;78L#it-jTh+@htn zHtZQL{VNT#K6&2J)MwGnpgI0KWi0YBYVIX&+{t=+NtB9HlbO(hXy>EFF-f8ZLrqa# z8l~YYLtW1_YkvmjRPOM0nXxOjAqw_WR=W#JVL|jzrk&B z;vlo%AADcB&B3D;!+Y0prQ$z&Y%S~xp$YZ27oTjE4t%n5S@=}Kto!=WH?x3Tg9kYn zUeLNN1XO5MRXF#<0S7^|xn#NOL*F&7KnJjnbYSCZo%W;SN)0-+lFz6zYxDEVj8Rms zKYJdBO5?;eO-dp~heUP=Tnjrp(22%>B(?4gyKmqpDLIj|l91qVT?#I|Z$ z)4ER$NyymH8=Zi4FCY*|e-#&tgi&k!9D6uC_7L)9rfK?L?(I{5+Gb~2Pny?*ej;+N z38Bg1%_vEJF+#O@cqrw1j8Nb$iZFhx8a!tF_CKEK*3rjO3JlB)ba&<)g{!qk9n)@+ zqoC?avFOhgTl#j0fHW3VC52we{Tj#^6H{Uc4b`9*{D;usEgkq1u4-s&C?@j^hkJ&j zb~eTI=TfPscB@bju2=+kH~8;_lWdhR+Uf3#tA{(MRg+y1=`B)Ko?WdK=~qUU^NeeV z&Wn>aLR0M=VXR`XXyx4_o_EhK+I;DYh=4URRqxlYXqD2fS5mEyp4$o|fm$Ns<#*jk zt){9e$1$w$fyq?U+X2~Q*Wrgu)}W1!kA%H*sd!KojyGcbu#idSpp5vT#Abb`C1A2ep*Bl@{Z%CB1U)mfeLlRFRKnBd@} z{QG8gV)qgV|DY9vq1uz}W0y(CUnL!9^#oXtm9LV$gU*Xr3J-qn%do(t9se;}131Lp zvc5G~78CQWM5Mi$>@XzvI&u7Y?qoOd&ymgln{+lQU6hh(w~i`tGnXzsHtyw3`SbDX zDW`>=H6G}vyT*olB&6$-dW&F1WAdV{H9z$PaxE)v>Z?^6(*o56U0+pY<#2);InQ(3m**rE z#w?hm79g98ppv?Ak*)LpeQsIsx#3S#M0eY$JASA!)EsqJ{xQKt}eP7)cmc&|MTrZVUr2+2etwm0E6-YN@)SejMy781X zC7XZVIwgadXh9@??#{2XcW|@v0rUmjic$UPNJL_n))J!4&~g?Ibf|+;ST+7avT?)0 zcaSH*&!yS3)5VP9GZ^tGPRnhB3H7-!Vd!GG>*%UN(Vww1sVpzOO5Z34y{LPZ1ff?L zR)5a?aJ_V3*(5vV&wmD;bPCq@V10{}4GsBV=%1d~mp-+6Y51y$y`tHDi3R#5JZ(%% z(7YZiPp!Z|iCXM8c)?-0WOs3=*V1~AX+n(?-Zr_MiPKr3-dP#PAV=x+pPEf^JYAL| zq`9Wo?B4;J;^WDMHhLv%?zrC1;>%|3&U0Ir>B=sZgns$Z2V_*#Y{7oSwt8KCa%F-1 zc&CX6cc!olPq_6`Gwv0(K%?fw@TLp`cW5}+4tE+)T7R_gghX@9u7n3xQrql&Ra8h6 z1agNVNByBNZY!adg{nj{ed|J@Z2LPuYEA`5_V3;m1{AiD5OHhDloY;7r!)iW-WrwG zIlN^`X=J7-RnT|U#3ba$)KQB}*1X@IYM_7VQI6iUNWBO1G`8{kjbV1i3!Ccn}}KgZMaDE7*38z#|}u`lyl&n(Q^U%D9s4m<&Tb zpQC&W|0Y+N;yl+|{PW;Nr;_<_Q3QyR2lBvNAr__>gW%h2%AJ2CtcD`4aru9`m8}9R z!eljx44c6gBA(v%gv{00d=$82wwvLzrn6B$NAjY*tO0-SsI8@C$_P4c7KaEo_A z9}toX@uWN0zB&pZjcOo#_A47s)=#H;0>{`QKbv6+53@doqnk0!DjV=-P5^ewH z&qtH{SmZnf5TGM%UPr{N1c{r&_zH88J3FdHD^$DtV%llWLSxk{Js5iRe9;@IC)J~yxUM-4ujw5^LGNXC>xTS54mGi@;ZzL?BOgT3d@OLe)k zNM%kM3U^_+;2`UP914MW3Kt8H7bF-s9+``2MS~hxM{g#;G0xOTbuEF`;ZN8OYh?` zb&!0QD&sgmTyoua2ebh;_b*3;QUoc8(am9)n{R24p0q+Jv-ux$&@Jp7PHLJQCzdNO zct3}im&VRuigU`yg0JF8o6LrXO({YowzbUVn(YV;$pd$1NcmyM#;F8jl^wWxcutFg zK!IyO4k~-SD_QpKKlS1Yy+1zKlTG1p&WVKTvEoNgPLMKmaR^3=f9bB_^LX>l=0r|T zQgAz-k%RiMEqE;D%$6H11BjNp$vuF88jfd~rev|85Kpz~^qnRB#Spxn5PBauRZJ&; zyoM=;x~0kTw5Iq31ys<^v`_R{ViB;c->LOQT8Slp6mxPz#J(kM3ZZtM4-_AsMxV8T z(FF;6Y;+dO+;BkpSXIE%-8=fbgM~8S1F~}#BY@B17n~~XgrQtmZeX3(S67=mI)1j$ zc$5AVyRUo{(0%YR%k`7f$+lr=S@1d37>md=IUrz1*11qUoGEgh7%EPCq>4Rg4 z)dh2*EZGCgQv(X4CUn?PQ>#j}M52+QZvDm+SNC-ExzPIYKq77xKdV44jqkLatA!GR zFx>cR=;$I#VZ~f+eK|4-N|5AJN@qqJ^jWfZb~uhJ$Z=kliw@+Xf0Eh%cQNI8J3UUn;pjPrAN(|*Sghxfd7G;Vf5#Jy8> z{rgIk5T{i@9h9|#PKQlyGUS>r2AX|;PEFLj@%3Ydd*F7;9YI{&NBYboG-tJYfmta$1ii05ddBJ=Q;K#wp0lr_xF z;OwIIbz-}aJKNnY!Xgolcx?RR>27{*?#2%h7eiELO5bVaspC0Lw?^dOzVpBI`xE=K z#d6!dDoU>7XfitYR90Jt`v!lpv9iQ#&n9EqX&QcE`zwSC@53l%CdV2dTu0x{orY{83|L2;)u^2{eXl zQi()FbQjHyJB=kzofJ22f%OKndWJVX!K+3a#9}iF-H{gH;bJSS01u%B7N_yWG^fHC zV^!Gv=~nJ-Fu9@u8t=V0`7J-doR`IMnxYSNdmaRr51Ohu9@55Wb~&>fj(!>oE$-v( zANrV)mY$I!hdwU+voXBrWvS$n@NVH?`Lv_aG+Jq&w0BqJnb_GYx{CnG@t5l9!&SARvi4Lgg*HF65OfjxBA>A)phkz{%@_o%R9k~7?b=PAfUOeMrSrO}qcnT>mDlUI z9V#eAO9vW^7)I8K$h{9}g5;PL^X_x+Js}$PXZkEMqNyskhZ=`I8*$ezj>fe;w52nA z6`6=ovl3X32_SRvZL2e*dCN**L6y6lmBkCj_0|=|g|?qBbIL!3gI6t0Dko10?eIE` z$f)E@>|HDykzvWMSZ7~qJ1CI-s`kIhsqJ-eu6?}RU*$=~2_dK8qSI1(gQBt+7ykW6 ziPbh#O@@QV(|ourAR-z0*2f1-xy#GHm8Yk?t|qS6$!&X{^Ca${PtyQ9%aqsd_5yT3 zNx+QENU}LbqZP)(Q=uIk3Qj#R*1cRc<+Af&$Z%)FXWas6fZ1WnfQ#o>x}vzGgjnj^j)~LJ2!$h~<43GX-mJGp_+l z=S6~e(TW41yk8$(xy&?Wpwe1zDx-av&;=4fata1EfDY-OktRzuf%~IILw6=0Y!rH7 zV7ru)D55<#FlydJnTRslmT^y|1hE*jLc zeg+3f%6E?baT2XMaW5umnr$W_Es`P(Ub)yoCDz^~S_N41R=^7fIcf28f~(Kerk`ID zQ6Lw$@7ZYmsWJM(RyPY3;fP?#5~Zq;z_4&rwD2c~#<5e|U~D7q4f$XPN$}DYDR@o= zfT1>1e=K^<{7&-p?9_t8i*%@p1o?7D|hetiO>F=Fa=R%O|JvP zx0!WReW}oPbmV~bWUY(K0|1MUMP9*#b~v(em;TrTs{q|U>_~;(-_qEE%~C~8r1)o>L`N-_*qAw(P>tFGSyZ(8u;tC@ zwj9l1HHgxDZ2Z8UK0wB;cEl)lcP|gf+`~hv4%^Y}FH1deRe`$8*Wm~&>9a0~Z8_BR zod9eLd8=6GsPOK|#b-MCEidD?d+?OI_T9|L$0~XBvF-Vh(dm52Tsx3FbOEfjXortG z3$}F2FZ0KqtCozLs%Z~iHQvQsdB2qRo~NVwRSoDTyE(fY%>|>d=FxqlvZ^u7vne(i z8O#}R5#%7gue_MgI)izK_uCxG?1F%6M15L;TuCBgSu_??NF5gCUbIp>q7OV?Y_d*p z>$PLtATL$6!Vg!r_MLGXiEc<7@QMOiDYceK?&2HI&qFf`iQ?a(;|vA3QHYWnYxZ@P zZ0ta^*BUF(Fd8hxHJO5FO?WE{GwAo>vI;LHPv!Pca-&as^YFadDXHoc4&sFo7uCJ* zLBocm$LiAO{Vs{()`Z%=>mzQvgY@nnhWSrXYCvy+v@iM8(^}3W*Dc$jTpDE$WI~Q`RQ~md{ehT={d? zjNf>Yk)C6qG4as83QE+z-2Ljmt@fE8&Ib}XzJBz0!U#Q>ugD`3wL{)$Nvzqy=HId_ zzI$W&otsnKeqVuaCj~A4v1q$nu=K3GgtI8HxJ+I^l!gWa^3E~-uK|- zxpz@n{{5%FDvgn+Ly}LTyN#`QYoj?dp{nzZ@>390*-ZVC*GwB7E>Ss>G;+hSW(tvHSY~4-{8_SlU#e#`E-d~+A_VQt2Sy?E0t=vF@E6xkT(n?OilMc$N{UO{ z)@Ox9u=!*jw1UV&N5JS8s8$wsSfx@%2Ye*0{{es;2Mt-FXvpD(#{ zzIot&@gISoYCXMuV)B9t@ zso+%tBelQdLqn=J<&|Z{Hj=e6Le)_%8TsWzD_OwAiA6BlZ}M#T_C@2) zWwBMArcTK=lVP410QX6ak!ThA>}J&~`3o!bxIPG>K6jG4&IsNpw%9etDv0vcyXD!tOn01hT@LD+8JbzsW%SZ zWgfE$8SLCO>_t~-b$ws=Bk-v&~LZSDxtgJOl@<*=vm2Ir=U%b*s z*rzHEH+PZAEJha^PjM$9Za-l$n3Uy+DZDj<>2vYf^7}oaz%{=6R1YI zY#~ol(Rf|%^zRi${~Pbsbmz4%7DZ-!=L=L0rnzjqBt^}leR!wDSY7Cu>1u?Y)s5Es zBdrAti5SL~?*lpS7@i&%sdOQ)=+>nR1=4y=QRJ{P`g9jl2+u-w1Q`R7`LhT7b^oYX z^@cK%xkF7baT2ELiq_hxSG2zUQ$^NN;F*=2e-`&9l&WByN^449;K9cA{OS@1mH+$N zvZu+IMbxRT?_R&YyS88P?mbEOmy@H1WJ7ABR8@7_KPTgq^seJ>KoILhn_939eM5+Z zVjN#5++{d1ef#OePk$3GNEZxH=SLdc$)a#$+H1l#D!knJ<%9T2t+5C4;Bwc{6UX1T z%B_e;IQ&qtLX`uuLU!Z^8OR%d$Do9N0qNAi{apyt^e6QlPaPNSH*OeU?eh_h@Od)7 z6>=T&hcha<+;_5HjM`a%?dqs75E)vVg%{*vxbPSWOcGZ^!PCjY=I`1n8LX#VwtlKB zcQ5V?OEARwj-ubGq303c*3*B^eDcQflL%dVQH<@|%x}`}q{FZkd#TBc@uSah6y=Dj z_%hv(44Cio`s?DU`GhGs*R9v;vc>UI=%dkaE?pOa8&)V~Etc2s%`ngUdX9}9R+loX zEy$QPde9r*BeIle)fz7@rU(|)_7(YgYgG30l!CoRj-a9Y&yid8R zxllBhRP%GgaF`;!u^nn_@ab>oBSSF?f zc44+3vVG`4hs)}2hP)|r{$t+H3tmS<=;rVcGqswm!uWo~pjj#Nw*V){Y>d1MLNG0rmFx=&<@10I9P`o_*=uyd8v{Uu(Q!HWxMQ+85+ z_??-`FbS~0k*WZogs@Ufg#r?TRrcC1no<6I1{51h^{WrzD;G`PU&@P933Z2gy0YCrF#AW-DrfzCR-meE= zKS#@pAGIE;YCv5Cxj%k1s9uLV>VAZR<1zeYgd`yF;HiIDP;!f zV!xjrYYe5y0Zi=<7Q34n(9)Ksgokhw+}m!MW`oT#Jmz-$4C3NbJIJ=fd8u+Y$TUDc z$N}PO09gnP2aBZ2Jy(D4Y~%iNQdJ^S?=52>-JHI_8u<))X8cymha6dm4Tej=OzI_M zX@x$DKkDCK;o|0)LHz2RA5pmS%>EjDHtjK}1#ACz1E~-dvosf8*XA>f7dg5%r0*ii z%aa(%C}PYHe-ao4Ir1NTmXS)LtJ^yHCA-YXebalrCO|;@)xVWMsAyv$Fh-?6erg@6 z>-dV!0s|v@Ip3lut`5%x(46l)jLVP5WD!O5rCir*ehnGzeAH>!J?K{?`K)_%8h|@y z@)c0Zc>uHtoc)Og^pemWEOkpL6wgT6!cU&<#?K=_8T9BWe1Qyl8KI0a(^z_b7kUtbuaFQa= zI(co&ZyB#NmHSd=ym`sbu<`y`vv>pxH%MW9ZwoNL?+pYwFBJ_Ca;FJBwGSqIj=}Pt z%y;6U=+9qc(!0&Mnask60Re3ykCusR2Ls4M5+I}N2pc76bU>=KT0izDQa*?4S@rGM z;Kcs0*LPXogXsY1GT04E(%pHP&mXSe2Xupu6Z%r8CuPj5qNYR6+lLC=baBWw6bGGq zdg9Vkkbf<+SX2M*X-Lg2ZWLGpDn=e;t) zNv32bLX>%mGKUaOWEKulh71kJl#q_hL*^+-2;orXF*0PzoWnVMd*7b-UGMidd|&IV zm30sM-uoJV*R}V)_TnvTy%B1Bbfnd%khrltE{iwsV{{A|g*dH4sJ6Tp_^|chyfdS# zXGVapHT|DaWl2&Q-+draQ~ny1PaJG=GQL#^LH%3_CaC&BW~<*i!He`dP4f& zo(m;lgd2m5FDa_D7;Et07jWiv@zdoxK{}b9-`wKUK<=o2^%MK%lUe4FFC{m*f1VRC zJ_}F$x&;?kyWb-VjjeOPpMJUA*ICbZ$tfdyV)3Ea>q=|VtTBILjnD66)8QCZ{L@h_ z%k!~qC(`PYBvvjD9J^eKNJUN6DrA{mA{XgG*qUpjG~lJ(k#MOUlpqd$xcET1uLpcF=p^VV_s zFB64UoEnOyOW^D&uu8m>?slGYYBc|bEI>G4zkL(vyFX2AxaPgTap8*jo3`VenOj?1 z;X-*6v}8f9SocTw25_M6>Cb}2HD3+yc`tf!C6imq z=4n(i1!?gwRkHp$6mnhZgVt9@ZPA)zl#h;nwpEwMU@;Q_B>gGDcI3^Kg9?q2T=O}< z7k_$1P8-q59Cl!-FxZv8+N0XT&>{uiQ)HjOS%K*>2LbEZyXibiKu) zu52(XMatDN)N_26wxt%)f;+(z-i;l6^1Gt0lCDX9Xqfk&*rdafrC_$Smr6tD4N(f0 z_CUc9FA!ZgJ-w}W@2=*o{?7nqZL2@ZS=~3wbW}L0ePnBjzlyWS+ggTP=b03L9fF$- z+zkr3@Lga!JoaggNlA0h>T5+#B~XGcCHjJfe`$714EH5PkRY6ow6bOtdeF}P9`g*Z z_awmVA7_(u5UU9H4Js&l$v#)PcI-wu%}w(0fp{e$D#J>k-S*psCEI$>y zA6e>h9-t4X+cAj_c;PuW-;?M_TIo%`Y(*P?!VJ_#HfFQNUzr(KF%y*%@ml*LqH{x8 z!t@&#XPsCM6%Bv1zGAc*cl|$lIt>;=oQCaSQ`&K(_U^_*{IsC(Xv_8=UozOi^)_z$ zaGgn0;rBcMnc142NkyHuu(EW*DBd|EaxXoU(Ne9?QPb;uvG2zKgB%iFfd9(}oKHDf zuk}0AYBX9ZGJ%Q(`@W1V_QZR6u3YF6K=kHFF1iRod{7$bfYzaO>nY~g??%H%ZTe2M zKJPS)4d%T`GQ2`kZJ0OO!AMbth2fvv9&Eh$SWAZ|^RwU?&Ro}sqTX=-gUwW{aL%_Dv~S)dyiSY_rz)RBI6=nZ&^%KwxJc#)jV|L&o4_3~wRy?gAsuj10cxjt<`4_HOn^V$Uq3Q4_eU+Vg@n2@v>IZ=LpxyfIR zdz!_k3?KWd?uuRQmXl;uVzs2I;*$cpJZ2;T>c@s{m8?-t^QWK??K*nuogs6-QG%SG zWgcB=N(uL+p!v>D5pP#D3DUqsZH^CfoKR!)^XLolDo-8>++0pRHb`S~%_*;V!&of* zZAE{4bD(s2=Wd(uIqjZjm1x<7*ze+4%sMUc$|J>@!9^ z0j!P}@SB(eiRfeUU%r+E{K4i3uy;j~pxRG7? zoz7w95Vh2Q@W1fSgzzGI`b0#K;qIjhwZ^<^&eBD;g9S2tvBB6?VTROQR-^~)8}zSH znXdn6711^Na4zhVK4gId=P9X&Gv-`*M@8S3@Z)(bZR?|(_5FBS(hc^Fz6XV)!gzQr zO_#;|4eXDueTm_XnwZd2BPfd6^0<-R&h2(UpGjl4qy+ z0t@aQc8fA2*|)tOKQ&G6_z5Tb{VB}z+avfmI5e=+XPin(D4PN}!XqO%WL%Hp$I@@4 zKcsC-bS}HtEgsSq67Njoy3CAMmr_*8w7PWrW8s1kKbX3hRN+q2$j`~~oB88YpFO%c zu+8h}Xy$bCs$xuM>;ND67?8%V4&>FpqBMg!A0jH4qF8Mjh175DxFWNmb3)hlc)}Y-pPP@`Ha9&qOl~$8_W&tZE90ly6hHkTodOh z1!Y%8Y4?rKn?KYZ9Cun53%Yq=d z{Q5RjrHZX>nQe^OOcuRLt6t2m6HCu2;2HFDF7dN<849<5jT_N>ig^)HiGN}6dh;vc zIhW$;L&ZAQ*sY{wv#bOfb;U-ER7e$TGP7%D5KC}tTL*%$L2=!>KAkok9bivsF~|_{ z^H#*_f(uLS`r4{W*7+s%2S<7uHPv!V0zZRTz#+}~BC`ufK1*{Ri##9eBKGv7;#yTL z@pKhqpS5)w-Nd){x(^qouY34xP0AA*lRD0(8&i~q#tK<~EwHc=UzP3<I+Z& z47E{uKOM_$!|c$0@`QxAh|oQUn8MAZ%s9%n3xB5t*?e0835~g9RQk6Wz)?>QZ*Ze4 zjR!}z8o?5K(H^kxPDtLVvHVWu8m2n?>2nKj*)_24&F0J3=R8Rv*7#R8T=Uys&rB@O zZhLH|#9qRO*LV#UO;_5nVF!^2?ci~z3Sz(fuJB&R+)6>ml)e{ar7#(rX-cD!r=Fqu znaARS&bH1zZ#NS9oS8*)iuaXoS|>iDXP3F}cuS);kp&m_*O0m_{6XDU43^PR;;*~r z*4ek`Y3(EFczLb}*I`(7v-ao9zB93l;AiP*9aW26#ex||l`qo#fSqr1tvx6%k3@#m z-)!r+<$qw0fkAycnPYsZOw}HfGxq(alY&n{xxBgY$k%jlLd|1jaj$qkg|pb#%J*4* zGC0RA%0s&EdMC!q(;)da?Pp78G>9lqz{Ubo?LN3k0lzPOPp--O2vp0SE zRr)kD?{|TiDL$Ih)p74jE(NJAW0qzwozXWIGBTBK#l&nN!lF***AJZr>IR}5T}DO` zjvC|5l34Bak$Nr#JPXy_+V9c>jpCi9PcwXqx~nEfTsS%5)Mqrkz3?S`QQc(!t;y8> zpz!*5_1ddP`7d5{#Ie4aK0?n@O9@uZ=t{AffWO|lG zT^keK0~V=^KO}8-jdwLtsw$PvluJ^F2{kVIEZ4|~IDm@O>$6*Y%D!ZBjjykx@5Fw; zwNL>Eci-c^*1}6BWyYEd$-xtEcohbm4==gLcjp*6Jf5<8npV)mT#sRRQD5P-U|fV< zkeppmU*Aa2;Jo0{DbejY;~Jiom>lMJ29(p^BV0XJk9tI#q3;8JOi|i>jZjx6FxifRM;!>3e3w z=e>I`%akc9CMRw)Gq9gxt)6=|H+Y$se$Jy9XS)* zB2CK)-@m)ee0>pNAqdV!G;{q^16CJpa+~?-I~3gwes)sg;kaVkj8S`yx0p=XkMtYm z0`X3d1se~Yo;9E-)l}7+&l>n=|2d1r+0@Y|x%6bPi{rD~Nyn%PV8%zklv$!%^4lYO ztqS++IKzWFp$*o7_FsAtYl-x9TgepWE*Mt|WZT-}ipPD2&@9}q(Qd}Sl(KON@g~N3 zr<8{#M>Z1-a{MM{p3tPR@LLu2V@H|+CK&bn^}<)J_HObvh`w@idaMj?h+JoCc9l%j zut~dq?1j*P?O6;q4rg!D(wTM;yVYjpJh{eeeDF8RKzrqs&4=3GFK`s*s{buEH{)Jr zB1|yrKT;F?R8WI5|F0AWxiw56Jl}8o)xp4fCWEVdHtlQce_YCl+at?cgnX+!Eo{id<-90v*)H|?N#gAD2I_?_oD(b!S8Shrx+$OKj z1#+>k;2h@JwX?&*y?U-?i@Cff-Usv!o}n`HtEZbAH9hW|DbgjtP%^J}(J1_;ihWv% z=qZ+JrpI$^{(NkKzOose8`Yv1y;EBaUwLPcX@T(cYy+QoA6xEMvzH&TBa(2ZQt@*U zz{0!c!1ZqRn~^$1MolF+cxygflDP85^!9hn5zgPec5R#mU=f>TaUsR6qb^Sh{$#QG zd+Xk4kLP&2(Vav!)+G}R0NS{=N-*v)SAGhYnsHM6>o>Q}cG%5y;(RHQ!AHViailze zO3Ccm9j7&4Q|JAz-D&CQmBvU5%V6oLG>t2qcjo)E*1YGxg!bI1Rb2ZboVM+ISVXW9 z+iY+swZyvHmtcQ}@d3a11l^`Vx&9VifRmjaGhma6WMXa%PKd~gxWoE3hN_Yrz6+#z zl`A=|@=f@dwV6K8I{H~Bv!rqLe6I#Sa!j(U?{H{VnN3lDCk;#1z?;wvu+gqVH~-fD zo!++?8zr_ThQv`dp2_X2sbgY`Dt3|8;{jY``R%NVzvnr|X9SRW1d{Eo6yHV)_53QEF%NA?EX(0&`M@!d-GuG z3wwAuXs6_>CnM-di;lRjAIV_yz;Nln;?TFXD`r>uM9G<fZ0dOdMg!t7n~lT@e}e ztaQ`FcXQ0i%&`L$G4-1J!~M`(*B2njDBU6oCDW_#Z((t!Msw_DFSng#5JFGs&cWkI z%JJ8r7`bYtO&&X#ju|W{ez_5dc)!dd{Q$M|ab}-OzjgHFQI;HO6cflw(>d?>q4YD*hx$^kB6?U920uD?NRf9BmW)4t%i5DO>P#;m`-b; z6iw!wfYy=IqKlk!hYqurrd5KCTszmGc(H;$yGB;V1`IwB*B}1gz(HR0JH^44c#s zo|+vM)tK|@A+%W^=hR2F|273I7Ir3IVu~<|z~a)~Mo(Op-7m)u4N()ov5Z`q;8!^W z7SU1f%%jJ0^zEb&41B=f8(rbrQ1bHMs3bhgBeUii&s5VWyv(V zp)QS>bJp4yx`mxLk22tOq`cqWaetT)Vx9gz0_1UXQb$RZ1|KKa^XyQxV3J3l~F3t~lQ_FDQ%v&Up#yGK>spF%>N(0el#(jM`3OUXe5VdN#-&?UO53K)FU zy14UFy>@rO0Qd2BEImQ)dV|!<_Ng~Df`+(mz3MfRS}}*g0VE0H^-2vcl{$so;&Ykt zxWDwf2^h8-={pKbN)&NVPsjBf9MkPRCF{EoD)w<|QoYLBe@fxZ$QL3_?4|x3--;C) zk&huj+cIQf*OVg8Q_?TkiR+&uE^+w14&jebcR1y*Y2xxaB(HX3lkJq}?Wp{tDfoW| zI|rt0vNjWbDF_#y|2z2-NhyMY@K>G(I6#%MYvZn>f9Id5W*jT5&t!ypW=RR z!#k=ISxs}hn>0t^zX;D-mwT)O%vG-YH;}tq7|Whj`6I8NN}@sN;KIs+tgHC3=J@_x zM`}ciKG%$MuLbatX2aDiwg#U5Klo^r4fqp4Awn#$j|Q^%|PEzzr_%iauX*kuYJ zL~?YHxeO|G2!VP-j6OCYIXe{qgM|HnO(~^~F#$p+X%ItYVWV_=QCT*qK#UUGzTgyl zKCBOa*-IMD@ON9frZtN$56+q#3P|1AQIyut-#&n3#AeYN=YN^h{OJU(EzVGTe~}_Z#)Y}5UiES z>Z%=4>=`ue^zeP}?hqZPI(pC@|5DKhZrn+^WYXB?TZfefaP%B}z>PrA5wrjdOHxlr zhH(K0kTK+kdZ0>OlyFg4>u<@CbBJ7;x<-C7$>9(nO2H_=@5pQ>;A2BfOmnVu6JO?ekv=on@Op27&eBQ9q;b)ExHlaaAXe~$jbB#WuTlr9XBl)sN#ad> z{U-Iq>0a|?0i<{zP!NAADBzVXU)F-6k~;zEav=szDV#N=Hq8Y&A#lMOK#xE$Mq38> zr8ccvouygL=cP>262fKEkduMqzoy+JSRfB2VRsYWUETkd2Ius^VILkucxI4O=n_$a z-uijUUR$nWy!jV!gC$V|_X^(Zfs#ycfHybq9jh-y%xjqvlQXvj-uq<0KJhj5|Di#U z8J!82iWp;0i!WA|HDC2b9YE9z21wQVSALw|-S_WZzX-OY5kNvNx1WLX zHw<(ZDSm1a6Hs4RZCd;QN{#e! zL#F5#*Xt}DF{FEMJ(W&iphLX1hDpX0kicl&Bg0+|mAXN4C`g)<%&8&0%I1GfC;0sa z@Cz|K3AI+${)@luw&h%x$_dDW%-Ja$!-&tTagds5;Vn9ce5UG&QUxGT=E^N?9do;t zb>n7;Ee&My!X0n?82e@U*i;|@gI0;jz30i;fHfKfO97=L$svuYI*~RU5Xm7^Y1O6K z2Zc_>l(6rj-$Qikn1Lu3ya3qp<_%OX;C5GJper3eHVk5YD8%U}K$t;NJpd*f1`fd< zR}B<1Rd3U(DXQ}R?kqeEp81sJfXmxb-4DlYiZ(E)0mxU{`7BRY*aiHmJ{lo2v@vZR z_86$9+fYqyz1gAz=Cq=CAbL9>`Ie&c_QI46C$wk9r(yq&XlFB3zgL>MMBB7)#_(oT zvU-Cm6he2HTO&wpT>9tXDu*SoYAH0>K2mry_;@M&xMTz$WvpqojV~46+&@pJn`BHZ zI}8#`#XM-M(e27+-qxi*j+i7X+0uYV_=O~7jQUB&_pYGIV*u&OJ~!0oe2vG`sFcXX z2S}8JjheENCjt^c1SfZnHzr^%Sr6Fa8S|*`MK?7=V$4|0LK1+zM%Lk6CV>be;F{|4 zHnu+*`3&GI?L{|MU9S8Mdyi$s#CS0N$jvmZ)V3%#kZCq~rZEFCNcl*Em2=49%{sy3oOqX~3}~iixiR z{>D9`!3l~;m6ZqR2+D2@8jkV%Vm$Gy>*~P9IXrSA*|T_oR=C__a(!gOUW}YsAjT zj5^PNiks7jT8WmbPZ@TLA4&|uW(e+BQkAlIVj?LE1bRed^NIB`&?(Glpm@BZPV8ZypsJ{sdabTXLu=y#ZT2VkBsQZH$*ns3F9||AT`b|2 z&EJVv1TyR@3aEtiJzB09>TgvQ@u2*pmJ1b8j?tlu^qO7-wgG_)@P2@E6@_?Sm;RcO zPAXaJG|-TENkX#0?s?PO&(%Mk0>^>Ca-06cnB-O&lUN-mK48;DgnL7z4$c_D2Tg8= zBzOrS$TqBRh`944(RVBsGF9Kpzxy9TX$b&2AWj?G`w%3hCZc1qt77KM^(@)Rg{cWT zhQWEVZjnpqEr)_y0fuJ_i_{eUqOVEDv^6lJW3ms}X{0YZM&?yO*#)UP;>@2;;fpGo zSugH@t+2qYBePcd__5rJx$k^s<#;gWEZjEM_A=kC4u$zqq_jd%fVdTH>nv?N)dZ$= z{xd+<*C1E&0)X#Fe)RLJ))}!=@hgB=w?RsSEgF^TVJi69j9L@$RkDjXp#5NRb0;c7 zp#Bi}>IusC0Tdesg+#Bw_pc!TP2iDgh;0gcY=B2!c&LuxCneD&k^{Uo45xjO6Zd`i zJwTA8!avlL>#-wD>{5BS)}mNP?@oGwB+&qkLFOPwxzt~I;z2P%X^_pcmFnaI(= z<%#T|T2+FIqL;fq^+<*MQ6PoS`*Vn_QGlP>=-ay3GN$U_!-Ybexjf`Q9?o zCfP;RC_e^KiriuJ-B)y~b#5)6Q0X&BuG())(G5)cZUJmP8XbFUe*u$mEwSXuKbV?J zkHFXcSD?SU0-2!IjDiCs1K`cr_G{;qbjUpUSLuaBq+&xFsJ6F{E#C?}wJ)Vp)DhsK zjC7O3@F*zAX+whu9jBVuI16#+;u7aom$qetRaO{0$(;om1|+5XW^4c*L-wk|e?)LV zwSriY?vpWH1i-@HiUy?k(JW|W?h}CF#i`D#t5lO_@5~T?JBVfF25;$7-CM&AHG&l%~tbFv2aWm|#)3P3=17m~!4J31iM(qRNl5I~^=Cs2TTz>rar zz(f*Qd&Fc7B5UQhD3QJ^D1YjKX~bfES6$A+Ue7}ifw+E*F&1{G6v%z2oUdkXAoxSO zZECt@MZnsO;y+J>fzN=ntNVRB>%3HYiebz8<_^Wj4YeRH2!?`c@=gLjCVw>CG+s{m z@_Z+ATL%R*VD~&G$eaR^>jl9cezg-#h}aLMnTs>eHz-9Vw~{+h22!FuV-v|`0*Y85 z6_qI31wo>@2TG8|CxD~r5edmyq8NuQfar+(L1CYhcQ(2&cq(m<&f~)rDMPz&VcVUv&{26Q=xdmFEPXl}HgP!BS78ulQ`HLjn zrXG=AkSpT>kDuR#84l#fen=`gI)J8&Vq^84?sa@MCRJ$BNTR+ByJ=fOl;Vpm&Gu4SC8@ zifBS#?2j*7GA5$7ECsy3rX#io1zb_0S!3+^d2ew_Ld36TXg^0d=F^Vk>X13*nn95u8$lkOx3y_SaD(nbneLZ5ojR%H~e*+cw4Pp z?hTTvukP95Tm-#S`bfvA_2CgH2?G?Ub-+^L$Mjc+W+p9$J?hapa5+^JP^FMln1&?b zoKZ>ys$Yb>1T)l+e6}o!o#r>mGx>WKbpgc~l-QzRbRDLqm6sB+?e)h4%u!U6u=Bg47SX@hs(R`0GFbrOz!nnTZ{UB0C4*e zr5rE;l_G+A6T%~KIYXqaqr@B}@8#@K43{FD$~5%PBpU9(xP$NkHIOq_Ai-czLGsfo(owjvQGPKu=+y=L!RvZ}x4g$mrC%=#s-tdmec2iTsW|4cBAE3yWiIgHi z-+R(X1Ba@4m;%jZ(Dkxg%};>jez`vTtM1K|^f)^Hx$wxDmZ@n&RTO;^sP;a{-zGrm z0o3Y5RC4c>ITVm$)Te>O=PJl#UpblmfmmU^$7d`B%4hx(N4&X`{Z8+SohB~tYaJ9r zTNYGyT=U61lm|hi$98utA*P3{)laI{CYIgq1*qJ6kK|?c?mT_;=mOF*$>ZPb$oaqj zrx_Gj0OCall%-iy-M^Jv?!M|miFS(xmD`kzYXD8#fg5%!MYmTu_Jo8D3JG$nsxcyRxt7ToMx>#Vu8NMMjW5cxh+Z{?UJzPs1fISHK3kCbKhg%M%^z&pO~aK1 z!t$HwoQPZ^Dhh~bkpO;d3#={xfH$`Jz9=zXaGM%Q!J^WG7|p!_d7{bDW2p5vL&^E6 z)K!7Uv#7Zv2!YLC5(D0}^fX9{$j z+SZJ2@Ac#$*b)PMj^Y0$9~N+0!5FSkw0#%pQ3WDs9b#GJvqLc6;v>Kjp8KzZ{(h+z$^3MbD9)M&N zA44;HBmhjNBZ!t*o9uBlZ??!1ZDMcq4$M-3NE>P)^5b?DkOf;@BZyZ0BLnxYna#xp zhyk^{xZAV3g@7F09tk75TJU2Fd(jo)&K?1%>_9}#{kzO~huz55^!`C$+V?se#P&b% zshx|Nlrq)<(syaWN?9W%NzEOWtJeV#V?TrvfIz$bffuBw!N6};1(aRf^JuzzpKbe2 z+LNTS<3fWBHItoYFpshqKO=Iw3$2;vgA5De79hc5v~vxh#=WQ;DWC^aPOqD%#A>fz zArKL0W7%nJpa(biyS%Gc5Y4u@Ub2SE>?W?sGD1&UiNYG*2LmVOOo23pSrb z9qZk_$GX8|aLUpDJ{o)>So3H2vGqwa==W#jQO`yYoxKUF`<(!@IR9ockOeCb{8$$I zb;yIg*cKV+&`_<5k$UdD2j)Cb@Zn!tJf~Y{xw>vF0Guv%uO9<_qTsN$)3;(lUK^>z zK&Ousm+^sXYp8SQN6|HsWG;m_|MqboGUL40jnkqRC None: + """Step the scripted smoothie sequence, optionally rendering it live.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--headless", action="store_true") + parser.add_argument("--max_steps", type=int) + parser.add_argument("--seed", type=int, default=70044) + parser.add_argument("--robot_usd_path", type=Path, help="Optional local Franka asset instead of the bundled asset.") + playback = parser.add_mutually_exclusive_group() + playback.add_argument("--replay", type=Path, help="Replay joint actions from a previously saved .npz recording.") + playback.add_argument( + "--record", + type=Path, + help=("Run the live, fully controlled sequence and write its actions/stages to this .npz path after success."), + ) + args = parser.parse_args() + if args.max_steps is not None and args.max_steps < 1: + parser.error("--max_steps must be positive.") + if not args.headless and not os.environ.get("DISPLAY"): + parser.error("Run from a graphical desktop terminal, or pass --headless.") + if args.robot_usd_path is not None: + if not args.robot_usd_path.is_file(): + parser.error("--robot_usd_path must be an existing USD file.") + os.environ["ISAACLAB_FRANKA_POUR_ROBOT_USD_PATH"] = str(args.robot_usd_path.resolve()) + os.environ.setdefault("PXR_WORK_THREAD_LIMIT", "1") + os.environ.setdefault("OMP_NUM_THREADS", "4") + os.environ.setdefault("OPENBLAS_NUM_THREADS", "1") + + import numpy as np + import torch + + from isaaclab.app import launch_simulation + + from isaaclab_tasks.contrib.franka_smoothie.recorded_controller import RecordedSequenceController + from isaaclab_tasks.contrib.franka_smoothie.smoothie_controller import SmoothieSequenceController + from isaaclab_tasks.contrib.franka_smoothie.smoothie_env import SmoothieBlenderEnv + from isaaclab_tasks.contrib.franka_smoothie.smoothie_env_cfg import FrankaSmoothieEnvCfg + + cfg = FrankaSmoothieEnvCfg() + cfg.seed = args.seed + + with contextlib.ExitStack() as resources: + resources.enter_context(launch_simulation(cfg)) + with torch.inference_mode(): + env = SmoothieBlenderEnv(cfg) + resources.callback(env.close) + env.reset(seed=args.seed) + + viewer = visuals = None + if not args.headless: + from isaaclab_visualizers.newton import NewtonGLVisualizer, NewtonGLVisualizerCfg + + from isaaclab_tasks.contrib.franka_smoothie.smoothie_visuals import SmoothieVisuals + + viewer = NewtonGLVisualizer( + NewtonGLVisualizerCfg( + enable_picking=False, + show_particles=False, + streaming_view=False, + window_width=1280, + window_height=800, + eye=(1.4, -1.2, 1.05), + lookat=(0.45, 0.0, 0.20), + ) + ) + resources.callback(viewer.close) + viewer.initialize(env.sim.get_scene_data_provider()) + visuals = SmoothieVisuals() + resources.callback(visuals.close) + + recording = args.record is not None + controller = ( + RecordedSequenceController(env, recording_path=args.replay) + if args.replay is not None + else SmoothieSequenceController(env) + ) + resources.callback(controller.close_trace) + recorded_actions: list[np.ndarray] = [] + recorded_stages: list[str] = [] + mode = "replaying recorded sequence" if args.replay is not None else "live sequence" + print(f"Running franka smoothie task ({mode}); seed {args.seed}", flush=True) + for step in range(args.max_steps or env.max_episode_length): + if viewer is not None and not viewer.is_running(): + print("Window closed.", flush=True) + break + actions = controller.compute(step) + if recording: + recorded_actions.append(actions[0].detach().cpu().numpy().copy()) + recorded_stages.append(controller.stage) + _, _, terminated, truncated, _ = env.step(actions) + done = bool(terminated[0] | truncated[0]) + if viewer is not None and not done: + visuals.update( + viewer, + env.pose("cup")[0].cpu().numpy(), + float(env.task.fill_fraction[0]), + bool(env.task.tap_on[0]), + ) + viewer.step(env.step_dt) + if step % 30 == 0 or done: + print(f"{(step + 1) * env.step_dt:.1f}s {controller.stage}", flush=True) + if done: + succeeded = bool(env.termination_manager.get_term("success")[0]) + print(f"Terminated at step {step + 1}: {'success' if succeeded else 'failure'}.", flush=True) + if recording: + if not succeeded: + parser.error("Refusing to save a recording from a run that did not succeed.") + np.savez( + args.record, + actions=np.stack(recorded_actions).astype(np.float32), + stages=np.asarray(recorded_stages), + ) + print(f"Saved recording to {args.record}.", flush=True) + break + else: + print("Reached step limit without terminating.", flush=True) + + +if __name__ == "__main__": + main() diff --git a/source/isaaclab_tasks/changelog.d/franka-smoothie.minor.rst b/source/isaaclab_tasks/changelog.d/franka-smoothie.minor.rst new file mode 100644 index 000000000000..9270f523a1ed --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/franka-smoothie.minor.rst @@ -0,0 +1,9 @@ +Added +^^^^^ + +* Added ``IsaacContrib-Franka-Smoothie``, a contributed Franka task with a repository-owned + scripted demonstration for fruit pouring, visual tap filling, physical lid fastening, + docking, and pressing the blender button. The task uses no MPM solver and no + liquid particles; liquid filling is a visual scalar approximation. Optional + ``--record`` and ``--replay`` runner arguments saved successful live joint-action + sequences and replayed them without per-step IK or live tracking gates. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/.gitattributes b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/.gitattributes new file mode 100644 index 000000000000..163f0f8ecb73 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/.gitattributes @@ -0,0 +1,2 @@ +assets/*.usdc filter=lfs diff=lfs merge=lfs -text +assets/*.npz filter=lfs diff=lfs merge=lfs -text diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/.gitignore b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/.gitignore new file mode 100644 index 000000000000..b14e4f44321b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/.gitignore @@ -0,0 +1,4 @@ +# Include the USD geometry and textures required by this task. +!assets/*.usda +!assets/*.usdc +!assets/overrides/*.usda diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/README.md b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/README.md new file mode 100644 index 000000000000..5a2c47381eeb --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/README.md @@ -0,0 +1,80 @@ +# Franka smoothie task + +`IsaacContrib-Franka-Smoothie` runs the five-stage task in one physical scene: + +1. Pour all 16 fruits from the basket into the cup and return the basket. +2. Place the cup under the tap, press its spring button to start filling, then press again to stop. +3. Pick up and screw on the lid. +4. Invert and dock the closed cup on the blender. +5. Press the physical blender button. + +The tap fill is deliberately a visual mesh and a scalar: two simulated seconds +of eligible filling represents 86.4 mL. It has no liquid mass, liquid contacts, +MPM solver, or liquid particles. The robot and all manipulated objects retain +rigid-body physics. The controller uses joint actions; it does not attach or +teleport objects between phases. This is a scripted demonstration, not a trained +policy or a verified successful full-sequence dataset. + +By default the runner uses the live scripted controller. All robot and scene +assets are bundled; no recording, checkpoint, or private output directory is +required. Optional playback uses a recording generated by `--record`. + +Use a Linux machine with a CUDA-capable NVIDIA GPU and the repository’s uv +environment and a driver compatible with the CUDA version pinned by this branch. +The interactive viewer also needs a graphical desktop with OpenGL. +Install Git LFS and uv first. From this branch’s repository root, fetch the +bundled assets and install the pinned dependencies: + +```bash +git lfs install +git lfs pull +uv sync --frozen +``` + +Then launch: + +```bash +uv run --frozen python scripts/environments/run_franka_smoothie.py +``` + +The script opens a NewtonGL window and uses the bundled Franka robot asset, +including the arm collision proxies required by this task. To use another local +copy, add `--robot_usd_path /absolute/path/to/franka_panda.usda`. No Python code or +local task assets are loaded from an `outputs/` directory. The scene, controllers +and authored USD live in this package; `assets/overrides/` holds the small USD +layers that adjust a base mesh for this scene. + +Pass `--headless` to run without a window and `--max_steps` to cap the number of +control steps. Close the window or press Ctrl+C to stop. + +To record a successful live run and then replay its raw joint actions: + +```bash +uv run --frozen python scripts/environments/run_franka_smoothie.py --headless --record /tmp/smoothie.npz +uv run --frozen python scripts/environments/run_franka_smoothie.py --replay /tmp/smoothie.npz +``` + +The runner saves the recording only if the success termination is reached. +Playback performs no IK or live tracking gates. The scene has no randomization; +`--seed` is passed through to the environment for reproducibility. + +The task uses Newton with MJWarp at a 50 Hz control rate. Playback is tied to +this physics configuration and the bundled assets; changes to either require a +new recording. Contact dynamics can also vary across hardware and dependency +versions, so confirm the final success message when testing. + +For a short headless launch check: + +```bash +uv run --frozen python scripts/environments/run_franka_smoothie.py --headless --max_steps 100 +``` + +For a complete sequence test, omit `--max_steps`. A successful run prints +`Terminated at step ...: success.`; a step-limit message only confirms that the +requested number of steps ran. + +Run the task's contract tests with: + +```bash +uv run --frozen --extra test python -m pytest source/isaaclab_tasks/test/contrib/test_franka_smoothie.py -q +``` diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/__init__.py new file mode 100644 index 000000000000..de2a000cf8a7 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/__init__.py @@ -0,0 +1,18 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Franka fruit preparation with a visual tap and physical lid assembly.""" + +import gymnasium as gym + +gym.register( + id="IsaacContrib-Franka-Smoothie", + entry_point=f"{__name__}.smoothie_env:SmoothieBlenderEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.smoothie_env_cfg:FrankaSmoothieEnvCfg", + "rsl_rl_cfg_entry_point": f"{__name__}.agents:FrankaSmoothiePPORunnerCfg", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/agents.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/agents.py new file mode 100644 index 000000000000..2d9b4c7bcbda --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/agents.py @@ -0,0 +1,25 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""PPO baseline for the contact-rich smoothie task.""" + +from isaaclab.utils.configclass import configclass + +from isaaclab_tasks.core.cabinet.config.franka.agents.rsl_rl_ppo_cfg import CabinetPPORunnerCfg + + +@configclass +class FrankaSmoothiePPORunnerCfg(CabinetPPORunnerCfg): + """PPO configuration for the rigid-body smoothie task's state observations.""" + + experiment_name = "franka_smoothie" + num_steps_per_env = 32 + max_iterations = 3000 + save_interval = 25 + obs_groups = {"actor": ["policy"], "critic": ["policy"]} + + def __post_init__(self): + self.actor.obs_normalization = True + self.critic.obs_normalization = True diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assembly_controller.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assembly_controller.py new file mode 100644 index 000000000000..053fda916ef9 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assembly_controller.py @@ -0,0 +1,827 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Measured lid assembly and cup docking targets; no object-state writes. + +This candidate uses physical grasp/contact gates and ordinary robot actions. CPU +trajectory checks establish neither collision clearance nor physical success. +""" + +from __future__ import annotations + +import copy +import hashlib +import math +from collections.abc import Callable +from pathlib import Path +from typing import Any + +import numpy as np +from scipy.spatial.transform import Rotation, Slerp + +OPTIONS = { + "algorithm": "anchored_lid_feedback_preinsert_docking_v5", + "position_tolerance_m": 0.008, + "rotation_tolerance_rad": 0.060, + "endpoint_hold_s": 0.25, + "lid_release_support_hold_s": 0.25, + "lid_release_target_separation_m": 0.110, + "cup_release_support_hold_s": 0.25, + "maximum_stage_wall_sim_s": 90.0, + "transit_tcp_z_m": 0.50, + "lid_empty_transit_tcp_z_m": 0.60, + "lid_empty_joint_waypoint_rad": [ + 0.4207511672711693, + -0.7518490166840933, + -0.34360682206715804, + -1.9573984580515424, + -0.24441385486798692, + 1.2257383113611597, + -0.7005853407533812, + ], + "lid_grasp_local_m": [0.0, 0.0, 0.018], + "lid_hand_yaw_rad": math.pi / 2, + "cup_grasp_local_m": [0.0, -0.057, 0.155], + "cup_hand_local_xyzw": [0.5, 0.5, 0.5, -0.5], + "cup_placement_tolerance_m": [0.0003, 0.0003, 0.0003], + "cup_placement_rotation_tolerance_rad": 0.0025, + "cup_approach_integral_gain_s_inv": 2.0, + "cup_approach_bias_lower_b_m": [-0.002, -0.002, 0.0], + "cup_approach_bias_upper_b_m": [0.002, 0.002, 0.008], + "cup_approach_bias_rate_m_s": 0.003, + "cup_approach_rotation_bias_limit_rad": 0.0075, + "cup_approach_rotation_bias_rate_rad_s": 0.020, + "cup_pregrasp_backoff_m": 0.05, + "cup_empty_transit_tcp_z_m": 0.60, + "cup_empty_joint_waypoint_rad": [ + -1.4434593462045706, + 1.1258680751677366, + 1.4417102527766354, + -1.2891767046596574, + 0.4635819171476493, + 1.389154505537075, + -2.348137811750025, + ], + "joint_tracking_tolerance_rad": 0.020, + "joint_endpoint_tolerance_rad": 0.008, + "joint_action": { + "arm_alpha": 0.2, + "arm_scale": 0.03, + "maximum_joint_increment_rad": 0.012, + "joint_limit_interior_margin_rad": 0.020, + }, + "lid_seated_axial_m": 0.220, + "thread_pitch_m": 0.008, + "clockwise_turns_per_grasp": 0.505, + "motor_cap_target_m": [0.64, 0.25, 0.145], + "motor_preinsert_lid_z_m": 0.210, + "motor_alignment_free_min_z_m": 0.205, + "motor_alignment_free_rotation_limit_rad": 0.075, + "motor_alignment_xy_tolerance_m": 0.0002, + "motor_alignment_rotation_tolerance_rad": 0.0025, + "motor_preinsert_z_tolerance_m": 0.001, + "motor_bias_lower_w_m": [-0.003, -0.003, 0.0], + "motor_bias_upper_w_m": [0.003, 0.003, 0.012], + "motor_rotation_bias_limit_rad": 0.020, + "motor_bias_rate_m_s": 0.003, + "motor_rotation_bias_rate_rad_s": 0.020, + "motor_integral_gain_s_inv": 2.0, + "physical_validation_passed": False, +} + +# Duration is a minimum tracking-gated motion time, never proof of completion. +STAGES = ( + ("raise_empty_hand", 8.0), + ("lid_joint_posture", 25.0), + ("lid_approach", 8.0), + ("lid_close", 2.5), + ("lid_lift", 4.0), + ("lid_carry", 6.0), + ("lid_lower_thread", 5.0), + ("lid_turn_first", 12.0), + ("lid_open_regrasp", 2.5), + ("lid_clear_regrasp", 3.0), + ("lid_unwind_hand", 7.0), + ("lid_lower_regrasp", 3.0), + ("lid_close_second", 2.5), + ("lid_turn_second", 12.0), + ("lid_seat_hold", 1.0), + ("lid_release", 2.5), + ("lid_retreat", 3.0), + ("cup_raise_empty_high", 8.0), + ("cup_joint_posture", 25.0), + ("cup_pregrasp_descent", 8.0), + ("cup_approach", 4.0), + ("cup_close", 2.5), + ("cup_lift", 7.0), + ("cup_invert", 12.0), + ("cup_carry_motor", 7.0), + ("cup_preinsert_motor", 4.0), + ("cup_lower_motor", 6.0), + ("cup_support_hold", 1.0), + ("cup_release", 2.5), + ("cup_retreat", 4.0), + ("complete", 1.0), +) + + +def assembly_controller_identity() -> dict: + """Return candidate controls and source identity without validation claims.""" + return { + "schema": "measured_assembly_controller_v1", + "options": copy.deepcopy(OPTIONS), + "stages": [list(row) for row in STAGES], + "source_sha256": {str(Path(__file__).resolve()): hashlib.sha256(Path(__file__).read_bytes()).hexdigest()}, + "physics_state_modified": False, + } + + +def _pose(value: Any) -> np.ndarray: + result = np.asarray(value, dtype=float) + if result.shape != (7,) or not np.isfinite(result).all() or np.linalg.norm(result[3:]) < 1e-12: + raise ValueError("Require a finite position [m] and nonzero XYZW quaternion.") + result = result.copy() + result[3:] /= np.linalg.norm(result[3:]) + return result + + +def _make_pose(position: np.ndarray, rotation: Rotation) -> np.ndarray: + return np.r_[position, rotation.as_quat()] + + +def _joint_array(value: Any, shape: tuple[int, ...], name: str) -> np.ndarray: + result = np.asarray(value) + if result.shape != shape or result.dtype.kind not in "fiu" or not np.isfinite(result).all(): + raise ValueError(f"Require finite {name} [rad] with shape {shape}.") + return result.astype(float, copy=False) + + +def bounded_joint_posture_step( + target: np.ndarray, joints: np.ndarray, limits: np.ndarray, previous_delta: np.ndarray +) -> tuple[np.ndarray, dict]: + """Return raw joint actions with unchanged live EMA and increment constraints. + + Args: + target: Desired named arm joint positions [rad], shape (7,). + joints: Measured named arm joint positions [rad], shape (7,). + limits: Authored joint limits [rad], shape (7, 2). + previous_delta: Live processed arm action [rad], shape (7,). + + Returns: + Raw seven-joint actions and diagnostics. Unreachable inset bounds cause + the strongest reachable restoration or braking, without a history reset. + """ + target = _joint_array(target, (7,), "joint target") + joints = _joint_array(joints, (7,), "joint positions") + limits = _joint_array(limits, (7, 2), "joint limits") + previous_delta = _joint_array(previous_delta, (7,), "processed action") + options = OPTIONS["joint_action"] + margin = options["joint_limit_interior_margin_rad"] + if np.any(limits[:, 1] - limits[:, 0] <= 2 * margin): + raise ValueError("Joint limits must have a nonempty declared interior.") + if np.any(target < limits[:, 0] + margin) or np.any(target > limits[:, 1] - margin): + raise ValueError("The requested joint posture must lie inside the authored limit inset.") + alpha, scale = options["arm_alpha"], options["arm_scale"] + center, radius = (1 - alpha) * previous_delta, alpha * scale + reachable_lower, reachable_upper = center - radius, center + radius + cap = options["maximum_joint_increment_rad"] + lower, upper = np.maximum(reachable_lower, -cap), np.minimum(reachable_upper, cap) + increment_unreachable = lower > upper + braking = np.clip(np.zeros(7), reachable_lower, reachable_upper) + lower = np.where(increment_unreachable, braking, lower) + upper = np.where(increment_unreachable, braking, upper) + interior_lower = limits[:, 0] + margin - joints + interior_upper = limits[:, 1] - margin - joints + bounded_lower, bounded_upper = np.maximum(lower, interior_lower), np.minimum(upper, interior_upper) + interior_unreachable = bounded_lower > bounded_upper + restoring = np.where(interior_lower > upper, upper, lower) + lower = np.where(interior_unreachable, restoring, bounded_lower) + upper = np.where(interior_unreachable, restoring, bounded_upper) + delta = np.clip(target - joints, lower, upper) + raw = (delta - center) / radius + if np.any(np.abs(raw) > 1 + 1e-10): + raise RuntimeError("Joint posture action escaped the live EMA reachable interval.") + raw = np.clip(raw, -1, 1) + effective = center + radius * raw + return raw, { + "controller": "bounded_measured_joint_posture_v1", + "maximum_joint_error_rad": float(np.max(np.abs(target - joints))), + "effective_delta_rad": effective.tolist(), + "joint_interior_unreachable": interior_unreachable.tolist(), + "increment_unreachable_due_to_ema": increment_unreachable.tolist(), + "restoration_or_braking_active": bool(interior_unreachable.any() or increment_unreachable.any()), + "target_minimum_authored_joint_margin_rad": float( + np.minimum(joints + effective - limits[:, 0], limits[:, 1] - joints - effective).min() + ), + } + + +class AssemblyTrajectory: + """Produce targets from current physical measurements and confirmed grasps. + + Input poses are scene-local [m, XYZW]. ``hand_pose`` uses the measured TCP as + its translation and the measured hand orientation. ``metrics`` contains the + assembly geometry/state measurements; missing required gates fail closed. + """ + + def __init__(self) -> None: + self.index = 0 + self.elapsed_s = 0.0 + self.stage_wall_s = 0.0 + self.hold_s = 0.0 + self.start: np.ndarray | None = None + self.goal: np.ndarray | None = None + self.joint_start: np.ndarray | None = None + self.reference: dict[str, tuple[np.ndarray, Rotation]] = {} + self.turn_lid_start: np.ndarray | None = None + self.turn_cup: np.ndarray | None = None + self.assembly_relative: tuple[np.ndarray, Rotation] | None = None + self.complete = False + self.release_support_hold_s = 0.0 + self.release_authorized = False + self.cup_approach_bias = np.zeros(3) + self.cup_approach_rotation_bias = np.zeros(3) + self.cup_close_target: np.ndarray | None = None + self.last_command: np.ndarray | None = None + self.motor_lid_start: np.ndarray | None = None + self.motor_bias = np.zeros(3) + self.motor_rotation_bias = np.zeros(3) + self.motor_preinsert_verified = False + self.motor_entry_command: np.ndarray | None = None + + @property + def stage(self) -> str: + """Return the controller substage, independent of measured task milestones.""" + return STAGES[self.index][0] + + def _capture(self, name: str, hand: np.ndarray, obj: np.ndarray) -> None: + rotation = Rotation.from_quat(obj[3:]) + self.reference[name] = ( + rotation.inv().apply(hand[:3] - obj[:3]), + rotation.inv() * Rotation.from_quat(hand[3:]), + ) + + def _held_target(self, name: str, obj: np.ndarray) -> np.ndarray: + if name not in self.reference: + raise RuntimeError(f"No measured physical grasp reference for {name}.") + local_position, local_rotation = self.reference[name] + rotation = Rotation.from_quat(obj[3:]) + return _make_pose(obj[:3] + rotation.apply(local_position), rotation * local_rotation) + + def _grasp_target(self, name: str, obj: np.ndarray) -> np.ndarray: + rotation = Rotation.from_quat(obj[3:]) + hand = ( + Rotation.from_euler("z", OPTIONS["lid_hand_yaw_rad"]) * Rotation.from_euler("x", math.pi) + if name == "lid" + else Rotation.from_quat(OPTIONS["cup_hand_local_xyzw"]) + ) + return _make_pose(obj[:3] + rotation.apply(OPTIONS[name + "_grasp_local_m"]), rotation * hand) + + def _dock_cup_pose(self, clearance_m: float) -> np.ndarray: + if self.assembly_relative is None: + raise RuntimeError("Docking requires this episode's measured seated cup/lid transform.") + relative_position, relative_rotation = self.assembly_relative + cap_rotation = Rotation.from_euler("x", math.pi) * Rotation.from_euler("z", math.pi) + cup_rotation = cap_rotation * relative_rotation.inv() + cap_position = np.asarray(OPTIONS["motor_cap_target_m"]) + [0, 0, clearance_m] + return _make_pose(cap_position - cup_rotation.apply(relative_position), cup_rotation) + + def _enter_motor(self, hand: np.ndarray, lid: np.ndarray) -> None: + """Initialize measured lid feedback while preserving the prior command [m, rad].""" + stage = self.stage + if stage == "cup_lower_motor" and not self.motor_preinsert_verified: + raise RuntimeError("Motor insertion requires the measured pre-insertion alignment dwell.") + self.motor_lid_start = lid.copy() + if stage == "cup_lower_motor": + self.motor_lid_start[2] = OPTIONS["motor_preinsert_lid_z_m"] + self.motor_entry_command = None if self.last_command is None else self.last_command.copy() + if stage == "cup_preinsert_motor": + self._capture("motor_lid", hand, lid) + self.motor_bias[:] = 0.0 + self.motor_rotation_bias[:] = 0.0 + if self.last_command is not None: + # Retain the existing actuator drive when changing controlled frames. + change = Rotation.from_quat(self.last_command[3:]) * Rotation.from_quat(hand[3:]).inv() + driven_lid = self.last_command[:3] + change.apply(lid[:3] - hand[:3]) + self.motor_bias[:] = driven_lid - lid[:3] + self.motor_rotation_bias[:] = change.as_rotvec() + if ( + np.any(self.motor_bias < OPTIONS["motor_bias_lower_w_m"]) + or np.any(self.motor_bias > OPTIONS["motor_bias_upper_w_m"]) + or np.linalg.norm(self.motor_rotation_bias) > OPTIONS["motor_rotation_bias_limit_rad"] + ): + raise RuntimeError("Existing carry drive exceeds the bounded motor alignment trim.") + if self.last_command is not None: + self.start = self.goal = self.last_command.copy() + + def _enter( + self, hand: np.ndarray, lid: np.ndarray, cup: np.ndarray, joint_positions: np.ndarray | None = None + ) -> None: + self.start, self.goal = hand.copy(), hand.copy() + self.joint_start = None + self.release_support_hold_s = 0.0 + self.release_authorized = False + stage = self.stage + high = OPTIONS["transit_tcp_z_m"] + if stage == "raise_empty_hand": + self.goal[2] = max(OPTIONS["lid_empty_transit_tcp_z_m"], hand[2]) + elif stage == "lid_joint_posture": + self.joint_start = _joint_array(joint_positions, (7,), "measured entry joints").copy() + elif stage == "lid_approach": + self.goal = self._grasp_target("lid", lid) + elif stage in ("lid_lift", "lid_carry", "lid_lower_thread"): + obj = lid.copy() + if stage == "lid_lift": + self._capture("lid", hand, lid) + obj[2] += 0.14 + else: + rotation = Rotation.from_quat(cup[3:]) + axial = ( + OPTIONS["lid_seated_axial_m"] + 2 * OPTIONS["clockwise_turns_per_grasp"] * OPTIONS["thread_pitch_m"] + ) + if stage == "lid_carry": + axial += 0.11 + obj[:3] = cup[:3] + rotation.apply([0, 0, axial]) + obj[3:] = rotation.as_quat() + self.goal = self._held_target("lid", obj) + elif stage in ("lid_turn_first", "lid_turn_second"): + self._capture("lid", hand, lid) + self.turn_lid_start, self.turn_cup = lid.copy(), cup.copy() + elif stage == "lid_clear_regrasp": + self.goal[2] += 0.10 + elif stage == "lid_retreat": + # Grasp compression must not consume the measured release clearance. + grasp = self._grasp_target("lid", lid) + self.goal[2] = max(hand[2], grasp[2] + OPTIONS["lid_release_target_separation_m"]) + elif stage == "lid_unwind_hand": + self.goal[3:] = (Rotation.from_euler("z", math.pi) * Rotation.from_quat(hand[3:])).as_quat() + elif stage == "lid_lower_regrasp": + self.goal[:3] = self._grasp_target("lid", lid)[:3] + elif stage == "cup_raise_empty_high": + self.goal[2] = max(OPTIONS["cup_empty_transit_tcp_z_m"], hand[2]) + elif stage == "cup_joint_posture": + self.joint_start = _joint_array(joint_positions, (7,), "measured entry joints").copy() + elif stage == "cup_pregrasp_descent": + self.goal = self._grasp_target("cup", cup) + self.goal[:3] -= Rotation.from_quat(self.goal[3:]).apply([0, 0, OPTIONS["cup_pregrasp_backoff_m"]]) + elif stage == "cup_approach": + self.goal = self._grasp_target("cup", cup) + self.cup_approach_bias[:] = 0.0 + self.cup_approach_rotation_bias[:] = 0.0 + self.cup_close_target = None + elif stage == "cup_close": + if self.cup_close_target is None: + raise RuntimeError("Cup closing requires a precisely placed approach endpoint.") + # Preserve corrective drive while the fingers begin to close. + self.start = self.cup_close_target.copy() + self.goal = self.cup_close_target.copy() + elif stage == "cup_lift": + self._capture("cup", hand, cup) + cup_rotation = Rotation.from_quat(cup[3:]) + self.assembly_relative = ( + cup_rotation.inv().apply(lid[:3] - cup[:3]), + cup_rotation.inv() * Rotation.from_quat(lid[3:]), + ) + self.goal[2] = max(high, hand[2] + 0.20) + elif stage == "cup_invert": + obj = self._dock_cup_pose(0.20) + self.goal = self._held_target("cup", obj) + self.goal[:3] = hand[:3] + elif stage == "cup_carry_motor": + self.goal = self._held_target("cup", self._dock_cup_pose(0.20)) + elif stage in ("cup_preinsert_motor", "cup_lower_motor"): + self._enter_motor(hand, lid) + elif stage in ("cup_support_hold", "cup_release") and self.motor_preinsert_verified: + if self.last_command is None: + raise RuntimeError("Dock support must preserve the last corrected insertion command.") + self.start = self.goal = self.last_command.copy() + elif stage == "cup_retreat": + # Retreat along the measured approach axis, keeping clear of the cup. + self.goal[:3] -= Rotation.from_quat(hand[3:]).apply([0, 0, 0.12]) + + def _sample(self, u: float) -> np.ndarray: + if self.start is None or self.goal is None: + raise RuntimeError("Stage targets have not been initialized.") + blend = u * u * (3 - 2 * u) + if self.stage in ("lid_turn_first", "lid_turn_second"): + if self.turn_lid_start is None or self.turn_cup is None: + raise RuntimeError("A thread motion needs the measured entry poses.") + cup_r = Rotation.from_quat(self.turn_cup[3:]) + angle = -2 * math.pi * OPTIONS["clockwise_turns_per_grasp"] * blend + rotation = ( + cup_r * Rotation.from_euler("z", angle) * cup_r.inv() * Rotation.from_quat(self.turn_lid_start[3:]) + ) + position = self.turn_lid_start[:3] - cup_r.apply( + [0, 0, OPTIONS["thread_pitch_m"] * OPTIONS["clockwise_turns_per_grasp"] * blend] + ) + return self._held_target("lid", _make_pose(position, rotation)) + if self.stage == "lid_unwind_hand": + rotation = Rotation.from_euler("z", math.pi * blend) * Rotation.from_quat(self.start[3:]) + elif self.stage == "cup_invert": + start = Rotation.from_quat(self.start[3:]) + delta = (Rotation.from_quat(self.goal[3:]) * start.inv()).as_rotvec() + angle = np.linalg.norm(delta) + # Near pi, measured tilt must not flip to the unreachable wrist arc. + if angle > 1e-12 and delta[1] < 0: + delta *= 1 - 2 * math.pi / angle + rotation = Rotation.from_rotvec(delta * blend) * start + else: + rotation = Slerp([0, 1], Rotation.from_quat([self.start[3:], self.goal[3:]]))([blend])[0] + return _make_pose((1 - blend) * self.start[:3] + blend * self.goal[:3], rotation) + + def _gates(self, metrics: dict, endpoint: bool, dt: float) -> tuple[bool, bool, str]: + stage = self.stage + + def get(name: str) -> bool: + return bool(metrics.get(name, False)) + + opened = get("fingers_open") + lid_motion = stage in ( + "lid_lift", + "lid_carry", + "lid_lower_thread", + "lid_turn_first", + "lid_turn_second", + "lid_seat_hold", + ) + cup_motion = stage in ( + "cup_lift", + "cup_invert", + "cup_carry_motor", + "cup_preinsert_motor", + "cup_lower_motor", + "cup_support_hold", + ) + close = lid_motion or cup_motion or stage in ("lid_close", "lid_close_second", "cup_close") + gate = True + if lid_motion: + gate = get("lid_held") + if cup_motion: + gate = get("cup_held") and get("lid_seated") and not get("lid_retention_failed") + if stage.startswith("lid_turn"): + gate &= get("lid_near_thread") + if stage in ("lid_clear_regrasp", "lid_unwind_hand", "lid_lower_regrasp"): + supported = get("lid_near_thread") and get("lid_stable") and get("cup_stable") + gate = supported and opened + # These stages follow a verified open release. Settling pauses hand + # motion without closing the fingers onto the supported lid again. + close = False + if stage in ("lid_open_regrasp", "lid_release"): + retained = get("lid_near_thread") + if stage == "lid_release": + retained &= get("lid_twist_complete") and get("lid_seated") + supported = retained and get("lid_stable") and get("cup_stable") + if not retained: + self.release_authorized = False + self.release_support_hold_s = 0.0 + elif not self.release_authorized: + self.release_support_hold_s = self.release_support_hold_s + dt if supported else 0.0 + self.release_authorized = self.release_support_hold_s >= OPTIONS["lid_release_support_hold_s"] - 1e-9 + # Opening can cause brief settling. Do not close again solely because + # velocity changes after the measured support dwell authorized release. + close = not self.release_authorized + gate = self.release_authorized and retained + if endpoint: + gate &= supported + if stage == "cup_release": + ready = get("dock_support_ready") + if not self.release_authorized: + self.release_support_hold_s = self.release_support_hold_s + dt if ready else 0.0 + self.release_authorized = self.release_support_hold_s >= OPTIONS["cup_release_support_hold_s"] - 1e-9 + gate, close = ready and self.release_authorized, not self.release_authorized + if stage in ("raise_empty_hand", "lid_joint_posture", "lid_approach"): + gate = opened + if stage in ("cup_raise_empty_high", "cup_joint_posture", "cup_pregrasp_descent", "cup_approach"): + gate = get("lid_release_complete") and get("lid_seated") and opened + if endpoint: + if stage in ("lid_close", "lid_close_second"): + gate &= get("lid_held") + elif stage == "cup_close": + gate &= get("cup_held") + elif stage in ("lid_open_regrasp", "lid_release", "cup_release"): + gate &= opened + elif stage == "lid_seat_hold": + gate &= get("lid_twist_complete") and get("lid_seated") + elif stage == "lid_lower_thread": + gate &= get("lid_near_thread") + elif stage == "lid_retreat": + gate &= get("lid_release_complete") + elif stage == "cup_support_hold": + gate &= get("dock_support_ready") + elif stage in ("cup_retreat", "complete"): + gate &= get("assembly_complete") + elif not close: + gate &= opened + return gate, close, "measured_gates_passed" if gate else "waiting_for_physical_gate" + + def _cup_approach_endpoint( + self, hand: np.ndarray, cup: np.ndarray, dt: float, allow_bias: bool + ) -> tuple[np.ndarray, bool, dict]: + """Refine the open-hand bridge placement with bounded target bias [m, rad].""" + desired = self._grasp_target("cup", cup) + cup_rotation = Rotation.from_quat(cup[3:]) + local_error = cup_rotation.inv().apply(hand[:3] - desired[:3]) + angular_error = cup_rotation.inv().apply( + (Rotation.from_quat(desired[3:]) * Rotation.from_quat(hand[3:]).inv()).as_rotvec() + ) + ready = bool( + np.all(np.abs(local_error) <= OPTIONS["cup_placement_tolerance_m"]) + and np.linalg.norm(angular_error) <= OPTIONS["cup_placement_rotation_tolerance_rad"] + ) + if not ready and allow_bias: + for bias, residual, rate in ( + (self.cup_approach_bias, -local_error, "cup_approach_bias_rate_m_s"), + ( + self.cup_approach_rotation_bias, + angular_error, + "cup_approach_rotation_bias_rate_rad_s", + ), + ): + increment = OPTIONS["cup_approach_integral_gain_s_inv"] * dt * residual + increment *= min(1.0, OPTIONS[rate] * dt / max(np.linalg.norm(increment), 1e-15)) + bias += increment + self.cup_approach_bias[:] = np.clip( + self.cup_approach_bias, OPTIONS["cup_approach_bias_lower_b_m"], OPTIONS["cup_approach_bias_upper_b_m"] + ) + self.cup_approach_rotation_bias *= min( + 1.0, + OPTIONS["cup_approach_rotation_bias_limit_rad"] + / max(np.linalg.norm(self.cup_approach_rotation_bias), 1e-15), + ) + sample = desired.copy() + sample[:3] += cup_rotation.apply(self.cup_approach_bias) + sample[3:] = ( + Rotation.from_rotvec(cup_rotation.apply(self.cup_approach_rotation_bias)) * Rotation.from_quat(desired[3:]) + ).as_quat() + self.cup_close_target = sample.copy() + diagnostics = { + "cup_placement_error_b_m": local_error.tolist(), + "cup_placement_rotation_error_rad": float(np.linalg.norm(angular_error)), + "cup_placement_ready": ready, + "cup_approach_bias_b_m": self.cup_approach_bias.tolist(), + "cup_approach_rotation_bias_b_rad": self.cup_approach_rotation_bias.tolist(), + } + return sample, ready, diagnostics + + def _motor_target( + self, hand: np.ndarray, lid: np.ndarray, metrics: dict, u: float, endpoint: bool, dt: float + ) -> tuple[np.ndarray, bool, dict]: + """Control the measured lid frame with bounded world-frame trim [m, rad]. + + The trim compensates actuator compliance, not permissible object error. + The moving preinsert follows the measured key corridor above the free plane. + Exact alignment remains mandatory at the preinsert endpoint and throughout insertion. + """ + if self.motor_lid_start is None: + raise RuntimeError("Motor alignment requires its measured entry pose.") + preinsert = self.stage == "cup_preinsert_motor" + position = np.asarray(OPTIONS["motor_cap_target_m"], dtype=float).copy() + if preinsert: + position[2] = OPTIONS["motor_preinsert_lid_z_m"] + blend = u * u * (3 - 2 * u) + position[2] = (1 - blend) * self.motor_lid_start[2] + blend * position[2] + rotation = Rotation.from_euler("x", math.pi) * Rotation.from_euler("z", math.pi) + lid_rotation = Rotation.from_quat(lid[3:]) + error = position - lid[:3] + angular_error = (rotation * lid_rotation.inv()).as_rotvec() + angle = float(np.linalg.norm(angular_error)) + aligned = bool( + np.all(np.abs(error[:2]) <= OPTIONS["motor_alignment_xy_tolerance_m"]) + and angle <= OPTIONS["motor_alignment_rotation_tolerance_rad"] + ) + key_fit = bool(metrics.get("dock_key_fit", False)) + held = bool(metrics.get("cup_held", False)) and bool(metrics.get("lid_seated", False)) + first = self.motor_entry_command is not None + if held: + if angle > OPTIONS["motor_alignment_free_rotation_limit_rad"]: + raise RuntimeError("Measured lid rotation escaped the motor alignment envelope.") + if lid[2] < OPTIONS["motor_alignment_free_min_z_m"]: + if preinsert or not key_fit: + raise RuntimeError("Measured lid left the free-space/key corridor; refuse further insertion.") + if not first: + residual = error.copy() + # Axial compensation is learned above the motor, never against contact. + if not (preinsert and endpoint): + residual[2] = 0.0 + for bias, value, rate in ( + (self.motor_bias, residual, OPTIONS["motor_bias_rate_m_s"]), + (self.motor_rotation_bias, angular_error, OPTIONS["motor_rotation_bias_rate_rad_s"]), + ): + increment = OPTIONS["motor_integral_gain_s_inv"] * dt * value + increment *= min(1.0, rate * dt / max(np.linalg.norm(increment), 1e-15)) + bias += increment + self.motor_bias[:] = np.clip( + self.motor_bias, OPTIONS["motor_bias_lower_w_m"], OPTIONS["motor_bias_upper_w_m"] + ) + self.motor_rotation_bias *= min( + 1.0, + OPTIONS["motor_rotation_bias_limit_rad"] / max(np.linalg.norm(self.motor_rotation_bias), 1e-15), + ) + desired_rotation = Rotation.from_rotvec(self.motor_rotation_bias) * rotation + # Repeated recapture would chase roll around the weak jaw-contact axis. + # The loaded entry reference stays fixed; only bounded measured-error trim adapts. + if "motor_lid" not in self.reference: + raise RuntimeError("Motor insertion requires the loaded alignment-entry grasp reference.") + local_hand, relative = self.reference["motor_lid"] + sample = _make_pose( + position + self.motor_bias + desired_rotation.apply(local_hand), desired_rotation * relative + ) + if first: + sample = self.motor_entry_command.copy() + self.motor_entry_command = None + elif not held and self.last_command is not None: + sample = self.last_command.copy() + # Above the motor, follow the safe key corridor without endpoint-level alignment stops. + moving_preinsert = preinsert and not endpoint + ready = held and key_fit and not first and (moving_preinsert or aligned) + if preinsert and endpoint: + ready &= abs(error[2]) <= OPTIONS["motor_preinsert_z_tolerance_m"] + elif not preinsert and endpoint: + ready &= bool(metrics.get("dock_support_ready", False)) + else: + ready &= abs(error[2]) <= OPTIONS["position_tolerance_m"] + return ( + sample, + bool(ready), + { + "motor_lid_position_error_w_m": error.tolist(), + "motor_lid_rotation_error_rad": angle, + "motor_alignment_ready": aligned, + "motor_actual_key_fit": key_fit, + "motor_bias_w_m": self.motor_bias.tolist(), + "motor_rotation_bias_w_rad": self.motor_rotation_bias.tolist(), + "motor_axial_bias_frozen": not preinsert, + "motor_command_handoff": first, + }, + ) + + def step(self, state: dict, dt: float) -> dict: + """Advance only with measured tracking and contact gates; timestep [s].""" + if not math.isfinite(dt) or dt <= 0: + raise ValueError("Require a positive finite policy timestep [s].") + hand, lid, cup = (_pose(state[name]) for name in ("hand_pose", "lid_pose", "cup_pose")) + metrics = state["metrics"] + if not bool(metrics.get("valid", False)) or bool(metrics.get("lid_retention_failed", False)): + raise RuntimeError("Invalid assembly physical state or lost threaded lid.") + if self.start is None: + self._enter(hand, lid, cup, state.get("joint_positions")) + duration = STAGES[self.index][1] + u = min(self.elapsed_s / duration, 1.0) + sample = self._sample(u) + endpoint = self.elapsed_s >= duration - 1e-9 + placement_ready, placement_diagnostics = None, {} + if endpoint and self.stage == "cup_approach": + allow_bias = all( + bool(metrics.get(key, False)) for key in ("fingers_open", "lid_release_complete", "lid_seated") + ) + sample, placement_ready, placement_diagnostics = self._cup_approach_endpoint(hand, cup, dt, allow_bias) + if self.stage in ("cup_preinsert_motor", "cup_lower_motor"): + sample, placement_ready, placement_diagnostics = self._motor_target(hand, lid, metrics, u, endpoint, dt) + joint_target = None + joint_error = None + if self.stage in ("lid_joint_posture", "cup_joint_posture"): + joints = _joint_array(state.get("joint_positions"), (7,), "measured joints") + if self.joint_start is None: + raise RuntimeError("A joint posture requires measured entry joints.") + blend = u * u * (3 - 2 * u) + name = "lid" if self.stage == "lid_joint_posture" else "cup" + waypoint = np.asarray(OPTIONS[name + "_empty_joint_waypoint_rad"]) + joint_target = self.joint_start + blend * (waypoint - self.joint_start) + joint_error = float(np.max(np.abs(joint_target - joints))) + sample = hand.copy() + position_error = float(np.linalg.norm(sample[:3] - hand[:3])) + rotation_error = float((Rotation.from_quat(sample[3:]) * Rotation.from_quat(hand[3:]).inv()).magnitude()) + tracking = ( + position_error <= OPTIONS["position_tolerance_m"] and rotation_error <= OPTIONS["rotation_tolerance_rad"] + ) + if endpoint and self.stage in ("lid_lower_thread", "lid_lower_regrasp"): + # Contact compliance can leave millimetres of TCP error even when the + # measured lid is aligned. Physical support/grasp gates remain. + tracking = position_error <= OPTIONS["position_tolerance_m"] and rotation_error <= 0.025 + elif endpoint and self.stage == "cup_lower_motor": + tolerance = OPTIONS["position_tolerance_m"] if bool(metrics.get("dock_support_ready", False)) else 0.0015 + tracking = position_error <= tolerance and rotation_error <= 0.025 + if joint_target is not None: + tolerance = OPTIONS["joint_endpoint_tolerance_rad" if endpoint else "joint_tracking_tolerance_rad"] + tracking = joint_error <= tolerance + if placement_ready is not None: + # The physical nominal pose, not the compensating target, permits closure. + tracking = placement_ready + gate, close, reason = self._gates(metrics, endpoint, dt) + self.stage_wall_s += dt + if self.stage_wall_s > OPTIONS["maximum_stage_wall_sim_s"]: + raise RuntimeError(f"Assembly substage {self.stage} exceeded its bounded physical time.") + self.hold_s = self.hold_s + dt if endpoint and tracking and gate else 0.0 + result = { + "phase": self.stage, + "assembly_controller_stage_id": self.index, + "tcp_position": sample[:3], + "hand_xyzw": sample[3:], + "raw_gripper_sign": -1.0 if close else 1.0, + "elapsed_s": self.elapsed_s, + "stage_wall_s": self.stage_wall_s, + "position_error_m": position_error, + "rotation_error_rad": rotation_error, + "tracking": tracking, + "gate_reason": reason, + "hold_s": self.hold_s, + "complete": self.complete, + "release_support_hold_s": self.release_support_hold_s, + "release_authorized": self.release_authorized, + "control_mode": "joint_posture" if joint_target is not None else "cartesian_pose", + "joint_position_target_rad": joint_target, + "maximum_joint_error_rad": joint_error, + **placement_diagnostics, + } + self.last_command = sample.copy() + if tracking and gate: + self.elapsed_s = min(duration, self.elapsed_s + dt) + if self.hold_s >= OPTIONS["endpoint_hold_s"] - 1e-9: + if self.stage == "cup_preinsert_motor": + self.motor_preinsert_verified = True + if self.index == len(STAGES) - 1: + self.complete = True + result["complete"] = True + else: + self.index += 1 + self.elapsed_s = self.stage_wall_s = self.hold_s = 0.0 + self.start = self.goal = None + return result + + +class AssemblyController: + """Adapt scene-local measured targets to ordinary bounded 8-D robot actions.""" + + def __init__(self, env: Any, metrics_fn: Callable[[Any], dict] | None = None) -> None: + from .bounded_return_pose import BoundedReturnPoseController + + if env.num_envs != 1: + raise ValueError("Assembly candidate supports one uninterrupted physical world.") + self.env = env + self.metrics_fn = metrics_fn + self.trajectory = AssemblyTrajectory() + self.pose_controller = BoundedReturnPoseController(env) + self.stage = self.trajectory.stage + self.diagnostics: dict = {} + self.previous_step: int | None = None + + def compute(self, step: int): + """Return raw actions from current physical poses; global step is monotonic.""" + import torch + + if type(step) is not int or step < 0 or (self.previous_step is not None and step != self.previous_step + 1): + raise ValueError("Assembly requires consecutive nonnegative global policy steps.") + self.previous_step = step + env = self.env + origin = env.scene.env_origins[0].detach().cpu().numpy() + hand = env.robot.data.body_link_pose_w.torch[0, env.hand_id].detach().cpu().numpy().copy() + hand[:3] = env.tcp()[0].detach().cpu().numpy() - origin + lid = env.pose("blade_cap")[0].detach().cpu().numpy().copy() + cup = env.pose("cup")[0].detach().cpu().numpy().copy() + lid[:3] -= origin + cup[:3] -= origin + metrics = self.metrics_fn(env) if self.metrics_fn is not None else env.assembly_measurements() + metrics = { + key: value[0].item() if torch.is_tensor(value) and value.numel() == 1 else value + for key, value in metrics.items() + } + joints = env.robot.data.joint_pos.torch[:, self.pose_controller.joint_ids] + sample = self.trajectory.step( + { + "hand_pose": hand, + "lid_pose": lid, + "cup_pose": cup, + "joint_positions": joints[0].detach().cpu().numpy(), + "metrics": metrics, + }, + env.step_dt, + ) + position = torch.as_tensor(sample["tcp_position"] + origin, device=env.device, dtype=env.tcp().dtype)[None] + rotation = torch.as_tensor(sample["hand_xyzw"], device=env.device, dtype=env.tcp().dtype)[None] + if sample["joint_position_target_rad"] is None: + actions = self.pose_controller.compute(position, rotation, sample["raw_gripper_sign"] < 0) + control_diagnostics = self.pose_controller.diagnostics + else: + self.pose_controller._check_action_contract() + limits = env.robot.data.joint_pos_limits.torch[:, self.pose_controller.joint_ids] + previous = env.action_manager.get_term("arm_action").processed_actions + raw, control_diagnostics = bounded_joint_posture_step( + sample["joint_position_target_rad"], + joints[0].detach().cpu().numpy(), + limits[0].detach().cpu().numpy(), + previous[0].detach().cpu().numpy(), + ) + actions = torch.zeros((1, 8), device=env.device, dtype=joints.dtype) + actions[0, :7] = torch.as_tensor(raw, device=env.device, dtype=joints.dtype) + actions[0, -1] = sample["raw_gripper_sign"] + control_diagnostics = { + "controller": control_diagnostics["controller"], + "per_environment": [control_diagnostics], + } + self.stage, self.diagnostics = sample["phase"], sample + self.diagnostics["bounded_pose_control"] = control_diagnostics + if actions.shape != (1, 8) or not torch.isfinite(actions).all(): + raise RuntimeError("Assembly produced invalid raw actions.") + return actions diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/blackberry.usdc b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/blackberry.usdc new file mode 100644 index 000000000000..fd55cd4b55f9 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/blackberry.usdc @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1a5dd9ab3eaa8cc22ec632a544700ef9dfb42f188cb915126170109ceb3a5364 +size 320331 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/blade_cap.usda b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/blade_cap.usda new file mode 100644 index 000000000000..960c1cff5ee3 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/blade_cap.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5eae5b1ee637a532232c7d112e1e03ee32ee4a1c5b84b754a3a3b3755f7f5c48 +size 86107 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/blueberry.usdc b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/blueberry.usdc new file mode 100644 index 000000000000..d629bdb2843b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/blueberry.usdc @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b6df7916c1de16e819ebe0311cdb55a5262bd61ca248f04004f9459fb0254fd3 +size 49238 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/cup.usda b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/cup.usda new file mode 100644 index 000000000000..15b4397a2c9a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/cup.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:076cae11fb235cb880e6fbb0da7a0b581ac2fcd062a761e48a0c6f25bd8baa52 +size 83877 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/franka.usdc b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/franka.usdc new file mode 100644 index 000000000000..c92616d69899 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/franka.usdc @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d143da26e899f44e65c45441f46d161e773d21d9f0bcd0dc67224c65f921b622 +size 7117657 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/fruit_basket.usda b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/fruit_basket.usda new file mode 100644 index 000000000000..038f3f9f3ad8 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/fruit_basket.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:62d78a0be9626a8335800d900d18b777e4c9ad91f36fbb2cfea28fbab3dd660b +size 464264 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/mango.usda b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/mango.usda new file mode 100644 index 000000000000..9b1b58671eb7 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/mango.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4b4bc1849256d77ee31d3a98df0d7370c9caad9d047d188ae0084c11a69e075f +size 12362 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/motor.usda b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/motor.usda new file mode 100644 index 000000000000..d78954be8950 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/motor.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0b09e8a68ff9998103ac209301bafc8cf65c5c6c9d5aca3b50f7865af8c76f4e +size 100554 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/overrides/strawberry.usda b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/overrides/strawberry.usda new file mode 100644 index 000000000000..3c3cf451d63c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/overrides/strawberry.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e2b0fa31e3133b2f5f5205162527630fb2344baaf2564603d5005c9b56a0d27f +size 427 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/overrides/workstation.usda b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/overrides/workstation.usda new file mode 100644 index 000000000000..e43d104bbabd --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/overrides/workstation.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4d4b2a2571c4d587e5c49f7f0381b1ffef5b8ac4313ed8fddb54adba1e037d0a +size 165 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/strawberry.usdc b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/strawberry.usdc new file mode 100644 index 000000000000..f5679dd82388 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/strawberry.usdc @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7b48325f46aed22be4fc9e8921aeff9c26ddae59e7f837a625afa6c40fd302f1 +size 237235 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/tap.usda b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/tap.usda new file mode 100644 index 000000000000..b985be65e2fa --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/tap.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:59a51256a2c190fc5d301f7f29f318d2dd869c18ff16931f220989690c14c188 +size 9506 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/workstation.usda b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/workstation.usda new file mode 100644 index 000000000000..75d2bcd8a3a5 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/assets/workstation.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:aa2a18c4c0b7940379a8d46f9a711f8c5ba50ad13888da11ba0117abe087fed4 +size 38212 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_controller.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_controller.py new file mode 100644 index 000000000000..e4d41012a8eb --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_controller.py @@ -0,0 +1,285 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Scripted pickup followed by a measured fruit pour and gated basket return. + +The maintained basket expert commands raw robot actions and retains live EMA +history. The environment owns receipt/support measurements and all task latches; +physical success requires a complete simulated recording. +""" + +from __future__ import annotations + +from typing import Any + +import numpy as np +import torch +from scipy.spatial.transform import Rotation + +from .basket_pose_controller import OPTIONS as POSE_OPTIONS +from .basket_pose_controller import BasketPoseController +from .controlled_fruit_pour import OPTIONS as POUR_OPTIONS +from .controlled_fruit_pour import ControlledFruitPour +from .pose_control import BasketExpert + +POLICY_DT_S = 1.0 / 30.0 +ARM_NAMES = tuple(f"panda_joint{i}" for i in range(1, 8)) +PICKUP_RECIPE = { + "controller": "BasketExpert", + "reference": "configured live basket rim grasp pose; actual TCP captured when closing starts", + "grasp_frame": "continuous_env.make_cfg; exact basket offset/quaternion and rationale recorded in profile", + "approach_above_grasp_m": 0.15, + "descent_start_s": 3.0, + "descent_duration_s": 3.5, + "close_start_s": 7.5, + "lift_start_s": 11.0, + "lift_duration_s": 3.5, + "lift_height_m": 0.15, + "after_lift": "hold the raised target until the measured pickup handoff qualifies", +} +OPTIONS = { + "pickup_recipe": PICKUP_RECIPE, + "basket_pose_adapter": POSE_OPTIONS, + "clock": "basket-local policy steps at 30 Hz; measured gates own phase transitions", + "arm_scale": 0.03, + "arm_alpha": 0.2, + "gripper_alpha": 0.04, + "return_gate": "delivery_complete and current receipt and held and handoff_hold_s >= 0.25", + "minimum_fruit_count": 16, + "pickup_handoff": "valid held grasp and measured strict upright lift complete", + "pour": POUR_OPTIONS, + "held_handoff_s": 0.25, + "support_hold_s": 0.5, + "tracking_position_tolerance_m": 0.015, + "tracking_rotation_tolerance_rad": 0.15, + "return_nominal_duration_s": 42.0, + "return_grasp": "measured once at qualified handoff; no object attachment", + "return_clock": "pauses on tracking/grasp/support/open gates", + "global_timeout": "owned by supervisor; no controller timeout or environment reset", + "phase0_gripper_open_m": 0.015, + "phase0_gripper_close_m": 0.0, + "physical_transfer_validated": False, +} +from .return_path import ReturnCandidate + + +def _one(value: Any) -> Any: + if isinstance(value, torch.Tensor): + if value.numel() != 1: + raise ValueError("Basket controller requires one-world scalar measurements.") + return value.item() + return value + + +class BasketController: + """Compute one world's eight raw actions using measured state, without state writes. + + Args: + env: Combined environment exposing basket-specific measurements/latches, + actual named joints, arm EMA history, TCP and rigid-body poses [m, XYZW]. + """ + + def __init__(self, env: Any): + arm = env.cfg.actions.arm_action + if ( + arm.scale != OPTIONS["arm_scale"] + or arm.alpha != OPTIONS["arm_alpha"] + or not arm.use_zero_offset + or arm.clip is not None + ): + raise ValueError("Named-joint arm scale, EMA, zero offset and clipping must be preserved.") + gripper = env.cfg.actions.gripper_action + if ( + gripper.open_positions_by_phase != {0: 0.015, 1: 0.04, 2: 0.04, 3: 0.04, 4: 0.04, 5: 0.04} + or gripper.close_positions_by_phase != {0: 0.0, 1: 0.0, 2: 0.0, 3: 0.0, 4: 0.0, 5: 0.0} + or gripper.default_position != 0.015 + or gripper.alpha != OPTIONS["gripper_alpha"] + ): + raise ValueError("The combined phase gripper must preserve original basket targets and EMA.") + ids, names = env.robot.find_joints(list(ARM_NAMES), preserve_order=True) + if tuple(names) != ARM_NAMES or len(set(ids)) != 7: + raise ValueError("Expected seven distinct, correctly ordered named Panda arm joints.") + self.env, self.joint_ids = env, ids + self.pickup = BasketExpert(env) + self.pose_controller = BasketPoseController(env) + self.stage = "scripted_pickup" + self.diagnostics: dict[str, Any] = {} + self._return: Any | None = None + self._pour: ControlledFruitPour | None = None + self._return_elapsed = 0.0 + self._open_started = False + self._last_step = -1 + + @torch.no_grad() + def compute(self, local_step: int) -> torch.Tensor: + """Return raw actions, shape [1, 8], at a basket-local policy step. + + A new local step zero clears only this controller's phase state. It never + resets joints, objects, action filters or the environment's task latches. + """ + if type(local_step) is not int or local_step < 0: + raise ValueError("Require a nonnegative integer component-local step.") + if local_step == 0: + self._return, self._return_elapsed, self._open_started = None, 0.0, False + self._pour = None + elif local_step <= self._last_step: + raise ValueError("Local steps must advance, except for an explicit component reset to zero.") + if int(_one(self.env.phase)) != 0: + raise ValueError("BasketController may execute only in basket phase zero.") + self._last_step = local_step + m, state = self.env.basket_measurements(), self.env.basket_state + valid = bool(_one(m["finite"])) and not bool(_one(m["failed"])) + receipt = int(_one(m["fruit_count"])) >= OPTIONS["minimum_fruit_count"] and bool(_one(m["all_types"])) + held = bool(_one(m["held"])) + eligible = ( + valid + and receipt + and held + and bool(_one(state["delivery_complete"])) + and float(_one(state["handoff_hold_s"])) >= OPTIONS["held_handoff_s"] - 1e-6 + ) + self.diagnostics = { + "local_step": local_step, + "local_time_s": local_step * POLICY_DT_S, + "valid": valid, + "held": held, + "strict_held": bool(_one(m.get("strict_held", m["held"]))), + "grasp_continuation": bool(_one(m.get("grasp_continuation", False))), + "receipt_current": receipt, + "return_handoff_eligible": eligible, + "pickup_recipe": PICKUP_RECIPE, + } + if self._return is None and eligible: + basket, tcp, hand, cup = self._measured_pose() + self._return = ReturnCandidate(basket, tcp, hand, cup) + self._return_elapsed = 0.0 + self.diagnostics["captured_grip_b_m"] = self._return.grip_b.tolist() + self.diagnostics["captured_hand_b_xyzw"] = self._return.hand_b.as_quat().tolist() + if ( + self._return is None + and self._pour is None + and valid + and held + and bool(_one(self.env.basket_strict_lift_complete)) + ): + basket, tcp, hand, cup = self._measured_pose() + self._pour = ControlledFruitPour(basket[:3], basket[3:], tcp, hand.as_quat(), cup[:3], cup[3:], POLICY_DT_S) + if self._return is None and self._pour is None: + self.stage = "scripted_pickup" + actions = self._scripted_actions(local_step) + elif self._return is None: + actions = self._pour_actions(valid, held, int(_one(m["fruit_count"]))) + else: + actions = self._return_actions(m, state, valid, held) + if actions.shape != (1, 8) or not torch.isfinite(actions).all() or (actions.abs() > 1).any(): + raise ValueError("Basket controller produced invalid raw actions.") + self.diagnostics["stage"] = self.stage + return actions + + def _scripted_actions(self, local_step: int) -> torch.Tensor: + actions = self.pickup.compute(local_step) + elapsed = local_step * POLICY_DT_S + if elapsed < PICKUP_RECIPE["close_start_s"]: + phase = "approach" + elif elapsed < PICKUP_RECIPE["lift_start_s"]: + phase = "close_dwell" + elif elapsed < PICKUP_RECIPE["lift_start_s"] + PICKUP_RECIPE["lift_duration_s"]: + phase = "lift" + else: + phase = "hold_raised" + self.diagnostics.update( + pickup_phase=phase, + raw_gripper_sign=float(actions[0, 7]), + ) + return actions + + def _measured_pose(self) -> tuple[np.ndarray, np.ndarray, Rotation, np.ndarray]: + origin = self.env.scene.env_origins[0].detach().cpu().numpy() + basket = self.env.pose("basket")[0].detach().cpu().numpy().copy() + cup = self.env.pose("cup")[0].detach().cpu().numpy().copy() + basket[:3] -= origin + cup[:3] -= origin + tcp = self.env.tcp()[0].detach().cpu().numpy() - origin + quat = self.env.robot.data.body_link_pose_w.torch[0, self.env.hand_id, 3:].detach().cpu().numpy() + if not all(np.isfinite(a).all() for a in (basket, cup, tcp, quat)): + raise ValueError("Nonfinite measured basket handoff geometry.") + return basket, tcp, Rotation.from_quat(quat), cup + + def _pour_actions(self, valid: bool, held: bool, fruit_count: int) -> torch.Tensor: + """Command the measured pouring reference without changing physical state.""" + basket, tcp, hand, cup = self._measured_pose() + sample = self._pour.step( + basket_position=basket[:3], + basket_quaternion=basket[3:], + tcp_position=tcp, + hand_quaternion=hand.as_quat(), + cup_position=cup[:3], + cup_quaternion=cup[3:], + held=held, + valid=valid, + delivered_count=fruit_count, + ) + target = self.env.tcp().clone() + target[0] = target.new_tensor(sample["tcp"]) + self.env.scene.env_origins[0] + rotation = target.new_tensor(sample["hand_quat"])[None] + self.stage = "measured_pour_" + sample["diagnostics"]["phase"] + self.diagnostics.update(sample["diagnostics"]) + actions = self.pose_controller.compute(target, rotation, True) + self.diagnostics["raw_gripper_sign"] = float(actions[0, 7]) + return actions + + def _return_actions(self, m: dict, state: dict, valid: bool, held: bool) -> torch.Tensor: + _, tcp, hand, _ = self._measured_pose() + t = self._return_elapsed + supported = bool(_one(m["supported"])) + support_ready = ( + valid + and bool(_one(state["controlled_descent"])) + and supported + and float(_one(state["support_hold_s"])) >= OPTIONS["support_hold_s"] - 1e-6 + ) + paused = False + if t >= 34.0 and not self._open_started: + if support_ready: + self._open_started = True + else: + t, paused = 34.0 - 1e-6, True + if t >= 37.0 and not (valid and bool(_one(m["open"])) and supported): + t, paused = 37.0 - 1e-6, True + sample = self._return.sample(t) + error_p = float(np.linalg.norm(sample["tcp_position"] - tcp)) + error_r = float((Rotation.from_quat(sample["hand_xyzw"]) * hand.inv()).magnitude()) + tracking = ( + error_p < OPTIONS["tracking_position_tolerance_m"] and error_r < OPTIONS["tracking_rotation_tolerance_rad"] + ) + advance = valid and tracking and (t >= 29.0 or held) and not paused + self._return_elapsed = min(t + POLICY_DT_S, OPTIONS["return_nominal_duration_s"]) if advance else t + self.stage = "return_" + sample["phase"] + if paused: + self.stage = "await_measured_open" if self._open_started else "await_supported_setdown" + if ( + bool(_one(state["finished"])) + and valid + and supported + and bool(_one(m["open"])) + and bool(_one(m["separated"])) + ): + self.stage = "basket_released" + target = self.env.tcp().clone() + target[0] = target.new_tensor(sample["tcp_position"]) + self.env.scene.env_origins[0] + rotation = target.new_tensor(sample["hand_xyzw"])[None] + actions = self.pose_controller.compute(target, rotation, not self._open_started) + self.diagnostics.update( + return_elapsed_s=t, + return_clock_advanced=advance, + support_ready=support_ready, + open_started=self._open_started, + measured_open=bool(_one(m["open"])), + measured_separated=bool(_one(m["separated"])), + tracking_position_error_m=error_p, + tracking_rotation_error_rad=error_r, + raw_gripper_sign=float(actions[0, 7]), + ) + return actions diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_geometry.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_geometry.py new file mode 100644 index 000000000000..42b22fab0551 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_geometry.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Dimensions shared by the rigid fruit basket, the reset layout and the basket stage [m].""" + +BASKET_POSITION = (0.36, 0.27, 0.002) +BASKET_BOTTOM_RADIUS = 0.0375 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_grasp_continuation.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_grasp_continuation.py new file mode 100644 index 000000000000..4008bb535d9d --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_grasp_continuation.py @@ -0,0 +1,257 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Acquire basket grasp evidence from coupled motion, then track measured continuation.""" + +from __future__ import annotations + +import math +from collections import deque + +import torch + +OPTIONS = { + "version": 3, + "initialization": "One second of closed bilateral deflection, raised upright basket and coupled measured motion.", + "minimum_each_deflection_m": 0.0009, + "minimum_sustained_each_deflection_m": 0.0008, + "maximum_closed_command_m": 0.0001, + "maximum_relative_speed_m_s": 0.02, + "minimum_finger_width_m": 0.001, + "maximum_finger_width_m": 0.065, + "stability_window_s": 1.0, + "maximum_relative_translation_drift_m": 0.002, + "maximum_relative_rotation_drift_rad": math.radians(3), + "acquisition_minimum_clearance_m": 0.08, + "acquisition_minimum_upright_cosine": 0.9, + "acquisition_minimum_each_path_m": 0.005, + "acquisition_minimum_each_displacement_m": 0.005, + "acquisition_minimum_motion_correlation": 0.95, + "interface_tcp_radial_range_m": [0.035, 0.075], + "interface_tcp_height_range_m": [0.060, 0.110], + "loss_each_deflection_below_m": 0.0002, + "loss_reference_translation_drift_m": 0.01, + "loss_reference_rotation_drift_rad": math.radians(30), + "maximum_frame_consistency_error_m": 0.00002, + "window_rule": ( + "Every sample qualifies across the full1s stability window. Acquisition requires each finger>=0.9mm; " + "only an existing acquisition credential permits sustained each-finger deflection>=0.8mm. " + "Captured basket point and TCP motion must agree for acquisition." + ), + "scope": "Alternate basket azimuth allowed; nominal-point strict held and strict lift remain separate telemetry.", + "evidence": ( + "v2 acquisition used coupled travel120mm with .401mm/1.86deg drift. Closed v4 probe retained its " + "acquisition credential but late0.893mm deflection samples interrupted current-held qualification " + "despite stable relative geometry. Sustained hysteresis requires a new physical validation." + ), + "limitation": "Deflection, material-interface proximity and coupled-motion evidence; no recorded force assertion.", +} + + +def rotate(rotation, vector): + """Rotate vectors [m] with normalized XYZW quaternions.""" + uv = torch.linalg.cross(rotation[..., :3], vector) + return vector + 2 * (rotation[..., 3:] * uv + torch.linalg.cross(rotation[..., :3], uv)) + + +def stable_window(qualified, positions, rotations, span_s): + """Evaluate a measured relative-pose window [m, rad, s].""" + translation = torch.linalg.vector_norm(positions - positions[0], dim=-1).amax(0) + dots = (rotations * rotations[0]).sum(-1).abs().clamp(0, 1) + rotation = (2 * torch.acos(dots)).amax(0) + accepted = ( + qualified.all(0) + & (span_s >= OPTIONS["stability_window_s"] - 1e-6) + & (translation <= OPTIONS["maximum_relative_translation_drift_m"]) + & (rotation <= OPTIONS["maximum_relative_rotation_drift_rad"]) + ) + return accepted, translation, rotation + + +class BasketGraspContinuation: + """Track one world's grasp evidence without modifying physical state. + + Args: + device: Tensor device. + step_dt: Policy interval [s], required to be 1/30. + """ + + def __init__(self, device: str, step_dt: float): + self.device, self.step_dt = device, step_dt + self.window_steps = round(OPTIONS["stability_window_s"] / step_dt) + self.history = deque(maxlen=self.window_steps + 1) + self.reset() + + def reset(self) -> None: + """Discard all acquisition credentials and sample coverage on reset.""" + self.history.clear() + self.last_step = None + self.current = torch.zeros(1, dtype=torch.bool, device=self.device) + self.acquired = torch.zeros_like(self.current) + self.acquisition_step = torch.full((1,), -1, dtype=torch.long, device=self.device) + self.reference_position = torch.zeros(1, 3, device=self.device) + self.reference_rotation = torch.tensor([[0.0, 0.0, 0.0, 1.0]], device=self.device) + for name in ( + "window_s", + "translation_drift_m", + "rotation_drift_rad", + "hand_path_m", + "basket_path_m", + "hand_displacement_m", + "basket_displacement_m", + "motion_correlation", + ): + setattr(self, name, torch.zeros(1, device=self.device)) + + @torch.no_grad() + def update( + self, + step: int, + *, + valid: torch.Tensor, + command_m: torch.Tensor, + deflection_m: torch.Tensor, + finger_width_m: torch.Tensor, + relative_speed_m_s: torch.Tensor, + grip_position_b_m: torch.Tensor, + hand_rotation_b_xyzw: torch.Tensor, + tcp_position_w_m: torch.Tensor, + basket_pose_w: torch.Tensor, + clearance_m: torch.Tensor, + upright: torch.Tensor, + ) -> torch.Tensor: + """Return current grasp evidence from copied measured inputs [m, rad, s]. + + Acquisition requires nonzero coupled motion and strong bilateral deflection. + Later stationary holding permits the lower sustained deflection threshold + only with that credential and a full rolling stable window. + Opening, invalid state, clear loss, reset or missing cadence clears it. + """ + if type(step) is not int or step < 0: + raise ValueError("Require a nonnegative integer global policy step.") + if self.last_step == step: + return self.current + if self.last_step is not None and step != self.last_step + 1: + self.reset() + expected = ( + (valid, (1,)), + (command_m, (1, 1)), + (deflection_m, (1, 2)), + (finger_width_m, (1,)), + (relative_speed_m_s, (1,)), + (grip_position_b_m, (1, 3)), + (hand_rotation_b_xyzw, (1, 4)), + (tcp_position_w_m, (1, 3)), + (basket_pose_w, (1, 7)), + (clearance_m, (1,)), + (upright, (1,)), + ) + if any(value.shape != shape for value, shape in expected): + raise ValueError("Require one-world measured tensors.") + finite = torch.stack([torch.isfinite(v).reshape(1, -1).all(-1) for v, _ in expected]).all(0) + hand_norm = torch.linalg.vector_norm(hand_rotation_b_xyzw, dim=-1, keepdim=True) + body_norm = torch.linalg.vector_norm(basket_pose_w[:, 3:], dim=-1, keepdim=True) + rotation = hand_rotation_b_xyzw / hand_norm.clamp_min(1e-8) + body_rotation = basket_pose_w[:, 3:] / body_norm.clamp_min(1e-8) + radial = torch.linalg.vector_norm(grip_position_b_m[:, :2], dim=-1) + radius_range, height_range = OPTIONS["interface_tcp_radial_range_m"], OPTIONS["interface_tcp_height_range_m"] + interface = ( + (radial >= radius_range[0]) + & (radial <= radius_range[1]) + & (grip_position_b_m[:, 2] >= height_range[0]) + & (grip_position_b_m[:, 2] <= height_range[1]) + ) + closed = (command_m[:, 0] >= 0) & (command_m[:, 0] <= OPTIONS["maximum_closed_command_m"]) + frame_error = torch.linalg.vector_norm( + rotate(body_rotation, grip_position_b_m) + basket_pose_w[:, :3] - tcp_position_w_m, dim=-1 + ) + measured_valid = ( + valid + & finite + & (hand_norm[:, 0] > 1e-8) + & (body_norm[:, 0] > 1e-8) + & (frame_error <= OPTIONS["maximum_frame_consistency_error_m"]) + ) + reference_distance = torch.linalg.vector_norm(grip_position_b_m - self.reference_position, dim=-1) + reference_angle = 2 * torch.acos((rotation * self.reference_rotation).sum(-1).abs().clamp(0, 1)) + lost = ( + ~measured_valid + | ~closed + | ~interface + | (deflection_m.amin(-1) < OPTIONS["loss_each_deflection_below_m"]) + | ( + self.acquired + & ( + (reference_distance > OPTIONS["loss_reference_translation_drift_m"]) + | (reference_angle > OPTIONS["loss_reference_rotation_drift_rad"]) + ) + ) + ) + if bool(lost.any()): + self.reset() + minimum_deflection = torch.where( + self.acquired, + OPTIONS["minimum_sustained_each_deflection_m"], + OPTIONS["minimum_each_deflection_m"], + ) + qualified = ( + measured_valid + & closed + & interface + & (deflection_m.amin(-1) >= minimum_deflection) + & (finger_width_m > OPTIONS["minimum_finger_width_m"]) + & (finger_width_m < OPTIONS["maximum_finger_width_m"]) + & (relative_speed_m_s >= 0) + & (relative_speed_m_s < OPTIONS["maximum_relative_speed_m_s"]) + ) + raised = (clearance_m >= OPTIONS["acquisition_minimum_clearance_m"]) & ( + upright >= OPTIONS["acquisition_minimum_upright_cosine"] + ) + self.history.append( + ( + step, + qualified.clone(), + torch.nan_to_num(grip_position_b_m).clone(), + torch.nan_to_num(rotation).clone(), + torch.nan_to_num(tcp_position_w_m).clone(), + torch.nan_to_num(basket_pose_w[:, :3]).clone(), + torch.nan_to_num(body_rotation).clone(), + raised.clone(), + ) + ) + self.last_step = step + self.window_s.fill_((step - self.history[0][0]) * self.step_dt) + positions = torch.stack([row[2] for row in self.history]) + rotations = torch.stack([row[3] for row in self.history]) + stable, self.translation_drift_m, self.rotation_drift_rad = stable_window( + torch.stack([row[1] for row in self.history]), positions, rotations, self.window_s + ) + hand_path = torch.stack([row[4] for row in self.history]) + body_positions = torch.stack([row[5] for row in self.history]) + body_rotations = torch.stack([row[6] for row in self.history]) + object_path = rotate(body_rotations, positions[0].expand_as(body_positions)) + body_positions + dh, db = torch.diff(hand_path, dim=0), torch.diff(object_path, dim=0) + self.hand_path_m = torch.linalg.vector_norm(dh, dim=-1).sum(0) + self.basket_path_m = torch.linalg.vector_norm(db, dim=-1).sum(0) + self.hand_displacement_m = torch.linalg.vector_norm(hand_path[-1] - hand_path[0], dim=-1) + self.basket_displacement_m = torch.linalg.vector_norm(object_path[-1] - object_path[0], dim=-1) + denominator = torch.sqrt((dh.square().sum((0, 2))) * (db.square().sum((0, 2)))) + self.motion_correlation = (dh * db).sum((0, 2)) / denominator.clamp_min(1e-16) + acquisition = ( + stable + & torch.stack([row[7] for row in self.history]).all(0) + & (self.hand_path_m >= OPTIONS["acquisition_minimum_each_path_m"]) + & (self.basket_path_m >= OPTIONS["acquisition_minimum_each_path_m"]) + & (self.hand_displacement_m >= OPTIONS["acquisition_minimum_each_displacement_m"]) + & (self.basket_displacement_m >= OPTIONS["acquisition_minimum_each_displacement_m"]) + & (self.motion_correlation >= OPTIONS["acquisition_minimum_motion_correlation"]) + ) + newly_acquired = acquisition & ~self.acquired + self.reference_position = torch.where(newly_acquired[:, None], grip_position_b_m, self.reference_position) + self.reference_rotation = torch.where(newly_acquired[:, None], rotation, self.reference_rotation) + self.acquisition_step = torch.where(newly_acquired, step, self.acquisition_step) + self.acquired |= acquisition + self.current = self.acquired & stable & ~lost + return self.current diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_pose_controller.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_pose_controller.py new file mode 100644 index 000000000000..74aae4cccdfc --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_pose_controller.py @@ -0,0 +1,63 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Local basket pose adapter preserving DLS direction before existing action limits. + +Pickup retains the maintained controller. Pour and return use the same IK, TCP +Jacobian, EMA inversion and gripper commands, changing only joint-step limiting. +Final raw-action clipping remains unchanged and can still change the direction. +""" + +from __future__ import annotations + +import torch + +from isaaclab.utils import math as math_utils + +from .pose_control import PoseController + +OPTIONS = { + "algorithm": "basket_dls_uniform_joint_increment_scaling_v1", + "scope": "measured basket pour and return only; maintained pickup unchanged", + "maximum_joint_increment_rad": 0.012, + "ik": "inherited PoseController DLS and TCP-offset Jacobian", + "limiting": "delta *= min(1, 0.012 / max(abs(delta))) independently per world", + "action_filter": "unchanged live EMA inversion followed by raw [-1, 1] clamp", + "rationale": "Closed S2 seed 70042: elementwise joint clipping reversed the TCP correction near a singularity.", + "physical_validation_passed": False, +} + + +def basket_joint_increment(delta: torch.Tensor) -> torch.Tensor: + """Limit each world's joint increment [rad], shape [..., 7], by one positive scale. + + Zero and unsaturated inputs are preserved. The input tensor is not modified. + """ + limit = OPTIONS["maximum_joint_increment_rad"] + peak = delta.abs().amax(dim=-1, keepdim=True) + return torch.where(peak > limit, (delta / peak.clamp_min(limit)) * limit, delta) + + +class BasketPoseController(PoseController): + """Use the maintained pose/action mapping with uniformly limited joint steps.""" + + def compute(self, position: torch.Tensor, rotation: torch.Tensor, close: bool | torch.Tensor) -> torch.Tensor: + """Convert world TCP position [m], XYZW rotation and gripper commands to raw actions.""" + env = self.env + hand = env.robot.data.body_link_pose_w.torch[:, env.hand_id] + jacobian = env.robot.data.body_link_jacobian_w.torch[:, env.hand_id - 1, :, self.joint_ids].clone() + offset = env.tcp() - hand[:, :3] + jacobian[:, :3] -= torch.bmm(math_utils.skew_symmetric_matrix(offset), jacobian[:, 3:]) + joints = env.robot.data.joint_pos.torch[:, self.joint_ids] + self.controller.set_command(torch.cat((position, rotation), -1)) + goal = self.controller.compute(env.tcp(), hand[:, 3:], jacobian, joints) + term = env.action_manager.get_term("arm_action") + alpha = env.cfg.actions.arm_action.alpha + delta = basket_joint_increment(goal - joints) + raw = (delta - (1 - alpha) * term.processed_actions) / (alpha * env.cfg.actions.arm_action.scale) + actions = torch.zeros((env.num_envs, 8), device=env.device) + actions[:, :7] = raw.clamp(-1, 1) + actions[:, -1] = torch.where(torch.as_tensor(close, device=env.device), -1.0, 1.0) + return actions diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_reference.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_reference.py new file mode 100644 index 000000000000..a0b7d39cca97 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_reference.py @@ -0,0 +1,113 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""CPU-only basket-pour reference geometry and current-robot IK audit. + +This constructs commands, not an attachment or a prescribed simulated object pose. +Only the TCP target is intended to be sent to PoseController. +""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np +from scipy.spatial.transform import Rotation, Slerp + +ROOT = Path(__file__).resolve().parent +LIP_B = np.array([0.0, 0.0605, 0.100]) +CUP_RIM_LOCAL_Z = 0.210 + + +def smooth(u): + u = np.clip(u, 0.0, 1.0) + return u * u * (3.0 - 2.0 * u) + + +class BasketPourReference: + """Preserve a measured TCP-to-basket transform through a slow reference path.""" + + def __init__( + self, + basket_pose, + tcp_position, + hand_xyzw, + cup_pose, + yaw_deg=0.0, + angle_deg=100.0, + clearance_m=0.030, + follow_clearance=False, + align_grasp_radial=False, + ): + self.start_p = np.asarray(basket_pose[:3]).copy() + self.start_r = Rotation.from_quat(basket_pose[3:]) + self.grip_b = self.start_r.inv().apply(np.asarray(tcp_position) - self.start_p) + self.hand_b = self.start_r.inv() * Rotation.from_quat(hand_xyzw) + self.cup = np.asarray(cup_pose).copy() + self.lip_b = LIP_B.copy() + self.pour_yaw = Rotation.from_euler("z", yaw_deg, degrees=True) + self.grasp_alignment = Rotation.identity() + if align_grasp_radial: + radial_distance = np.linalg.norm(self.grip_b[:2]) + if radial_distance < 1e-6: + raise ValueError("Radial alignment requires a grasp away from the basket axis.") + self.lip_b[:2] = -self.grip_b[:2] * LIP_B[1] / radial_distance + phi = np.arctan2(-self.grip_b[0], -self.grip_b[1]) + self.grasp_alignment = Rotation.from_rotvec([0.0, 0.0, phi]) + self.upright = self.pour_yaw * self.grasp_alignment + self.to_upright = Slerp([0.0, 1.0], Rotation.concatenate([self.start_r, self.upright])) + self.angle = np.deg2rad(angle_deg) + self.clearance_m = clearance_m + self.follow_clearance = follow_clearance + angles = np.linspace(0, 2 * np.pi, 128, endpoint=False) + self.boundary_b = np.concatenate( + [ + np.stack([radius * np.cos(angles), radius * np.sin(angles), np.full_like(angles, z)], -1) + for radius, z in [(0.039, 0.0), (0.0605, 0.1)] + ] + ) + + def sample(self, time_s): + # Relative to an already established grasp: raise3s, carry6s, tilt12s, hold6s. + high = self.start_p.copy() + high[2] = max(high[2], self.cup[2] + CUP_RIM_LOCAL_Z + (self.clearance_m if self.follow_clearance else 0.07)) + cup_xy = self.cup[:2] + hover = np.r_[cup_xy, high[2] + 0.100] - self.upright.apply(self.lip_b) + if time_s < 3.0: + fraction = smooth(time_s / 3.0) + position = (1 - fraction) * self.start_p + fraction * high + rotation = self.start_r + phase = "raise" + elif time_s < 9.0: + fraction = smooth((time_s - 3.0) / 6.0) + position = (1 - fraction) * high + fraction * hover + rotation = self.to_upright(float(fraction)) + phase = "carry" + else: + fraction = smooth((time_s - 9.0) / 12.0) + rotation = self.pour_yaw * Rotation.from_rotvec([-self.angle * fraction, 0, 0]) * self.grasp_alignment + # Keep the lowest basket point above the receiving rim throughout tilt. + min_relative_z = rotation.apply(self.boundary_b - self.lip_b)[:, 2].min() + floor = self.cup[2] + CUP_RIM_LOCAL_Z + self.clearance_m - min_relative_z + initial_lip_z = high[2] + 0.100 + # Descend gradually from the high carry position, without a phase jump. + proposed = initial_lip_z * (1 - fraction) + (self.cup[2] + CUP_RIM_LOCAL_Z + self.clearance_m) * fraction + lip = np.r_[cup_xy, floor if self.follow_clearance else max(floor, proposed)] + position = lip - rotation.apply(self.lip_b) + phase = "tip" if time_s < 21.0 else "hold" + tcp = position + rotation.apply(self.grip_b) + hand = rotation * self.hand_b + lip = position + rotation.apply(self.lip_b) + minimum_z = (position + rotation.apply(self.boundary_b))[:, 2].min() + return { + "phase": phase, + "basket_position": position, + "basket_xyzw": rotation.as_quat(), + "tcp_position": tcp, + "hand_xyzw": hand.as_quat(), + "lip_position": lip, + "lowest_basket_z": float(minimum_z), + } diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_stage.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_stage.py new file mode 100644 index 000000000000..eebdbb2cd74f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/basket_stage.py @@ -0,0 +1,141 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""Measured fruit delivery and basket return gates.""" + +import math + +import torch + +from isaaclab.utils import math as math_utils + +from .basket_geometry import BASKET_POSITION + +CRITERIA = { + "receipt_metric": "whole_authored_collision_hulls_in_actual_cup_zero_tolerance", + "geometry_tolerance_m": 0.0, + "fruit_count": 16, + "minimum_delivered_fruit": 16, + "all_four_fruit_types_required": True, + "legacy_center_region_for_comparison_only": {"radial_lt_m": 0.035, "z_gt_m": 0.01, "z_lt_m": 0.185}, + "held_upright_lift_m": 0.08, + "held_lift_upright_cosine": 0.9, + "held_lift_hold_s": 1.0, + "delivery_hold_s": 1.0, + "held_return_handoff_s": 0.25, + "home_position_m": list(BASKET_POSITION), + "home_xy_tolerance_m": 0.02, + "upright_cosine": math.cos(math.radians(5.0)), + "floor_radius_m": 0.039, + "floor_height_m": 0.003, + "table_height_m": 0.0, + "controlled_descent_floor_max_m": 0.015, + "supported_floor_min_m": -0.001, + "supported_floor_max_m": 0.003, + "maximum_linear_speed_m_s": 0.01, + "maximum_angular_speed_rad_s": 0.1, + "support_hold_before_open_s": 0.5, + "minimum_open_fraction_each_finger": 0.85, + "minimum_open_command_fraction": 0.9, + "released_tcp_to_grasp_m": 0.10, + "released_stable_hold_s": 2.0, + "support_semantics": ( + "Basket floor at the authored table plane, upright and stationary; no direct contact-force assertion." + ), + "failure": "Unchanged maintained mdp.failure; nonfinite or failed samples never advance milestones.", +} + +FLOAT_STATES = ("lift_hold_s", "delivery_hold_s", "handoff_hold_s", "support_hold_s", "released_hold_s") + +BOOL_STATES = ("lift_complete", "delivery_complete", "controlled_descent", "support_complete", "finished") + + +def new_milestones(num_envs: int, device: str) -> dict[str, torch.Tensor]: + """Create resettable per-world holds [s] and ordered milestone flags.""" + return { + **{name: torch.zeros(num_envs, device=device) for name in FLOAT_STATES}, + **{name: torch.zeros(num_envs, device=device, dtype=torch.bool) for name in BOOL_STATES}, + } + + +def reset_milestones(state: dict[str, torch.Tensor], env_ids: torch.Tensor) -> None: + """Clear only the resetting worlds' task history.""" + for value in state.values(): + value[env_ids] = 0 + + +def placement_geometry(pose, velocity, tcp, fingers, commanded, origin, open_position, grasp): + """Measure support, opening and hand separation from raw SI-unit state tensors.""" + up = math_utils.quat_apply(pose[:, 3:], pose.new_tensor([0.0, 0.0, 1.0]).expand(len(pose), -1))[:, 2] + local_position = pose[:, :3] - origin + # Exact lower support height of the authored floor cylinder [m]. + low = ( + local_position[:, 2] + + 0.5 * CRITERIA["floor_height_m"] * (up - up.abs()) + - CRITERIA["floor_radius_m"] * (1 - up.square()).clamp_min(0).sqrt() + ) + home_xy = (local_position[:, :2] - pose.new_tensor(BASKET_POSITION[:2])).norm(dim=-1) + linear, angular = velocity[:, :3].norm(dim=-1), velocity[:, 3:].norm(dim=-1) + separation = (tcp - grasp).norm(dim=-1) + near_home = (home_xy <= CRITERIA["home_xy_tolerance_m"]) & (up >= CRITERIA["upright_cosine"]) + supported = ( + near_home + & (low >= CRITERIA["supported_floor_min_m"]) + & (low <= CRITERIA["supported_floor_max_m"]) + & (linear <= CRITERIA["maximum_linear_speed_m_s"]) + & (angular <= CRITERIA["maximum_angular_speed_rad_s"]) + ) + opened = (fingers >= open_position * CRITERIA["minimum_open_fraction_each_finger"]).all(-1) & ( + commanded >= open_position * CRITERIA["minimum_open_command_fraction"] + ).all(-1) + finite = torch.isfinite(torch.cat((pose, velocity, tcp, fingers, commanded, grasp), -1)).all(-1) + return { + "floor_min_z_m": low, + "home_xy_error_m": home_xy, + "basket_upright": up, + "basket_linear_speed_m_s": linear, + "basket_angular_speed_rad_s": angular, + "tcp_to_grasp_m": separation, + "near_home": near_home & finite, + "supported": supported & finite, + "open": opened & finite, + "separated": (separation > CRITERIA["released_tcp_to_grasp_m"]) & finite, + "finite": finite, + } + + +def advance_milestones(state: dict[str, torch.Tensor], measurements: dict[str, torch.Tensor], step_dt: float) -> None: + """Advance valid ordered basket milestones with a policy timestep [s].""" + m = measurements + valid = ~m["failed"] & m["finite"] + + def hold(name: str, condition: torch.Tensor) -> None: + state[name].copy_(torch.where(condition & valid, state[name] + step_dt, 0.0)) + + hold( + "lift_hold_s", + m["held"] + & (m["clearance_m"] >= CRITERIA["held_upright_lift_m"]) + & (m["basket_upright"] >= CRITERIA["held_lift_upright_cosine"]), + ) + state["lift_complete"] |= state["lift_hold_s"] >= CRITERIA["held_lift_hold_s"] - 1e-6 + delivered = (m["fruit_count"] >= CRITERIA["minimum_delivered_fruit"]) & m["all_types"] + hold("delivery_hold_s", state["lift_complete"] & delivered) + state["delivery_complete"] |= state["delivery_hold_s"] >= CRITERIA["delivery_hold_s"] - 1e-6 + hold("handoff_hold_s", state["delivery_complete"] & delivered & m["held"]) + state["controlled_descent"] |= ( + valid + & state["delivery_complete"] + & m["held"] + & m["near_home"] + & (m["floor_min_z_m"] >= CRITERIA["supported_floor_min_m"]) + & (m["floor_min_z_m"] <= CRITERIA["controlled_descent_floor_max_m"]) + ) + hold("support_hold_s", state["controlled_descent"] & state["delivery_complete"] & m["supported"]) + state["support_complete"] |= state["support_hold_s"] >= CRITERIA["support_hold_before_open_s"] - 1e-6 + release = state["delivery_complete"] & state["support_complete"] & m["supported"] & m["open"] & m["separated"] + hold("released_hold_s", release) + state["finished"] |= valid & (state["released_hold_s"] >= CRITERIA["released_stable_hold_s"] - 1e-6) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/bounded_return_pose.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/bounded_return_pose.py new file mode 100644 index 000000000000..3e03dc542bc1 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/bounded_return_pose.py @@ -0,0 +1,243 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Full-pose return control with joint, increment and live action-filter bounds. + +This controller issues ordinary actions. A target inside a joint limit does not +guarantee physical clearance or prevent inertial overshoot; task gates are unchanged. +""" + +from __future__ import annotations + +import copy +import hashlib +import importlib +from pathlib import Path +from typing import Any + +import numpy as np +import scipy +import torch +from scipy.optimize import lsq_linear + +from isaaclab.utils import math as math_utils + +JOINT_NAMES = tuple(f"panda_joint{i}" for i in range(1, 8)) +OPTIONS = { + "algorithm": "bounded_full_pose_return_dls_v1", + "scope": "milk carton measured return only", + "damping": 0.01, + "joint_limit_interior_margin_rad": 0.020, + "maximum_joint_increment_rad": 0.012, + "arm_alpha": 0.2, + "arm_scale": 0.03, + "raw_action_limit": 1.0, + "joint_order": list(JOINT_NAMES), + "task_error": "unweighted world TCP position [m] and shortest world hand rotation vector [rad]", + "jacobian": "live Newton body_link_jacobian_w, shifted from hand origin to measured TCP", + "bounds": "intersection of inset authored joint limits, +/-0.012 rad and 0.8*live_processed +/-0.006 rad", + "infeasible_interior": "nearest reachable endpoint toward the inset; fixed coordinate before solving", + "infeasible_increment": "maximum braking within EMA/raw bounds; report unavoidable increment violation", + "solver": "scipy.optimize.lsq_linear, BVLS, tolerance 1e-12, maximum 100 iterations", + "fixed_interval_width_rad": 1e-12, + "physical_validation_passed": False, +} + +DEPENDENCY_PATHS = tuple( + sorted( + { + Path(importlib.import_module(name).__file__).resolve() + for name in ( + "scipy.optimize._lsq.lsq_linear", + "scipy.optimize._lsq.bvls", + "scipy.optimize._lsq.common", + "isaaclab.utils.math", + "isaaclab.assets.articulation.base_articulation_data", + "isaaclab_tasks.contrib.franka_pour.mdp.actions", + ) + } + ) +) + + +def return_pose_controller_identity() -> dict: + """Return declared controls and deterministic source bindings, without success claims.""" + return { + "schema": "bounded_return_pose_controller_v1", + "options": copy.deepcopy(OPTIONS), + "source_sha256": { + str(path): hashlib.sha256(path.read_bytes()).hexdigest() + for path in (Path(__file__).resolve(), *DEPENDENCY_PATHS) + }, + "numpy_version": np.__version__, + "scipy_version": scipy.__version__, + "torch_version": torch.__version__, + "physics_state_modified": False, + } + + +def _finite_array(value: Any, shape: tuple[int, ...], name: str) -> np.ndarray: + result = np.asarray(value) + if result.shape != shape or result.dtype.kind not in "fiu" or not np.isfinite(result).all(): + raise ValueError(f"Invalid {name}: require finite real array with shape {shape}.") + return result.astype(np.float64, copy=False) + + +def bounded_return_step( + jacobian: np.ndarray, + error: np.ndarray, + joints: np.ndarray, + limits: np.ndarray, + previous_delta: np.ndarray, +) -> tuple[np.ndarray, dict]: + """Solve one full-pose action step using joint positions/increments [rad]. + + Args: + jacobian: World TCP Jacobian [m/rad for translation], shape (6, 7). + error: Unweighted position [m] and rotation-vector [rad] error, shape (6,). + joints: Measured named joint positions [rad], shape (7,). + limits: Authored simulation lower/upper joint limits [rad], shape (7, 2). + previous_delta: Live filtered arm action [rad], shape (7,). + + Returns: + Raw bounded arm actions and JSON-safe diagnostics. If existing EMA history + makes the increment or inset infeasible, diagnostics identify the affected + joints. No history is reset and no robot state is written. + """ + jacobian = _finite_array(jacobian, (6, 7), "Jacobian") + error = _finite_array(error, (6,), "pose error") + joints = _finite_array(joints, (7,), "joint positions") + limits = _finite_array(limits, (7, 2), "joint limits") + previous_delta = _finite_array(previous_delta, (7,), "previous filtered delta") + margin = OPTIONS["joint_limit_interior_margin_rad"] + if np.any(limits[:, 1] - limits[:, 0] <= 2 * margin): + raise ValueError("Joint limits must have a nonempty interior after the declared margin.") + alpha, scale = OPTIONS["arm_alpha"], OPTIONS["arm_scale"] + step_limit = OPTIONS["maximum_joint_increment_rad"] + center, radius = (1 - alpha) * previous_delta, alpha * scale + reachable_lower, reachable_upper = center - radius, center + radius + lower, upper = np.maximum(reachable_lower, -step_limit), np.minimum(reachable_upper, step_limit) + increment_unreachable = lower > upper + # Old EMA can make even the increment cap impossible. Brake at the reachable + # endpoint closest to zero rather than hiding a reset or clipping filtered state. + braking = np.clip(np.zeros(7), reachable_lower, reachable_upper) + lower = np.where(increment_unreachable, braking, lower) + upper = np.where(increment_unreachable, braking, upper) + interior_lower = limits[:, 0] + margin - joints + interior_upper = limits[:, 1] - margin - joints + bounded_lower, bounded_upper = np.maximum(lower, interior_lower), np.minimum(upper, interior_upper) + interior_unreachable = bounded_lower > bounded_upper + restoring = np.where(interior_lower > upper, upper, lower) + lower = np.where(interior_unreachable, restoring, bounded_lower) + upper = np.where(interior_unreachable, restoring, bounded_upper) + fixed = upper - lower <= OPTIONS["fixed_interval_width_rad"] + delta = np.zeros(7) + delta[fixed] = (lower[fixed] + upper[fixed]) / 2 + free = ~fixed + iterations = 0 + if free.any(): + damping = OPTIONS["damping"] + matrix = np.vstack((jacobian[:, free], damping * np.eye(int(free.sum())))) + target = np.r_[error - jacobian[:, fixed] @ delta[fixed], np.zeros(int(free.sum()))] + result = lsq_linear(matrix, target, bounds=(lower[free], upper[free]), method="bvls", tol=1e-12, max_iter=100) + if not result.success or not np.isfinite(result.x).all(): + raise RuntimeError(f"Bounded return solve failed: {result.message}") + delta[free] = result.x + iterations = result.nit + raw = (delta - center) / radius + if np.any(np.abs(raw) > 1 + 1e-10): + raise RuntimeError("Bounded return solution escaped its EMA reachable interval.") + raw = np.clip(raw, -1, 1) + effective = center + radius * raw + diagnostics = { + "algorithm": OPTIONS["algorithm"], + "position_error_m": float(np.linalg.norm(error[:3])), + "rotation_error_rad": float(np.linalg.norm(error[3:])), + "effective_delta_rad": effective.tolist(), + "raw_arm_action": raw.tolist(), + "effective_lower_rad": lower.tolist(), + "effective_upper_rad": upper.tolist(), + "joint_interior_unreachable": interior_unreachable.tolist(), + "increment_unreachable_due_to_ema": increment_unreachable.tolist(), + "restoration_or_braking_active": bool(interior_unreachable.any() or increment_unreachable.any()), + "fixed_variables": fixed.tolist(), + "linear_residual_norm": float(np.linalg.norm(jacobian @ effective - error)), + "target_minimum_authored_joint_margin_rad": float( + np.minimum(joints + effective - limits[:, 0], limits[:, 1] - joints - effective).min() + ), + "solver_iterations": int(iterations), + } + return raw, diagnostics + + +class BoundedReturnPoseController: + """Issue ordinary filtered joint actions for a full TCP/hand return target.""" + + def __init__(self, env: Any) -> None: + self.env = env + self.joint_ids, names = env.robot.find_joints(list(JOINT_NAMES), preserve_order=True) + if tuple(names) != JOINT_NAMES or len(set(self.joint_ids)) != 7: + raise ValueError("Return control requires each literal panda_joint1..7 in named order.") + term = env.action_manager.get_term("arm_action") + if tuple(term._joint_names) != JOINT_NAMES: + raise ValueError("Arm action columns must match the literal named joint order.") + self._check_action_contract() + self.identity = return_pose_controller_identity() + self.diagnostics: dict = {} + + def _check_action_contract(self) -> None: + cfg = self.env.cfg.actions.arm_action + if cfg.alpha != OPTIONS["arm_alpha"] or cfg.scale != OPTIONS["arm_scale"]: + raise ValueError("Bounded return requires the unchanged alpha=0.2 and scale=0.03 arm action.") + + def compute(self, position: torch.Tensor, rotation: torch.Tensor, close: bool | torch.Tensor) -> torch.Tensor: + """Map world TCP position [m] and XYZW hand orientation into raw bounded actions.""" + self._check_action_contract() + env = self.env + n = env.num_envs + for value, shape, name in ((position, (n, 3), "position"), (rotation, (n, 4), "rotation")): + if ( + not isinstance(value, torch.Tensor) + or not value.is_floating_point() + or value.shape != shape + or not torch.isfinite(value).all() + ): + raise ValueError(f"Invalid return {name}: require finite tensor {shape}.") + if torch.any(torch.linalg.vector_norm(rotation, dim=-1) < 1e-12): + raise ValueError("Return rotation quaternion must be nonzero.") + closing = torch.as_tensor(close, device=env.device) + if closing.dtype != torch.bool or closing.shape not in (torch.Size([]), torch.Size([n])): + raise ValueError("Gripper command must be a bool or one bool per environment.") + hand = env.robot.data.body_link_pose_w.torch[:, env.hand_id] + tcp = env.tcp() + if ( + hand.shape != (n, 7) + or tcp.shape != (n, 3) + or not torch.isfinite(hand).all() + or not torch.isfinite(tcp).all() + or torch.any(torch.linalg.vector_norm(hand[:, 3:], dim=-1) < 1e-12) + ): + raise ValueError("Return controller requires a finite measured hand pose and TCP.") + jacobian = env.robot.data.body_link_jacobian_w.torch[:, env.hand_id - 1, :, self.joint_ids].clone() + jacobian[:, :3] -= torch.bmm(math_utils.skew_symmetric_matrix(tcp - hand[:, :3]), jacobian[:, 3:]) + target_rotation = rotation / torch.linalg.vector_norm(rotation, dim=-1, keepdim=True) + position_error, rotation_error = math_utils.compute_pose_error( + tcp, hand[:, 3:], position, target_rotation, rot_error_type="axis_angle" + ) + error = torch.cat((position_error, rotation_error), dim=-1) + joints = env.robot.data.joint_pos.torch[:, self.joint_ids] + limits = env.robot.data.joint_pos_limits.torch[:, self.joint_ids] + previous = env.action_manager.get_term("arm_action").processed_actions + arrays = [value.detach().cpu().numpy() for value in (jacobian, error, joints, limits, previous)] + expected = [(n, 6, 7), (n, 6), (n, 7), (n, 7, 2), (n, 7)] + for value, shape in zip(arrays, expected): + if value.shape != shape: + raise ValueError(f"Unexpected return-controller state shape: {value.shape}, require {shape}.") + results = [bounded_return_step(*(value[index] for value in arrays)) for index in range(n)] + actions = torch.zeros((n, 8), device=env.device, dtype=joints.dtype) + actions[:, :7] = torch.as_tensor(np.stack([row[0] for row in results]), device=env.device, dtype=joints.dtype) + actions[:, -1] = torch.where(closing, -1.0, 1.0) + self.diagnostics = {"controller": OPTIONS["algorithm"], "per_environment": [row[1] for row in results]} + return actions diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/controlled_fruit_pour.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/controlled_fruit_pour.py new file mode 100644 index 000000000000..30b4652e6233 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/controlled_fruit_pour.py @@ -0,0 +1,461 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Scripted basket-pour command geometry, without simulator or object-state writes. + +The caller acquires the basket and owns every task milestone and return decision. +This helper emits TCP/hand references from live measured geometry. Its local clock +pauses when grasp or tracking is lost. CPU checks do not establish physical success. +The nominal pour follows the earlier physical FullBasketExpert recipe; measured +gates, live cup tracking, and clearance continuity make this a new candidate. +""" + +from __future__ import annotations + +import copy +from pathlib import Path +from typing import Any + +import numpy as np +from scipy.spatial.transform import Rotation, Slerp + +FROZEN_ROOT = Path(__file__).parent +DEPENDENCY_PATHS = (FROZEN_ROOT / "basket_reference.py", FROZEN_ROOT / "grasp_feedback.py") +OPTIONS = { + "schema": "scripted_measured_fruit_pour", + "version": 11, + "actions_only": True, + "physical_validation_passed": False, + "capture_gate": "caller requires strict upright lift complete and current held", + "goal_fruit_count": 16, + "return_owned_by_caller": True, + "height_alignment_s": 3.0, + "level_carry_s": 6.0, + "rock_cycles": 2, + "rock_cycle_s": 6.0, + "rock_deg": 15.0, + "tilt_s": 24.0, + "tilt_deg": 130.0, + "late_tilt_lift_start_deg": 115.0, + "late_tilt_lift_m": 0.030, + "terminal_tip_additional_deg": 20.0, + "terminal_tip_s": 4.0, + "terminal_tip_lift_m": 0.050, + "terminal_tip_rationale": ( + "Preserve the selected initial tilt path through 115 degrees, then tip once from 130 to 150 degrees " + "over 4 s and hold. " + "Keep lip XY fixed; add 30 mm world-Z lift with cubic smoothstep from 115 to 130 degrees, " + "then add the remaining 20 mm during the terminal tip for the same total 50 mm at 150 degrees. " + "The late rise addresses sub-mm palm/cup clearance with the full04 peak measured grasp drift. " + "Observed berry/strawberry releases precede the rise, but it changes late mango release height; " + "actual fruit receipt requires fresh physical validation. " + "Apply this offset after the original excess-height descent so its magnitude is not attenuated. " + "Existing measured clock gates apply; actual hand clearance still requires geometry screening. " + "This bounded continuation is an unvalidated candidate for fruit retained inside the basket. " + "The caller still owns strict receipt, return and all task milestones." + ), + "tilt_timing_rationale": ( + "After measured carry alignment, perform two 0-to-15-to-0 degree cycles of 6 s each, " + "then restore the original 24 s forward tilt on the same angle-dependent geometry. " + "Each cubic 3 s half-cycle has zero endpoint velocity and peak angular speed 7.5 degrees/s. " + "Earlier successful S19 motion included low-angle rocking; the full04 uniform 48 s tilt " + "spilled fruit on different sides of the cup. This bounded settling candidate does not " + "establish causality or physical success. Measured gates and physics remain unchanged." + ), + "align_grasp_radial": True, + "grasp_alignment_rationale": ( + "Select the physical rim point opposite the captured grasp and turn upright during carry. " + "The 135-degree pickup with a fixed original lip intersected the cup late in nominal tilt; " + "in that prior 95 mm-height case, radial alignment gave 13.197 mm minimum exact hand-capsule clearance " + "over the sampled nominal poses. This is historical S7 geometry evidence, not an S8 measurement. " + "This does not bound actual tracking or grasp drift; physical validation is still required." + ), + "minimum_rim_clearance_m": 0.030, + "clearance_rationale": ( + "S15 restores 30 mm commanded clearance to retain hand/cup separation with a larger inward aim. " + "A bounded nominal screen using the S12 captured grasp found 18.595 mm whole-hand/cup separation " + "and reachable key poses for 30 mm clearance and -25 mm cup-Y aim. This is sampled rigid geometry, " + "not a guarantee under actual tracking, fruit contacts or grasp drift. " + "Position-only grip, physics, tracking gates and completion criteria remain unchanged." + ), + "clearance_geometry": "outer basket boundary radius60.5mm; entire basket above highest actual cup rim", + "cup_rim_local_z_m": 0.210, + "cup_outer_rim_radius_m": 0.0515, + "lip_aim_offset_cup_xy_m": [0.0, -0.025], + "lip_aim_rationale": ( + "S15 applies -25 mm cup-frame Y aim to both the command and measured alignment gate. " + "S14's -10 mm aim still spilled a blackberry beyond the positive-Y cup wall. " + "A frozen-trajectory screen including the added flight height improved this berry's predicted " + "margin from -3.270 to +3.964 mm, without making a baseline-positive footprint negative among " + "16 comparable releases. Four already-below-rim mango releases were unassessed; two already-negative " + "predictions worsened despite those fruits being received through contacts in the actual S14 run. " + "These observations motivate a physical retry and do not prove receipt or contact-free execution. " + "Actual cup rim clearance geometry, task gates and acceptance remain unchanged." + ), + "reference_recipe": { + "controller": "historical fruit-pour reference", + "episode": "demonstrations.hdf5/demo_16: world 11/reset 0/seed 20260918", + "evidence": "Strict whole-hull count 20 for 2.4 s at 29.6-31.9667 s; fruit-only, not combined success.", + "adaptation": ( + "Raise keeps captured rotation; carry turns upright with measured radial grasp alignment; " + "pour yaw 0, cup-Y aim -25 mm, two 6 s/15 degree rocking cycles and 24 s/130 degree forward tilt " + "with 30 mm base clearance; smoothly add 30 mm lift from 115 to 130 degrees, then 4 s to " + "150 degrees with the remaining 20 mm lift for 50 mm total. " + "Current strict-lift handoff, measured clock gates, grasp feedback and live cup remain; " + "extra captured height descends continuously during tilt. Not an exact old-episode replay." + ), + }, + "tracking_position_tolerance_m": 0.008, + "tracking_rotation_tolerance_rad": 0.08, + "lip_alignment_tolerance_m": 0.010, + "upright_cosine": 0.995, + "alignment_hold_s": 0.4, + "cup_reference_translation_rate_m_s": 0.02, + "cup_reference_rotation_rate_rad_s": 0.15, + "grasp_feedback": { + "enabled": True, + "time_constant_s": 0.5, + "maximum_translation_rate_m_s": 0.004, + "maximum_rotation_rate_rad_s": 0.15, + "maximum_translation_from_capture_m": 0.015, + "maximum_rotation_from_capture_rad": 1.1, + }, +} + + +from . import basket_reference as _geometry +from . import grasp_feedback as _feedback + + +def _pose(position: np.ndarray, quaternion: np.ndarray) -> np.ndarray: + value = np.r_[position, quaternion].astype(np.float64) + if value.shape != (7,) or not np.isfinite(value).all() or np.linalg.norm(value[3:]) < 1e-10: + raise ValueError("Require a finite position [m] and nonzero XYZW quaternion.") + value[3:] = Rotation.from_quat(value[3:]).as_quat() + return value + + +class ControlledFruitPour: + """Generate a measured, tracking-gated TCP pour trajectory after a stable lift. + + Args: + basket_position: Measured basket origin [m], shape [3]. + basket_quaternion: Measured basket XYZW quaternion, shape [4]. + tcp_position: Measured tool centre position [m], shape [3]. + hand_quaternion: Measured hand XYZW quaternion, shape [4]. + cup_position: Measured cup origin [m], shape [3]. + cup_quaternion: Measured cup XYZW quaternion, shape [4]. + dt: Positive policy interval [s]. + options: Optional complete option mapping, recorded by the caller's manifest. + """ + + def __init__( + self, + basket_position: np.ndarray, + basket_quaternion: np.ndarray, + tcp_position: np.ndarray, + hand_quaternion: np.ndarray, + cup_position: np.ndarray, + cup_quaternion: np.ndarray, + dt: float, + options: dict[str, Any] | None = None, + ): + if not np.isfinite(dt) or dt <= 0: + raise ValueError("Require positive finite dt [s].") + self.options = copy.deepcopy(OPTIONS if options is None else options) + aim_offset = np.asarray(self.options["lip_aim_offset_cup_xy_m"], dtype=np.float64) + if aim_offset.shape != (2,) or not np.isfinite(aim_offset).all(): + raise ValueError("Require a finite cup-frame XY lip offset [m], shape [2].") + if type(self.options["rock_cycles"]) is not int or self.options["rock_cycles"] <= 0: + raise ValueError("Require a positive integer rocking cycle count.") + if not np.isfinite(self.options["rock_cycle_s"]) or self.options["rock_cycle_s"] <= 0: + raise ValueError("Require a positive finite rocking cycle duration [s].") + if not np.isfinite(self.options["rock_deg"]) or not 0 < self.options["rock_deg"] <= self.options["tilt_deg"]: + raise ValueError("Rocking angle [deg] must be positive, finite, and within the forward tilt arc.") + for key in ("terminal_tip_additional_deg", "terminal_tip_s"): + if not np.isfinite(self.options[key]) or self.options[key] <= 0: + raise ValueError("Terminal tip angle [deg] and duration [s] must be positive and finite.") + if not np.isfinite(self.options["terminal_tip_lift_m"]) or self.options["terminal_tip_lift_m"] < 0: + raise ValueError("Terminal tip lift [m] must be nonnegative and finite.") + if ( + not np.isfinite(self.options["late_tilt_lift_start_deg"]) + or not 0 <= self.options["late_tilt_lift_start_deg"] < self.options["tilt_deg"] + ): + raise ValueError("Late lift start [deg] must be finite and within the forward tilt arc.") + if ( + not np.isfinite(self.options["late_tilt_lift_m"]) + or not 0 <= self.options["late_tilt_lift_m"] <= self.options["terminal_tip_lift_m"] + ): + raise ValueError("Late lift [m] must be finite, nonnegative, and no larger than the final total lift.") + self.dt = float(dt) + basket = _pose(basket_position, basket_quaternion) + hand_pose = _pose(tcp_position, hand_quaternion) + self.cup = _pose(cup_position, cup_quaternion) + self.geometry = _geometry.BasketPourReference( + basket, + hand_pose[:3], + hand_pose[3:], + self.cup, + angle_deg=self.options["tilt_deg"], + clearance_m=self.options["minimum_rim_clearance_m"], + follow_clearance=True, + align_grasp_radial=self.options["align_grasp_radial"], + ) + self.feedback = _feedback.GraspFeedback(basket, hand_pose[:3], hand_pose[3:], self.options["grasp_feedback"]) + self.elapsed_s = 0.0 + self.alignment_hold_s = 0.0 + self.tilt_armed = False + self._first_step = True + self._level_rotation = self.geometry.upright + self._level_slerp = Slerp([0.0, 1.0], Rotation.concatenate([self.geometry.start_r, self._level_rotation])) + self._last_reference = self._command(0.0) + self.diagnostics: dict[str, Any] = {} + + @property + def tilt_start_s(self) -> float: + """Return the gated rocking start on the local clock [s], excluding waiting.""" + return float(self.options["height_alignment_s"] + self.options["level_carry_s"]) + + @property + def forward_tilt_start_s(self) -> float: + """Return the forward tilt start after the finite rocking schedule [s].""" + return self.tilt_start_s + self.options["rock_cycles"] * float(self.options["rock_cycle_s"]) + + @property + def tilt_end_s(self) -> float: + """Return the original tilt endpoint on the local clock [s].""" + return self.forward_tilt_start_s + float(self.options["tilt_s"]) + + @property + def duration_s(self) -> float: + """Return the bounded trajectory duration including the terminal tip [s].""" + return self.tilt_end_s + float(self.options["terminal_tip_s"]) + + def _rim(self, cup: np.ndarray) -> tuple[np.ndarray, float]: + rotation = Rotation.from_quat(cup[3:]) + centre = cup[:3] + rotation.apply([0.0, 0.0, self.options["cup_rim_local_z_m"]]) + normal = rotation.apply([0.0, 0.0, 1.0]) + highest = centre[2] + self.options["cup_outer_rim_radius_m"] * np.linalg.norm(normal[:2]) + return centre, float(highest) + + def _command(self, time_s: float) -> dict[str, Any]: + result = self.feedback.command(self._sample(time_s)) + result["tcp"] = result["tcp_position"] + result["hand_quat"] = result["hand_xyzw"] + return result + + def _lip_aim(self, cup: np.ndarray, rim_center: np.ndarray) -> np.ndarray: + # Only aim XY moves; clearance still uses the unshifted actual rim geometry. + offset = np.r_[self.options["lip_aim_offset_cup_xy_m"], 0.0] + return rim_center + Rotation.from_quat(cup[3:]).apply(offset) + + def _sample(self, time_s: float) -> dict[str, Any]: + geometry, options = self.geometry, self.options + terminal_lift_m = 0.0 + rim, rim_top = self._rim(self.cup) + aim = self._lip_aim(self.cup, rim) + high = geometry.start_p.copy() + clearance_floor = rim_top + options["minimum_rim_clearance_m"] + captured_lowest = float(geometry.start_r.apply(geometry.boundary_b)[:, 2].min()) + high[2] = max(high[2], clearance_floor - min(0.0, captured_lowest)) + hover = np.r_[aim[:2], high[2] + geometry.lip_b[2]] - self._level_rotation.apply(geometry.lip_b) + if time_s < options["height_alignment_s"]: + fraction = float(_geometry.smooth(time_s / options["height_alignment_s"])) + position = (1.0 - fraction) * geometry.start_p + fraction * high + rotation = geometry.start_r + phase = "height_alignment" + elif time_s < self.tilt_start_s: + fraction = float(_geometry.smooth((time_s - options["height_alignment_s"]) / options["level_carry_s"])) + position = (1.0 - fraction) * high + fraction * hover + rotation = self._level_slerp(fraction) + position[2] = max(position[2], clearance_floor - float(rotation.apply(geometry.boundary_b)[:, 2].min())) + phase = "level_carry" + else: + if time_s < self.forward_tilt_start_s: + half_cycle_s = options["rock_cycle_s"] / 2.0 + cycle_time_s = (time_s - self.tilt_start_s) % options["rock_cycle_s"] + rock_fraction = float(_geometry.smooth(cycle_time_s / half_cycle_s)) + if cycle_time_s >= half_cycle_s: + rock_fraction = 1.0 - float(_geometry.smooth((cycle_time_s - half_cycle_s) / half_cycle_s)) + # Reuse the original angle-dependent arc, including excess-height descent, in both directions. + fraction = options["rock_deg"] / options["tilt_deg"] * rock_fraction + else: + fraction = float(_geometry.smooth((time_s - self.forward_tilt_start_s) / options["tilt_s"])) + angle = geometry.angle * fraction + late_fraction = float( + _geometry.smooth( + (np.rad2deg(angle) - options["late_tilt_lift_start_deg"]) + / (options["tilt_deg"] - options["late_tilt_lift_start_deg"]) + ) + ) + # This cumulative offset is applied after the original base-clearance height calculation. + terminal_lift_m = options["late_tilt_lift_m"] * late_fraction + if time_s >= self.tilt_end_s: + tip_fraction = float(_geometry.smooth((time_s - self.tilt_end_s) / options["terminal_tip_s"])) + angle += np.deg2rad(options["terminal_tip_additional_deg"]) * tip_fraction + terminal_lift_m += (options["terminal_tip_lift_m"] - options["late_tilt_lift_m"]) * tip_fraction + rotation = geometry.pour_yaw * Rotation.from_rotvec([-angle, 0.0, 0.0]) * geometry.grasp_alignment + min_relative_z = float(rotation.apply(geometry.boundary_b - geometry.lip_b)[:, 2].min()) + # Preserve the carry endpoint, then remove excess height without a tilt-boundary jump. + excess_height = (1.0 - fraction) * (high[2] - clearance_floor) + lip = np.r_[aim[:2], clearance_floor - min_relative_z + excess_height] + lip[2] += terminal_lift_m + position = lip - rotation.apply(geometry.lip_b) + if time_s < self.forward_tilt_start_s: + phase = "rock" + elif time_s < self.tilt_end_s: + phase = "tilt" + else: + phase = "terminal_tip" if time_s < self.duration_s else "hold" + tcp = position + rotation.apply(geometry.grip_b) + hand = (rotation * geometry.hand_b).as_quat() + return { + "phase": phase, + "basket_position": position, + "basket_xyzw": rotation.as_quat(), + "tcp_position": tcp, + "hand_xyzw": hand, + "tcp": tcp, + "hand_quat": hand, + "lip_position": position + rotation.apply(geometry.lip_b), + "lowest_basket_z": float((position + rotation.apply(geometry.boundary_b))[:, 2].min()), + "reference_cup_rim_highest_z": rim_top, + "reference_lip_aim_position_m": aim, + # Preserve the diagnostic key; its value includes the preceding late-forward lift. + "terminal_tip_commanded_lift_m": terminal_lift_m, + } + + def step( + self, + *, + basket_position: np.ndarray, + basket_quaternion: np.ndarray, + tcp_position: np.ndarray, + hand_quaternion: np.ndarray, + cup_position: np.ndarray, + cup_quaternion: np.ndarray, + held: bool, + valid: bool = True, + delivered_count: int = 0, + ) -> dict[str, Any]: + """Return TCP [m]/hand XYZW command geometry plus gate diagnostics. + + Positions share a common world or environment-local frame. The return keys + are ``tcp_position`` (alias ``tcp``), ``hand_xyzw`` (alias ``hand_quat``), + ``basket_position``, ``basket_xyzw``, ``lip_position``, ``phase``, and + ``diagnostics``. Receipt is diagnostic only; + this helper neither changes task state nor starts the return trajectory. + """ + try: + basket = _pose(basket_position, basket_quaternion) + hand = _pose(tcp_position, hand_quaternion) + cup = _pose(cup_position, cup_quaternion) + except ValueError: + valid = False + if not valid: + self.alignment_hold_s = 0.0 + self.diagnostics = { + "phase": self._last_reference["phase"], + "local_elapsed_s": self.elapsed_s, + "clock_advanced": False, + "valid": False, + "held": bool(held), + "tilt_armed": self.tilt_armed, + "alignment_hold_s": 0.0, + "delivered_count": int(delivered_count), + "goal_fruit_count": self.options["goal_fruit_count"], + } + return {**self._last_reference, "diagnostics": dict(self.diagnostics)} + + options = self.options + if held: + self.cup[:3] += _feedback.bounded_vector( + cup[:3] - self.cup[:3], options["cup_reference_translation_rate_m_s"] * self.dt + ) + rotation = Rotation.from_quat(self.cup[3:]) + change = _feedback.bounded_vector( + (rotation.inv() * Rotation.from_quat(cup[3:])).as_rotvec(), + options["cup_reference_rotation_rate_rad_s"] * self.dt, + ) + self.cup[3:] = (rotation * Rotation.from_rotvec(change)).as_quat() + self.feedback.update(basket, hand[:3], hand[3:], self.dt, held=held, valid=True) + reference = self._command(self.elapsed_s) + basket_rotation = Rotation.from_quat(basket[3:]) + errors = { + "tcp_position_error_m": float(np.linalg.norm(reference["tcp_position"] - hand[:3])), + "hand_rotation_error_rad": float( + (Rotation.from_quat(reference["hand_xyzw"]).inv() * Rotation.from_quat(hand[3:])).magnitude() + ), + "basket_position_error_m": float(np.linalg.norm(reference["basket_position"] - basket[:3])), + "basket_rotation_error_rad": float( + (Rotation.from_quat(reference["basket_xyzw"]).inv() * basket_rotation).magnitude() + ), + } + tracking = ( + max(errors["tcp_position_error_m"], errors["basket_position_error_m"]) + <= options["tracking_position_tolerance_m"] + and max(errors["hand_rotation_error_rad"], errors["basket_rotation_error_rad"]) + <= options["tracking_rotation_tolerance_rad"] + ) + rim, actual_rim_top = self._rim(cup) + aim = self._lip_aim(cup, rim) + measured_lip = basket[:3] + basket_rotation.apply(self.geometry.lip_b) + lip_error = float(np.linalg.norm(measured_lip[:2] - aim[:2])) + cup_center_error = float(np.linalg.norm(measured_lip[:2] - rim[:2])) + aligned = lip_error <= options["lip_alignment_tolerance_m"] + upright = float(basket_rotation.as_matrix()[2, 2]) >= options["upright_cosine"] + cup_upright = float(Rotation.from_quat(cup[3:]).as_matrix()[2, 2]) >= options["upright_cosine"] + gate = held and tracking and aligned and upright and cup_upright + if self.elapsed_s >= self.tilt_start_s and not self.tilt_armed: + self.alignment_hold_s = self.alignment_hold_s + self.dt if gate else 0.0 + self.tilt_armed = self.alignment_hold_s >= options["alignment_hold_s"] - 1e-10 + advance = held and tracking and cup_upright and not self._first_step + if self.elapsed_s >= self.tilt_start_s: + advance = advance and self.tilt_armed and aligned + previous_time = self.elapsed_s + if advance: + if self.elapsed_s < self.tilt_start_s: + boundary = self.tilt_start_s + elif self.elapsed_s < self.forward_tilt_start_s: + boundary = self.forward_tilt_start_s + elif self.elapsed_s < self.tilt_end_s: + boundary = self.tilt_end_s + else: + boundary = self.duration_s + self.elapsed_s = min(self.elapsed_s + self.dt, boundary) + self._first_step = False + reference = self._command(self.elapsed_s) + translation, angle, updated = self.feedback.diagnostics() + self.diagnostics = { + "phase": reference["phase"], + "local_elapsed_s": self.elapsed_s, + "clock_advanced": self.elapsed_s > previous_time, + "valid": True, + "held": bool(held), + "tracking": bool(tracking), + "tilt_armed": bool(self.tilt_armed), + "alignment_gate": bool(gate), + "alignment_hold_s": self.alignment_hold_s, + "measured_lip_alignment_error_m": lip_error, + "measured_lip_cup_center_error_m": cup_center_error, + "actual_cup_rim_center_m": rim.tolist(), + "lip_aim_position_m": aim.tolist(), + "lip_aim_offset_cup_xy_m": list(options["lip_aim_offset_cup_xy_m"]), + "lip_aim_offset_world_m": (aim - rim).tolist(), + "reference_lip_aim_position_m": reference["reference_lip_aim_position_m"].tolist(), + "measured_lip_position_m": measured_lip.tolist(), + "measured_basket_upright_cosine": float(basket_rotation.as_matrix()[2, 2]), + "measured_cup_upright": bool(cup_upright), + "command_clearance_above_actual_rim_m": float(reference["lowest_basket_z"] - actual_rim_top), + "terminal_tip_commanded_lift_m": reference["terminal_tip_commanded_lift_m"], + "grasp_translation_correction_m": translation, + "grasp_rotation_correction_rad": angle, + "grasp_feedback_updated": bool(updated), + "delivered_count": int(delivered_count), + "goal_fruit_count": options["goal_fruit_count"], + **errors, + } + self._last_reference = reference + return {**reference, "diagnostics": dict(self.diagnostics)} diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/cup_contact_spacing.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/cup_contact_spacing.py new file mode 100644 index 000000000000..e8daada43f45 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/cup_contact_spacing.py @@ -0,0 +1,162 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Task-local cup rigid-contact spacing and read-only native startup identity.""" + +from __future__ import annotations + +import copy +import hashlib +import importlib.util +import math +from pathlib import Path + +import newton +import numpy as np +import warp as wp + +OPTIONS = { + "version": 1, + "margin_m": 0.00025, + "baseline_margin_m": 0.00015, + "expected_shape_count": 33, + "scope": "Only receiving-cup rigid Floor and Wall/S000..S031, after the inherited world hook.", + "excluded": "Particle-only _MPM proxies, threads, handle, fruits, robot and all other shapes.", + "rationale": ( + "Conditional contact-spacing candidate after S18 completed milk delivery and supported release, " + "but blackberry_3 repeatedly crossed authored floor/wall planes by micrometres. " + "Native startup verified the cup surfaces, witness pair eligibility and existing 0.150 mm margins. " + "The extra 0.100 mm changes compliant contact reference spacing; it does not guarantee equal " + "measured separation or packed-cup success. Receipt geometry and zero-tolerance gates are unchanged." + ), + "physical_validation_passed": False, +} +_COMPARISON_TOLERANCE_M = 1e-10 +_NEWTON_ROOT = Path(importlib.util.find_spec("newton").origin).parent +DEPENDENCY_PATHS = ( + Path(__file__).resolve(), + _NEWTON_ROOT / "_src/solvers/mujoco/kernels.py", + _NEWTON_ROOT / "_src/solvers/mujoco/solver_mujoco.py", +) + + +def cup_contact_spacing_contract() -> dict: + """Return the declared rigid cup margin [m] without mutable shared metadata.""" + return copy.deepcopy(OPTIONS) + + +def _selected_indices(labels: list[str], env_id: int) -> list[int]: + prefix = f"/World/envs/env_{env_id}/Cup/Cup/" + expected = [prefix + "Floor", *(prefix + f"Wall/S{i:03d}" for i in range(32))] + selected = [ + i + for i, label in enumerate(labels) + if not label.endswith("_MPM") and (label == prefix + "Floor" or label.startswith(prefix + "Wall/")) + ] + actual = [labels[i] for i in selected] + if len(actual) != OPTIONS["expected_shape_count"] or set(actual) != set(expected): + raise ValueError("Require exactly one cup Floor and 32 declared rigid Wall shapes.") + return selected + + +def apply_cup_contact_spacing(builder: newton.ModelBuilder, env_id: int) -> None: + """Set only the selected cup rigid margins [m], after inherited builder configuration.""" + selected = _selected_indices(list(builder.shape_label), env_id) + for i in selected: + flags = int(builder.shape_flags[i]) + if ( + builder.shape_world[i] != env_id + or not flags & int(newton.ShapeFlags.COLLIDE_SHAPES) + or flags & int(newton.ShapeFlags.COLLIDE_PARTICLES) + or not math.isclose( + builder.shape_margin[i], OPTIONS["baseline_margin_m"], rel_tol=0, abs_tol=_COMPARISON_TOLERANCE_M + ) + ): + raise ValueError("Cup rigid spacing requires the unchanged inherited world-hook baseline.") + # Validate the complete selection before mutating any builder field. + for i in selected: + builder.shape_margin[i] = OPTIONS["margin_m"] + + +def validate_cup_contact_runtime(rigid) -> dict: + """Verify native selected rigid margins [m] before stepping, without writing solver state.""" + view = rigid.model + if rigid.model is not view or rigid._use_mujoco_contacts: + raise ValueError("Require the actual external-contact rigid solver and its entry view.") + wp.synchronize_device(view.device) + labels, bodies = list(view.shape_label), list(view.body_label) + selected = _selected_indices(labels, 0) + arrays = { + "margin": np.array(view.shape_margin.numpy(), copy=True), + "flags": np.array(view.shape_flags.numpy(), copy=True), + "world": np.array(view.shape_world.numpy(), copy=True), + "owner": np.array(view.shape_body.numpy(), copy=True), + "mapping": np.array(rigid.mjc_geom_to_newton_shape.numpy(), copy=True), + "body_mapping": np.array(rigid.mjc_body_to_newton.numpy(), copy=True), + "geom_body": np.array(rigid.mjw_model.geom_bodyid.numpy(), copy=True), + "native_margin": np.array(rigid.mjw_model.geom_margin.numpy(), copy=True), + } + for name, array in arrays.items(): + kind = "f" if name in ("margin", "native_margin") else "iu" + if array.dtype.kind not in kind or not np.isfinite(array).all(): + raise ValueError(f"Invalid cup native readback array: {name}.") + for name in ("margin", "flags", "world", "owner"): + if arrays[name].shape != (len(labels),): + raise ValueError(f"Invalid cup entry array shape: {name}.") + ngeom, nbody = rigid.mj_model.ngeom, rigid.mj_model.nbody + if ( + arrays["mapping"].shape != (1, ngeom) + or arrays["native_margin"].shape != (1, ngeom) + or arrays["body_mapping"].shape != (1, nbody) + or arrays["geom_body"].shape != (ngeom,) + or np.any((arrays["mapping"] < -1) | (arrays["mapping"] >= len(labels))) + or np.any((arrays["geom_body"] < 0) | (arrays["geom_body"] >= nbody)) + ): + raise ValueError("Require valid one-world cup native shape/body mappings.") + rows = [] + for i in selected: + geoms = np.flatnonzero(arrays["mapping"][0] == i) + owner = int(arrays["owner"][i]) + flags = int(arrays["flags"][i]) + if ( + len(geoms) != 1 + or not 0 <= owner < len(bodies) + or bodies[owner] != "/World/envs/env_0/Cup/Cup" + or arrays["world"][i] != 0 + or not flags & int(newton.ShapeFlags.COLLIDE_SHAPES) + or flags & int(newton.ShapeFlags.COLLIDE_PARTICLES) + ): + raise ValueError("Missing or invalid selected rigid cup geometry ownership/mapping.") + g = int(geoms[0]) + if arrays["body_mapping"][0, arrays["geom_body"][g]] != owner: + raise ValueError("Selected cup native body mapping differs from entry ownership.") + margin, native_margin = float(arrays["margin"][i]), float(arrays["native_margin"][0, g]) + if not all( + math.isclose(value, OPTIONS["margin_m"], rel_tol=0, abs_tol=_COMPARISON_TOLERANCE_M) + for value in (margin, native_margin) + ): + raise ValueError("Selected cup Newton/native margin differs from the declared candidate.") + rows.append( + { + "label": labels[i], + "entry_shape": i, + "native_geom": g, + "newton_margin_m": margin, + "native_margin_m": native_margin, + "shape_flags": flags, + } + ) + return { + "schema": "cup_contact_runtime_v1", + "passed": True, + "shape_count": len(rows), + "margin_m": OPTIONS["margin_m"], + "comparison_tolerance_m": _COMPARISON_TOLERANCE_M, + "geometries": rows, + "source_sha256": {str(path): hashlib.sha256(path.read_bytes()).hexdigest() for path in DEPENDENCY_PATHS}, + "physics_state_modified": False, + "physics_steps_executed_by_check": 0, + "scope": "Startup margin/mapping readback only; no solved-force, separation or task-success claim.", + } diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/fruit_receipt_geometry.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/fruit_receipt_geometry.py new file mode 100644 index 000000000000..f9a4f9fd110f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/fruit_receipt_geometry.py @@ -0,0 +1,187 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Zero-tolerance receipt from enabled authored fruit colliders and actual cup planes.""" + +from __future__ import annotations + +import copy +import hashlib +from functools import lru_cache +from pathlib import Path + +import numpy as np +import torch +from scipy.spatial import ConvexHull + +from pxr import Usd, UsdGeom, UsdPhysics + +ASSETS = Path(__file__).resolve().parent / "assets" +KINDS = ("strawberry", "blueberry", "blackberry", "mango") + + +def enabled(prim): + """Return whether the authored collision API is enabled.""" + return ( + prim.HasAPI(UsdPhysics.CollisionAPI) + and UsdPhysics.CollisionAPI(prim).GetCollisionEnabledAttr().Get() is not False + ) + + +def transformed_points(prim, cache, body_inverse): + matrix = np.asarray(cache.GetLocalToWorldTransform(prim) * body_inverse) + points = np.asarray(UsdGeom.Mesh(prim).GetPointsAttr().Get(), dtype=np.float64) + return (np.column_stack((points, np.ones(len(points)))) @ matrix)[:, :3] + + +@lru_cache(maxsize=1) +def load_geometry(): + """Load convex support vertices [m] and cup inward containment bounds [m].""" + stage = Usd.Stage.Open(str(ASSETS / "cup.usda")) + cache = UsdGeom.XformCache() + root = stage.GetPrimAtPath("/Asset/Cup") + assert root.HasAPI(UsdPhysics.RigidBodyAPI) + inverse = cache.GetLocalToWorldTransform(root).GetInverse() + normals, distances, rims = [], [], [] + for prim in stage.GetPrimAtPath("/Asset/Cup/Wall").GetChildren(): + assert enabled(prim) and prim.IsA(UsdGeom.Mesh) + points = transformed_points(prim, cache, inverse) + radii = np.linalg.norm(points[:, :2], axis=-1) + inner = np.unique(points[np.abs(radii - radii.min()) < 1e-7, :2], axis=0) + assert inner.shape == (2, 2) + edge = inner[1] - inner[0] + normal = np.array([edge[1], -edge[0]]) + normal /= np.linalg.norm(normal) + if normal @ inner.mean(0) < 0: + normal *= -1 + distance = float(inner[0] @ normal) + assert np.max(np.abs(inner @ normal - distance)) < 1e-14 + normals.append(normal) + distances.append(distance) + rims.append(float(points[:, 2].max())) + assert len(normals) == 32 and max(rims) - min(rims) < 1e-9 + floor_prim = stage.GetPrimAtPath("/Asset/Cup/Floor") + assert enabled(floor_prim) + floor = UsdGeom.Cylinder(floor_prim) + matrix = np.asarray(cache.GetLocalToWorldTransform(floor_prim) * inverse) + assert str(floor.GetAxisAttr().Get()) == "Z" and np.allclose(matrix[:3, :3], np.eye(3)) + floor_top = float(matrix[3, 2] + floor.GetHeightAttr().Get() / 2) + geometry, descriptions = {}, {} + for kind in KINDS: + path = ASSETS / f"{kind}.{'usda' if kind == 'mango' else 'usdc'}" + fruit = Usd.Stage.Open(str(path)) + cache = UsdGeom.XformCache() + roots = [p for p in fruit.Traverse() if p.HasAPI(UsdPhysics.RigidBodyAPI)] + assert len(roots) == 1 + inverse = cache.GetLocalToWorldTransform(roots[0]).GetInverse() + vertices, parts = [], [] + for prim in fruit.Traverse(): + if not enabled(prim): + continue + assert prim.IsA(UsdGeom.Mesh) + assert prim.GetAttribute("physics:approximation").Get() == "convexHull" + points = transformed_points(prim, cache, inverse) + vertices.append(points) + parts.append({"path": str(prim.GetPath()), "point_count": len(points)}) + all_points = np.concatenate(vertices) + hull = ConvexHull(all_points) + support = np.ascontiguousarray(all_points[hull.vertices], dtype=np.float64) + geometry[kind] = support + descriptions[kind] = { + "asset_sha256": hashlib.sha256(path.read_bytes()).hexdigest(), + "enabled_colliders": parts, + "authored_point_count": len(all_points), + "support_point_count": len(support), + "support_vertices_float64_sha256": hashlib.sha256(support.tobytes()).hexdigest(), + } + metadata = { + "schema": "authored_fruit_hull_receipt", + "version": 1, + "tolerance_m": 0.0, + "method": ( + "Every enabled authored convexHull vertex is inside all32 actual inner-wall planes and floor/rim " + "planes. Outer support vertices preserve exact maxima in this convex receiving region." + ), + "floating_point": "Normalized recorded XYZW quaternions, float64 transforms and plane comparisons <=0.", + "cup_sha256": hashlib.sha256((ASSETS / "cup.usda").read_bytes()).hexdigest(), + "inner_wall_normals_xy": np.asarray(normals).tolist(), + "inner_wall_distances_m": distances, + "floor_top_m": floor_top, + "rim_m": min(rims), + "fruit_geometry": descriptions, + "legacy_center_metric": "Recorded separately; does not advance this version3 receipt gate.", + } + return geometry, np.asarray(normals), np.asarray(distances), floor_top, min(rims), metadata + + +def geometry_contract(): + """Return the serialized authoritative geometry identity without sharing mutable configuration.""" + return copy.deepcopy(load_geometry()[-1]) + + +def rotation_matrix(q): + """Return rotation matrices from finite nonzero XYZW quaternions.""" + q = q / torch.linalg.vector_norm(q, dim=-1, keepdim=True).clamp_min(1e-30) + x, y, z, w = q.unbind(-1) + return torch.stack( + ( + 1 - 2 * (y * y + z * z), + 2 * (x * y - z * w), + 2 * (x * z + y * w), + 2 * (x * y + z * w), + 1 - 2 * (x * x + z * z), + 2 * (y * z - x * w), + 2 * (x * z - y * w), + 2 * (y * z + x * w), + 1 - 2 * (x * x + y * y), + ), + dim=-1, + ).reshape(*q.shape[:-1], 3, 3) + + +class WholeHullReceipt: + """Measure each fruit's whole-collider containment in the actual receiving cup.""" + + def __init__(self, names: tuple[str, ...], device: str): + geometry, normals, distances, self.floor, self.rim, _ = load_geometry() + assert len(names) == 16 and set(n.split("_")[0] for n in names) == set(KINDS) + self.indices = { + kind: torch.tensor([i for i, n in enumerate(names) if n.split("_")[0] == kind], device=device) + for kind in KINDS + } + self.vertices = { + kind: torch.tensor(value, dtype=torch.float64, device=device) for kind, value in geometry.items() + } + self.normals = torch.tensor(normals, dtype=torch.float64, device=device) + self.distances = torch.tensor(distances, dtype=torch.float64, device=device) + + def measure(self, cup_pose: torch.Tensor, fruit_poses: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]: + """Return contained masks [N,16] and maximum plane violations [m] from measured world poses.""" + cup = cup_pose.to(torch.float64) + fruit = fruit_poses.to(torch.float64) + valid_cup = torch.isfinite(cup).all(-1) & (torch.linalg.vector_norm(cup[:, 3:], dim=-1) > 1e-12) + valid = ( + torch.isfinite(fruit).all(-1) + & (torch.linalg.vector_norm(fruit[:, :, 3:], dim=-1) > 1e-12) + & valid_cup[:, None] + ) + cup_r = rotation_matrix(cup[:, 3:]).transpose(-1, -2) + fruit_r = rotation_matrix(fruit[:, :, 3:]) + relative_r = torch.einsum("nij,nfjk->nfik", cup_r, fruit_r) + center = torch.einsum("nij,nfj->nfi", cup_r, fruit[:, :, :3] - cup[:, None, :3]) + violations = torch.empty(fruit.shape[:2], dtype=torch.float64, device=fruit.device) + for kind in KINDS: + ids = self.indices[kind] + points = torch.einsum("nfij,vj->nfvi", relative_r[:, ids], self.vertices[kind]) + center[:, ids, None, :] + sides = (points[:, :, :, :2] @ self.normals.T - self.distances).amax(dim=(-1, -2)) + low = self.floor - points[:, :, :, 2].amin(-1) + high = points[:, :, :, 2].amax(-1) - self.rim + violations[:, ids] = torch.maximum(torch.maximum(sides, low), high) + violations = torch.where(valid, violations, torch.full_like(violations, float("inf"))) + return violations <= 0.0, violations + + def all_types(self, inside: torch.Tensor) -> torch.Tensor: + """Require at least one completely contained fruit of every authored kind.""" + return torch.stack([inside[:, ids].any(-1) for ids in self.indices.values()], dim=-1).all(-1) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/grasp_feedback.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/grasp_feedback.py new file mode 100644 index 000000000000..d5d419e051d1 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/grasp_feedback.py @@ -0,0 +1,81 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Bounded measured-grasp estimation; this changes TCP commands, never object states.""" + +from __future__ import annotations + +import numpy as np +from scipy.spatial.transform import Rotation + + +def bounded_vector(vector: np.ndarray, maximum: float) -> np.ndarray: + """Limit a translation [m] or rotation vector [rad] without changing its direction.""" + norm = float(np.linalg.norm(vector)) + return vector * min(1.0, maximum / max(norm, 1e-15)) + + +class GraspFeedback: + """Estimate the basket-to-hand transform from measured poses with rate and total bounds.""" + + def __init__(self, basket: np.ndarray, tcp: np.ndarray, hand_xyzw: np.ndarray, options: dict): + self.options = dict(options) + rotation = Rotation.from_quat(basket[3:]) + self.initial_grip = rotation.inv().apply(tcp - basket[:3]) + self.initial_hand = rotation.inv() * Rotation.from_quat(hand_xyzw) + self.grip = self.initial_grip.copy() + self.hand = self.initial_hand + self.updated = False + + def update(self, basket, tcp, hand_xyzw, step_dt: float, *, held: bool, valid: bool) -> bool: + """Update only while a finite, valid grasp is measured; rate limits use policy dt [s].""" + self.updated = False + if not held or not valid: + return False + if not np.isfinite(step_dt) or step_dt <= 0: + raise ValueError("Require a positive finite policy timestep.") + if not np.isfinite(np.r_[basket, tcp, hand_xyzw]).all(): + return False + rotation = Rotation.from_quat(basket[3:]) + measured_grip = rotation.inv().apply(tcp - basket[:3]) + measured_hand = rotation.inv() * Rotation.from_quat(hand_xyzw) + options = self.options + fraction = -np.expm1(-step_dt / options["time_constant_s"]) + target_grip = self.initial_grip + bounded_vector( + measured_grip - self.initial_grip, options["maximum_translation_from_capture_m"] + ) + target_hand = self.initial_hand * Rotation.from_rotvec( + bounded_vector( + (self.initial_hand.inv() * measured_hand).as_rotvec(), + options["maximum_rotation_from_capture_rad"], + ) + ) + self.grip += bounded_vector( + fraction * (target_grip - self.grip), options["maximum_translation_rate_m_s"] * step_dt + ) + change = bounded_vector( + fraction * (self.hand.inv() * target_hand).as_rotvec(), + options["maximum_rotation_rate_rad_s"] * step_dt, + ) + self.hand = self.hand * Rotation.from_rotvec(change) + self.updated = True + return True + + def command(self, reference: dict) -> dict: + """Compensate measured grasp drift while preserving basket pose and any TCP retreat offset.""" + result = dict(reference) + key = "basket_xyzw" if "basket_xyzw" in reference else "basket_reference_xyzw" + rotation = Rotation.from_quat(reference[key]) + result["tcp_position"] = np.asarray(reference["tcp_position"]) + rotation.apply(self.grip - self.initial_grip) + result["hand_xyzw"] = (rotation * self.hand).as_quat() + return result + + def diagnostics(self) -> tuple[float, float, bool]: + """Return bounded translation [m], rotation [rad], and the last update flag.""" + return ( + float(np.linalg.norm(self.grip - self.initial_grip)), + float((self.initial_hand.inv() * self.hand).magnitude()), + self.updated, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/pad_torsional_contact.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/pad_torsional_contact.py new file mode 100644 index 000000000000..2cd4326eec2d --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/pad_torsional_contact.py @@ -0,0 +1,219 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Activation of authored pad spin friction, with native startup and active-contact evidence.""" + +from __future__ import annotations + +import hashlib +import importlib.util +from pathlib import Path +from typing import Any + +import newton +import numpy as np +import warp as wp + +PAD_SUFFIXES = ("/panda_leftfinger/left_finger_pad", "/panda_rightfinger/right_finger_pad") +BRIDGE_SUFFIX = "/Cup/Handle/Bridge1" +_MATERIAL_FIELDS = ("shape_material_mu", "shape_material_mu_torsional", "shape_material_mu_rolling") + + +def _indices(labels: list[str], env_id: int) -> list[int]: + selected = [] + for suffix in (*PAD_SUFFIXES, BRIDGE_SUFFIX): + matches = [ + i + for i, label in enumerate(labels) + if label.startswith(f"/World/envs/env_{env_id}/") and label.endswith(suffix) + ] + if len(matches) != 1: + raise ValueError(f"Require exactly one active shape for {suffix}.") + selected.append(matches[0]) + return selected + + +def apply_pad_torsional_contact(builder: Any, env_id: int) -> dict: + """Change only two pad contact dimensions to four, preserving friction [m for spin/roll].""" + selected = _indices(list(builder.shape_label), env_id) + condim = builder.custom_attributes["mujoco:condim"] + priority = builder.custom_attributes["mujoco:geom_priority"] + if condim.default != 3 or not isinstance(condim.values, dict) or not isinstance(priority.values, dict): + raise ValueError("Require the registered sparse contact attributes with baseline condim three.") + rows = [] + for index in selected: + flags = int(builder.shape_flags[index]) + friction = [float(getattr(builder, name)[index]) for name in _MATERIAL_FIELDS] + if ( + builder.shape_world[index] != env_id + or not flags & int(newton.ShapeFlags.COLLIDE_SHAPES) + or flags & int(newton.ShapeFlags.COLLIDE_PARTICLES) + or condim.values.get(index, condim.default) != 3 + or priority.values.get(index, priority.default) != 0 + or not np.isfinite(friction).all() + or any(value < 0 for value in friction) + ): + raise ValueError("Require unchanged active rigid pad/Bridge1 contact baseline.") + if index in selected[:2] and not np.allclose(friction, [1.0, 0.005, 0.0001], rtol=1e-6, atol=1e-10): + raise ValueError("Require the pinned pad's existing authored friction values.") + rows.append( + { + "label": builder.shape_label[index], + "builder_shape": index, + "friction": friction, + "priority": 0, + "before_condim": 3, + "requested_condim": 4 if index in selected[:2] else 3, + } + ) + # Validate the complete selection before touching either sparse entry. + for index in selected[:2]: + condim.values[index] = 4 + root = Path(importlib.util.find_spec("newton").origin).parent + dependencies = ( + Path(__file__).resolve(), + root / "_src/solvers/mujoco/kernels.py", + root / "_src/solvers/mujoco/solver_mujoco.py", + ) + return { + "schema": "pad_torsional_contact_v1", + "geometries": rows, + "native_verified": False, + "active_bilateral_verified": False, + "changed_field": "mujoco:condim", + "changed_shape_count": 2, + "source_sha256": {str(path): hashlib.sha256(path.read_bytes()).hexdigest() for path in dependencies}, + "scope": "Rigid pad contact formulation only; no full-task or physical success claim.", + } + + +def _array(value: Any) -> np.ndarray: + return np.array(value.numpy() if hasattr(value, "numpy") else value, copy=True) + + +def verify_pad_torsional_contact(rigid: Any, receipt: dict) -> None: + """Verify imported, compiled and native pad contact settings before issuing robot actions.""" + view = rigid.model + if rigid.model is not view or rigid._use_mujoco_contacts or int(rigid.mjw_model.opt.cone) != 1: + raise ValueError("Require the actual external-contact elliptic rigid solver.") + wp.synchronize_device(view.device) + labels, bodies = list(view.shape_label), list(view.body_label) + selected = _indices(labels, 0) + mapping = _array(rigid.mjc_geom_to_newton_shape) + body_mapping = _array(rigid.mjc_body_to_newton) + owner = _array(view.shape_body) + native = rigid.mjw_model + geom_owner = _array(native.geom_bodyid) + n = rigid.mj_model.ngeom + if mapping.shape != (1, n) or body_mapping.shape != (1, rigid.mj_model.nbody) or geom_owner.shape != (n,): + raise ValueError("Require valid single-world native shape/body mappings.") + imported_dim, imported_priority = _array(view.mujoco.condim), _array(view.mujoco.geom_priority) + imported_friction = np.stack([_array(getattr(view, name)) for name in _MATERIAL_FIELDS], axis=-1) + native_dim, native_priority, native_friction = ( + _array(getattr(native, name)) for name in ("geom_condim", "geom_priority", "geom_friction") + ) + if native_dim.shape != (n,) or native_priority.shape != (n,) or native_friction.shape != (1, n, 3): + raise ValueError("Invalid native pad contact readback array dimensions.") + for index, row in zip(selected, receipt["geometries"], strict=True): + geoms = np.flatnonzero(mapping[0] == index) + if len(geoms) != 1 or row["label"] != labels[index]: + raise ValueError("Missing, duplicated or mismatched pad/Bridge1 geometry mapping.") + geom = int(geoms[0]) + if ( + not 0 <= owner[index] < len(bodies) + or not 0 <= geom_owner[geom] < rigid.mj_model.nbody + or body_mapping[0, geom_owner[geom]] != owner[index] + ): + raise ValueError("Native pad/Bridge1 geometry body ownership differs from the entry view.") + states = { + "imported": { + "condim": int(imported_dim[index]), + "priority": int(imported_priority[index]), + "friction": imported_friction[index].tolist(), + }, + "compiled": { + "condim": int(rigid.mj_model.geom_condim[geom]), + "priority": int(rigid.mj_model.geom_priority[geom]), + "friction": rigid.mj_model.geom_friction[geom].tolist(), + }, + "native": { + "condim": int(native_dim[geom]), + "priority": int(native_priority[geom]), + "friction": native_friction[0, geom].tolist(), + }, + } + row.update(entry_shape=index, native_geom=geom, body_label=bodies[int(owner[index])], readback=states) + if any( + state["condim"] != row["requested_condim"] + or state["priority"] != 0 + or not np.allclose(state["friction"], row["friction"], rtol=1e-6, atol=1e-10) + for state in states.values() + ): + raise ValueError(f"Pad/Bridge1 contact native mismatch: {row}") + receipt["native_verified"] = True + + +def capture_pad_bridge_contacts(rigid: Any, receipt: dict, step: int) -> dict: + """Require actual bilateral pad/Bridge1 spin constraint rows at the measured held step.""" + if not receipt["native_verified"]: + raise ValueError("Require startup native verification before recording active contacts.") + wp.synchronize_device(rigid.model.device) + data, contact = rigid.mjw_data, rigid.mjw_data.contact + count_array, nefc = _array(data.nacon), _array(data.nefc) + arrays = { + name: _array(getattr(contact, name)) for name in ("geom", "worldid", "dim", "friction", "efc_address", "dist") + } + if count_array.shape != (1,) or nefc.shape != (1,): + raise ValueError("Require single-world active-contact counts.") + count = int(count_array[0]) + if not 0 <= count <= len(arrays["geom"]) or any(len(value) != len(arrays["geom"]) for value in arrays.values()): + raise ValueError("Invalid active-contact count or array capacities.") + bridge = receipt["geometries"][2] + rows = [] + for pad in receipt["geometries"][:2]: + wanted = {pad["native_geom"], bridge["native_geom"]} + resolved = np.maximum(pad["friction"], bridge["friction"]) + expected = np.maximum(resolved[[0, 0, 1, 2, 2]], 1e-5) + active = [] + for index in range(count): + if arrays["worldid"][index] != 0 or set(arrays["geom"][index].tolist()) != wanted: + continue + addresses = arrays["efc_address"][index, :4] + if np.all(arrays["efc_address"][index] < 0): + continue + if ( + arrays["dim"][index] != 4 + or len(addresses) != 4 + or np.any(addresses < 0) + or np.any(addresses >= nefc[0]) + or len(set(addresses.tolist())) != 4 + or not np.allclose(arrays["friction"][index], expected, rtol=1e-6, atol=1e-10) + or not np.isfinite(arrays["dist"][index]) + ): + raise ValueError( + "Active pad/Bridge1 contact does not carry the expected four-dimensional friction constraint." + ) + active.append( + { + "contact_index": index, + "geom": arrays["geom"][index].tolist(), + "dim": int(arrays["dim"][index]), + "friction": arrays["friction"][index].tolist(), + "efc_address": addresses.tolist(), + "distance_m": float(arrays["dist"][index]), + } + ) + if not active: + raise ValueError(f"No active four-dimensional pad/Bridge1 contact for {pad['label']} at held step {step}.") + rows.append({"pad_label": pad["label"], "bridge_label": bridge["label"], "contacts": active}) + receipt["active_bilateral_verified"] = True + receipt["active_contact_evidence"] = { + "step": step, + "nacon": count, + "nefc": int(nefc[0]), + "pairs": rows, + "scope": "Actual active constraint rows; no solved-force or task-success claim.", + } + return receipt["active_contact_evidence"] diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/pose_control.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/pose_control.py new file mode 100644 index 000000000000..844d0bb83545 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/pose_control.py @@ -0,0 +1,81 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""TCP pose control and the scripted basket-pickup candidate used by the tap controller.""" + +from __future__ import annotations + +import math +from typing import TYPE_CHECKING + +import torch + +from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg +from isaaclab.utils import math as math_utils + +if TYPE_CHECKING: + from .smoothie_env import SmoothieBlenderEnv + + +class PoseController: + """Map TCP pose commands to the filtered joint actions the task's action terms expect.""" + + def __init__(self, env: SmoothieBlenderEnv) -> None: + self.env = env + self.joint_ids = env.robot.find_joints([f"panda_joint{i}" for i in range(1, 8)], preserve_order=True)[0] + self.controller = DifferentialIKController( + DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"), + env.num_envs, + env.device, + ) + + def compute(self, position: torch.Tensor, rotation: torch.Tensor, close: bool | torch.Tensor) -> torch.Tensor: + """Convert world TCP position [m], XYZW rotation, and per-world gripper commands to actions.""" + env = self.env + hand = env.robot.data.body_link_pose_w.torch[:, env.hand_id] + jacobian = env.robot.data.body_link_jacobian_w.torch[:, env.hand_id - 1, :, self.joint_ids].clone() + offset = env.tcp() - hand[:, :3] + jacobian[:, :3] -= torch.bmm(math_utils.skew_symmetric_matrix(offset), jacobian[:, 3:]) + joints = env.robot.data.joint_pos.torch[:, self.joint_ids] + self.controller.set_command(torch.cat((position, rotation), -1)) + goal = self.controller.compute(env.tcp(), hand[:, 3:], jacobian, joints) + term = env.action_manager.get_term("arm_action") + alpha = env.cfg.actions.arm_action.alpha + delta = (goal - joints).clamp(-0.012, 0.012) + raw = (delta - (1 - alpha) * term.processed_actions) / (alpha * env.cfg.actions.arm_action.scale) + actions = torch.zeros((env.num_envs, 8), device=env.device) + actions[:, :7] = raw.clamp(-1, 1) + actions[:, -1] = torch.where(torch.as_tensor(close, device=env.device), -1.0, 1.0) + return actions + + +class BasketExpert: + """Scripted approach/close/lift candidate using physical contacts and policy actions. + + Success must be measured by the environment. This controller does not attach, + teleport, or prescribe motion to the basket, and does not demonstrate pouring. + """ + + def __init__(self, env: SmoothieBlenderEnv) -> None: + self.env = env + self.controller = PoseController(env) + self.position, self.rotation = env.grasp_pose() + + def compute(self, step: int | torch.Tensor) -> torch.Tensor: + """Return actions for scalar or per-world episode steps, restarting each world at step zero.""" + env = self.env + steps = torch.as_tensor(step, device=env.device).expand(env.num_envs) + time_s = steps * env.step_dt + close_step = math.ceil(7.5 / env.step_dt) + approach = steps < close_step + position, rotation = env.grasp_pose() + position[:, 2] += 0.15 * (1 - ((time_s - 3.0) / 3.5).clamp(0, 1)) + self.position[approach] = position[approach] + self.rotation[approach] = rotation[approach] + closing = steps == close_step + self.position[closing] = env.tcp()[closing] + target = self.position.clone() + target[:, 2] += 0.15 * ((time_s - 11.0) / 3.5).clamp(0, 1) + return self.controller.compute(target, self.rotation, steps >= close_step) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/recorded_controller.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/recorded_controller.py new file mode 100644 index 000000000000..601feb3bc93b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/recorded_controller.py @@ -0,0 +1,63 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Playback of a baked, time-sampled recording of the live smoothie controller. + +The scene has no randomization. A recorded run of +``SmoothieSequenceController`` supplies the actions for the demonstration. +Playback indexes the recorded raw actions by policy step; it performs no IK, +no bounded least-squares solve, and no live tracking/physical gating. +""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +import numpy as np +import torch + + +class RecordedSequenceController: + """Replay a baked ``(actions, stages)`` recording as raw robot actions.""" + + def __init__(self, env: Any, *, recording_path: Path, trace_path: Path | None = None): + path = Path(recording_path) + with np.load(path) as data: + actions = data["actions"] + stages = data["stages"] + if actions.ndim != 2 or actions.shape[1] != 8: + raise ValueError(f"Recorded actions at {path} must have shape [N, 8].") + if stages.shape[0] != actions.shape[0]: + raise ValueError(f"Recorded actions and stages at {path} must have matching length.") + self.env = env + self.actions = actions.astype(np.float32, copy=False) + self.stages = stages + self.previous_step: int | None = None + # The runner closes this owned stream through close_trace in its finally block. + self.trace = None if trace_path is None else Path(trace_path).open("x", buffering=1) # noqa: SIM115 + + @property + def stage(self) -> str: + """Recorded stage label for the most recently computed step.""" + index = 0 if self.previous_step is None else self.previous_step + return str(self.stages[min(index, len(self.stages) - 1)]) + + def compute(self, step: int) -> torch.Tensor: + """Return the recorded raw robot actions for ``step``, shape [1, 8].""" + if type(step) is not int or step < 0 or (self.previous_step is not None and step != self.previous_step + 1): + raise ValueError("Controller calls require consecutive nonnegative policy steps.") + if step >= len(self.actions): + raise IndexError(f"Recorded trajectory has {len(self.actions)} steps; step {step} was requested.") + self.previous_step = step + actions = torch.as_tensor(self.actions[step], device=self.env.device)[None] + if self.trace is not None: + self.trace.write(f"{step}\t{step * self.env.step_dt:.3f}\t{self.stage}\n") + return actions + + def close_trace(self) -> None: + """Close the optional controller event trace.""" + if self.trace is not None: + self.trace.close() diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/return_path.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/return_path.py new file mode 100644 index 000000000000..6ac60a100be5 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/return_path.py @@ -0,0 +1,86 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""CPU-only measured-grasp basket placement candidate; no simulated object writes.""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np +from scipy.spatial.transform import Rotation, Slerp + +ROOT = Path(__file__).resolve().parent + +from .basket_reference import BasketPourReference, smooth + +HOME = np.array([0.36, 0.27, 0.002]) + + +class ReturnCandidate: + """Command poses [m, XYZW] through guarded height, upright return, and release.""" + + def __init__(self, basket, tcp, hand, cup): + self.p = basket[:3].copy() + self.rotation = Rotation.from_quat(basket[3:]) + self.grip_b = self.rotation.inv().apply(tcp - self.p) + self.hand_b = self.rotation.inv() * hand + self.boundary = BasketPourReference(basket, tcp, hand.as_quat(), cup).boundary_b + self.lip_b = np.array([0.0, 0.0605, 0.1]) + self.safe_z = cup[2] + 0.210 + 0.05 + self.raise_m = max(0.0, self.safe_z - (self.p + self.rotation.apply(self.boundary))[:, 2].min()) + self.lip = self.p + self.rotation.apply(self.lip_b) + [0.0, 0.0, self.raise_m] + self.slerp = Slerp([0.0, 1.0], Rotation.concatenate([self.rotation, Rotation.identity()])) + self.upright_p = self.untilt(1.0)[0] + self.home_high = np.array([*HOME[:2], max(self.upright_p[2], self.safe_z)]) + self.home_pre = np.array([*HOME[:2], 0.025]) + self.home_rest = np.array([*HOME[:2], 0.0015]) + + def untilt(self, fraction): + rotation = self.slerp(float(fraction)) + lip = self.lip.copy() + lip[2] = max(lip[2], self.safe_z - rotation.apply(self.boundary - self.lip_b)[:, 2].min()) + return lip - rotation.apply(self.lip_b), rotation + + def sample(self, t): + closed = t < 34.0 + if t < 3.0: + p, r, stage = self.p + np.array([0.0, 0.0, self.raise_m * smooth(t / 3.0)]), self.rotation, "clear_cup" + elif t < 15.0: + p, r = self.untilt(smooth((t - 3.0) / 12.0)) + stage = "upright" + elif t < 23.0: + u = smooth((t - 15.0) / 8.0) + p = (1 - u) * self.upright_p + u * self.home_high + r = Rotation.identity() + stage = "return_above_home" + elif t < 29.0: + u = smooth((t - 23.0) / 6.0) + p = (1 - u) * self.home_high + u * self.home_pre + r = Rotation.identity() + stage = "lower_coarse" + elif t < 32.0: + u = smooth((t - 29.0) / 3.0) + p = (1 - u) * self.home_pre + u * self.home_rest + r = Rotation.identity() + stage = "lower_fine" + else: + p = self.home_rest.copy() + r = Rotation.identity() + stage = ( + "settle_closed" if t < 34.0 else ("open" if t < 37.0 else ("retreat" if t < 40.0 else "released_dwell")) + ) + tcp = p + r.apply(self.grip_b) + if t >= 37.0: + tcp[2] += 0.12 * smooth((t - 37.0) / 3.0) + return { + "phase": stage, + "basket_reference_position": p, + "basket_reference_xyzw": r.as_quat(), + "tcp_position": tcp, + "hand_xyzw": (r * self.hand_b).as_quat(), + "close": closed, + "lowest_basket_reference_z": float((p + r.apply(self.boundary))[:, 2].min()), + } diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/scene_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/scene_cfg.py new file mode 100644 index 000000000000..5a9bd46f33aa --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/scene_cfg.py @@ -0,0 +1,150 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Scene layout for the smoothie task: workbench, robot, containers, lid, blender and packed fruit.""" + +import math +import os +from pathlib import Path + +from isaaclab_newton.sim.schemas import NewtonMeshCollisionPropertiesCfg + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import CollisionPropertiesCfg +from isaaclab.utils.configclass import configclass + +from ..franka_pour.pour_env_cfg import PourSceneCfg +from .basket_geometry import BASKET_POSITION +from .smoothie_asset import tap_articulation_cfg + +ASSETS = Path(__file__).parent / "assets" +CUP_POSITION = (0.57, -0.07, 0.013) +MOTOR_POSITION = (0.64, 0.25, 0.0) +CAP_POSITION = (0.29, -0.075, 0.078) +FRUITS = ("strawberry", "blueberry", "blackberry", "mango") +# Basket-local fruit centers [m], packed with clearance at the reset yaw angles. +FRUIT_LAYOUT = { + "strawberry": (-0.016504, 0.013801, 0.029275), + "strawberry_2": (0.011796, -0.025169, 0.107484), + "strawberry_3": (0.007000, -0.017021, 0.044672), + "strawberry_4": (0.002145, 0.011383, 0.087000), + "blueberry": (-0.026109, -0.022603, 0.051921), + "blueberry_2": (-0.029348, 0.023428, 0.074840), + "blueberry_3": (0.014665, 0.037911, 0.116507), + "blueberry_4": (-0.017944, -0.038494, 0.123598), + "blackberry": (-0.014852, 0.036472, 0.112907), + "blackberry_2": (-0.017227, -0.029619, 0.077301), + "blackberry_3": (0.032341, 0.020091, 0.112470), + "blackberry_4": (0.028775, 0.012079, 0.063778), + "mango": (-0.010620, 0.016723, 0.060005), + "mango_2": (-0.005598, -0.003316, 0.012698), + "mango_3": (-0.024086, 0.010443, 0.131901), + "mango_4": (0.011269, 0.022617, 0.129368), +} +FRUIT_LAYOUT = { + name: (BASKET_POSITION[0] + x, BASKET_POSITION[1] + y, BASKET_POSITION[2] + z) + for name, (x, y, z) in FRUIT_LAYOUT.items() +} +FRUIT_GROUPS = {kind: tuple(name for name in FRUIT_LAYOUT if name.split("_")[0] == kind) for kind in FRUITS} +# Keep the packing rotations of the retained fruits when changing the population. +_FRUIT_RESET_YAW_INDICES = { + "strawberry": 0, + "strawberry_2": 1, + "strawberry_3": 2, + "strawberry_4": 3, + "blueberry": 5, + "blueberry_2": 6, + "blueberry_3": 7, + "blueberry_4": 8, + "blackberry": 11, + "blackberry_2": 12, + "blackberry_3": 13, + "blackberry_4": 14, + "mango": 16, + "mango_2": 17, + "mango_3": 18, + "mango_4": 19, +} + + +def rigid(name, file, position): + """Configure a scene-owned dynamic body at a position [m].""" + return RigidObjectCfg( + prim_path=f"{{ENV_REGEX_NS}}/{name}", + spawn=sim_utils.UsdFileCfg(usd_path=str(ASSETS / file)), + init_state=RigidObjectCfg.InitialStateCfg(pos=position), + ) + + +@configclass +class TapSceneCfg(InteractiveSceneCfg): + """Keyed workbench, robot, cup, threaded lid, blender, spring-button tap and packed fruit.""" + + robot = PourSceneCfg().robot.copy() + robot.init_state.joint_pos = { + "panda_joint1": 0.0, + "panda_joint2": -0.569, + "panda_joint3": 0.0, + "panda_joint4": -2.81, + "panda_joint5": 0.0, + "panda_joint6": 3.037, + "panda_joint7": 0.741, + "panda_finger_joint.*": 0.04, + } + workstation = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Workstation", + spawn=sim_utils.UsdFileCfg(usd_path=str(ASSETS / "overrides" / "workstation.usda")), + ) + light = AssetBaseCfg(prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=2500.0)) + cup = rigid("Cup", "cup.usda", CUP_POSITION) + basket = rigid("FruitBasket", "fruit_basket.usda", BASKET_POSITION) + tap = tap_articulation_cfg() + blade_cap = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/BladeCap", + spawn=sim_utils.UsdFileCfg(usd_path=str(ASSETS / "blade_cap.usda")), + init_state=ArticulationCfg.InitialStateCfg(pos=CAP_POSITION, joint_pos={"Bearing": 0.0}), + actuators={ + "motor": ImplicitActuatorCfg( + joint_names_expr=["Bearing"], + stiffness=0.0, + damping=0.02, + joint_effort_limit=0.5, + joint_velocity_limit=150.0, + ) + }, + ) + motor = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Motor", + spawn=sim_utils.UsdFileCfg(usd_path=str(ASSETS / "motor.usda")), + init_state=ArticulationCfg.InitialStateCfg(pos=MOTOR_POSITION, joint_pos={"PowerButton": 0.0}), + actuators={ + "button": ImplicitActuatorCfg( + joint_names_expr=["PowerButton"], stiffness=300.0, damping=3.0, joint_effort_limit=10.0 + ) + }, + ) + + def __post_init__(self): + self.robot.spawn.usd_path = os.environ.get("ISAACLAB_FRANKA_POUR_ROBOT_USD_PATH", str(ASSETS / "franka.usdc")) + self.robot.actuators["panda_hand"].stiffness = 5000.0 + self.robot.actuators["panda_hand"].damping = 100.0 + self.robot.actuators["panda_hand"].joint_effort_limit = 200.0 + for name, position in FRUIT_LAYOUT.items(): + kind = name.split("_")[0] + fruit = rigid(name.capitalize(), f"{kind}.{'usda' if kind == 'mango' else 'usdc'}", position) + yaw = _FRUIT_RESET_YAW_INDICES[name] * 2.399963229728653 + fruit.init_state.rot = (0.0, 0.0, math.sin(yaw / 2), math.cos(yaw / 2)) + # Convex hulls keep the packed fruit from interpenetrating in the basket. + fruit.spawn.collision_props = CollisionPropertiesCfg( + mesh_collision_property=NewtonMeshCollisionPropertiesCfg( + mesh_approximation_name="convexHull", max_hull_vertices=-1 + ) + ) + if kind == "strawberry": + fruit.spawn.usd_path = str(ASSETS / "overrides" / "strawberry.usda") + setattr(self, name, fruit) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_assembly_geometry.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_assembly_geometry.py new file mode 100644 index 000000000000..784d74f77e13 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_assembly_geometry.py @@ -0,0 +1,505 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Measured lid threading and inverted cup docking; no simulation state writes.""" + +from __future__ import annotations + +import copy +import math +from enum import IntEnum + +import torch + + +class AssemblyStage(IntEnum): + """Ordered physical milestones within the lid and docking phases.""" + + LID_PICKUP = 0 + LID_ALIGN = 1 + LID_THREAD = 2 + LID_RELEASE = 3 + CUP_PICKUP = 4 + CUP_INVERT = 5 + CUP_DOCK = 6 + FINAL_RELEASE = 7 + COMPLETE = 8 + + +CRITERIA = { + "lid_seated_axial_m": 0.220, + "lid_seated_tolerance_m": 0.0025, + "lid_radial_tolerance_m": 0.004, + "lid_axis_cosine": 0.985, + "lid_near_axial_min_m": 0.217, + "lid_near_axial_max_m": 0.232, + "lid_clockwise_turns": 1.0, + "lid_engaged_descent_m": 0.003, + "maximum_turn_increment_rad": 0.2, + "lid_lift_height_m": 0.04, + "cup_lift_height_m": 0.08, + "grasp_distance_m": 0.015, + "grasp_relative_speed_m_s": 0.08, + "grasp_relative_angular_speed_rad_s": 0.5, + "contact_deflection_m": 0.0005, + "stable_linear_speed_m_s": 0.01, + "stable_angular_speed_rad_s": 0.1, + "finger_open_min_m": 0.0395, + "hand_separation_m": 0.09, + "dock_position_tolerance_m": 0.006, + "dock_yaw_tolerance_rad": 0.06, + "dock_key_surface_clearance_m": 0.0003, + "lid_lift_hold_s": 0.25, + "lid_align_hold_s": 0.10, + "lid_seat_hold_s": 0.25, + "lid_release_hold_s": 0.50, + "cup_lift_hold_s": 0.25, + "cup_inverted_hold_s": 0.25, + "dock_support_hold_s": 0.50, + "final_release_hold_s": 1.0, + "minimum_fruit_count": 16, + "minimum_fill_fraction": 1.0, +} +OBSERVATION_SCALES = ( + ("assembly_stage", 8.0), + ("lid_clockwise_turns", 1.0), + ("lid_engaged_descent_m", 0.012), + ("lid_lift_hold_s", 0.25), + ("lid_align_hold_s", 0.10), + ("lid_seat_hold_s", 0.25), + ("lid_release_hold_s", 0.50), + ("cup_lift_hold_s", 0.25), + ("cup_inverted_hold_s", 0.25), + ("dock_support_hold_s", 0.50), + ("final_release_hold_s", 1.0), + ("lid_twist_complete", 1.0), + ("lid_release_complete", 1.0), + ("cup_lift_complete", 1.0), + ("lid_retention_failed", 1.0), + ("assembly_complete", 1.0), +) + + +def assembly_contract() -> dict: + """Return independent geometry, chronology and observation contracts [SI units].""" + return { + "schema": "measured_threaded_lid_and_inverted_dock_v1", + "criteria": copy.deepcopy(CRITERIA), + "stages": {stage.name: int(stage) for stage in AssemblyStage}, + "observation_names": [name for name, _ in OBSERVATION_SCALES], + "observation_scales": [scale for _, scale in OBSERVATION_SCALES], + "observation_size": len(OBSERVATION_SCALES), + "cup_home_m": [0.57, -0.07, 0.013], + "lid_home_m": [0.29, -0.075, 0.078], + "lid_grasp_local_m": [0.0, 0.0, 0.018], + "cup_grasp_local_m": [0.0, -0.061, 0.150], + "cup_grasp_feature": { + "prim_path": "/Asset/Cup/Handle/Bridge1", + "bounds_cup_m": [[-0.011, -0.075, 0.143], [0.011, -0.047, 0.157]], + "anchor": "Authored upper bridge center, independent of the commanded TCP offset.", + }, + "dock_lid_root_motor_local_m": [0.0, 0.0, 0.145], + "lid_tab_half_widths_m": [0.0135, 0.0175], + "socket_key_half_gaps_m": [0.015, 0.019], + "seated_lid_floor_underside_cup_local_m": 0.210, + "cup_receipt_mouth_cup_local_m": 0.210, + "thread_semantics": ( + "Signed clockwise credit only between consecutive aligned, near, held samples; " + "counterclockwise motion subtracts credit. Regrasp gives no unheld rotation credit. " + "Require a measured engaged axial descent and stable seating." + ), + "support_semantics": ( + "Supported placement inferred from authored support geometry and measured velocities; " + "this gate does not claim independent contact-force sensing." + ), + "retention_semantics": ( + "Lid seating must remain true after screw-on completion through transport and final release. " + "Any observed loss is latched as failure. " + "Current whole-fruit receipt and scalar tap fill gate final success." + ), + "state_write_policy": "Only milestone tensors change; never object, robot, particle or joint state.", + } + + +def _rotate(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + xyz, w = q[..., :3], q[..., 3:] + cross = torch.linalg.cross(xyz, v) + return v + 2 * (w * cross + torch.linalg.cross(xyz, cross)) + + +def _conjugate(q: torch.Tensor) -> torch.Tensor: + return torch.cat((-q[..., :3], q[..., 3:]), -1) + + +def _multiply(q: torch.Tensor, r: torch.Tensor) -> torch.Tensor: + return torch.cat( + ( + q[..., 3:] * r[..., :3] + r[..., 3:] * q[..., :3] + torch.linalg.cross(q[..., :3], r[..., :3]), + q[..., 3:] * r[..., 3:] - (q[..., :3] * r[..., :3]).sum(-1, keepdim=True), + ), + -1, + ) + + +def assembly_geometry( + *, + cup_pose: torch.Tensor, + cap_pose: torch.Tensor, + motor_pose: torch.Tensor, + hand_pose: torch.Tensor, + cup_velocity: torch.Tensor, + cap_velocity: torch.Tensor, + hand_velocity: torch.Tensor, + tcp_position: torch.Tensor, + finger_positions: torch.Tensor, + commanded_positions: torch.Tensor, + contact_deflection: torch.Tensor, + bilateral_contact: torch.Tensor, + origins: torch.Tensor, + fruit_count: torch.Tensor, + all_types: torch.Tensor, + fill_fraction: torch.Tensor, + failed: torch.Tensor, +) -> dict[str, torch.Tensor]: + """Measure batched physical geometry without changing any simulator state. + + Poses use world position [m] and XYZW quaternion, shape [N, 7]. Velocities + contain world linear [m/s] and angular [rad/s] values, shape [N, 6]. TCP and + origins are [m], shape [N, 3]. Finger positions, commands and deflections + are per-finger [m], shape [N, 2]. Remaining inputs have shape [N]. + """ + n = cup_pose.shape[0] + poses = (cup_pose, cap_pose, motor_pose, hand_pose) + velocities = (cup_velocity, cap_velocity, hand_velocity) + groups = ((poses, 7), (velocities, 6), ((tcp_position, origins), 3)) + groups += (((finger_positions, commanded_positions, contact_deflection), 2),) + for values, width in groups: + if any(value.shape != (n, width) or value.device != cup_pose.device for value in values): + raise ValueError(f"Expected same-device physical tensors with shape [N, {width}].") + scalars = (bilateral_contact, fruit_count, all_types, fill_fraction, failed) + if any(value.shape != (n,) or value.device != cup_pose.device for value in scalars): + raise ValueError("Expected same-device scalar measurements with shape [N].") + if any(value.dtype != torch.bool for value in (bilateral_contact, all_types, failed)): + raise ValueError("Contact, fruit-type and failure flags must be Boolean tensors.") + finite = torch.ones(n, dtype=torch.bool, device=cup_pose.device) + for values, _ in groups: + for value in values: + finite &= torch.isfinite(value).all(-1) + for pose in poses: + finite &= (pose[:, 3:].norm(dim=-1) - 1.0).abs() <= 0.001 + finite &= torch.isfinite(fruit_count) & torch.isfinite(fill_fraction) + finite &= (fruit_count >= 0) & (fruit_count <= 16) & (fill_fraction >= 0) & (fill_fraction <= 1) + finite &= ((finger_positions >= -0.001) & (finger_positions <= 0.041)).all(-1) + finite &= ((commanded_positions >= 0) & (commanded_positions <= 0.04)).all(-1) + valid = finite & ~failed + up = cup_pose.new_tensor((0.0, 0.0, 1.0)).expand(n, -1) + relative_position = _rotate(_conjugate(cup_pose[:, 3:]), cap_pose[:, :3] - cup_pose[:, :3]) + relative_rotation = _multiply(_conjugate(cup_pose[:, 3:]), cap_pose[:, 3:]) + relative_up = _rotate(relative_rotation, up)[:, 2] + q = relative_rotation + yaw = torch.atan2(2 * (q[:, 3] * q[:, 2] + q[:, 0] * q[:, 1]), 1 - 2 * (q[:, 1].square() + q[:, 2].square())) + aligned = (relative_position[:, :2].norm(dim=-1) <= CRITERIA["lid_radial_tolerance_m"]) & ( + relative_up > CRITERIA["lid_axis_cosine"] + ) + axial = relative_position[:, 2] + seated = aligned & ((axial - CRITERIA["lid_seated_axial_m"]).abs() <= CRITERIA["lid_seated_tolerance_m"]) + near = aligned & (axial > CRITERIA["lid_near_axial_min_m"]) & (axial < CRITERIA["lid_near_axial_max_m"]) + finger_width = finger_positions.sum(-1) + closing_contact = ( + bilateral_contact + & (contact_deflection >= CRITERIA["contact_deflection_m"]).all(-1) + & ((finger_positions - commanded_positions) >= CRITERIA["contact_deflection_m"]).all(-1) + & (finger_width > 0.001) + & (finger_width < 0.065) + ) + result = { + "finite": finite, + "valid": valid, + "failed": ~valid, + "lid_near_thread": near & valid, + "lid_seated": seated & valid, + } + for name, pose, velocity, offset in ( + ("lid", cap_pose, cap_velocity, (0.0, 0.0, 0.018)), + ("cup", cup_pose, cup_velocity, (0.0, -0.061, 0.150)), + ): + grasp = pose[:, :3] + _rotate(pose[:, 3:], pose.new_tensor(offset).expand(n, -1)) + distance = (tcp_position - grasp).norm(dim=-1) + source_speed = velocity[:, :3] + torch.linalg.cross(velocity[:, 3:], grasp - pose[:, :3]) + hand_speed = hand_velocity[:, :3] + torch.linalg.cross(hand_velocity[:, 3:], grasp - hand_pose[:, :3]) + relative_speed = (source_speed - hand_speed).norm(dim=-1) + relative_angular_speed = (velocity[:, 3:] - hand_velocity[:, 3:]).norm(dim=-1) + held = ( + closing_contact + & (distance <= CRITERIA["grasp_distance_m"]) + & (relative_speed <= CRITERIA["grasp_relative_speed_m_s"]) + & (relative_angular_speed <= CRITERIA["grasp_relative_angular_speed_rad_s"]) + & valid + ) + result.update( + { + f"{name}_held": held, + f"{name}_grasp_position_w_m": grasp, + f"{name}_hand_separation_m": distance, + f"{name}_grasp_relative_speed_m_s": relative_speed, + f"{name}_grasp_relative_angular_speed_rad_s": relative_angular_speed, + f"{name}_grip_position_b_m": _rotate(_conjugate(pose[:, 3:]), tcp_position - pose[:, :3]), + f"{name}_hand_rotation_b_xyzw": _multiply(_conjugate(pose[:, 3:]), hand_pose[:, 3:]), + f"{name}_stable": (velocity[:, :3].norm(dim=-1) < CRITERIA["stable_linear_speed_m_s"]) + & (velocity[:, 3:].norm(dim=-1) < CRITERIA["stable_angular_speed_rad_s"]) + & valid, + } + ) + cup_up, cap_up = _rotate(cup_pose[:, 3:], up)[:, 2], _rotate(cap_pose[:, 3:], up)[:, 2] + motor_local = _rotate(_conjugate(motor_pose[:, 3:]), cap_pose[:, :3] - motor_pose[:, :3]) + dock_error = (motor_local - cup_pose.new_tensor((0.0, 0.0, 0.145))).norm(dim=-1) + motor_relative = _multiply(_conjugate(motor_pose[:, 3:]), cap_pose[:, 3:]) + tab_x = _rotate(motor_relative, cup_pose.new_tensor((1.0, 0.0, 0.0)).expand(n, -1)) + tab_yaw = torch.atan2(tab_x[:, 1], tab_x[:, 0]) + yaw_error = 0.5 * torch.atan2(torch.sin(2 * tab_yaw), torch.cos(2 * tab_yaw)).abs() + corners = cup_pose.new_tensor( + [[x, y, z] for x in (-0.0135, 0.0135) for y in (-0.0175, 0.0175) for z in (0.0, 0.036)] + ) + motor_corners = motor_local[:, None] + _rotate( + motor_relative[:, None].expand(-1, 8, -1), corners[None].expand(n, -1, -1) + ) + key_extent = motor_corners[:, :, :2].abs().amax(1) + key_fit = (key_extent + CRITERIA["dock_key_surface_clearance_m"] <= cup_pose.new_tensor((0.015, 0.019))).all(-1) + dock = ( + (dock_error <= CRITERIA["dock_position_tolerance_m"]) + & (cup_up < -CRITERIA["lid_axis_cosine"]) + & seated + & key_fit + & (yaw_error <= CRITERIA["dock_yaw_tolerance_rad"]) + & result["cup_stable"] + & result["lid_stable"] + & valid + ) + opened = (finger_positions >= CRITERIA["finger_open_min_m"]).all(-1) & valid + cup_home = cup_pose[:, :3] - origins + cup_supported = ( + ((cup_home[:, :2] - cup_pose.new_tensor((0.57, -0.07))).norm(dim=-1) <= 0.006) + & (cup_home[:, 2] >= 0.011) + & (cup_home[:, 2] <= 0.015) + & (cup_up > CRITERIA["lid_axis_cosine"]) + & result["cup_stable"] + ) + result.update( + lid_relative_yaw_rad=yaw, + lid_axial_m=axial, + lid_radial_error_m=relative_position[:, :2].norm(dim=-1), + lid_axis_cosine=relative_up, + lid_upright=cap_up, + cup_up=cup_up, + lid_lift_height_m=cap_pose[:, 2] - origins[:, 2] - 0.078, + cup_lift_height_m=cup_pose[:, 2] - origins[:, 2] - 0.013, + cup_home_supported=cup_supported, + fingers_open=opened, + dock_position_error_m=dock_error, + dock_yaw_error_rad=yaw_error, + dock_key_fit=key_fit & valid, + dock_key_projected_half_extent_m=key_extent, + dock_support_ready=dock, + dock_released=dock + & opened + & (result["cup_hand_separation_m"] >= CRITERIA["hand_separation_m"]) + & ~result["cup_held"], + ingredients_retained=(fruit_count == CRITERIA["minimum_fruit_count"]) + & all_types + & (fill_fraction >= CRITERIA["minimum_fill_fraction"]) + & valid, + ) + return result + + +class AssemblyGeometryState: + """Ordered measured state, with no object attachment or pose mutation.""" + + def __init__(self, num_envs: int, device: str | torch.device, step_dt: float): + if type(num_envs) is not int or num_envs < 1 or not math.isfinite(step_dt) or step_dt <= 0: + raise ValueError("Require positive world count and timestep [s].") + self.step_dt = step_dt + self.state: dict[str, torch.Tensor] = {} + floats = [name for name, _ in OBSERVATION_SCALES if name.endswith("_s")] + floats += ["lid_clockwise_turns", "lid_engaged_descent_m", "previous_yaw_rad", "maximum_engaged_axial_m"] + for name in floats: + self.state[name] = torch.zeros(num_envs, device=device) + for name in ( + "lid_twist_complete", + "lid_release_complete", + "cup_lift_complete", + "cup_inverted", + "dock_complete", + "lid_retention_failed", + "assembly_failed", + "assembly_complete", + "previous_turn_eligible", + "engagement_seen", + ): + self.state[name] = torch.zeros(num_envs, device=device, dtype=torch.bool) + self.state["assembly_stage"] = torch.zeros(num_envs, device=device, dtype=torch.long) + self.state["last_step"] = torch.full((num_envs,), -1, device=device, dtype=torch.long) + self.measurements: dict[str, torch.Tensor] = self.state.copy() + + def reset(self, env_ids: torch.Tensor) -> None: + """Reset only selected episode milestones, without modifying physics.""" + for name, value in self.state.items(): + value[env_ids] = -1 if name == "last_step" else 0 + self.measurements = self.state.copy() + + def observations(self) -> torch.Tensor: + """Return the documented 16 normalized state features, shape [N, 16].""" + return torch.stack([self.state[name].float() / scale for name, scale in OBSERVATION_SCALES], -1) + + def update( + self, + measurements: dict[str, torch.Tensor], + active: torch.Tensor, + receipt_complete: torch.Tensor, + *, + step: int, + ) -> dict[str, torch.Tensor]: + """Advance once per observed policy boundary, in physical milestone order. + + Args: + measurements: Output of :func:`assembly_geometry` from this boundary. + active: Boolean worlds currently in the assembly phase, shape [N]. + receipt_complete: Boolean prior fruit/tap completion proof, shape [N]. + step: Nonnegative global policy step; first activation may occur at any step. + """ + if type(step) is not int or step < 0: + raise ValueError("Require a nonnegative integer global policy step.") + s, m = self.state, measurements + if any( + value.shape != s["assembly_stage"].shape or value.dtype != torch.bool + for value in (active, receipt_complete) + ): + raise ValueError("Require one Boolean active and receipt flag per world.") + if bool((active & (s["last_step"] > step)).any()): + raise ValueError("Assembly policy steps cannot move backward without reset.") + fresh = active & (s["last_step"] != step) + if not bool(fresh.any()): + return self.measurements + consecutive = fresh & (s["last_step"] == step - 1) + gap = fresh & (s["last_step"] >= 0) & ~consecutive + for name in s: + if name.endswith("_hold_s"): + s[name][gap] = 0 + s["previous_turn_eligible"][gap] = False + s["last_step"][fresh] = step + s["assembly_failed"] |= fresh & ~m["valid"] + valid = fresh & receipt_complete & m["valid"] & ~s["assembly_failed"] + stage = s["assembly_stage"].clone() + retention_required = stage >= AssemblyStage.LID_RELEASE + s["lid_retention_failed"] |= fresh & retention_required & ~m["lid_seated"] + s["assembly_failed"] |= s["lid_retention_failed"] + valid &= ~s["lid_retention_failed"] + + def hold(name: str, condition: torch.Tensor) -> torch.Tensor: + updated = torch.where(valid & condition, s[name] + self.step_dt, 0.0) + s[name].copy_(torch.where(fresh, updated, s[name])) + return s[name] >= CRITERIA[name] - 1e-6 + + lifted_lid = hold( + "lid_lift_hold_s", + (stage == AssemblyStage.LID_PICKUP) + & m["lid_held"] + & (m["lid_lift_height_m"] >= CRITERIA["lid_lift_height_m"]) + & (m["lid_upright"] > CRITERIA["lid_axis_cosine"]), + ) + s["assembly_stage"][valid & lifted_lid & (stage == AssemblyStage.LID_PICKUP)] = AssemblyStage.LID_ALIGN + aligned = hold("lid_align_hold_s", (stage == AssemblyStage.LID_ALIGN) & m["lid_near_thread"] & m["lid_held"]) + s["assembly_stage"][valid & aligned & (stage == AssemblyStage.LID_ALIGN)] = AssemblyStage.LID_THREAD + eligible = valid & (stage == AssemblyStage.LID_THREAD) & m["lid_near_thread"] & m["lid_held"] + delta = torch.atan2( + torch.sin(m["lid_relative_yaw_rad"] - s["previous_yaw_rad"]), + torch.cos(m["lid_relative_yaw_rad"] - s["previous_yaw_rad"]), + ) + pair = eligible & s["previous_turn_eligible"] & consecutive + plausible = delta.abs() <= CRITERIA["maximum_turn_increment_rad"] + s["assembly_failed"] |= pair & ~plausible + increment = torch.where(pair & plausible, -delta / (2 * math.pi), 0.0) + s["lid_clockwise_turns"] += increment + first_engagement = eligible & ~s["engagement_seen"] + s["maximum_engaged_axial_m"].copy_( + torch.where(first_engagement, m["lid_axial_m"], s["maximum_engaged_axial_m"]) + ) + s["maximum_engaged_axial_m"].copy_( + torch.where( + eligible, torch.maximum(s["maximum_engaged_axial_m"], m["lid_axial_m"]), s["maximum_engaged_axial_m"] + ) + ) + descent = (s["maximum_engaged_axial_m"] - m["lid_axial_m"]).clamp_min(0) + s["lid_engaged_descent_m"].copy_( + torch.where(eligible, torch.maximum(s["lid_engaged_descent_m"], descent), s["lid_engaged_descent_m"]) + ) + s["engagement_seen"] |= eligible + s["previous_turn_eligible"].copy_(torch.where(fresh, eligible, s["previous_turn_eligible"])) + s["previous_yaw_rad"].copy_(torch.where(fresh, m["lid_relative_yaw_rad"], s["previous_yaw_rad"])) + valid &= ~s["assembly_failed"] + twisted = hold( + "lid_seat_hold_s", + (stage == AssemblyStage.LID_THREAD) + & eligible + & m["lid_seated"] + & m["lid_stable"] + & m["cup_stable"] + & (s["lid_clockwise_turns"] >= CRITERIA["lid_clockwise_turns"] - 1e-6) + & (s["lid_engaged_descent_m"] >= CRITERIA["lid_engaged_descent_m"]), + ) + s["lid_twist_complete"] |= valid & twisted + s["assembly_stage"][valid & twisted & (stage == AssemblyStage.LID_THREAD)] = AssemblyStage.LID_RELEASE + released_lid = hold( + "lid_release_hold_s", + (stage == AssemblyStage.LID_RELEASE) + & s["lid_twist_complete"] + & m["cup_home_supported"] + & m["lid_stable"] + & m["lid_seated"] + & m["fingers_open"] + & (m["lid_hand_separation_m"] >= CRITERIA["hand_separation_m"]) + & ~m["lid_held"], + ) + s["lid_release_complete"] |= valid & released_lid + s["assembly_stage"][valid & released_lid & (stage == AssemblyStage.LID_RELEASE)] = AssemblyStage.CUP_PICKUP + lifted_cup = hold( + "cup_lift_hold_s", + (stage == AssemblyStage.CUP_PICKUP) + & s["lid_release_complete"] + & m["cup_held"] + & m["lid_seated"] + & (m["cup_up"] > CRITERIA["lid_axis_cosine"]) + & (m["cup_lift_height_m"] >= CRITERIA["cup_lift_height_m"]), + ) + s["cup_lift_complete"] |= valid & lifted_cup + s["assembly_stage"][valid & lifted_cup & (stage == AssemblyStage.CUP_PICKUP)] = AssemblyStage.CUP_INVERT + inverted = hold( + "cup_inverted_hold_s", + (stage == AssemblyStage.CUP_INVERT) + & s["cup_lift_complete"] + & m["cup_held"] + & m["lid_seated"] + & (m["cup_up"] < -CRITERIA["lid_axis_cosine"]), + ) + s["cup_inverted"] |= valid & inverted + s["assembly_stage"][valid & inverted & (stage == AssemblyStage.CUP_INVERT)] = AssemblyStage.CUP_DOCK + supported = hold( + "dock_support_hold_s", + (stage == AssemblyStage.CUP_DOCK) & s["cup_inverted"] & m["cup_held"] & m["dock_support_ready"], + ) + s["dock_complete"] |= valid & supported + s["assembly_stage"][valid & supported & (stage == AssemblyStage.CUP_DOCK)] = AssemblyStage.FINAL_RELEASE + finished = hold( + "final_release_hold_s", + (stage >= AssemblyStage.FINAL_RELEASE) + & s["dock_complete"] + & m["dock_released"] + & m["ingredients_retained"] + & s["lid_twist_complete"], + ) + s["assembly_complete"].copy_(torch.where(fresh, finished & valid, s["assembly_complete"])) + s["assembly_stage"][valid & finished] = AssemblyStage.COMPLETE + self.measurements = {**m, **s} + return self.measurements diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_asset.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_asset.py new file mode 100644 index 000000000000..0cbba2fa9d3f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_asset.py @@ -0,0 +1,276 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Build a fixed tap with a physical spring button and a separate cup station floor.""" + +from __future__ import annotations + +import argparse +import hashlib +import json +import math +from pathlib import Path + +import numpy as np + +from pxr import Gf, Sdf, Usd, UsdGeom, UsdPhysics, UsdShade + +HERE = Path(__file__).resolve().parent +ASSET_PATH = HERE / "assets/tap.usda" +TAP_ORIGIN_M = (0.28, -0.30, 0.0) +CUP_STATION_POSITION_M = (0.38, -0.30, 0.013) +NOZZLE_POSITION_M = (0.38, -0.30, 0.290) +BUTTON_TOP_POSITION_M = (0.28, -0.30, 0.100) +BUTTON_PRESSED_POSITION_M = (0.28, -0.30, 0.096) +BUTTON_BODY_CENTER_LOCAL_M = (0.0, 0.0, 0.096) +BUTTON_TRAVEL_M = 0.004 +BUTTON_PRESS_THRESHOLD_M = 0.002 +BUTTON_RELEASE_THRESHOLD_M = 0.001 +BUTTON_RADIUS_M = 0.022 +BUTTON_MASS_KG = 0.02 +BUTTON_STIFFNESS_N_M = 300.0 +BUTTON_DAMPING_N_S_M = 3.0 +BUTTON_EFFORT_LIMIT_N = 10.0 +STATION_FLOOR_RADIUS_M = 0.067 +STATION_FLOOR_TOP_M = 0.012 +BASE_BODY_PATH = "/Asset/Base" +BUTTON_BODY_PATH = "/Asset/Button" +BUTTON_JOINT_PATH = "/Asset/TapButton" +BUTTON_JOINT_NAME = "TapButton" + +# Each part contributes a simple primitive collider and its component mass. +# Overlapping fixed fittings are intentional; the spring button is a separate link. +PARTS = ( + ("Foot", "box", (-0.005, 0.045, 0.009), (0.070, 0.180, 0.018), "charcoal"), + ("CupStationFloor", "cylinder", (0.100, 0.0, 0.006), (0.067, 0.012, "Z"), "charcoal"), + ("Mast", "cylinder", (0.0, 0.105, 0.172), (0.019, 0.308, "Z"), "steel"), + ("ArmX", "cylinder", (0.050, 0.105, 0.326), (0.015, 0.100, "X"), "steel"), + ("ArmY", "cylinder", (0.100, 0.0525, 0.326), (0.015, 0.105, "Y"), "steel"), + ("Nozzle", "cylinder", (0.100, 0.0, 0.308), (0.015, 0.036, "Z"), "steel"), + ("ControlStand", "cylinder", (0.0, 0.0, 0.053), (0.026, 0.070, "Z"), "charcoal"), +) +COLORS = { + "steel": (0.62, 0.69, 0.76), + "charcoal": (0.065, 0.085, 0.105), + "button": (0.075, 0.50, 0.78), + "white": (0.88, 0.95, 1.0), + "outlet": (0.018, 0.024, 0.030), +} + + +def _material(stage: Usd.Stage, name: str) -> UsdShade.Material: + material = UsdShade.Material.Define(stage, f"/Asset/Materials/{name}") + shader = UsdShade.Shader.Define(stage, material.GetPath().AppendChild("Surface")) + shader.CreateIdAttr("UsdPreviewSurface") + shader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).Set(Gf.Vec3f(*COLORS[name])) + shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(0.23 if name == "steel" else 0.38) + shader.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(0.85 if name == "steel" else 0.0) + material.CreateSurfaceOutput().ConnectToSource(shader.ConnectableAPI(), "surface") + return material + + +def _primitive(stage, path, kind, center, dimensions, material, *, collision=True): + if kind == "box": + shape = UsdGeom.Cube.Define(stage, path) + shape.CreateSizeAttr(1.0) + shape.AddTranslateOp().Set(Gf.Vec3d(*center)) + shape.AddScaleOp().Set(Gf.Vec3f(*dimensions)) + else: + radius, height, axis = dimensions + shape = UsdGeom.Cylinder.Define(stage, path) + shape.CreateRadiusAttr(radius) + shape.CreateHeightAttr(height) + shape.CreateAxisAttr(axis) + shape.AddTranslateOp().Set(Gf.Vec3d(*center)) + shape.CreateDisplayColorAttr([Gf.Vec3f(*COLORS[material])]) + UsdShade.MaterialBindingAPI.Apply(shape.GetPrim()).Bind( + UsdShade.Material(stage.GetPrimAtPath(f"/Asset/Materials/{material}")) + ) + if collision: + UsdPhysics.CollisionAPI.Apply(shape.GetPrim()) + return shape + + +def _base_mass_properties() -> tuple[float, np.ndarray, np.ndarray]: + """Return additive component mass [kg], COM [m], and inertia tensor [kg m²].""" + total_mass = 1.2 + volumes, inertias, centers = [], [], [] + for _, kind, center, dimensions, _ in PARTS: + centers.append(center) + if kind == "box": + x, y, z = dimensions + volumes.append(x * y * z) + inertias.append(np.array([y * y + z * z, x * x + z * z, x * x + y * y]) / 12) + else: + radius, height, axis = dimensions + volumes.append(math.pi * radius * radius * height) + inertia = np.full(3, (3 * radius * radius + height * height) / 12) + inertia["XYZ".index(axis)] = radius * radius / 2 + inertias.append(inertia) + masses = total_mass * np.asarray(volumes) / sum(volumes) + centers = np.asarray(centers) + com = np.sum(masses[:, None] * centers, axis=0) / total_mass + tensor = np.zeros((3, 3)) + for mass, center, inertia in zip(masses, centers, inertias, strict=True): + delta = center - com + tensor += mass * (np.diag(inertia) + np.dot(delta, delta) * np.eye(3) - np.outer(delta, delta)) + return total_mass, com, tensor + + +def _body(stage, path, mass, center, tensor): + body = UsdGeom.Xform.Define(stage, path) + UsdPhysics.RigidBodyAPI.Apply(body.GetPrim()) + values, axes = np.linalg.eigh(tensor) + if np.linalg.det(axes) < 0: + axes[:, 0] *= -1 + rotation = Gf.Matrix3d(*axes.T.reshape(-1).tolist()).ExtractRotation().GetQuat() + physics = UsdPhysics.MassAPI.Apply(body.GetPrim()) + physics.CreateMassAttr(mass) + physics.CreateCenterOfMassAttr(Gf.Vec3f(*center)) + physics.CreateDiagonalInertiaAttr(Gf.Vec3f(*values)) + physics.CreatePrincipalAxesAttr(Gf.Quatf(rotation)) + return body + + +def build_tap_asset(*, output: Path = ASSET_PATH) -> dict: + """Write the tap articulation in SI units and verify its authored structure. + + The root is placed at :data:`TAP_ORIGIN_M`. The button travels downward with + negative joint displacement [m], and a force drive returns it to zero. + """ + stage = Usd.Stage.CreateInMemory() + UsdGeom.SetStageMetersPerUnit(stage, 1.0) + UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z) + root = UsdGeom.Xform.Define(stage, "/Asset") + stage.SetDefaultPrim(root.GetPrim()) + UsdPhysics.ArticulationRootAPI.Apply(root.GetPrim()) + for name in COLORS: + _material(stage, name) + mass, com, inertia = _base_mass_properties() + _body(stage, BASE_BODY_PATH, mass, com, inertia) + for name, kind, center, dimensions, material in PARTS: + _primitive(stage, f"{BASE_BODY_PATH}/{name}", kind, center, dimensions, material) + # Dark outlet and light accents are display-only, with no hidden colliders. + _primitive( + stage, + f"{BASE_BODY_PATH}/Outlet", + "cylinder", + (0.100, 0.0, 0.2898), + (0.010, 0.0003, "Z"), + "outlet", + collision=False, + ) + _primitive( + stage, + f"{BASE_BODY_PATH}/StationMark", + "cylinder", + (0.100, 0.0, 0.0122), + (0.052, 0.0003, "Z"), + "steel", + collision=False, + ) + mount = UsdPhysics.FixedJoint.Define(stage, "/Asset/Mount") + mount.CreateBody1Rel().SetTargets([Sdf.Path(BASE_BODY_PATH)]) + radius, height, mass = BUTTON_RADIUS_M, 0.008, BUTTON_MASS_KG + button_tensor = np.diag([mass * (3 * radius * radius + height * height) / 12] * 2 + [mass * radius * radius / 2]) + button = _body(stage, BUTTON_BODY_PATH, mass, (0.0, 0.0, 0.0), button_tensor) + button.AddTranslateOp().Set(Gf.Vec3d(*BUTTON_BODY_CENTER_LOCAL_M)) + _primitive(stage, f"{BUTTON_BODY_PATH}/Face", "cylinder", (0.0, 0.0, 0.0), (radius, height, "Z"), "button") + _primitive( + stage, + f"{BUTTON_BODY_PATH}/Indicator", + "cylinder", + (0.0, 0.0, 0.00415), + (0.004, 0.0002, "Z"), + "white", + collision=False, + ) + joint = UsdPhysics.PrismaticJoint.Define(stage, BUTTON_JOINT_PATH) + joint.CreateAxisAttr("Z") + joint.CreateBody0Rel().SetTargets([Sdf.Path(BASE_BODY_PATH)]) + joint.CreateBody1Rel().SetTargets([Sdf.Path(BUTTON_BODY_PATH)]) + joint.CreateLocalPos0Attr(Gf.Vec3f(*BUTTON_BODY_CENTER_LOCAL_M)) + joint.CreateLocalPos1Attr(Gf.Vec3f(0.0)) + joint.CreateLowerLimitAttr(-BUTTON_TRAVEL_M) + joint.CreateUpperLimitAttr(0.0) + drive = UsdPhysics.DriveAPI.Apply(joint.GetPrim(), "linear") + drive.CreateTypeAttr("force") + drive.CreateStiffnessAttr(BUTTON_STIFFNESS_N_M) + drive.CreateDampingAttr(BUTTON_DAMPING_N_S_M) + drive.CreateTargetPositionAttr(0.0) + drive.CreateMaxForceAttr(BUTTON_EFFORT_LIMIT_N) + output = output.resolve() + output.parent.mkdir(parents=True, exist_ok=True) + content = stage.GetRootLayer().ExportToString() + if not output.is_file() or output.read_text() != content: + temporary = output.with_name(".tap.tmp.usda") + temporary.write_text(content) + temporary.replace(output) + return inspect_tap_asset(output) + + +def inspect_tap_asset(path: Path = ASSET_PATH) -> dict: + """Check body/joint identities, positive mass/inertia, and button limits [m].""" + stage = Usd.Stage.Open(str(path)) + bodies = [prim for prim in stage.Traverse() if prim.HasAPI(UsdPhysics.RigidBodyAPI)] + assert {str(prim.GetPath()) for prim in bodies} == {BASE_BODY_PATH, BUTTON_BODY_PATH} + for prim in bodies: + physics = UsdPhysics.MassAPI(prim) + assert physics.GetMassAttr().Get() > 0 + assert min(physics.GetDiagonalInertiaAttr().Get()) > 0 + joint = UsdPhysics.PrismaticJoint(stage.GetPrimAtPath(BUTTON_JOINT_PATH)) + assert joint and joint.GetAxisAttr().Get() == "Z" + assert math.isclose(joint.GetLowerLimitAttr().Get(), -BUTTON_TRAVEL_M, abs_tol=1e-9) + assert joint.GetUpperLimitAttr().Get() == 0.0 + collisions = [str(p.GetPath()) for p in stage.Traverse() if p.HasAPI(UsdPhysics.CollisionAPI)] + assert len(collisions) == len(PARTS) + 1 + return { + "asset": str(path.resolve()), + "sha256": hashlib.sha256(path.read_bytes()).hexdigest(), + "origin_m": TAP_ORIGIN_M, + "body_paths": [BASE_BODY_PATH, BUTTON_BODY_PATH], + "joint_path": BUTTON_JOINT_PATH, + "joint_limits_m": [-BUTTON_TRAVEL_M, 0.0], + "button_top_world_m": BUTTON_TOP_POSITION_M, + "button_pressed_top_world_m": BUTTON_PRESSED_POSITION_M, + "press_threshold_depression_m": BUTTON_PRESS_THRESHOLD_M, + "release_threshold_depression_m": BUTTON_RELEASE_THRESHOLD_M, + "nozzle_outlet_world_m": NOZZLE_POSITION_M, + "cup_station_pose_m": CUP_STATION_POSITION_M, + "station_floor_top_m": STATION_FLOOR_TOP_M, + "station_floor_radius_m": STATION_FLOOR_RADIUS_M, + "collision_paths": collisions, + "rigid_body_count": 2, + "particle_count": 0, + "physical_rollout_performed": False, + } + + +def tap_articulation_cfg(prim_path: str = "{ENV_REGEX_NS}/Tap"): + """Return the fixed tap articulation with its spring drive [N/m, N s/m].""" + from isaaclab.actuators import ImplicitActuatorCfg + from isaaclab.assets import ArticulationCfg + from isaaclab.sim import UsdFileCfg + + return ArticulationCfg( + prim_path=prim_path, + spawn=UsdFileCfg(usd_path=str(ASSET_PATH)), + init_state=ArticulationCfg.InitialStateCfg(pos=TAP_ORIGIN_M, joint_pos={BUTTON_JOINT_NAME: 0.0}), + actuators={ + "button": ImplicitActuatorCfg( + joint_names_expr=[BUTTON_JOINT_NAME], + stiffness=BUTTON_STIFFNESS_N_M, + damping=BUTTON_DAMPING_N_S_M, + joint_effort_limit=BUTTON_EFFORT_LIMIT_N, + ) + }, + ) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--check", action="store_true") + args = parser.parse_args() + print(json.dumps(inspect_tap_asset() if args.check else build_tap_asset(), indent=2)) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_controller.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_controller.py new file mode 100644 index 000000000000..128c67646a7c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_controller.py @@ -0,0 +1,508 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Measured robot actions for fruit, visual tap filling, lid, docking and button. + +Only robot actions are commanded. Cup and basket transforms are measured grasp +references, never physical attachments. The environment owns tap state, fill +timers, contact measurements and task success. +""" + +from __future__ import annotations + +import json +from pathlib import Path +from typing import Any + +import numpy as np +from scipy.spatial.transform import Rotation, Slerp + +from .assembly_controller import OPTIONS as ASSEMBLY_OPTIONS +from .assembly_controller import AssemblyTrajectory, bounded_joint_posture_step + +OPTIONS = { + "tap_cup_position": [0.38, -0.30, 0.013], + "tap_button_position": [0.28, -0.30, 0.10], + "tap_button_direction": [0.0, 0.0, -1.0], + "tap_button_rotation": [1.0, 0.0, 0.0, 0.0], + "motor_button_position": [0.64, 0.177, 0.09], + "motor_button_direction": [0.0, 1.0, 0.0], + "motor_button_rotation": [-(2**-0.5), 0.0, 0.0, 2**-0.5], + "button_prepress_distance_m": 0.045, + "button_press_distance_m": 0.004, + # The pinned closed finger pads extend 4.9 mm beyond the measured TCP. + "finger_pad_forward_offset_m": 0.0049, + "button_contact_overtravel_m": 0.0005, + "cup_lift_m": 0.17, + "tap_clearance_lift_m": 0.03, + "tap_front_offset_m": [0.0, -0.14, 0.0], + "cup_pregrasp_distance_m": 0.05, + "position_tolerance_m": 0.008, + "rotation_tolerance_rad": 0.08, + "endpoint_hold_s": 0.20, + "support_hold_s": 0.35, + "tap_wait_s": 2.0, + "maximum_stage_s": 60.0, +} + +TAP_STAGES = ( + ("raise_empty", 4.0), + ("cup_joint_posture", 12.0), + ("cup_pregrasp", 4.0), + ("cup_approach", 3.0), + ("cup_close", 2.5), + ("cup_lift", 3.0), + ("cup_carry_corner", 3.0), + ("cup_carry_tap", 4.0), + ("cup_lower_tap_front", 3.0), + ("cup_enter_tap", 3.0), + ("cup_lower_tap", 3.0), + ("cup_support_tap", 0.5), + ("cup_release_tap", 2.5), + ("cup_retreat_tap", 2.0), + ("tap_raise", 2.0), + ("tap_approach", 3.0), + ("tap_press_on", 1.0), + ("tap_retract_on", 1.2), + ("tap_wait", 0.0), + ("tap_press_off", 1.0), + ("tap_retract_off", 1.2), + ("return_raise", 2.0), + ("return_pregrasp", 4.0), + ("return_approach", 3.0), + ("return_close", 2.5), + ("return_lift", 3.0), + ("return_leave_tap", 3.0), + ("return_raise_cup", 3.0), + ("return_carry_corner", 3.0), + ("return_carry_home", 4.0), + ("return_lower_home", 3.0), + ("return_support_home", 0.5), + ("return_release_home", 2.5), + ("return_retreat_home", 2.0), + ("complete", 0.25), +) +BUTTON_STAGES = (("raise", 3.0), ("approach", 3.0), ("press", 1.0), ("retract", 1.5), ("complete", 0.25)) + + +def _one(value: Any) -> Any: + return value.item() if hasattr(value, "numel") and value.numel() == 1 else value + + +from .basket_controller import BasketController + + +def _pose(position: np.ndarray, rotation: Rotation) -> np.ndarray: + return np.r_[position, rotation.as_quat()] + + +def _interpolate(start: np.ndarray, goal: np.ndarray, fraction: float) -> np.ndarray: + fraction = float(np.clip(fraction, 0.0, 1.0)) + blend = fraction * fraction * (3.0 - 2.0 * fraction) + rotation = Slerp([0.0, 1.0], Rotation.from_quat([start[3:], goal[3:]]))([blend])[0] + return _pose(start[:3] + blend * (goal[:3] - start[:3]), rotation) + + +class _SmoothieTrajectory: + """Track physical cup and button targets using measured completion gates.""" + + def __init__(self, options: dict[str, Any], *, button_only: bool = False): + self.options = options + self.button_only = button_only + self.stages = BUTTON_STAGES if button_only else TAP_STAGES + self.index = 0 + self.elapsed_s = self.stage_wall_s = self.hold_s = self.support_s = 0.0 + self.start: np.ndarray | None = None + self.goal: np.ndarray | None = None + self.joint_start: np.ndarray | None = None + self.home: np.ndarray | None = None + self.grasp_position: np.ndarray | None = None + self.grasp_rotation: Rotation | None = None + self.close_target: np.ndarray | None = None + self.release_authorized = False + self.complete = False + self.placement = AssemblyTrajectory() + + @property + def stage(self) -> str: + return self.stages[self.index][0] + + def _grasp(self, cup: np.ndarray, *, backoff: bool = False) -> np.ndarray: + rotation = Rotation.from_quat(cup[3:]) + local = np.asarray(ASSEMBLY_OPTIONS["cup_grasp_local_m"]).copy() + if backoff: + local[1] -= self.options["cup_pregrasp_distance_m"] + return _pose( + cup[:3] + rotation.apply(local), rotation * Rotation.from_quat(ASSEMBLY_OPTIONS["cup_hand_local_xyzw"]) + ) + + def _held(self, cup: np.ndarray) -> np.ndarray: + if self.grasp_position is None or self.grasp_rotation is None: + raise RuntimeError("Cup motion requires a measured closed grasp.") + rotation = Rotation.from_quat(cup[3:]) + return _pose(cup[:3] + rotation.apply(self.grasp_position), rotation * self.grasp_rotation) + + def _button(self, *, pressed: bool) -> np.ndarray: + prefix = "motor" if self.button_only else "tap" + direction = np.asarray(self.options[prefix + "_button_direction"], dtype=float) + direction /= np.linalg.norm(direction) + distance = ( + self.options["button_press_distance_m"] + - self.options["finger_pad_forward_offset_m"] + + self.options["button_contact_overtravel_m"] + if pressed + else -self.options["button_prepress_distance_m"] + ) + return _pose( + np.asarray(self.options[prefix + "_button_position"]) + distance * direction, + Rotation.from_quat(self.options[prefix + "_button_rotation"]), + ) + + def _enter(self, hand: np.ndarray, cup: np.ndarray, joints: np.ndarray) -> None: + stage = self.stage + self.start, self.goal = hand.copy(), hand.copy() + self.release_authorized = False + self.support_s = 0.0 + if self.home is None: + self.home = cup.copy() + if self.button_only: + if stage == "raise": + self.goal[2] = max(hand[2], 0.40) + elif stage in ("approach", "retract"): + self.goal = self._button(pressed=False) + elif stage == "press": + self.goal = self._button(pressed=True) + return + if stage in ("raise_empty", "tap_raise", "return_raise"): + self.goal[2] = max(hand[2], 0.60 if stage == "raise_empty" else 0.42) + elif stage == "cup_joint_posture": + self.joint_start = joints.copy() + elif stage in ("cup_pregrasp", "return_pregrasp"): + self.goal = self._grasp(cup, backoff=True) + self.placement = AssemblyTrajectory() + elif stage in ("cup_approach", "return_approach"): + self.goal = self._grasp(cup) + elif stage in ("cup_close", "return_close"): + self.goal = self.close_target.copy() if self.close_target is not None else hand.copy() + elif stage in ("cup_lift", "return_lift"): + rotation = Rotation.from_quat(cup[3:]) + self.grasp_position = rotation.inv().apply(hand[:3] - cup[:3]) + self.grasp_rotation = rotation.inv() * Rotation.from_quat(hand[3:]) + target = cup.copy() + target[2] += self.options["tap_clearance_lift_m" if stage == "return_lift" else "cup_lift_m"] + self.goal = self._held(target) + elif stage in ( + "cup_carry_corner", + "cup_carry_tap", + "cup_lower_tap_front", + "cup_enter_tap", + "cup_lower_tap", + "return_leave_tap", + "return_raise_cup", + "return_carry_corner", + "return_carry_home", + "return_lower_home", + ): + target = self.home.copy() + if stage not in ("cup_carry_corner", "return_carry_corner", "return_carry_home", "return_lower_home"): + target[:3] = self.options["tap_cup_position"] + if stage in ("cup_carry_corner", "return_carry_corner"): + target[1] = self.options["tap_cup_position"][1] + self.options["tap_front_offset_m"][1] + # Stay in front of the nozzle until the cup rim is below its outlet. + if stage in ("cup_carry_tap", "cup_lower_tap_front", "return_leave_tap", "return_raise_cup"): + target[:3] += self.options["tap_front_offset_m"] + if stage in ( + "cup_carry_corner", + "cup_carry_tap", + "return_raise_cup", + "return_carry_corner", + "return_carry_home", + ): + target[2] += self.options["cup_lift_m"] + elif stage in ("cup_lower_tap_front", "cup_enter_tap", "return_leave_tap"): + target[2] += self.options["tap_clearance_lift_m"] + self.goal = self._held(target) + elif stage in ("cup_retreat_tap", "return_retreat_home"): + self.goal[:3] += Rotation.from_quat(cup[3:]).apply([0.0, -0.11, 0.03]) + elif stage in ("tap_approach", "tap_retract_on", "tap_retract_off", "tap_wait"): + self.goal = self._button(pressed=False) + elif stage in ("tap_press_on", "tap_press_off"): + self.goal = self._button(pressed=True) + + def _gates(self, m: dict, endpoint: bool, dt: float) -> tuple[bool, bool]: + def get(name: str) -> bool: + return bool(m.get(name, False)) + + stage = self.stage + if self.button_only: + gate = get("assembly_complete") + if stage == "press" and endpoint: + gate &= get("motor_on") or get("motor_button_pressed") + if stage in ("retract", "complete") and endpoint: + gate &= get("motor_on") + return gate, True + held_stages = { + "cup_lift", + "cup_carry_corner", + "cup_carry_tap", + "cup_lower_tap_front", + "cup_enter_tap", + "cup_lower_tap", + "cup_support_tap", + "return_lift", + "return_leave_tap", + "return_raise_cup", + "return_carry_corner", + "return_carry_home", + "return_lower_home", + "return_support_home", + } + close = stage in held_stages or stage in ("cup_close", "return_close") or stage.startswith("tap_") + gate = get("cup_held") if stage in held_stages else True + if stage in ("cup_close", "return_close") and endpoint: + gate &= get("cup_held") + if stage in ("cup_support_tap", "cup_release_tap", "return_support_home", "return_release_home"): + positioned = get("cup_under_tap") if stage.startswith("cup_") else get("cup_home") + supported = get("cup_supported") and positioned + self.support_s = self.support_s + dt if supported else 0.0 + if "release" in stage: + self.release_authorized |= self.support_s >= self.options["support_hold_s"] - 1e-9 + close = not self.release_authorized + gate = self.release_authorized and supported + if endpoint: + gate &= get("fingers_open") + elif endpoint: + gate &= supported and self.support_s >= self.options["support_hold_s"] - 1e-9 + if stage.startswith("tap_"): + gate &= get("cup_under_tap") and get("cup_supported") + if stage == "tap_press_on" and endpoint: + gate &= get("tap_on") + elif stage in ("tap_retract_on", "tap_wait"): + gate &= get("tap_on") + if endpoint: + gate &= not get("tap_button_pressed") + elif stage in ("tap_press_off", "tap_retract_off") and endpoint: + gate &= not get("tap_on") + if stage == "tap_wait": + gate &= float(m.get("tap_fill_time_s", 0.0)) >= self.options["tap_wait_s"] + if stage.startswith("return_"): + gate &= not get("tap_on") + if stage in ( + "raise_empty", + "cup_joint_posture", + "cup_pregrasp", + "cup_approach", + "return_raise", + "return_pregrasp", + "return_approach", + ): + gate &= get("fingers_open") + if stage in ("return_retreat_home", "complete"): + gate &= get("cup_home") and get("cup_supported") and get("fingers_open") and not get("tap_on") + return gate, close + + def step(self, state: dict, dt: float) -> dict: + if not np.isfinite(dt) or dt <= 0: + raise ValueError("Policy interval must be positive and finite [s].") + hand, cup = np.asarray(state["hand_pose"]), np.asarray(state["cup_pose"]) + joints, m = np.asarray(state["joints"]), state["metrics"] + if not bool(m.get("finite", m.get("valid", False))) or bool(m.get("failed", False)): + raise RuntimeError("Tap sequence encountered invalid physical state.") + if not all(np.isfinite(a).all() for a in (hand, cup, joints)): + raise RuntimeError("Nonfinite measured robot or cup state.") + if self.start is None: + self._enter(hand, cup, joints) + duration = self.stages[self.index][1] + endpoint = self.elapsed_s >= duration - 1e-9 + fraction = min(self.elapsed_s / max(duration, dt), 1.0) + sample = _interpolate(self.start, self.goal, fraction) + joint_target = None + placement = {} + position_error = float(np.linalg.norm(sample[:3] - hand[:3])) + rotation_error = float((Rotation.from_quat(sample[3:]) * Rotation.from_quat(hand[3:]).inv()).magnitude()) + tracking = ( + position_error <= self.options["position_tolerance_m"] + and rotation_error <= self.options["rotation_tolerance_rad"] + ) + if self.stage == "cup_joint_posture": + blend = fraction * fraction * (3.0 - 2.0 * fraction) + joint_target = self.joint_start + blend * ( + np.asarray(ASSEMBLY_OPTIONS["cup_empty_joint_waypoint_rad"]) - self.joint_start + ) + tracking = np.max(np.abs(joint_target - joints)) <= (0.008 if endpoint else 0.020) + if self.stage in ("cup_approach", "return_approach") and endpoint: + sample, tracking, placement = self.placement._cup_approach_endpoint( + hand, cup, dt, bool(m.get("fingers_open", False)) + ) + self.close_target = sample.copy() + gate, close = self._gates(m, endpoint, dt) + self.stage_wall_s += dt + if self.stage_wall_s > self.options["maximum_stage_s"]: + raise RuntimeError(f"Tap substage {self.stage} exceeded its bounded physical time.") + self.hold_s = self.hold_s + dt if endpoint and tracking and gate else 0.0 + result = { + "stage": self.stage, + "tcp_position": sample[:3], + "hand_xyzw": sample[3:], + "close": close, + "joint_position_target_rad": joint_target, + "elapsed_s": self.elapsed_s, + "stage_wall_s": self.stage_wall_s, + "position_error_m": position_error, + "rotation_error_rad": rotation_error, + "tracking": bool(tracking), + "gate": bool(gate), + "hold_s": self.hold_s, + "complete": self.complete, + **placement, + } + if tracking and gate: + self.elapsed_s = min(duration, self.elapsed_s + dt) + if self.hold_s >= self.options["endpoint_hold_s"] - 1e-9: + if self.index == len(self.stages) - 1: + self.complete = True + result["complete"] = True + else: + self.index += 1 + self.elapsed_s = self.stage_wall_s = self.hold_s = 0.0 + self.start = self.goal = None + return result + + +class SmoothieSequenceController: + """Compose basket, cup/tap, lid/dock and physical blender-button actions. + + The one-world environment supplies world poses [m, XYZW], a 30 Hz policy + interval, arm EMA history, ``basket_measurements``, ``assembly_measurements`` + and ``tap_measurements``. The tap measurements include physical grasp/support, + cup home/tap alignment, switch state, fill time [s], and motor button state. + """ + + def __init__(self, env: Any, *, trace_path: Path | None = None): + from .bounded_return_pose import BoundedReturnPoseController + + self.env = env + self.basket = BasketController(env) + self.pose_controller = BoundedReturnPoseController(env) + options = {name: getattr(env.cfg, name, value) for name, value in OPTIONS.items()} + self.tap = _SmoothieTrajectory(options) + self.button = _SmoothieTrajectory(options, button_only=True) + self.assembly = None + self.stage = "basket" + self.diagnostics: dict[str, Any] = {} + self.previous_step: int | None = None + # The runner closes this owned stream through close_trace in its finally block. + self.trace = None if trace_path is None else Path(trace_path).open("x", buffering=1) # noqa: SIM115 + self.last_trace_stage: str | None = None + + @property + def tap_complete(self) -> bool: + """Whether the cup was filled, returned, released and cleared by the hand.""" + return self.tap.complete + + @property + def complete(self) -> bool: + """Whether the final physical button action and hand retreat completed.""" + return self.button.complete + + def _measured_state(self) -> dict: + env = self.env + origin = env.scene.env_origins[0].detach().cpu().numpy() + hand = env.robot.data.body_link_pose_w.torch[0, env.hand_id].detach().cpu().numpy().copy() + hand[:3] = env.tcp()[0].detach().cpu().numpy() - origin + cup = env.pose("cup")[0].detach().cpu().numpy().copy() + cup[:3] -= origin + metrics = {key: _one(value) for key, value in env.tap_measurements().items()} + if self.tap.complete: + metrics.update({key: _one(value) for key, value in env.assembly_measurements().items()}) + return { + "hand_pose": hand, + "cup_pose": cup, + "metrics": metrics, + "joints": env.robot.data.joint_pos.torch[0, self.pose_controller.joint_ids].detach().cpu().numpy(), + } + + def _actions(self, sample: dict): + import torch + + env = self.env + target = torch.as_tensor(sample["tcp_position"], device=env.device, dtype=env.tcp().dtype)[None] + target = target + env.scene.env_origins + rotation = target.new_tensor(sample["hand_xyzw"])[None] + if sample["joint_position_target_rad"] is None: + return self.pose_controller.compute(target, rotation, sample["close"]) + joints = env.robot.data.joint_pos.torch[0, self.pose_controller.joint_ids] + limits = env.robot.data.joint_pos_limits.torch[0, self.pose_controller.joint_ids] + previous = env.action_manager.get_term("arm_action").processed_actions[0] + raw, diagnostics = bounded_joint_posture_step( + sample["joint_position_target_rad"], + joints.detach().cpu().numpy(), + limits.detach().cpu().numpy(), + previous.detach().cpu().numpy(), + ) + sample["joint_control"] = diagnostics + actions = torch.zeros((1, 8), device=env.device, dtype=joints.dtype) + actions[0, :7] = actions.new_tensor(raw) + actions[0, 7] = -1.0 if sample["close"] else 1.0 + return actions + + def compute(self, step: int): + """Return consecutive raw robot actions, shape [1, 8], without state writes.""" + import torch + + from .assembly_controller import AssemblyController + + if type(step) is not int or step < 0 or (self.previous_step is not None and step != self.previous_step + 1): + raise ValueError("Controller calls require consecutive nonnegative policy steps.") + self.previous_step = step + phase = int(_one(self.env.phase)) + if phase == 0: + actions = self.basket.compute(step) + self.stage, self.diagnostics = "basket/" + self.basket.stage, dict(self.basket.diagnostics) + elif not self.tap.complete: + sample = self.tap.step(self._measured_state(), self.env.step_dt) + actions = self._actions(sample) + self.stage, self.diagnostics = "tap/" + sample["stage"], sample + elif self.assembly is None or not self.assembly.trajectory.complete: + if self.assembly is None: + self.assembly = AssemblyController(self.env) + actions = self.assembly.compute(step) + self.stage, self.diagnostics = "assembly/" + self.assembly.stage, dict(self.assembly.diagnostics) + else: + sample = self.button.step(self._measured_state(), self.env.step_dt) + actions = self._actions(sample) + self.stage, self.diagnostics = "button/" + sample["stage"], sample + if actions.shape != (1, 8) or not torch.isfinite(actions).all() or (actions.abs() > 1).any(): + raise RuntimeError("Tap controller produced invalid raw actions.") + if self.trace is not None and (self.stage != self.last_trace_stage or step % 30 == 0): + + def convert(value): + if isinstance(value, np.ndarray): + return value.tolist() + if isinstance(value, np.generic): + return value.item() + if torch.is_tensor(value): + return value.detach().cpu().tolist() + raise TypeError(type(value).__name__) + + self.trace.write( + json.dumps( + { + "step": step, + "time_s": step * self.env.step_dt, + "stage": self.stage, + "diagnostics": self.diagnostics, + }, + default=convert, + ) + + "\n" + ) + self.last_trace_stage = self.stage + return actions + + def close_trace(self) -> None: + """Close the optional controller event trace.""" + if self.trace is not None: + self.trace.close() diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_env.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_env.py new file mode 100644 index 000000000000..be786e64b172 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_env.py @@ -0,0 +1,472 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Rigid-body fruit, tap, threaded-lid and blender task with visual scalar filling. + +The legacy scene supplies the same authored rigid assets and robot controls. +Liquid objects, particle proxies and the MPM entry are removed before constructing +any simulator. Runtime actions only drive robot joints; filling changes a scalar. +""" + +from __future__ import annotations + +import newton +import torch +from isaaclab_newton.cloner import newton_builder_world_hook +from isaaclab_newton.physics import NewtonManager + +from isaaclab.envs import ManagerBasedRLEnv +from isaaclab.envs import mdp as base_mdp +from isaaclab.managers import EventTermCfg, ObservationGroupCfg, ObservationTermCfg, RewardTermCfg, TerminationTermCfg +from isaaclab.utils import math as math_utils +from isaaclab.utils.configclass import configclass + +from isaaclab_tasks.contrib.franka_pour.pour_env import _set_mjwarp_force_space_solref_mode + +from . import basket_stage +from .basket_grasp_continuation import BasketGraspContinuation +from .cup_contact_spacing import apply_cup_contact_spacing, validate_cup_contact_runtime +from .fruit_receipt_geometry import WholeHullReceipt +from .pad_torsional_contact import apply_pad_torsional_contact, verify_pad_torsional_contact +from .scene_cfg import CUP_POSITION, FRUIT_GROUPS, FRUIT_LAYOUT +from .smoothie_assembly_geometry import AssemblyGeometryState, assembly_geometry +from .smoothie_asset import CUP_STATION_POSITION_M, NOZZLE_POSITION_M +from .smoothie_task import FILL_DURATION_S, SmoothiePhase, SmoothieTaskState, all_fruit_delivered + +OBJECTS = ("cup", "blade_cap", "basket", *FRUIT_LAYOUT) + + +def failure(env: SmoothieBlenderEnv) -> torch.Tensor: + """Reject nonfinite, dropped or escaped rigid objects and lost fastened lids.""" + invalid = ~torch.isfinite(env.robot.data.joint_pos.torch).all(-1) + invalid |= ~torch.isfinite(env.robot.data.joint_vel.torch).all(-1) + for name in (*OBJECTS, "tap", "motor"): + pose = env.pose(name) + invalid |= ~torch.isfinite(pose).all(-1) + invalid |= ~torch.isfinite(env.scene[name].data.root_link_vel_w.torch).all(-1) + invalid |= (pose[:, 2] - env.scene.env_origins[:, 2]) < -0.12 + invalid |= (pose[:, :3] - env.scene.env_origins).norm(dim=-1) > 1.5 + return invalid | env.task.failed | env.assembly.state["assembly_failed"] + + +def success(env: SmoothieBlenderEnv) -> torch.Tensor: + """Advance measured milestones once and return ordered completion.""" + env.update_task() + return env.task.completed & ~failure(env) + + +def progress(env: SmoothieBlenderEnv) -> torch.Tensor: + """Reward each newly measured milestone once.""" + env.update_task() + return env.progress_reward + + +def state(env: SmoothieBlenderEnv) -> torch.Tensor: + """Observe rigid state, controls, button travel [m] and scalar fill fraction.""" + poses = [] + for name in (*OBJECTS, "tap", "motor"): + pose = env.pose(name).clone() + pose[:, :3] -= env.scene.env_origins + poses.extend((pose, env.scene[name].data.root_link_vel_w.torch)) + return torch.cat( + [ + env.robot.data.joint_pos.torch, + env.robot.data.joint_vel.torch * 0.1, + *poses, + torch.nn.functional.one_hot(env.phase, 6).float(), + env.task.fill_fraction[:, None], + env.task.tap_on[:, None].float(), + env.task.milestones.float(), + env.scene["tap"].data.joint_pos.torch * 250, + env.scene["motor"].data.joint_pos.torch * 250, + env.assembly.observations(), + ], + dim=-1, + ) + + +def reset(env: SmoothieBlenderEnv, env_ids: torch.Tensor) -> None: + """Reset physical rigid objects and task history at episode boundaries only.""" + env.reset_task(env_ids) + + +@configclass +class ObservationsCfg: + """Task observations independent of the removed liquid solver.""" + + @configclass + class PolicyCfg(ObservationGroupCfg): + state = ObservationTermCfg(func=state) + previous_action = ObservationTermCfg(func=base_mdp.last_action) + concatenate_terms = True + enable_corruption = False + + policy = PolicyCfg() + + +@configclass +class RewardsCfg: + """Measured milestone reward and the existing action-rate regularizer.""" + + progress = RewardTermCfg(func=progress, weight=1.0) + action_rate = RewardTermCfg(func=base_mdp.action_rate_l2, weight=-0.002) + + +@configclass +class TerminationsCfg: + """Ordered completion, physical failure and episode timeout.""" + + success = TerminationTermCfg(func=success) + failure = TerminationTermCfg(func=failure) + time_out = TerminationTermCfg(func=base_mdp.time_out, time_out=True) + + +@configclass +class EventsCfg: + """Reset only at episode boundaries.""" + + reset = EventTermCfg(func=reset, mode="reset") + + +class SmoothieBlenderEnv(ManagerBasedRLEnv): + """Physical manipulation with no liquid solver or runtime object attachment.""" + + def pose(self, name: str) -> torch.Tensor: + """Return object root pose in world coordinates [m, XYZW].""" + return self.scene[name].data.root_link_pose_w.torch + + def local(self, name: str, points: torch.Tensor) -> torch.Tensor: + """Transform world points [m], shape (N, P, 3), into an object frame.""" + pose = self.pose(name) + return math_utils.quat_apply_inverse( + pose[:, None, 3:].expand(-1, points.shape[1], -1), points - pose[:, None, :3] + ) + + def offset(self, name: str, xyz: tuple[float, float, float]) -> torch.Tensor: + """Return a local grasp or target point [m] in world coordinates.""" + pose = self.pose(name) + return pose[:, :3] + math_utils.quat_apply( + pose[:, 3:], torch.tensor(xyz, device=self.device).expand(self.num_envs, -1) + ) + + def tcp(self) -> torch.Tensor: + """Return the Franka finger midpoint [m] in world coordinates.""" + pose = self.robot.data.body_link_pose_w.torch[:, self.hand_id] + offset = torch.tensor((0.0, 0.0, 0.107), device=self.device).expand(self.num_envs, -1) + return pose[:, :3] + math_utils.quat_apply(pose[:, 3:], offset) + + def fruit_inside(self) -> torch.Tensor: + """Report whether the cup contains at least one fruit of each type.""" + occupied = [] + for names in FRUIT_GROUPS.values(): + centers = torch.stack([self.pose(name)[:, :3] for name in names], 1) + local = self.local("cup", centers) + # Margins require the fruit center to be fully below the lip. + inside = (local[:, :, :2].norm(dim=-1) < 0.035) & (local[:, :, 2] > 0.010) & (local[:, :, 2] < 0.185) + occupied.append(inside.any(-1)) + return torch.stack(occupied, -1) + + def fruit_fraction(self) -> torch.Tensor: + """Fraction of the fruit centers safely below the receiving cup's lip.""" + centers = torch.stack([self.pose(name)[:, :3] for name in FRUIT_LAYOUT], 1) + local = self.local("cup", centers) + inside = (local[:, :, :2].norm(dim=-1) < 0.035) & (local[:, :, 2] > 0.01) & (local[:, :, 2] < 0.185) + return inside.float().mean(-1) + + def __init__(self, cfg, render_mode=None, **kwargs): + if cfg.scene.num_envs != 1: + raise ValueError("The measured basket controller currently supports one environment.") + if cfg.sim.physics.solver_cfg.solver_type != "mujoco_warp": + raise ValueError("Smoothie task requires the direct rigid solver.") + self._pad_identity = None + with newton_builder_world_hook(self._configure_world): + super().__init__(cfg, render_mode, **kwargs) + model = NewtonManager.get_model() + if model.particle_count != 0 or any("_MPM" in label for label in model.shape_label): + raise RuntimeError("Unexpected liquid particles or MPM collision proxies.") + validate_cup_contact_runtime(NewtonManager._solver) + verify_pad_torsional_contact(NewtonManager._solver, self._pad_identity) + + def _configure_world(self, builder, env_id, position, quaternion): + for index, world in enumerate(builder.shape_world): + if world != env_id: + continue + label = builder.shape_label[index] + builder.shape_flags[index] = int(builder.shape_flags[index]) & ~int(newton.ShapeFlags.COLLIDE_PARTICLES) + if "/Robot/" not in label: + builder.shape_margin[index] = 0.00015 + builder.shape_material_mu[index] = 0.25 if "/Thread/" in label else 0.8 + builder.shape_material_ke[index] = 1e5 + builder.shape_material_kd[index] = 500.0 + builder.shape_material_kf[index] = 1000.0 + _set_mjwarp_force_space_solref_mode(builder, index) + apply_cup_contact_spacing(builder, env_id) + self._pad_identity = apply_pad_torsional_contact(builder, env_id) + + def load_managers(self) -> None: + self.robot = self.scene["robot"] + self.hand_id = self.robot.find_bodies("panda_hand")[0][0] + self.finger_ids = self.robot.find_joints("panda_finger_joint.*")[0] + self.task = SmoothieTaskState(self.num_envs, self.device, self.step_dt) + self.phase = self.task.phase + self.assembly = AssemblyGeometryState(self.num_envs, self.device, self.step_dt) + self.basket_state = basket_stage.new_milestones(self.num_envs, self.device) + self.basket_grasp = BasketGraspContinuation(self.device, self.step_dt) + self.basket_strict_lift_hold_s = torch.zeros(self.num_envs, device=self.device) + self.basket_strict_lift_complete = torch.zeros(self.num_envs, device=self.device, dtype=torch.bool) + self.hull_receipt = WholeHullReceipt(tuple(FRUIT_LAYOUT), self.device) + self.progress_reward = torch.zeros(self.num_envs, device=self.device) + self.motor_on = torch.zeros(self.num_envs, device=self.device, dtype=torch.bool) + self._last_update = -1 + super().load_managers() + + def reset_task(self, env_ids: torch.Tensor) -> None: + base_mdp.reset_scene_to_default(self, env_ids, reset_joint_targets=True) + self.task.reset(env_ids.to(dtype=torch.long)) + self.assembly.reset(env_ids) + basket_stage.reset_milestones(self.basket_state, env_ids) + self.basket_grasp.reset() + self.basket_strict_lift_hold_s[env_ids] = 0 + self.basket_strict_lift_complete[env_ids] = False + self.progress_reward[env_ids] = 0 + self.motor_on[env_ids] = False + positions = self.robot.data.default_joint_pos.torch[env_ids].clone() + positions[:, self.finger_ids] = 0.015 + self.robot.write_joint_state_to_sim_index( + position=positions, velocity=torch.zeros_like(positions), env_ids=env_ids + ) + self.robot.set_joint_position_target_index(target=positions, env_ids=env_ids) + self.action_manager.get_term("gripper_action").set_reset_position( + positions[:, self.finger_ids[:1]], env_ids=env_ids + ) + self._last_update = self.common_step_counter + + def grasp_pose(self) -> tuple[torch.Tensor, torch.Tensor]: + """Return the basket grasp point [m] and hand rotation [XYZW].""" + pose = self.pose("basket") + offset = pose.new_tensor(self.cfg.basket_grasp_offset).expand(self.num_envs, -1) + rotation = pose.new_tensor(self.cfg.basket_grasp_rotation).expand(self.num_envs, -1) + return pose[:, :3] + math_utils.quat_apply(pose[:, 3:], offset), math_utils.quat_mul(pose[:, 3:], rotation) + + def assembly_measurements(self, basket=None): + """Measure rigid lid/cup geometry with a dimensionless virtual fill fraction.""" + if basket is None: + basket = self.basket_measurements() + gripper = self.action_manager.get_term("gripper_action") + measured = assembly_geometry( + cup_pose=self.pose("cup"), + cap_pose=self.pose("blade_cap"), + motor_pose=self.pose("motor"), + hand_pose=self.robot.data.body_link_pose_w.torch[:, self.hand_id], + cup_velocity=self.scene["cup"].data.root_link_vel_w.torch, + cap_velocity=self.scene["blade_cap"].data.root_link_vel_w.torch, + hand_velocity=self.robot.data.body_link_vel_w.torch[:, self.hand_id], + tcp_position=self.tcp(), + finger_positions=self.robot.data.joint_pos.torch[:, self.finger_ids], + commanded_positions=gripper.processed_actions, + contact_deflection=gripper.contact_deflection, + bilateral_contact=gripper.bilateral_contact, + origins=self.scene.env_origins, + fruit_count=basket["fruit_count"], + all_types=basket["all_types"], + fill_fraction=self.task.fill_fraction, + failed=failure(self), + ) + return {**measured, **self.assembly.state} + + def tap_measurements(self, assembly=None): + """Measure station support, physical buttons and scalar filling [SI units].""" + if assembly is None: + assembly = self.assembly_measurements() + cup = self.pose("cup") + local = cup[:, :3] - self.scene.env_origins + up = math_utils.quat_apply(cup[:, 3:], cup.new_tensor((0.0, 0.0, 1.0)).expand(self.num_envs, -1)) + opening = local + 0.210 * up + nozzle = cup.new_tensor(NOZZLE_POSITION_M) + aligned = (opening[:, :2] - nozzle[:2]).norm(dim=-1) < 0.018 + aligned &= ((nozzle[2] - opening[:, 2]) > 0.020) & ((nozzle[2] - opening[:, 2]) < 0.10) + upright = up[:, 2] > 0.985 + station = (local - cup.new_tensor(CUP_STATION_POSITION_M)).abs() + station_support = (station[:, :2].norm(dim=-1) < 0.012) & (station[:, 2] < 0.003) + home = (local - cup.new_tensor(CUP_POSITION)).norm(dim=-1) < 0.006 + cap_local = self.local("cup", self.pose("blade_cap")[:, None, :3])[:, 0] + cup_open = (cap_local[:, :2].norm(dim=-1) > 0.070) | ((cap_local[:, 2] - 0.220).abs() > 0.055) + valve = -self.scene["tap"].data.joint_pos.torch[:, 0] + motor = self.scene["motor"].data.joint_pos.torch[:, 0] >= 0.002 + invalid = failure(self) + return dict( + finite=~invalid, + failed=invalid, + cup_held=assembly["cup_held"], + cup_supported=(station_support | home) & upright & assembly["cup_stable"], + cup_under_tap=aligned & upright, + cup_upright=upright, + cup_open=cup_open, + cup_home=home & upright, + fingers_open=assembly["fingers_open"], + tap_on=self.task.tap_on, + tap_fill_time_s=self.task.fill_fraction * FILL_DURATION_S, + tap_button_pressed=valve >= 0.002, + motor_button_pressed=motor, + motor_on=self.motor_on, + valve_depression_m=valve, + ) + + def update_task(self) -> None: + """Advance once per measured policy boundary, preserving the physical scene.""" + if self._last_update == self.common_step_counter: + return + self._last_update = self.common_step_counter + basket = self._update_basket_continuation(self.basket_measurements()) + active = self.phase == SmoothiePhase.FRUIT + strict_lift = ( + active + & basket["finite"] + & ~basket["failed"] + & basket["strict_held"] + & (basket["clearance_m"] >= basket_stage.CRITERIA["held_upright_lift_m"]) + & (basket["basket_upright"] >= basket_stage.CRITERIA["held_lift_upright_cosine"]) + ) + self.basket_strict_lift_hold_s.copy_( + torch.where(strict_lift, self.basket_strict_lift_hold_s + self.step_dt, 0.0) + ) + self.basket_strict_lift_complete |= ( + self.basket_strict_lift_hold_s >= basket_stage.CRITERIA["held_lift_hold_s"] - 1e-6 + ) + basket_stage.advance_milestones( + self.basket_state, {**basket, "failed": basket["failed"] | ~active}, self.step_dt + ) + measured = self.assembly_measurements(basket) + self.assembly.update( + measured, + (self.phase >= SmoothiePhase.LID) & (self.phase <= SmoothiePhase.BUTTON), + self.task.milestones[:, 1], + step=int(self.common_step_counter), + ) + measured.update(self.assembly.state) + tap = self.tap_measurements(measured) + before = self.task.milestones.sum(-1) + fruit = all_fruit_delivered(basket["fruit_hull_inside"]) & self.basket_state["finished"] + self.task.update( + fruit_delivered=fruit, + cup_under_tap=tap["cup_under_tap"], + cup_upright=tap["cup_upright"], + cup_open=tap["cup_open"], + lid_fastened=measured["lid_twist_complete"] & measured["lid_seated"] & measured["lid_release_complete"], + cup_docked=measured["assembly_complete"] & measured["dock_support_ready"], + blender_button_pressed=tap["motor_button_pressed"], + valve_depression_m=tap["valve_depression_m"], + failed=tap["failed"], + ) + self.progress_reward.copy_((self.task.milestones.sum(-1) - before).float()) + self.motor_on.copy_(self.task.completed) + self.extras.setdefault("log", {}).update( + {"Task/phase": self.phase.float().mean(), "Task/fill_fraction": self.task.fill_fraction.mean()} + ) + + def basket_measurements(self) -> dict[str, torch.Tensor]: + """Measure basket grasp/receipt/support [SI units] independently of active phase.""" + pose = self.pose("basket") + hand = self.robot.data.body_link_pose_w.torch[:, self.hand_id] + hv = self.robot.data.body_link_vel_w.torch[:, self.hand_id] + velocity = self.scene["basket"].data.root_link_vel_w.torch + grasp = pose[:, :3] + math_utils.quat_apply( + pose[:, 3:], pose.new_tensor(self.cfg.basket_grasp_offset).expand(self.num_envs, -1) + ) + rotation = math_utils.quat_mul( + pose[:, 3:], pose.new_tensor(self.cfg.basket_grasp_rotation).expand(self.num_envs, -1) + ) + fingers = self.robot.data.joint_pos.torch[:, self.finger_ids] + gripper = self.action_manager.get_term("gripper_action") + distance = (self.tcp() - grasp).norm(dim=-1) + speed = ( + velocity[:, :3] + + torch.linalg.cross(velocity[:, 3:], grasp - pose[:, :3]) + - hv[:, :3] + - torch.linalg.cross(hv[:, 3:], grasp - hand[:, :3]) + ).norm(dim=-1) + actual_grasp_speed = ( + velocity[:, :3] + + torch.linalg.cross(velocity[:, 3:], self.tcp() - pose[:, :3]) + - hv[:, :3] + - torch.linalg.cross(hv[:, 3:], self.tcp() - hand[:, :3]) + ).norm(dim=-1) + corners = pose.new_tensor([[x, y, z] for x in (-0.0605, 0.0605) for y in (-0.0605, 0.0605) for z in (0.0, 0.1)]) + world = ( + math_utils.quat_apply(pose[:, None, 3:].expand(-1, 8, -1), corners.expand(self.num_envs, -1, -1)) + + pose[:, None, :3] + ) + result = basket_stage.placement_geometry( + pose, velocity, self.tcp(), fingers, gripper.commanded_position, self.scene.env_origins, 0.015, grasp + ) + held = ( + gripper.bilateral_contact + & (distance < self.cfg.basket_grasp_distance) + & (fingers.sum(-1) > 0.001) + & (fingers.sum(-1) < 0.065) + & (speed < self.cfg.basket_relative_speed) + & result["finite"] + ) + inside, violations = self.hull_receipt.measure( + self.pose("cup"), torch.stack([self.pose(name) for name in FRUIT_LAYOUT], dim=1) + ) + continuation = self.basket_grasp.current & (self.basket_grasp.last_step == self.common_step_counter) + result.update( + strict_held=held, + grasp_continuation=continuation, + grasp_acquired=self.basket_grasp.acquired, + grasp_acquisition_step=self.basket_grasp.acquisition_step, + held=held | continuation, + grip_position_b_m=math_utils.quat_apply_inverse(pose[:, 3:], self.tcp() - pose[:, :3]), + hand_rotation_b_xyzw=math_utils.quat_mul(math_utils.quat_conjugate(pose[:, 3:]), hand[:, 3:]), + commanded_position_m=gripper.commanded_position, + contact_deflection_m=gripper.contact_deflection, + tcp_position_w_m=self.tcp(), + pose_w=pose, + grasp_relative_speed_m_s=actual_grasp_speed, + acquisition_hand_path_m=self.basket_grasp.hand_path_m, + acquisition_basket_path_m=self.basket_grasp.basket_path_m, + acquisition_hand_displacement_m=self.basket_grasp.hand_displacement_m, + acquisition_basket_displacement_m=self.basket_grasp.basket_displacement_m, + acquisition_motion_correlation=self.basket_grasp.motion_correlation, + continuation_window_s=self.basket_grasp.window_s, + continuation_translation_drift_m=self.basket_grasp.translation_drift_m, + continuation_rotation_drift_rad=self.basket_grasp.rotation_drift_rad, + reach_m=distance, + alignment=(hand[:, 3:] * rotation).sum(-1).square().clamp(0, 1), + relative_speed_m_s=speed, + finger_width_m=fingers.sum(-1), + clearance_m=world[:, :, 2].amin(-1) - self.scene.env_origins[:, 2], + upright=result["basket_upright"], + target_error_m=(pose[:, :3] - self.offset("cup", (0.0, 0.0, 0.30))).norm(dim=-1), + fruit_count=inside.sum(-1), + all_types=self.hull_receipt.all_types(inside), + fruit_hull_inside=inside, + fruit_hull_max_plane_violation_m=violations, + legacy_center_fruit_count=torch.round(self.fruit_fraction() * len(FRUIT_LAYOUT)).long(), + legacy_center_all_types=self.fruit_inside().all(-1), + failed=failure(self), + ) + return result + + def _update_basket_continuation(self, basket): + """Update grasp evidence once per physical policy boundary, before task gates.""" + continuation = self.basket_grasp.update( + int(self.common_step_counter), + valid=basket["finite"] & ~basket["failed"] & (self.phase == 0), + command_m=basket["commanded_position_m"], + deflection_m=basket["contact_deflection_m"], + finger_width_m=basket["finger_width_m"], + relative_speed_m_s=basket["grasp_relative_speed_m_s"], + grip_position_b_m=basket["grip_position_b_m"], + hand_rotation_b_xyzw=basket["hand_rotation_b_xyzw"], + tcp_position_w_m=basket["tcp_position_w_m"], + basket_pose_w=basket["pose_w"], + clearance_m=basket["clearance_m"], + upright=basket["upright"], + ) + basket["grasp_continuation"] = continuation + basket["held"] = basket["strict_held"] | continuation + return basket diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_env_cfg.py new file mode 100644 index 000000000000..bb8f5844eab2 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_env_cfg.py @@ -0,0 +1,88 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Rigid-only smoothie task at a fixed 50 Hz control rate.""" + +import math + +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg, NewtonCollisionPipelineCfg + +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.utils.configclass import configclass + +from ..franka_pour.pour_env_cfg import ActionsCfg +from .scene_cfg import TapSceneCfg +from .smoothie_asset import BUTTON_TOP_POSITION_M, CUP_STATION_POSITION_M +from .smoothie_env import EventsCfg, ObservationsCfg, RewardsCfg, TerminationsCfg +from .smoothie_gripper import SmoothieGripperPositionActionCfg + + +@configclass +class SmoothieActionsCfg(ActionsCfg): + """Filtered robot commands with six-phase finger targets [m].""" + + gripper_action = SmoothieGripperPositionActionCfg(asset_name="robot", joint_names=["panda_finger.*"]) + + +@configclass +class FrankaSmoothieEnvCfg(ManagerBasedRLEnvCfg): + """Ordered fruit, tap, threaded lid, docking and final button task. + + The liquid fill is a task scalar and a renderer mesh, with no liquid mass or + dynamics. + """ + + scene = TapSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) + actions = SmoothieActionsCfg() + observations = ObservationsCfg() + rewards = RewardsCfg() + events = EventsCfg() + terminations = TerminationsCfg() + basket_grasp_distance: float = 0.04 + """Maximum TCP distance from the basket grasp point that still counts as grasped [m].""" + basket_relative_speed: float = 0.15 + """Maximum basket-to-hand translational speed for a stable grasp [m/s].""" + basket_grasp_offset: tuple[float, float, float] = (0.059 / 2**0.5, 0.059 / 2**0.5, 0.090) + """Basket-local grasp position [m].""" + basket_grasp_rotation: tuple[float, float, float, float] = ( + math.cos(3 * math.pi / 8), + math.sin(3 * math.pi / 8), + 0.0, + 0.0, + ) + """Basket-relative hand quaternion in XYZW order.""" + tap_cup_position: tuple[float, float, float] = CUP_STATION_POSITION_M + """Cup support position relative to the environment origin [m].""" + tap_button_position: tuple[float, float, float] = BUTTON_TOP_POSITION_M + """Unpressed tap button face position [m].""" + tap_button_direction: tuple[float, float, float] = (0.0, 0.0, -1.0) + tap_button_rotation: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + + def __post_init__(self): + self.episode_length_s = 600.0 + self.sim.use_newton_actuators = True + self.sim.physics = NewtonCfg( + solver_cfg=MJWarpSolverCfg( + use_mujoco_contacts=False, + integrator="implicitfast", + nconmax=200, + njmax=300, + cone="elliptic", + impratio=100.0, + tolerance=1e-6, + ), + collision_cfg=NewtonCollisionPipelineCfg(rigid_contact_max=8192), + ) + + dt = 0.01 + decimation = 2 + substeps = 2 + iterations = 20 + line_search = 10 + self.sim.dt, self.decimation = dt, decimation + self.sim.render_interval = decimation + self.sim.physics.num_substeps = substeps + self.sim.physics.solver_cfg.iterations = iterations + self.sim.physics.solver_cfg.ls_iterations = line_search diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_gripper.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_gripper.py new file mode 100644 index 000000000000..3f04d8c47588 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_gripper.py @@ -0,0 +1,57 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""Six-phase gripper targets preserving the existing filtered joint action.""" + +from dataclasses import field + +import torch + +from isaaclab.utils.configclass import configclass + +from isaaclab_tasks.contrib.franka_pour.mdp.actions import CurriculumGripperPositionAction +from isaaclab_tasks.contrib.franka_pour.mdp.actions_cfg import CurriculumGripperPositionActionCfg + + +class SmoothieGripperPositionAction(CurriculumGripperPositionAction): + """Change physical finger targets [m] without clearing filter history.""" + + def __init__(self, cfg, env): + if cfg.open_positions_by_phase != {0: 0.015, 1: 0.04, 2: 0.04, 3: 0.04, 4: 0.04, 5: 0.04}: + raise ValueError("Require declared six-phase opening targets.") + if cfg.close_positions_by_phase != dict.fromkeys(range(6), 0.0): + raise ValueError("Require six zero closure targets.") + super().__init__(cfg, env) + self._open_command = self._open_command.expand(self.num_envs, -1).clone() + self._close_command = self._close_command.expand(self.num_envs, -1).clone() + + def process_actions(self, actions: torch.Tensor) -> None: + """Apply phase targets before the existing binary action filter.""" + phase = self._env.phase + if ( + phase.shape != (self.num_envs,) + or phase.dtype != torch.long + or not bool(((phase >= 0) & (phase <= 5)).all()) + ): + raise ValueError("Require one integer phase 0..5 per environment.") + self._open_command.copy_(torch.where((phase == 0)[:, None], 0.015, 0.04).expand_as(self._open_command)) + self._close_command.zero_() + super().process_actions(actions) + + +@configclass +class SmoothieGripperPositionActionCfg(CurriculumGripperPositionActionCfg): + """Explicit per-finger phase targets [m].""" + + class_type: type[SmoothieGripperPositionAction] = SmoothieGripperPositionAction + open_positions_by_phase: dict[int, float] = field( + default_factory=lambda: {0: 0.015, 1: 0.04, 2: 0.04, 3: 0.04, 4: 0.04, 5: 0.04} + ) + close_positions_by_phase: dict[int, float] = field(default_factory=lambda: dict.fromkeys(range(6), 0.0)) + neutral_position: float = 0.04 + close_position: float = 0.0 + default_position: float = 0.015 + alpha: float = 0.04 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_task.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_task.py new file mode 100644 index 000000000000..e2b7005a391b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_task.py @@ -0,0 +1,232 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Measured task progression with scalar tap filling and no fluid simulation.""" + +from __future__ import annotations + +import math +from enum import IntEnum + +import torch + +FRUIT_COUNT = 16 +TARGET_VOLUME_M3 = 86.4e-6 +FILL_DURATION_S = 2.0 +VALVE_PRESS_M = 0.002 +VALVE_RELEASE_M = 0.001 + + +class SmoothiePhase(IntEnum): + """Ordered physical milestones of the smoothie and blender demonstration.""" + + FRUIT = 0 + TAP = 1 + LID = 2 + DOCK = 3 + BUTTON = 4 + DONE = 5 + + +def all_fruit_delivered(whole_hull_inside: torch.Tensor) -> torch.Tensor: + """Require every one of the 16 authored fruit hulls inside its cup. + + Args: + whole_hull_inside: Measured whole-hull containment flags, shape [N, 16]. + + Returns: + Complete fruit delivery flags, shape [N]. + """ + if ( + not isinstance(whole_hull_inside, torch.Tensor) + or whole_hull_inside.dtype != torch.bool + or whole_hull_inside.ndim != 2 + or whole_hull_inside.shape[1] != FRUIT_COUNT + ): + raise ValueError("Require boolean whole-hull containment with shape [N, 16].") + return whole_hull_inside.all(dim=1) + + +class SmoothieTaskState: + """Track ordered task evidence independently for each environment. + + ``fill_volume_m3`` is a visual task scalar [m^3], not simulated liquid. + ``milestones`` has shape [N, 5], ordered as fruit, tap, lid, dock and button. + Failure latches until reset and freezes phase, filling and switch state. + + Args: + num_envs: Number of independent environments. + device: Torch device on which measurements and task state reside. + step_dt_s: Elapsed simulated time per update [s]. + """ + + def __init__(self, num_envs: int, device: torch.device | str, step_dt_s: float): + if type(num_envs) is not int or num_envs < 1: + raise ValueError("num_envs must be a positive integer.") + if isinstance(step_dt_s, bool) or not math.isfinite(step_dt_s) or step_dt_s <= 0: + raise ValueError("step_dt_s must be finite and positive.") + self.num_envs = num_envs + self.device = torch.empty(0, device=device).device + self.step_dt_s = float(step_dt_s) + self.phase = torch.zeros(num_envs, dtype=torch.long, device=self.device) + self.milestones = torch.zeros((num_envs, 5), dtype=torch.bool, device=self.device) + self.fill_volume_m3 = torch.zeros(num_envs, device=self.device) + self.tap_on = torch.zeros(num_envs, dtype=torch.bool, device=self.device) + self.tap_on_seen = torch.zeros_like(self.tap_on) + self.tap_off_seen = torch.zeros_like(self.tap_on) + self.failed = torch.zeros_like(self.tap_on) + self._valve_armed = torch.zeros_like(self.tap_on) + self._button_armed = torch.zeros_like(self.tap_on) + self._fill_time_s = torch.zeros(num_envs, dtype=torch.float64, device=self.device) + + @property + def fill_fraction(self) -> torch.Tensor: + """Return the visual cup fill fraction, shape [N], bounded to [0, 1].""" + return self.fill_volume_m3 / TARGET_VOLUME_M3 + + @property + def completed(self) -> torch.Tensor: + """Return ordered completion without a latched failure, shape [N].""" + return (self.phase == SmoothiePhase.DONE) & ~self.failed + + def reset(self, env_ids: torch.Tensor | None = None) -> None: + """Clear selected environments, including valve and final-button edge history. + + Args: + env_ids: Unique environment indices on the state device, shape [M]. + ``None`` resets every environment. + """ + if env_ids is None: + env_ids = torch.arange(self.num_envs, device=self.device) + if ( + not isinstance(env_ids, torch.Tensor) + or env_ids.dtype != torch.long + or env_ids.device != self.device + or env_ids.ndim != 1 + or bool(((env_ids < 0) | (env_ids >= self.num_envs)).any()) + or env_ids.unique().numel() != env_ids.numel() + ): + raise ValueError("Require unique valid environment indices on the state device.") + for value in ( + self.phase, + self.milestones, + self.fill_volume_m3, + self.tap_on, + self.tap_on_seen, + self.tap_off_seen, + self.failed, + self._valve_armed, + self._button_armed, + self._fill_time_s, + ): + value[env_ids] = 0 + + @torch.no_grad() + def update( + self, + *, + fruit_delivered: torch.Tensor, + cup_under_tap: torch.Tensor, + cup_upright: torch.Tensor, + cup_open: torch.Tensor, + lid_fastened: torch.Tensor, + cup_docked: torch.Tensor, + blender_button_pressed: torch.Tensor, + valve_depression_m: torch.Tensor, + failed: torch.Tensor, + ) -> None: + """Consume measured evidence and advance at most one phase per update. + + Boolean measurements have shape [N]. ``fruit_delivered`` must represent + all 16 whole fruit hulls; ``lid_fastened`` must include measured seating + and screw engagement; ``cup_docked`` must include supported placement. + A valve must first be released, then depressed at least 2 mm to toggle. + It rearms only below 1 mm. Filling requires a recognized on edge during + TAP, an open upright cup below the nozzle, and two seconds of eligible + filling. A later off edge proves the tap milestone. A final blender + button press is accepted only after release was observed during BUTTON. + + Args: + fruit_delivered: All-fruit whole-hull delivery flags. + cup_under_tap: Cup opening aligned beneath the nozzle. + cup_upright: Cup orientation suitable for receiving liquid. + cup_open: Cup has no lid obstructing its opening. + lid_fastened: Measured lid seating and screw-engagement acceptance. + cup_docked: Measured supported placement on the blender. + blender_button_pressed: Measured mechanical blender button press. + valve_depression_m: Measured tap button depression [m], shape [N]. + failed: Physical failure flags; true values latch until reset. + """ + for name, value in ( + ("fruit_delivered", fruit_delivered), + ("cup_under_tap", cup_under_tap), + ("cup_upright", cup_upright), + ("cup_open", cup_open), + ("lid_fastened", lid_fastened), + ("cup_docked", cup_docked), + ("blender_button_pressed", blender_button_pressed), + ("failed", failed), + ): + self._validate_measurement(name, value, boolean=True) + self._validate_measurement("valve_depression_m", valve_depression_m, boolean=False) + if not bool(torch.isfinite(valve_depression_m).all()): + raise ValueError("Valve depression must be finite.") + + self.failed |= failed + active = ~self.failed & (self.phase != SmoothiePhase.DONE) + prior_phase = self.phase.clone() + in_tap = active & (prior_phase == SmoothiePhase.TAP) + in_button = active & (prior_phase == SmoothiePhase.BUTTON) + + valve_pressed = valve_depression_m >= VALVE_PRESS_M + valve_released = valve_depression_m <= VALVE_RELEASE_M + valve_edge = active & self._valve_armed & valve_pressed + was_on = self.tap_on.clone() + self.tap_on ^= valve_edge + self._valve_armed[active & valve_released] = True + self._valve_armed[active & valve_pressed] = False + self.tap_on_seen |= in_tap & valve_edge & ~was_on + + filling = in_tap & self.tap_on & self.tap_on_seen & cup_under_tap & cup_upright & cup_open + self._fill_time_s[filling] = (self._fill_time_s[filling] + self.step_dt_s).clamp(max=FILL_DURATION_S) + # Remove only floating-point accumulation error at the exact fill duration. + at_target = filling & (self._fill_time_s >= FILL_DURATION_S - 1e-12) + self._fill_time_s[at_target] = FILL_DURATION_S + self.fill_volume_m3[filling] = (self._fill_time_s[filling] * (TARGET_VOLUME_M3 / FILL_DURATION_S)).to( + self.fill_volume_m3.dtype + ) + full = self.fill_volume_m3 >= TARGET_VOLUME_M3 + self.tap_off_seen |= in_tap & valve_edge & was_on & self.tap_on_seen & full + + button_edge = in_button & self._button_armed & blender_button_pressed + self._button_armed[in_button & ~blender_button_pressed] = True + self._button_armed[active & blender_button_pressed] = False + + transitions = ( + active & (prior_phase == SmoothiePhase.FRUIT) & fruit_delivered, + in_tap & self.tap_on_seen & self.tap_off_seen & ~self.tap_on & full & cup_under_tap & cup_upright, + active & (prior_phase == SmoothiePhase.LID) & lid_fastened, + active & (prior_phase == SmoothiePhase.DOCK) & cup_docked, + button_edge + & self.milestones[:, :4].all(dim=1) + & fruit_delivered + & full + & ~self.tap_on + & lid_fastened + & cup_docked, + ) + for phase, advance in enumerate(transitions): + self.milestones[:, phase] |= advance + self.phase[advance] = phase + 1 + + def _validate_measurement(self, name: str, value: torch.Tensor, *, boolean: bool) -> None: + if ( + not isinstance(value, torch.Tensor) + or value.shape != (self.num_envs,) + or value.device != self.device + or (value.dtype != torch.bool if boolean else not value.is_floating_point()) + ): + kind = "boolean" if boolean else "floating" + raise ValueError(f"{name} must be a {kind} tensor with shape [{self.num_envs}] on {self.device}.") diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_visuals.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_visuals.py new file mode 100644 index 000000000000..42f1385303c5 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/franka_smoothie/smoothie_visuals.py @@ -0,0 +1,140 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Renderer-only tap stream and cup fill; these meshes never enter the physics model.""" + +from __future__ import annotations + +import math +from typing import Any + +import numpy as np + +from .smoothie_asset import NOZZLE_POSITION_M + +FILL_RADIUS_M = 0.039 +FILL_BOTTOM_LOCAL_M = 0.012 +FILL_TOP_LOCAL_M = 0.145 +STREAM_RADIUS_M = 0.004 +STREAM_END_WORLD_Z_M = 0.18 +MILK_COLOR = (0.95, 0.96, 0.88) + + +def _cylinder(radius: float, bottom: float, top: float, segments: int = 24) -> tuple[np.ndarray, np.ndarray]: + angles = np.arange(segments) * (2 * math.pi / segments) + xy = radius * np.column_stack((np.cos(angles), np.sin(angles))) + points = np.concatenate( + ( + np.column_stack((xy, np.full(segments, bottom))), + np.column_stack((xy, np.full(segments, top))), + [[0, 0, bottom], [0, 0, top]], + ) + ) + faces = [] + for i in range(segments): + j = (i + 1) % segments + faces.extend( + ( + (i, j, segments + j), + (i, segments + j, segments + i), + (2 * segments, j, i), + (2 * segments + 1, segments + i, segments + j), + ) + ) + return points.astype(np.float32), np.asarray(faces, dtype=np.int32).reshape(-1) + + +def fill_vertices(cup_pose: np.ndarray, fill_level: float) -> tuple[np.ndarray, np.ndarray]: + """Return renderer vertices [m] from a measured cup pose [xyz, xyzw] and fill fraction. + + The decorative fill follows the cup, including after inversion; no fluid + dynamics, mass, contacts, particle emission, or rigid-state writes occur. + """ + pose = np.asarray(cup_pose, dtype=np.float64) + if pose.shape != (7,) or not np.isfinite(pose).all() or not np.isfinite(fill_level): + raise ValueError("Expected a finite cup pose [xyz, xyzw] and fill fraction.") + norm = np.linalg.norm(pose[3:]) + if norm <= 1e-12 or not 0.0 <= fill_level <= 1.0: + raise ValueError("Require a nonzero quaternion and fill fraction in [0, 1].") + x, y, z, w = pose[3:] / norm + rotation = np.array( + ( + (1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)), + (2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)), + (2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)), + ) + ) + height = FILL_BOTTOM_LOCAL_M + max(fill_level, 1e-5) * (FILL_TOP_LOCAL_M - FILL_BOTTOM_LOCAL_M) + points, faces = _cylinder(FILL_RADIUS_M, FILL_BOTTOM_LOCAL_M, height) + return (points @ rotation.T + pose[:3]).astype(np.float32), faces + + +class SmoothieVisuals: + """Render a decorative stream and fill through NewtonGL without adding simulation bodies. + + Call :meth:`update` immediately before ``viewer.step(dt)``. The installed + Isaac Lab wrapper exposes its native Newton viewer through ``_viewer``; + direct native NewtonGL instances are also accepted. This small adapter is + the only place that accesses that wrapper detail. + """ + + def __init__(self, name: str = "tap_demo") -> None: + self.name = name + self._raw = None + self._meshes = {} + self._last_fill = None + self._last_tap_on = None + + def update(self, viewer: Any, cup_pose: np.ndarray, fill_level: float, tap_on: bool) -> None: + """Update only renderer meshes using cup pose [m, xyzw] and a visual fill fraction.""" + import warp as wp + + raw = getattr(viewer, "_viewer", viewer) + if raw is None or not hasattr(raw, "log_mesh"): + raise TypeError("SmoothieVisuals requires an initialized NewtonGL viewer.") + if self._raw is not None and raw is not self._raw: + raise ValueError("Create one SmoothieVisuals instance per viewer.") + self._raw = raw + pose = np.asarray(cup_pose, dtype=np.float64) + points, indices = fill_vertices(pose, fill_level) + signature = (tuple(pose), float(fill_level)) + if signature != self._last_fill: + self._draw(raw, wp, "fill", points, indices, hidden=fill_level <= 0.0) + self._last_fill = signature + if bool(tap_on) != self._last_tap_on: + stream, triangles = _cylinder(STREAM_RADIUS_M, STREAM_END_WORLD_Z_M, NOZZLE_POSITION_M[2], 12) + stream[:, :2] += np.asarray(NOZZLE_POSITION_M[:2]) + self._draw(raw, wp, "stream", stream, triangles, hidden=not tap_on) + self._last_tap_on = bool(tap_on) + + def _draw(self, raw, wp, suffix, points, indices, *, hidden): + name = f"{self.name}/{suffix}" + if suffix not in self._meshes: + self._meshes[suffix] = ( + wp.array(points, dtype=wp.vec3, device=raw.device), + wp.array(indices, dtype=wp.int32, device=raw.device), + ) + vertices, faces = self._meshes[suffix] + vertices.assign(points) + raw.log_mesh( + name, + vertices, + faces, + color=MILK_COLOR, + roughness=0.25, + metallic=0.0, + dynamic=True, + backface_culling=False, + hidden=hidden, + ) + + def close(self) -> None: + """Hide this helper's two renderer meshes; physics remains untouched.""" + if self._raw is not None: + for suffix in self._meshes: + mesh = self._raw.objects.get(f"{self.name}/{suffix}") + if mesh is not None: + mesh.hidden = True + self._meshes.clear() diff --git a/source/isaaclab_tasks/test/contrib/test_franka_smoothie.py b/source/isaaclab_tasks/test/contrib/test_franka_smoothie.py new file mode 100644 index 000000000000..a7fadaa3faad --- /dev/null +++ b/source/isaaclab_tasks/test/contrib/test_franka_smoothie.py @@ -0,0 +1,437 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Contracts for the smoothie task's scene, authored assets, controllers and ordered milestones.""" + +import math +import unittest +from types import SimpleNamespace + +import pytest +import torch + +from pxr import Usd, UsdGeom, UsdPhysics + +from isaaclab_tasks.contrib.franka_smoothie.basket_geometry import BASKET_BOTTOM_RADIUS +from isaaclab_tasks.contrib.franka_smoothie.scene_cfg import ASSETS, FRUIT_GROUPS, FRUIT_LAYOUT +from isaaclab_tasks.contrib.franka_smoothie.smoothie_env import SmoothieBlenderEnv +from isaaclab_tasks.contrib.franka_smoothie.smoothie_env_cfg import FrankaSmoothieEnvCfg +from isaaclab_tasks.contrib.franka_smoothie.smoothie_task import ( + TARGET_VOLUME_M3, + SmoothiePhase, + SmoothieTaskState, + all_fruit_delivered, +) + + +def test_basket_expert_uses_independent_clocks_and_reset_poses(monkeypatch): + from isaaclab_tasks.contrib.franka_smoothie import pose_control + + class Controller: + def compute(self, position, rotation, close): + return position.clone(), rotation.clone(), close.clone() + + monkeypatch.setattr(pose_control, "PoseController", lambda _: Controller()) + grasp = torch.tensor([[0.1, 0.0, 0.2], [0.2, 0.0, 0.2]]) + rotations = torch.tensor([[1.0, 0.0, 0.0, 0.0]]).repeat(2, 1) + tcp = torch.tensor([[0.1, 0.0, 0.22], [0.2, 0.0, 0.24]]) + env = SimpleNamespace( + num_envs=2, + device="cpu", + step_dt=1 / 60, + grasp_pose=lambda: (grasp.clone(), rotations.clone()), + tcp=lambda: tcp.clone(), + ) + expert = pose_control.BasketExpert(env) + position, _, close = expert.compute(torch.tensor([0, 450])) + torch.testing.assert_close(position[:, 2], torch.tensor([0.35, 0.24])) + assert close.tolist() == [False, True] + position, _, close = expert.compute(torch.tensor([450, 720])) + torch.testing.assert_close(position[:, 2], torch.tensor([0.22, 0.24 + 0.15 / 3.5])) + assert close.all() + grasp[0, 0] += 0.1 + rotations[0] = torch.tensor([0.0, 1.0, 0.0, 0.0]) + position, rotation, close = expert.compute(torch.tensor([0, 840])) + torch.testing.assert_close(position[0], grasp[0] + torch.tensor([0.0, 0.0, 0.15])) + torch.testing.assert_close(position[1], tcp[1] + torch.tensor([0.0, 0.0, 0.15 * 3.0 / 3.5])) + torch.testing.assert_close(rotation, rotations) + assert close.tolist() == [False, True] + + +def test_scattered_fruits_are_separate_scene_objects(): + cfg = FrankaSmoothieEnvCfg() + fruits = [getattr(cfg.scene, name) for name in FRUIT_LAYOUT] + assert len(fruits) == 16 + assert [len(names) for names in FRUIT_GROUPS.values()] == [4, 4, 4, 4] + assert len({fruit.prim_path for fruit in fruits}) == 16 + assert len({fruit.init_state.pos for fruit in fruits}) == 16 + assert all( + fruit.spawn.rigid_props is None or fruit.spawn.rigid_props.kinematic_enabled is not True for fruit in fruits + ) + # Retained fruits keep the rotations used by the original packed basket. + for kind, original_start in (("strawberry", 0), ("blueberry", 5), ("blackberry", 11), ("mango", 16)): + for instance, name in enumerate(FRUIT_GROUPS[kind]): + yaw = (original_start + instance) * 2.399963229728653 + assert getattr(cfg.scene, name).init_state.rot == pytest.approx( + (0.0, 0.0, math.sin(yaw / 2), math.cos(yaw / 2)) + ) + + +def test_any_fruit_instance_can_satisfy_the_cup_receipt_check(): + poses = {name: torch.tensor([[0.4, 0.15, 0.03, 0.0, 0.0, 0.0, 1.0]]) for name in FRUIT_LAYOUT} + poses["cup"] = torch.tensor([[0.57, -0.07, 0.013, 0.0, 0.0, 0.0, 1.0]]) + env = SimpleNamespace(device="cpu", num_envs=1, pose=poses.__getitem__) + env.local = lambda name, points: SmoothieBlenderEnv.local(env, name, points) + assert not SmoothieBlenderEnv.fruit_inside(env).any() + assert SmoothieBlenderEnv.fruit_fraction(env).item() == 0.0 + for i, names in enumerate(FRUIT_GROUPS.values()): + # The original single fruit stays in the basket; a duplicate is loaded instead. + poses[names[-1]][:, :3] = poses["cup"][:, :3] + torch.tensor([[0.0, 0.0, 0.05]]) + expected = torch.arange(len(FRUIT_GROUPS)) <= i + assert torch.equal(SmoothieBlenderEnv.fruit_inside(env)[0], expected) + assert SmoothieBlenderEnv.fruit_fraction(env).item() == pytest.approx(4 / len(FRUIT_LAYOUT)) + + +def test_basket_is_one_rigid_body_with_an_open_lattice(): + cfg = FrankaSmoothieEnvCfg() + stage = Usd.Stage.Open(cfg.scene.basket.spawn.usd_path) + bodies = [prim for prim in stage.Traverse() if prim.HasAPI(UsdPhysics.RigidBodyAPI)] + assert len(bodies) == 1 + assert UsdPhysics.MassAPI(bodies[0]).GetMassAttr().Get() > 0 + assert not hasattr(cfg.scene, "bag") + floor = UsdGeom.Cylinder(stage.GetPrimAtPath("/Asset/Basket/Floor")) + assert floor.GetPrim().HasAPI(UsdPhysics.CollisionAPI) + assert floor.GetHeightAttr().Get() > 0 + # The floor must reach past the radius the lattice bars are held outside of. + assert floor.GetRadiusAttr().Get() >= BASKET_BOTTOM_RADIUS + bars = [UsdGeom.Mesh(prim) for prim in stage.Traverse() if prim.IsA(UsdGeom.Mesh)] + assert bars + for mesh in bars: + assert mesh.GetPrim().HasAPI(UsdPhysics.CollisionAPI) + assert UsdPhysics.MeshCollisionAPI(mesh).GetApproximationAttr().Get() == "convexHull" + # Individual bars stay outside the inner base radius, leaving the central mouth open. + points = torch.tensor(mesh.GetPointsAttr().Get(), dtype=torch.float64) + assert (points[:, :2].norm(dim=-1) >= BASKET_BOTTOM_RADIUS - 1.0e-6).all() + # The lattice uses separate uprights and hoops, not a convexified closed container. + assert stage.GetPrimAtPath("/Asset/Basket/Uprights") + assert stage.GetPrimAtPath("/Asset/Basket/Hoops") + assert not stage.GetPrimAtPath("/Asset/Film") + + +@pytest.mark.parametrize("file", ["cup.usda", "blade_cap.usda"]) +def test_threads_have_separate_convex_contact_surfaces(file): + stage = Usd.Stage.Open(str(ASSETS / file)) + threads = [p for p in stage.Traverse() if "/Thread/T" in str(p.GetPath())] + assert len(threads) > 32 + for prim in threads: + mesh = UsdGeom.Mesh(prim) + points = torch.tensor(mesh.GetPointsAttr().Get(), dtype=torch.float64) + faces = torch.tensor(mesh.GetFaceVertexIndicesAttr().Get()).reshape(-1, 4) + volume = 0.0 + for face in faces: + a, b, c, d = points[face] + volume += (a.dot(torch.linalg.cross(b, c)) + a.dot(torch.linalg.cross(c, d))) / 6.0 + assert volume > 0.0, "Inward faces corrupt the collider's mass and signed distance." + assert prim.HasAPI(UsdPhysics.CollisionAPI) + assert UsdPhysics.MeshCollisionAPI(prim).GetApproximationAttr().Get() == "convexHull" + # There is no joint or drive imposing the screw's pitch. + assert not any(p.IsA(UsdPhysics.PrismaticJoint) for p in stage.Traverse()) + + +def test_cup_handle_supports_leave_cavity_open(): + stage = Usd.Stage.Open(str(ASSETS / "cup.usda")) + wall = UsdGeom.Mesh(stage.GetPrimAtPath("/Asset/Cup/Wall/S000")) + radii = [math.hypot(point[0], point[1]) for point in wall.GetPointsAttr().Get()] + inner_radius, outer_radius = min(radii), max(radii) + bounds = UsdGeom.BBoxCache(Usd.TimeCode.Default(), ["default"]) + grip = bounds.ComputeWorldBound(stage.GetPrimAtPath("/Asset/Cup/Handle/Grip")).ComputeAlignedRange() + supports = [p for p in stage.GetPrimAtPath("/Asset/Cup/Handle").GetChildren() if p.GetName().startswith("Bridge")] + assert supports + for support in supports: + assert support.HasAPI(UsdPhysics.CollisionAPI) + bridge = bounds.ComputeWorldBound(support).ComputeAlignedRange() + # Supports approach from negative Y and must end within the wall, outside the cavity. + near_radius = -bridge.GetMax()[1] + assert inner_radius <= near_radius <= outer_radius + assert all( + min(bridge.GetMax()[axis], grip.GetMax()[axis]) > max(bridge.GetMin()[axis], grip.GetMin()[axis]) + for axis in range(3) + ), "Each support must remain connected to the exterior grip." + + +class TapTaskChecks(unittest.TestCase): + def setUp(self): + self.task = SmoothieTaskState(2, "cpu", 0.1) + self.inputs = { + name: torch.zeros(2, dtype=torch.bool) + for name in ( + "fruit_delivered", + "cup_under_tap", + "cup_upright", + "cup_open", + "lid_fastened", + "cup_docked", + "blender_button_pressed", + "failed", + ) + } + self.inputs["valve_depression_m"] = torch.zeros(2) + + def step(self, count=1, **values): + for name, value in values.items(): + self.inputs[name][:] = torch.as_tensor(value) + for _ in range(count): + self.task.update(**self.inputs) + + def start_tap(self): + self.step(fruit_delivered=True, cup_under_tap=True, cup_upright=True, cup_open=True) + self.assertTrue((self.task.phase == SmoothiePhase.TAP).all()) + self.step(valve_depression_m=0.002) + + def finish_tap(self): + self.start_tap() + self.step(19) + torch.testing.assert_close(self.task.fill_volume_m3, torch.full((2,), TARGET_VOLUME_M3)) + self.step(valve_depression_m=0.001) + self.step(valve_depression_m=0.002) + self.assertTrue((self.task.phase == SmoothiePhase.LID).all()) + + def reach_button(self): + self.finish_tap() + self.step(lid_fastened=True, cup_open=False) + self.step(cup_docked=True) + self.assertTrue((self.task.phase == SmoothiePhase.BUTTON).all()) + + def test_all_sixteen_whole_fruits_required(self): + hulls = torch.ones(2, 16, dtype=torch.bool) + hulls[0, -1] = False + self.step(fruit_delivered=all_fruit_delivered(hulls)) + self.assertEqual(self.task.phase.tolist(), [SmoothiePhase.FRUIT, SmoothiePhase.TAP]) + for invalid in (torch.ones(2, 15, dtype=torch.bool), torch.ones(2, 16), torch.ones(16, dtype=torch.bool)): + with self.assertRaises(ValueError): + all_fruit_delivered(invalid) + + def test_complete_ordered_sequence_and_press_edge(self): + self.reach_button() + self.assertFalse(self.task.completed.any()) + self.step(blender_button_pressed=False) + self.step(blender_button_pressed=True) + self.assertTrue(self.task.completed.all()) + self.assertTrue(self.task.milestones.all()) + self.assertFalse(self.task.tap_on.any()) + self.assertTrue((self.task.fill_fraction == 1).all()) + + def test_held_valve_toggles_once_and_hysteresis_requires_release(self): + self.start_tap() + self.step(3, valve_depression_m=0.003) + self.assertTrue(self.task.tap_on.all()) + self.step(valve_depression_m=0.0015) + self.step(valve_depression_m=0.002) + self.assertTrue(self.task.tap_on.all()) + self.step(valve_depression_m=0.001) + self.step(valve_depression_m=0.002) + self.assertFalse(self.task.tap_on.any()) + self.assertFalse(self.task.tap_off_seen.any()) + self.assertTrue((self.task.phase == SmoothiePhase.TAP).all()) + + def test_each_nozzle_gate_pauses_filling(self): + for gate in ("cup_under_tap", "cup_upright", "cup_open"): + with self.subTest(gate=gate): + self.setUp() + self.start_tap() + before = self.task.fill_volume_m3.clone() + self.step(30, **{gate: False}) + torch.testing.assert_close(self.task.fill_volume_m3, before) + self.step(19, **{gate: True}) + self.assertTrue((self.task.fill_fraction == 1).all()) + self.assertTrue((self.task.phase == SmoothiePhase.TAP).all()) + + def test_fill_requires_two_seconds_and_valve_off_after_filling(self): + self.start_tap() + self.step(18) + torch.testing.assert_close(self.task.fill_fraction, torch.full((2,), 0.95)) + self.step(valve_depression_m=0.001) + self.assertTrue((self.task.fill_fraction == 1).all()) + self.assertTrue((self.task.phase == SmoothiePhase.TAP).all()) + self.step(valve_depression_m=0.002, cup_under_tap=[False, True]) + self.assertEqual(self.task.phase.tolist(), [SmoothiePhase.TAP, SmoothiePhase.LID]) + self.step(cup_under_tap=True) + self.assertTrue((self.task.phase == SmoothiePhase.LID).all()) + + def test_early_held_valve_cannot_supply_ordered_tap_evidence(self): + self.step() + self.step(valve_depression_m=0.002) + self.assertTrue(self.task.tap_on.all()) + self.step(fruit_delivered=True, cup_under_tap=True, cup_upright=True, cup_open=True) + self.step(30) + self.assertFalse(self.task.tap_on_seen.any()) + self.assertFalse(self.task.fill_volume_m3.any()) + self.assertTrue((self.task.phase == SmoothiePhase.TAP).all()) + + def test_lid_and_dock_cannot_skip_a_phase(self): + self.step(lid_fastened=True, cup_docked=True, blender_button_pressed=True) + self.assertTrue((self.task.phase == SmoothiePhase.FRUIT).all()) + self.finish_tap() + self.assertTrue((self.task.phase == SmoothiePhase.LID).all()) + self.step(lid_fastened=False) + self.assertTrue((self.task.phase == SmoothiePhase.LID).all()) + self.step(lid_fastened=True, cup_docked=False) + self.step(2) + self.assertTrue((self.task.phase == SmoothiePhase.DOCK).all()) + + def test_early_held_blender_press_needs_release_during_button_phase(self): + self.inputs["blender_button_pressed"][:] = True + self.reach_button() + self.step(5) + self.assertFalse(self.task.completed.any()) + self.step(blender_button_pressed=False) + self.step(blender_button_pressed=True) + self.assertTrue(self.task.completed.all()) + + def test_final_press_rechecks_fruit_lid_and_dock(self): + for gate in ("fruit_delivered", "lid_fastened", "cup_docked"): + with self.subTest(gate=gate): + self.setUp() + self.reach_button() + self.step(blender_button_pressed=False) + self.step(blender_button_pressed=True, **{gate: False}) + self.assertFalse(self.task.completed.any()) + self.step(**{gate: True}) + self.assertFalse(self.task.completed.any()) + self.step(blender_button_pressed=False) + self.step(blender_button_pressed=True) + self.assertTrue(self.task.completed.all()) + + def test_late_tap_on_prevents_completion_without_further_filling(self): + self.reach_button() + self.step(valve_depression_m=0.001, blender_button_pressed=False) + self.step(valve_depression_m=0.002, blender_button_pressed=True) + self.assertTrue(self.task.tap_on.all()) + self.assertFalse(self.task.completed.any()) + before = self.task.fill_volume_m3.clone() + self.step(5) + torch.testing.assert_close(self.task.fill_volume_m3, before) + + def test_failure_latches_and_partial_reset_preserves_other_world(self): + self.start_tap() + before = self.task.fill_volume_m3.clone() + self.step(failed=[True, False], valve_depression_m=0.001) + self.step(30, failed=False, valve_depression_m=0.002) + self.assertTrue(self.task.failed[0]) + self.assertEqual(self.task.fill_volume_m3[0], before[0]) + self.assertTrue(self.task.tap_on[0]) + other = {name: value[1].clone() for name, value in vars(self.task).items() if isinstance(value, torch.Tensor)} + self.task.reset(torch.tensor([0])) + self.assertEqual(self.task.phase[0], SmoothiePhase.FRUIT) + self.assertFalse(self.task.failed[0]) + self.assertFalse(self.task.fill_volume_m3[0]) + self.assertFalse(self.task.tap_on[0]) + self.assertFalse(self.task.milestones[0].any()) + for name, expected in other.items(): + torch.testing.assert_close(getattr(self.task, name)[1], expected) + self.task.reset() + self.step(fruit_delivered=True, valve_depression_m=0.003) + self.step(5) + self.assertFalse(self.task.tap_on.any()) + + def test_invalid_inputs_and_reset_indices_leave_state_unchanged(self): + self.start_tap() + before = {name: value.clone() for name, value in vars(self.task).items() if isinstance(value, torch.Tensor)} + for name, value in ( + ("fruit_delivered", torch.ones(2)), + ("failed", torch.zeros(3, dtype=torch.bool)), + ("valve_depression_m", torch.tensor([float("nan"), 0.0])), + ("valve_depression_m", torch.zeros(2, dtype=torch.long)), + ): + with self.assertRaises(ValueError): + self.task.update(**{**self.inputs, name: value}) + for indices in (torch.tensor([-1]), torch.tensor([2]), torch.tensor([0, 0]), torch.tensor([0.0])): + with self.assertRaises(ValueError): + self.task.reset(indices) + for name, expected in before.items(): + torch.testing.assert_close(getattr(self.task, name), expected) + + def test_fill_duration_at_thirty_hertz(self): + self.task = SmoothieTaskState(2, "cpu", 1 / 30) + self.start_tap() + self.step(58) + self.assertTrue((self.task.fill_fraction < 1).all()) + self.step() + self.assertTrue((self.task.fill_fraction == 1).all()) + + +def test_smoothie_uses_rigid_physics_at_fifty_hertz(): + cfg = FrankaSmoothieEnvCfg() + cfg.validate() + assert cfg.sim.dt * cfg.decimation == pytest.approx(1 / 50) + assert not any(hasattr(cfg.scene, name) for name in ("media", "milk", "milk_cap")) + assert cfg.sim.physics.solver_cfg.solver_type == "mujoco_warp" + + +def test_tap_workstation_keeps_cup_support_and_removes_carton_slot(): + cfg = FrankaSmoothieEnvCfg() + stage = Usd.Stage.Open(cfg.scene.workstation.spawn.usd_path) + assert not stage.GetPrimAtPath("/Asset/MilkHolder").IsActive() + assert stage.GetPrimAtPath("/Asset/CupHolder").IsActive() + assert stage.GetPrimAtPath("/Asset/CapStand").IsActive() + tap = Usd.Stage.Open(cfg.scene.tap.spawn.usd_path) + joint = UsdPhysics.PrismaticJoint(tap.GetPrimAtPath("/Asset/TapButton")) + assert joint.GetLowerLimitAttr().Get() == pytest.approx(-0.004) + assert joint.GetUpperLimitAttr().Get() == 0 + + +def test_tap_bundled_robot_has_required_arm_collision_proxies(monkeypatch): + monkeypatch.delenv("ISAACLAB_FRANKA_POUR_ROBOT_USD_PATH", raising=False) + cfg = FrankaSmoothieEnvCfg() + stage = Usd.Stage.Open(cfg.scene.robot.spawn.usd_path) + assert not any(stage.GetRootLayer().GetExternalReferences()) + names = {prim.GetName() for prim in stage.Traverse()} + assert { + "link0_c", + "link1_c", + "link2_c", + "link3_c", + "link4_c", + "link5_c0", + "link5_c1", + "link5_c2", + "link6_c", + "link7_c", + } <= names + + +@pytest.mark.parametrize("succeeded", [False, True]) +def test_runner_only_records_successful_episodes(monkeypatch, tmp_path, succeeded): + import contextlib + import runpy + import sys + from pathlib import Path + + import isaaclab.app + + from isaaclab_tasks.contrib.franka_smoothie import smoothie_controller, smoothie_env + + env = SimpleNamespace( + reset=lambda **_: None, + close=lambda: None, + max_episode_length=1, + step_dt=0.02, + step=lambda _: (None, None, torch.tensor([True]), torch.tensor([False]), {}), + termination_manager=SimpleNamespace(get_term=lambda _: torch.tensor([succeeded])), + ) + controller = SimpleNamespace(compute=lambda _: torch.zeros((1, 8)), stage="test", close_trace=lambda: None) + monkeypatch.setattr(isaaclab.app, "launch_simulation", lambda _: contextlib.nullcontext()) + monkeypatch.setattr(smoothie_env, "SmoothieBlenderEnv", lambda _: env) + monkeypatch.setattr(smoothie_controller, "SmoothieSequenceController", lambda _: controller) + recording = tmp_path / "trajectory.npz" + runner = Path(__file__).resolve().parents[4] / "scripts/environments/run_franka_smoothie.py" + monkeypatch.setattr(sys, "argv", [str(runner), "--headless", "--record", str(recording)]) + if succeeded: + runpy.run_path(str(runner), run_name="__main__") + assert recording.is_file() + else: + with pytest.raises(SystemExit, match="2"): + runpy.run_path(str(runner), run_name="__main__") + assert not recording.exists()