From ce8e4951778abf769d2e5e8b165b5d86c837ad99 Mon Sep 17 00:00:00 2001 From: David Meger Date: Tue, 27 Jan 2026 16:00:29 -0500 Subject: [PATCH 1/7] Daves first effort to implement connect. Not yet planning. --- ...roller_collision_detection.cpython-310.pyc | Bin 0 -> 9646 bytes ...roller_collision_detection.cpython-313.pyc | Bin 0 -> 14939 bytes ...troller_collision_detection.cpython-38.pyc | Bin 0 -> 9638 bytes pybullet/arm_rrt.py | 114 +++--- ...gen3lite_controller_collision_detection.py | 379 ++---------------- 5 files changed, 93 insertions(+), 400 deletions(-) create mode 100644 pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-310.pyc create mode 100644 pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-313.pyc create mode 100644 pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-38.pyc diff --git a/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-310.pyc b/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..d49e6b58478d4afcd038130b764f708b9cd6c47d GIT binary patch literal 9646 zcma)CTWlLwdY%~$$>Bv5CCk^?@#MBK6I;%u=_Q*+mL*D2)}jsqO;yq5+DuDA1R-C?Ms{FZ;X&3alU7KJ}pw+d&^%6sRE-C|aOEkrs{n{r~VH z(NeM_%|GXyIp?2q{`21t8zUoW1=sDObYHroDE~s0!W8 zvtoE^HN76U;`M};s3)zYN_}HBqn@%-TvlsoD>JVMUBn+MBJQLf#H=jp36VrS`9QNq zgdtK76)WeAiu7GgzUSl~C>X1*@?FRrUtTL*X?0<3VX0)#ES1()mTp;N#nP-@oSQ4o ztSznBcwMye9~W;e%`B{a+V3=eq|@3b3#)676gdJCGWeg}>vb=>t(~M(n)Ah|tt?c_|Q#Pw2`| zj(OMhoI2iAfSJ2C9SIrW4^*YwL^WdSC%-%K)A@gH|H)KE!~XOx+O#BYdinw)+)=+! zY$yUiRcL^khyh{)0d;PV2Z_C8kk~TzRaHSbg>A*g?#H;6{zBPOqBmO^k*q4h_)4XX z{*v%y(SHOzvS`nIr2=NZQfVY(&IO}_-sb*X5xKvJJpg|682yT7lSe7fr7@JoxHOK^ zIG0ZG98LujTc;-!G4!O?3|jFfUQGhmncr77RAwwAI7{Dqpw21}=qu;IW~U!2Xgk+y z8T7a_va4bK=V|^O&}QpQM?0_3?6?0gxPUj$iFo&0@oH>eWsDcmehJiiUc~n`Qqf?~ zv!w$f`TeoqB=_Uy?NGlRx`Yx?XzRzSxsvyCua23$IOLJxSFOTzeMj6Z`_7Rbr+~Sj4|;RyfWFsG%r#nIwe}^m4Cx1pvdZ-P;=$4|Sx8p@p!Vc{ zAN}XQ|NP*ar%#_gA)cD}Y4UM(nV8_*s5Ilnew>1I^*$R;M4&zU~k}! z?m+E^Jb!55jo!c;h2)dB`8s>!fQ+QPK;XF}r2|q^h2+q>j;nTw{7+LAT6FKC2@wh+ zk5BlkN=Lbq+EoK(4_vXOb<}|=F~Z9_(( zS!`Dnv{0LTBUIf-FrvOw+hA-m2V7zLJ?P&6HmmvmZ_qSoP8XE+TgT#@v~AaO1KXZ_ z4;HOl^WR6IH~g8U5!qG==ZgTmw2@u41MlLz5-3|Ld;ly^sOObvi3LrbLq{eb%DmFt zhf52kHG8G_@zSl^Yeh6)Mwd|CidMk(ik2Z?0(ciU&;LcGa5>aO5RTm{&aK&V3#Iwu zip{9P@s)-74~E+E^D7I>%jmMSTr82r499ydGq+$rXQQ?wCUc=QTl_7aWSE4~^Q4g8aPwMbciXD|Mw4PU3~_~DBueGzL4Y#0&r z(P17o;i9-F_ug#4(}{Y=8FQG5KH~~M9A)}2o3FwKIiXIRW}N}=f7C) zN0MPiI2+|wEwEv4Y%5o&aAWEs($SH@SfZ@x=qY1eQXy?(>HGw!ByD96)*?n zida;>Lck=zjP(MQSV-WNMSM^4@4o?%P!k$%`io_?oSIe7=;zQjuBKyYmHsBx3#jE{ z=lI!v{@VrWi13h(AUT|&uVn!`ux+^24QPT#Y6)WS7Fy-o1bCKx$1R#~3Pf7rtzr~} zE339IA3ueD-y?G%;f>>F1hnF+0^!9xW|cd6*dujMLoL?P_O;zuN8QsqF%o66z}xW- zoce-RibOdsNiZXM9_eW=-qJ`A5@j%;Ctx3F6T$@MT?7AUuCOu@JNUH!LlPgJ2CA8s zl<-iPNv3(&98mZa^61!toSIj4wf*{u>wb3p5gO3?$pK{=ypMu_y}ol{7aj_pr$Ghm z32yi*LjMwUKshGjphTjhcC>0tB)^R9>f4EJgi@f0QF^3Rfd<#(pLb2dR+{rA2#n`Gew0F&vG$EJBvtn+>s3h)4UP?u2SPOv7Qf*Bkd? zRl-E{9Nw6*2*J&At=4FGi{-Ynh)**g8>)dVIj3sh=ug|P9=|Qmj6Sj@#L7wByd|%p z5U^m7BLz1}4Ya)&*ir}il`XJg*G-C3lv&W))9mje=rdPH`65J%+++&Xpd;Z?J5VEV zZG}m?sqYR)t}Kl!6%vx{q$H7Gn#%7}iB!7$bpXgFvGE}_(l=6U4(y;|6K*z)Wc+i` z8=%Gu$5R8d?2UNFfhG}UI(!cWav7Kv;WDUjr$LY8E<_ac&{!x_%z_Zg)9Bsh2T4pV zrvSpU6d%y1B9Vhujsk(MZVdG%sdKeCyxWFz->EF)odql$uH~@FSz6KwDz7)-Kwv~A z2GF}`V@Y;-QD2~NXmiIQT^jTaZ7!9txt-W~NTi6Q4=IW5IQnbM3a{+yV1>j!5)2?g za$nhv2gX)vFC7?r8F*DHKjZuAIcTiiL?!Leel6<0%f#wZG1Ngd}-9R4wW*U zcqa*g;%o^F7YWq&pjEOYM8U{j4n0RXz*hG1JPO7R#@`6W_QpGgFv$4trh>7pQ+pHK z1~KcTSisVqbVrAPRfE$aMd7uoc|U4Bxcg7XxW8k|#c4VJ0dlTEYb1#YdE8tJF``q)`~y7-6lB*SSE zmVP8{%0LJIb+b_u-zxf%6p;@ci9b>Kb>S%HzSInB9pq)#9ceZ;j@3Vb+&PNJk+LD}K84IH z8wia^i%nN5PBSR4*BrChs??C&Dto4YS}c1NhdxTFF65sI=DHJn?l@k5lW5^5U1a1* zy5TAEenkZ8)6pMHPJRY)E5xk~M^3jmaI!KSAoVJ&K_cJUN>R8273la@9<(X7>I;}% zlrJmE9|D&|&Rt=>Du0_w5l?7qL-er1l79ug*-u5%B* z%JJO_XCj~b0s{3483j0ooV-KEv75V#G%UGMhXRUUq3SyX*vDaSr%YAS#B!YgIk9q+ z0EJAPkGSP}cO$31LhTy_$iPd7z?%StT%?541E#lnW$>SL(Z?Dc;teaZKul!hE?@;% zV_SyfzyM-RVWzvaBt+Ioufk}o41BqAE$V2cq6&5E7U`=)k<(aDO;^e?fb^9;IvDXk z1kiJl@+b8l{b_3AzjGr9|Fmb#f|1Fp`B+XpqZ%=TjK(+lI2npGg2j_u-)Fvkvg=F4 zKWq+A)}fT)=)wfVp)d!gBGG$?Tx9QUQbZ;p%ef|lRXn^SV92o7^FojnIw@)qhq_I) zU+xRq5@NMT*B(jplI$_ZKhh+bOZhtlNSxZE^HDz7Y+$G(CQF5p$Z=+-V#k>r*oeKZ zQ0Y|y%y-9d8}kyrLIz!K5g6LSEPcoB&hD?!!Jh!g>j>4K<~5k{|Q9<4yHp z1Sf9+vG^e}Q}FklTkuP7fH|}Quk<%yhAh~(?SPa-l%u4N%-*+?zCgpioAlDBenT`L z;ScnP{)OIQC?k6KUF;0u? z6zj-obS>y%%7J;}(}|&Yt($SMMp>tm{)j@JUfRK>I673fhZlQ_g0i78zreSAnq*!b zr`V@({34GTc_adg0R!E2;YMog+{Te2HX&Bz+>(S1=+U;XG2nh~RK!*~L5CJB?*M%oZN*4e~)a#I{Be_oBFiO_Hm?vp%#ER+J<~aunbcZl6pZooT0&#uYdRNh4t- z)A95r5Ca6(zKdQyWH{LP{oquje`m9J-E1hpr+IXJf4W zDNDl|d~24r-X0@!Qm0dF2+visYu?R`md8IDKzg5kUlAQf52R!8qXNds{%$#?Edngh%|8J{xS@r6?1xyZJ?PWZ19xJH0jPEHaaKb<|Nh;e_DswDzT1eOUfs~_Wag=s`!pbH&w zMcjQI-L15A$X_Kra7Ye*l`#Dp0ZN<{3`!XDPi{a_`4P3R5LhL!M!*EHvaUa{QdXSL zg$~IOi4UD8xgL@dihl*wY$8V|FL18MRYNymp`yHUHjOrDT-s3k^#>{h6g`yk1Dg>jVd;}jB1{w+#G-yc&oNq}X2 l0C?KQd(p3W-p9<@u3~kQbI?Y)$Wj7dpS$Z z>`Ee&z)n)Pj1x3+U{|u^)=Uu~lHn#!Ycz5Y)N0U&Ql!W(No&_*6?G7yhESl8W%Wl; zpze3>?948yRqVL=Q=nJWojZ5#J@-R+ zC&eM!5f`xDIK={_3q>blmslYdvT{XYrRZWQw^)QP#iARrM=VBMB6<*)iY184#8Rbv zs?1xZRrO2q&ST-2)T7KCipnu18i`2CZ48l{zwPP~f@YpD8C8VXv?Lr2%h9s|fhr1A zQ&6I(qp@&M2q-f`SQbX-4#q(w*5PuEO@~z>7zwDVFsnq*hC`An%t*26Xh;=eQG+<6 zips63@I*8$$J8ytO!Tb8vL!jRRhpcXg0ZOLLKRR5D^f_9jjCaqSXBtfAt9=SB{>$* zGvaDko#GbE7n~6J(cSP5#+2=B0vr%%WO4In`3Z9cclxwdAbXHiJg?$#zSgH5<3=%AAN%%g{p{ zSHuaRDbq=*F>0eHJB)Ppi5^(Z9ta#4-6>Ymk;&;tSGKf~B8D77i$fbYZrm!`sRmkZ zHe05BXtw;xXXD8BHFNN=(PH-bpgA}x_#md%hH(q^ExV70cIthc*lU!`8R-K+t^wCG}|DgX@^OZj+xHjdx+VIT3AHUwM5PO(nFITy1nbA3~Wh-{yrtujS z4`eD@4k*MkjEV=0iU+&}@9$(`-GSRADHLMLO4X{=Z3kYx1#8BY?=>^Y?{zE0v%F23 zB^1-jj`jA9`TGWj`+I%&Af(!wzS^LKaG?yyH`jx=haBNO1LV7Gb zBZ)C-R&z$8fzY_`aG%!X&kH*Ie#mMd3x7DIH7XM7(khQ;1bvSj8p%p|O?VIu*{?6J zW}j7nCA5|Y6^umBNecGcOgN@KxKx|~!pD|kP5PC9JSC}G^MhK%m|}ha7nHF9l1yXv z!VfMz9i5RfrR|Ke=AM#bg9MNU4%C*vPT*v0kl&%^oW%0u@P` zEkand8ugFI@vg`~PJK#rrZ*0-rxk^y$v`|3^8-d{>#>@X;iKu`!!wQ|kt-?^SKOBh&<1$_qz`423L(@|5$!I*LGG;)$VC(E0;G*0)J9nCa zO7nw1i%gM>V1O8tKP1Jl6Orneozt9te^?I3{C=$=?}qCzz7rBq?|@l6&85pLzgh5J zQ}cpt#o3T<-bl~Jw7{PA>E=!B{f>j%*uqk`oUU)Vo^?IH`?I-(KbYJQB0}p?Fy(Ata<*UY zN^Rbs+`RuM6aTbj#d+er+6@b~Zr})dRLr%cOBd&A5R1} zXe~!G&u;zMyqlgi>5VOmfy))I#eR71%DF4`SFJw{y*2gb)SJcE)L)GK^2EyufsC_hf;%}e$ zot8^YmwH}qx#CIIj8W-(4z7HiKCl~QtqVOj-JX=YVaeTaDVA#NOg47DUY6?WO?LHO z3naUa+;9(kVCBl&P^(V(o+TyhT@v|HOttJ!w(P%p{B39VZ|;_HwO`XX*f9mKPoXk^yO`+?vp3Y2mK$!(NL*C{ghu^70DFdzD%clt&P2g*cWzopm(k z-=}7QJyey@3^rX+%@3UL`wQAJFQ~DUaM>yI-sx8 z(18A&;=In`E0!s}v1@N-kjJ%dGcVDtti#v$b)@%Z_Z41qD1lHouCgW0#i}_^ zL&gumK%lLo)FFeDi_?KwX+RE1=QTHdFkk?do$ZXupdw+FSC`j{`g=!){Nm`5ULT~O zUStT8EqpKg!*HV4g8@?Z(`q8QDhm!T97hE@MHl%7c zFV$>*{di*ZbBQNr64BX2IF_u5f7|h)ovUuT3y#-x|9&X|b+%tFA@Kq?Ve+i}yvR@o zu$M@+;U{dU#)2>JE&vs$Q6l?1S?Xi9xNHuk#`SYey+Oda&Zsu9idp8jyL^P@VDN#( zg-k@S;sA2NAS3~Iz&aBW22U1|Dw2}&GNh)zG zW5oz?`eRW)5!9OUuT@SFS|F8p>eCzq^Y12yXjR(uRWbP`DABC4Rrx9sSQW&2_*5sB z&njHw#1Eh$kSgt?wOOb*SoK$et5|qpw8)Y}r3XND>MDy76da{54}et0_Yu@m$!_p8$Dqw>DZJwBez&Y@p(pJwO}X2W z?zYQiH{6?PN~`rL&1@yqr;%i)^dKhsciQL}E!f?s+UVoC_3~`_lNH1)L@;Okv+gax zrpL6=q~{z&b4jpt*U%SRPG3yh7|3Kou1Oh-OzXBL7$#@gK4Ds~e1u`v1RrUUYeG&9 z(>jzb0L= z(mZ-?LuzVv81I9U<~ELbe;_Pt?z2)P8id|OZ}XmBMS+Ds5CeCX6k*;bz;-|H)JL`t zsSF|#lu>kM*ju`WM{8B!tPEQXMPaNzq+cBD&_P=%j0yu|10%!!LnFguzL8^^hlVVg zGoTEDsAiF8HAf_j>yjzWCP$SS%^pycz?{-bUu`ki5;VISk)&CsZ^AIbc$I@qgQ^vp zQ_Vz!#y_dqf&hIQFR8>bY2gqUB2*+Ixi!CxcmQu4LlrfR2=bydkGyDEa<`;w8vq8A zwe9H*bvNr8Qgxlly3TatrWfbZ)wSt*0e|;OoW&kpk6BMt=5CZVWi+4qC3pRbyD?p{ zE>*ELS+RAwqI2Q!&C>FVh0hgU^gQQzS$XCBOXn{;mK!_YX#R2AU$$NC`m2s)f|C~scuSv-6B%!+$EUN5v?np|;j(o>-o_r`Qd>4lx&IQLFz)h&1Fw+5db)HTx^ z?#8=STzRw3e2KFef!Gw2XMc_O=W!P`!z2GgSmjYlb|Wc zk@aNBTN4qE`QI|noRJA;qh{iPBg?z$ z2J4rd6U_z*UyG5-K0(zfI%(|2Jj;bSH3%B62|m)V*MyuJ`P(ps-gx0EVdgsdJrX}5 z?D~Fw2oPjqvq4%cGur8&^JDEKEX_yiBCC0MdL;K49ISz95felaZW#6s|0Db ze?{YNW0oO0tzLJ)1u4p8UKWKv=$4dSk_IENQiird?U^;CpAeU0;fQf3E0}k(Okm+! zm(cy%%h|PevrMkyhB$!IFi}oNBcWA7RTo-h^c0w2#l5Z|&vq~o0J(WMbN8EjC!Cei z6X&-Gb6bS@&+Ae*LklL&Pfjc68d+#NV_HhRA0x+ctA832Gdh`9qQ{guy}m^9)o+em zXq$vZobW(TFeuH&0;eOA&>Ig%a7!MLg%E&6Kn_X-8tBd(7E-TpT8f>MB$>eqU6$P+ z(?XEYgQbJbzdjM>JgFV7+ac^EuU#u*Ch%jr1-yt^x{VhmP%}oULb6)}Nx;Zb3IRJD zpBX@J^vm|)+xK{&6J622(#>=Y0){O$S!9XNPkP4OqcKZ)kh0Nu@t~}OL&)O&=UZJ#8MCV4=G?~t+ z(rb*OI7RameUYN|h?oifSXh2iw@00&{5guwQFNXnLMmR5j!RtG4Xqhz>@nrbD5I6E z*$TA2BAaD6_0|K(u1xg^|&Uph*WJnSzD-3l+;G(W**fa`kr6 zO=D#S+6e{}*v-NL`9Wrm`wG%Lsb9ek03fR4s@JEgb}UuxU|L;W(p|?A8)V8?ia?f}7FV}W190k^UW#GkuSB764er?AO_q@92^}*%M z-PblJHxDh>4KECl-LL*eNyp97szmjkmD0VyU?1${sy5x*!>y~i82xs1F_e1f;p9UP zU$v$7j3oDrELV>%6s0R07y0Lg7Yfb9EoX7Uv-R@)igO=s1rnZJug|^h?781QM7W9} zx1V5re!s{q2M>H=^u`dG9SR@1rHA|$5ui1jQ6l@ykNt9Jed2z=QG&1`^~6-L#T@j< zY~Xwj6NNdioN#&M#11+hGh?u#puqfpemYl+3WvXd=vT6J4U8$V|$(;kg*pWJV zDtYu&dRtd&TW@k(?{)F6l`r)1xNEz=#$a;i;4coQj`@?v{ORqxQrr8I+xrsR`mP75 zlE~jGtG=NAN$qO&YSmeX?gPVoc^xc(g`R|`@olH@TMpCJ|36GvKqLI0J4#iqLj-N} zoh9WZjB3px(sA%_KSbnm_6ZJ=@*R|0b$*H&b@Kpi%zuDZ6@n3qQ~wmvCqF^`$!-1D zRXPk~pWp=1nOjxk(;pwG9AhE5kpt=WyA?gsMY#vJD=fIQO6 zra0vQnuM1^p#+%DTtC;`C>R%sB@FWtw$dF-joZX9yM*G;s5THH{G_9%3c5#FTg97y z4f7ODfS5`k6!!6k*2^U~8uomB7%1Yt>+cOW+y`J7Z`^yoVVq`^&FUf&>}DFwX>G`w zb#c$i`KbJwuq*S9DPti3DpPbvoR6Am(*E73OJ+giq#-)R0 zb~~J6cC5a0@MPUPCKXB2TJlfw>QXCcM(suPH20p<4Y`qoq$yuaoEqFrF~>k1q{g$5gE5HOnvlg91{%N#c?K4MTx^mD z^}yC(o+;S^SHwJ)H0H$)Ts<)hWY=Kl+vc^;zwKgvLn`y=Gq-_iXv@qr!JPbSs@A~SrN#N zP7dp&F-!8|t%w1CHnF z6aOm|RbNH~NmkAg7HnB^wyZcGVh%yiE2McXuWP$e<^^;kGkV8TUB~4!%XQl`s#HqY zmK3%vmu$P)v@zAxwbaygwJWv%XmbD2gm@w)29x+}3O(lmtZQyrTi^@HroaUcsbmew zl1;BwTt2nzedOwyx4j46S-0VX4P1Hq9f2#YV#nTFqA;@olk*$UV;w*fY}VXryD9|LQ2BKjc2J*zL}Ho`>uQ_`9P#Z{K^@VYgS`_1NrZ`MYkr zeUQKFvD&xXb2Zq7dk6SO?Fae02P|%T)7?52S?oRhU8mDt`Jc6Qc763&6G2cnLm^U3 zYPJDu{V*Wk4M)}rn?upmHSwz>8+dv@@pRFS zleQ|im5B-zta%i25nsFZqqRydPv7|O)zNU9w)n+ z#ki8r;8L`eog+HiYeH83Le_Y^CG&ry|C$L*J0OJOGc$9-46aRu$+*lMP9a5bb$Tu; zx5glN1URaPOb_#5RFQ<99l~rRAj>2$a3BQN6x?1_VJ;dMRPuizvoEVL9X&@rGODEi zNXcNQ0)C-Izl%csEygdG%+}$e8v~!$f8vytFj;q4*L=Hc3_ONfUqvxK#r zj*bq%v2~aZCUK_pc}aKAH<|=YnFEu&;cwf~p$t)-5sHQ>I!+PIwjxqAhA3Rx3O-fF z!kO;*S|+!1B%;DZrxe;J41?{``Y;28&TSYUWZpn0>BDC!`W!`!NipMXoL=WC`XWV) z7r9_b#~{f=-CICzU&fEz6oM+s;}m_KqEi&@L!`OGY8IQS1;`@YkDx<;IB$n#<)4xL z5&o+hB1kb8SKhi@*7o!uB$(ZmaP3~P?Md4_DVvbA34h-BdV6Zek>rjeE4BfMBr33F z#kTd=#dQn(yJd9{a^wcKZK-D48|6Q4_))`kd$MMDVen?vx>Qx`QdR5gW!DE2li|dZ zk%TmxtUB{-AAG~g>tV?B)T3#;YoR^mYEQb_ml@rg#pM@z|M>F@JafLOS#s7e=VUiI zD__`s!?^(_56^}>POh@Syl8&<*iBnu!u8PNmsV_Dw``6-8v5$cGb4;;lRTk8`~BWh z&1v{z>xvYc?k$)D%@LcM#RgL7yeT${h+k?p(!3OcxSAcQ**WDbr5(}JPhd^-Ri?>Q zen!!CiVjiq2qMk-h%UVzRS3}oYjf&BM0cz_&%eu6y~`Eh?_asrB-i>c^jdz;7Psw<)bW*f9^q{6h52P`%{w;dGktfgoUN9%@jr|6rYHaa literal 0 HcmV?d00001 diff --git a/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-38.pyc b/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..333e8db8ae663ea83a1347913fcdad45133fe15f GIT binary patch literal 9638 zcma)CTWlQHd7j(u&R)1ACF(}&GPdPdYfDUG2SutnqA1?Xm|W5%<=PXe)8(Gwa;V*z z<(XMY+%A&`$*2ogMGB+^(xNT(QbZ5+OJCBWfQ3F4Z6A!L56(OkDT-EQ4HPMmqD_O= z{r+?ILQlAS`@$A;N6Nb~G8JA<^Jon_~c4loOAI3FLIcKugpy~rKo zwei4@cu=jny!q3Ng=fqvcdM+`fLqa;YrWz5!R?Yo{VeKe@n9v0ys}l|HOup@h0SXX z;D|;t0>|>27h2_NDGaST4{m$R4Xv6RtpzN!qM$Q?zzhU0hSqxE`B8Y$ss*=QQSJKd zlDoRUjJG4qZvjXn9epHebjnMNen9yyPhpu(Y^W7T0lz~|^ zt-HL0$TsI%8s)BqP$BKh}sEbnOJ!M^yU)HlYX!@3@=*(p42ZSky9(`z& z6JHE`5iMAzVh{!~LO$weS+vRjzQS^Upx?tdL>jCgWYNzC*^x>Lm@9$5h^YhR6ExQ-6`*`%dV9|cZWJRlT$$hl-4|BmCBI^3YFyAfA-x^-u$c4ib%h&JZ{g?eB9|Vx2bB) zL%Z7PakbOqYUNe&oVdC}5^yI;-SFc1(TD97|7f&KlkPl(nVKEN8x8|hl$LVy$d(!@ z+c>0kt))KHep}lxmXt>dGc@}*ey z9zp(vZgrIpVxa%o9KU?ydI(Ou!Ag(;H|BkJg46b0vgXRuUbSjn_fh2F=FJ=9Yi{{g z(1^ktoddd5--N{QZ`3zegly@4aC!m?*=p4bAq*VmMvxZBjnp^e%xkEP%0aDuWrP+^ zyOmTlrTNN%YZ-AI&-Wt78F{T5luOm{3Njr8Pi-e5TOzE_0f_A!-cnmoKc|#PSy!Pl z43yMUirm(M#{1Av&>`|ikhk++pPwl%JByQV&Cgz6o<#K!s$+Fs?3@HAsnaO{C_p8o zGo%&dmBP7LV^KUfJ2|!NOwAOhCl?)oE|W>*~h_mZ#GcoKInLg2erut6my(q?rNz~jU4E@_#{qb)vGkP zJK?T0DsOpVqg3^pXf`xH|CTdZ9J@9<3ChA5E`m4;IzNFU;m;5_Mu2#eA16Q*7MmT~ zA3!Pm5`dy6wWKP3dQ!`&c{OL8RL`RBh(4ss--tGhQeGWK+C2N&OtpuBNH&p78pj*v z09sIqc-2)65=bqA70#oUzd%6DQp{HXG2b*cX;B>2R|y5-Qq<zzMu! z4z%Km0tUuBCX|~N)RMZbp`^F8huW6jQn!tkPV7vIdn3~#J5Vc1c24k%=q`D>uOl_Z zkqSnLlO+(K&kBY(1Qh*B74)Nd%+5-32|!VGA6qH5{3$eU91{v z2FEg>HeWt;-B0)5vj?Os$+!`F@DVa3y70m(W!eLc^uxN|F2lgVPp`m6eV}h?EQx*k z!j{fbNKbDWXl0NI)6!coSz2mKW0?oWR$}89Hb%&L?V2Zpl~hY{5-nvr`B2$PB1L&U zAq*mA+eAK%H-lYAJ-#L77cq}4J3>F0L}D&tHb*fVQ;yz()qF~6nQRDk31Jo-D?X80 z|IbFDmy*21yhhkol0P2hw2>e-+SEABD=Ve(pc?Q;T3eq!?&#CUqusiXMmaHwIQl12 z6aQeeV8rGMB;6dW-8e_Nj?9ZsPzh+IhFV%H)!iANxp(6s`XD*CS6i9-i{JX*xv4)N zt$giIGJn|k%fI`u^3v~I`1s%6{>f8ZA`k*J$Q++I*5Ka#FqzmAMN7 z+&jrQJ3To+=PWI}KDjt)56ULHrW^OmsyWCc?^?h%W1|w3s+<@yJ_^Y%g*b^nX{g6! zQOtV&tsZM|3yp1440LV&?Lt~`h>e}8Rs*Q>o?-{D40dX73uy%_N3&Q_E=sT1L0j z=8FgOo7|6G*!J)m$*2@w!D}xe6A6h&hVo6A4w1I4gYIE85I3|)(&!{`4~XKjMFQSOH$fq#Jpra+x8vi0_Mhf ziKZ*B69{*d-8k2wndy?}$GO`sd;(a!RI;;}@LTN){)-r@Sm@u!Tk$b)fKc0W0iqY< zGMe&=GzbP@fzK?@%om;U`Qq~8{A`@3LrAF-8-BoRaiYX|34%6) z$sH%cs_WK;O;<=s!I_ksAE&!JDy+R&y~P(W%~;)UWN>m}*71g9Zz*OTMunkjc$0wQ z_upnN2l_YF(}t-UTJzGu41Wm2?fe*M+mV&~qh&Ixna&{pX-OwJu-V__P8O%-wS&KVA z0mt$wAe72D#owm4S%R|t^b*X*bSNqDS7Q{DX8wSt8%?ih*M>dR5PMNd_%x)18Mi{#t5SF%&MKdGuC*932Q0_$n!ccmd zat?uCB~T);N}xhOIMlP0TOr^QAk%@)%Qd^0= zM=%Y#LCA62bX{rL#@Y6X6K~mloq|APXJH3+P2jXFP_uTHMirJ_&%7J#NbhoqM_|g5 z|30P_z7C+|Nh6R>Ag$2O|5vgv4|Dh_ZShkxYpOYY0B-4VopeSLd8AtAP^0;&kq&q&wT_}_Z`6t-6I$$#r9cez&R$y_inz_hM&Fp=cwD~ zZ$1yH-9EPI69aTM{)cGs)W*vLXm9-U2XFiyEPK2q11OVt#|bqFCO8DR0)bz2@`kpB zxKuAn)zXE(Xp)D-Uqw@dB+@Q4m$>cP=kK*cQFgx&_dOTA6OZylfI{8aw_xFKJ)ybK z#K=W%77kt#xqtg`@9~Xvbf;G=jNCTi2Ew8e{*C$&Q9p8gMXhlCkfYj*RLc+~Ea=6l zWq7P_pf4g3bZ^j$7IeHkt^Q@iXJ6U3ia6g3E39(Y{RAjNGDa1MDVVs3su66SKX_?- zJMUo>+7+^J#i|yOq3abPXEfyWhqw{L(S23OA2|B3CsE(FZ#AYO;HD$UsNFKdsVGHO zEz@rqdSOH80Zjn!f+tIc~ zc-!NFUxV#%v5U3@x6j4J3{twsxwH%S1&I(&3C3OXqKIxbWf!d}+`qVT!gsvT?b$F0Y$a#~V=}oWW3bCp z1yic~Q;7A$gTs|}8}CHtN=QgoajgYQ1_3J%c1=HgUSv0M2er{~XP7-eL033yxZ_(8 z*U{2r)2puEW*Vu9`^0)3nh8QrHb_eJ0V~lR_CAW^o1lgN#4E`xZ9*$%)B(iNO}+WT zA!N3fq>BZW!@E_G{4ymPzlDTXDL+Wzd%uLM1}DxqPAy=#FhD-%IIwlA?Jp_EVL{n( zI7MCfR|rt3j=xEOh?T!ZfE+@;L7+k4HUVNX-XcH?*|j(!4@uW>l2RsK5j+F?q%s#w z-AtLocvG2YGgr+#eukM5Ez4-xeP~(KRdCG*$VAFLWlM>0dq&zAoZpU#AgnBWGU#%Z zC4{D?AoN$ZR1t)J3EX6?C-5f4T{?+J170KX6#T4~hOluKfnkF}(h;nPZP);WB}%iL z;LPLjCXMZk;MnXoLe|1AxQmv36spFk`BqZ+YsPk-TCsjeqJidL3!)eQcK{KWEY_&i zHWAeTldU#=mnCE%`n%)Jh|=1j-6-Mnh{e zXjmZy^~hlmz1D&|6!;5W`8S3R1tA(!qkm5T$HmTn8@e;V{ZIswmw$rjab54mRE*es zDYFc#pJuTKYdd0siztdO;`7MJNG}dfwlJQLN1Nxm-Mb73H?MOki#}J-j$b7(O5id; z`(FNsqn!Zt1brcL@CIJQ=Rxsoj5deG{qQc4!G(-**=0mG`vqkQn_JQ^-Iz+fH%&P* zX88;OLG~|GZl9?|)E#HwkYTlS-&sh-ncl+r9CaxI*r=_7_bKYk=c$%rrgqK?dpc+r zmk@){ZK3Dq-~feW?kR~R?BgNBY=@YKVVjYWkV846@%z*ROig_pr!Zifo%?UOpjCY5 z7pPG(+N|R=IN}8g*9njkiVf2IoODr~K(4+i{F68ttgK^Qa;Inz{!Icsv!=_?vH6;m dBUdnGC8Rqzvga{pnx{7KE#+HE(mZRb{|`!U2TA|{ literal 0 HcmV?d00001 diff --git a/pybullet/arm_rrt.py b/pybullet/arm_rrt.py index 349d105..4c292c3 100644 --- a/pybullet/arm_rrt.py +++ b/pybullet/arm_rrt.py @@ -3,7 +3,7 @@ from scipy.spatial import cKDTree import matplotlib.pyplot as plt import random -import math + import gen3lite_controller_collision_detection # ----------------------------- @@ -14,6 +14,18 @@ def __init__(self, point): self.point = np.array(point) self.parent = None +class Tree: + def __init__(self,node_list,kdtree): + self.node_list = node_list + self.kdtree = kdtree + + def add(self,new_point): + self.node_list.append(new_point) + + if len(self.node_list) % 50 == 0: + data = [n.point for n in self.node_list] + self.kdtree = cKDTree(data) + # ----------------------------- # RRT Planner # ----------------------------- @@ -26,44 +38,60 @@ def __init__( rand_area=[], step_size=0.1, goal_sample_rate=0.1, - max_iter=5000 + max_iter=500000 ): self.controller = gen3lite_controller_collision_detection.Gen3LiteArmController() self.controller.createBalloonMaze() self.start = Node(self.controller.getCurrentJointAngles()) - self.goal = Node([0.1026325022237283, -0.2931188624740633, 1.2717083400432991, 0.048794139164578594, 0.07744723004754135, -0.8437927483158898, -0.024709326684397483]) + self.goal = Node(self.controller.goal) self.rand_ranges = self.controller.getRanges() self.step_size = step_size self.goal_sample_rate = goal_sample_rate self.max_iter = max_iter - self.node_list = [self.start] - self.kdtree = cKDTree([self.start.point]) + self.start_tree = Tree([self.start],cKDTree([self.start.point])) + self.goal_tree = Tree([self.goal],cKDTree([self.start.point])) + + def add_node(self,tree): + rnd_point = self.sample() + nearest_node = self.nearest_node(rnd_point,tree) + new_node = self.steer(nearest_node, rnd_point) + + if self.collision_free(nearest_node.point, new_node.point): + tree.add(new_node) + + return new_node # ----------------------------- # Main planning loop + # returns: True when a path has been found + # False if we have no path by the set number of iterations # ----------------------------- def plan(self): - for _ in tqdm(range(self.max_iter)): + for k in tqdm(range(self.max_iter)): + if k % 2: + print("Adding to start") + new_node = self.add_node(self.start_tree) - rnd_point = self.sample() - nearest_node = self.nearest_node(rnd_point) - new_node = self.steer(nearest_node, rnd_point) + while(new_node is not None): + ret = self.add_node(self.goal_tree,new_node) + if ret == None: + break + if ret == self.TREES_CONNECT: + print("Plan found!") + + else: + print("Adding to goal tree") + new_node = self.add_node(self.goal_tree) - if self.collision_free(nearest_node.point, new_node.point): - self.node_list.append(new_node) - - if len(self.node_list) % 50 == 0: - data = [n.point for n in self.node_list] - self.kdtree = cKDTree(data) + while(self.add_node(self.start_tree,new_node)): if self.reached_goal(new_node): - p = self.extract_path(new_node) - self.controller.execPath(p) - return p + self.path_to_goal = self.extract_path(new_node) + return True - return None # Failed + return False # ----------------------------- # Sampling @@ -79,9 +107,9 @@ def sample(self): # ----------------------------- # Nearest node # ----------------------------- - def nearest_node(self, point): - _, idx = self.kdtree.query(point) - return self.node_list[idx] + def nearest_node(self, point,tree): + _, idx = tree.kdtree.query(point) + return tree.node_list[idx] # ----------------------------- # Steer @@ -117,37 +145,6 @@ def extract_path(self, node): path.append(node.point) node = node.parent return path[::-1] - - # ----------------------------- - # Visualization - # ----------------------------- - def draw(self, path=None): - plt.figure() - for node in self.node_list: - if node.parent: - plt.plot( - [node.point[0], node.parent.point[0]], - [node.point[1], node.parent.point[1]], - "-g" - ) - - for (ox, oy, r) in self.obstacles: - circle = plt.Circle((ox, oy), r, color="r") - plt.gca().add_patch(circle) - - plt.plot(self.start.point[0], self.start.point[1], "bo", label="Start") - plt.plot(self.goal.point[0], self.goal.point[1], "ro", label="Goal") - - if path: - px, py = zip(*path) - plt.plot(px, py, "-b", linewidth=2, label="Path") - - plt.axis("equal") - plt.grid(True) - plt.legend() - plt.show() - - # ----------------------------- # Example Usage # ----------------------------- @@ -155,7 +152,10 @@ def draw(self, path=None): rrt = RRT() - path = rrt.plan() - print("Planning complete.") - print(path) - #rrt.draw(path) + success = rrt.plan() + if success: + rrt.controller.execPath(rrt.path_to_goal) + print("Planning complete. Moving the robot to the goal.") + else: + print("Failed to find a path to the goal.") + \ No newline at end of file diff --git a/pybullet/gen3lite_controller_collision_detection.py b/pybullet/gen3lite_controller_collision_detection.py index a5be17b..283f39a 100644 --- a/pybullet/gen3lite_controller_collision_detection.py +++ b/pybullet/gen3lite_controller_collision_detection.py @@ -5,18 +5,7 @@ from enum import Enum import numpy as np - -class ControlModes(Enum): - """ - Pybullet control modes, only for the end effector. We use IK to translate - EF commands to joint velocities using a PID controller. - """ - - END_EFFECTOR_POSE = pb.POSITION_CONTROL - END_EFFECTOR_TWIST = pb.VELOCITY_CONTROL - - -class Gen3LiteArmController: +class Gen3LiteArmController(object): """ A controller for the Kinova Gen3 Lite robotic arm in PyBullet. @@ -52,7 +41,7 @@ def __init__(self, dt=1 / 50.0): self.__upper_limits: List = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05] self.__joint_ranges: List = [5.8, 4, 5.8, 4, 5.8, 4, 6] self.__rest_poses: List = [0, 0, 0, 0, 0, 0, 0] - self.__home_poses: List = [0, 0, 0.5 * math.pi, 0.5 * math.pi, 0.5 * math.pi, -math.pi * 0.5, 0] + self.__home_poses: List = [0, 0, -0.5 * math.pi, 0.5 * math.pi, 0.5 * math.pi, -math.pi * 0.5, 0] self.joint_ids = [pb.getJointInfo(self.__kinova_id, i) for i in range(self.__n_joints)] self.joint_ids = [j[0] for j in self.joint_ids if j[2] == pb.JOINT_REVOLUTE] @@ -75,10 +64,30 @@ def getCurrentJointAngles(self): return angles def createBalloonMaze(self): + balloon_collision_id = pb.createCollisionShape(pb.GEOM_SPHERE, radius=0.1) + balloon_visual_id = pb.createVisualShape(pb.GEOM_SPHERE, radius=0.1,rgbaColor=[1.0,0.0,0.0,1.0]) + for y in [-0.125, 0.125]: for z in [0.25, 0.5]: - col_box_id = pb.createCollisionShape(pb.GEOM_SPHERE, radius=0.1) - box_id = pb.createMultiBody(baseMass=0, baseCollisionShapeIndex=col_box_id, basePosition=[0.4, y, z]) + box_id = pb.createMultiBody(baseMass=0, basePosition=[0.3, y, z],baseCollisionShapeIndex=balloon_collision_id, + baseVisualShapeIndex=balloon_visual_id + ) + + # This is a hard-coded sensible goal to try to reach, in the middle of the four balloons. + self.goal = [0.1026325022237283, -0.2931188624740633, 1.2717083400432991, 0.048794139164578594, 0.07744723004754135, -0.8437927483158898, -0.024709326684397483] + + # Record where we were (usually home, but just to be safe...) + curr = self.getCurrentJointAngles() + # Move the arm to the goal, specified in joint angles + self.set_joint_positions(self.goal) + # Record the end effectors x,y,z position + goal_state = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) + # Move the arm back to where it began + self.set_joint_positions(curr) + + # Now we can make a visual marker for the goal + goal_visual_id = pb.createVisualShape(pb.GEOM_BOX, halfExtents=[0.05,0.05,0.05],rgbaColor=[0.0,0.0,1.0,1.0]) + box_id = pb.createMultiBody(baseMass=0, basePosition=goal_state[0], baseVisualShapeIndex=goal_visual_id ) def set_to_home(self): """ @@ -88,11 +97,13 @@ def set_to_home(self): pb.resetJointState(self.__kinova_id, i, self.__home_poses[i]) def execPath(self,path): + self.set_joint_positions(self.__home_poses) + pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) for p in path: self.move_to_joint_positions(p) - def move_to_joint_positions(self, joints, max_steps=100): + def move_to_joint_positions(self, joints, max_steps=20): """ Move to target joint positions with position control. @@ -108,7 +119,8 @@ def move_to_joint_positions(self, joints, max_steps=100): targetPosition=joints[i], force=2000, positionGain=1.0, - velocityGain=1.0 + velocityGain=1.0, + maxVelocity=0.3 ) # Step the simulation for a short duration to allow movement. @@ -116,9 +128,9 @@ def move_to_joint_positions(self, joints, max_steps=100): pb.stepSimulation() curr = self.getCurrentJointAngles() e = np.linalg.norm(np.array(joints) - np.array(curr)) - print("Error at iter ", k, " is ", e) - print("Target: ", joints) - print("Current ", curr) + #print("Error at iter ", k, " is ", e) + #print("Target: ", joints) + #print("Current ", curr) if e < 0.1: break @@ -230,9 +242,6 @@ def collision_free(self,p1,p2): return False return True - # ------------------------------------------------------------------ - # UPDATED COLLISION FUNCTION - # ------------------------------------------------------------------ def check_collision(self): """ Checks for collisions between the robot and ANY other body in the environment, @@ -247,14 +256,7 @@ def check_collision(self): # Iterate over all bodies in the PyBullet simulation for i in range(pb.getNumBodies()): other_body_id = pb.getBodyUniqueId(i) - - # Case 1: Self-collision (Body vs itself) - if other_body_id == self.__kinova_id: - contact_points = pb.getContactPoints(bodyA=self.__kinova_id, bodyB=self.__kinova_id) - - # Case 2: Environment collision (Body vs other object) - else: - contact_points = pb.getContactPoints(bodyA=self.__kinova_id, bodyB=other_body_id) + contact_points = pb.getContactPoints(bodyA=self.__kinova_id, bodyB=other_body_id) # If contacts are found, return True immediately if contact_points is None or len(contact_points) > 0: @@ -265,16 +267,10 @@ def check_collision(self): def main(): """ - Test the Gen3Lite Arm moving, gripper functionalities, and collision detection. + This is a dummy main function that won't be used for the core A2 planning but + gives you some idea for how to see the Gen3Lite Arm moving, gripper functionalities, and collision detection. """ - - # Initialize PyBullet simulation - pb.connect(pb.GUI) - pb.setGravity(0, 0, -9.8) - - # Create the controller controller = Gen3LiteArmController() - pb.setTimeStep(controller.dt) # Test homing functionality print("\nTesting Gen3Lite Arm controller homing...") @@ -289,318 +285,15 @@ def main(): col_box_id = pb.createCollisionShape(pb.GEOM_SPHERE, radius=0.125) box_id = pb.createMultiBody(baseMass=0, baseCollisionShapeIndex=col_box_id, basePosition=[0.4, y, z]) - #col_box_id = pb.createCollisionShape(pb.GEOM_SPHERE, halfExtents=[0.2, 0.2, 0.2]) - #box_id = pb.createMultiBody(baseMass=0, baseCollisionShapeIndex=col_box_id, basePosition=[-1.0, 0, 0.5]) - - #col_box_id = pb.createCollisionShape(pb.GEOM_SPHERE, halfExtents=[0.2, 0.2, 0.2]) - #box_id = pb.createMultiBody(baseMass=0, baseCollisionShapeIndex=col_box_id, basePosition=[0.0, 1.0, 0.5]) - print(controller.getCurrentJointAngles()) for i in range (10000): pb.stepSimulation() time.sleep(1./240.) - pb.disconnect() - - # Note: No arguments passed to check_collision - is_collision = controller.check_collision() - print(f"Box at [1.0, 0, 0.5]. Collision detected? {is_collision} (Expected: False)") - - return - - # 3. Move the box to where the arm currently is (approx [0.4, 0, 0.4]) - print("Moving box to collide with arm...") - pb.resetBasePositionAndOrientation(box_id, [0.4, 0, 0.4], [0, 0, 0, 1]) - pb.stepSimulation() - is_collision = controller.check_collision() - print(f"Box at [0.4, 0, 0.4]. Collision detected? {is_collision} (Expected: True)") - - # Remove the box to continue other tests cleanly - pb.removeBody(box_id) - # --- COLLISION DETECTION TEST END --- - - controller.move_to_cartesian([-0.4, 0, 0.4], controller.default_ori) - - # Test gripper functionality - print("\nTesting gripper open/close...") - controller.open_gripper() - controller.close_gripper() + print("Check collision returned: ",is_collision) - # Test the direct joint control - print("\nTesting direct joint control...") - home_ = [0, 0, 0, 0, 0, -math.pi * 0.5, 0] - controller.move_to_joint_positions(home_) - - print("All tests completed.") pb.disconnect() - if __name__ == "__main__": main() - -# import pybullet as pb -# import time -# import math -# from typing import Dict, List -# from enum import Enum -# import numpy as np -# -# -# class ControlModes(Enum): -# """ -# Pybullet control modes, only for the end effector. We use IK to translate -# EF commands to joint velocities using a PID controller. -# """ -# -# END_EFFECTOR_POSE = pb.POSITION_CONTROL -# END_EFFECTOR_TWIST = pb.VELOCITY_CONTROL -# -# -# class Gen3LiteArmController: -# """ -# A controller for the Kinova Gen3 Lite robotic arm in PyBullet. -# -# This class provides methods to control the arm's joints, move the end-effector -# to desired positions and orientations using inverse kinematics, and operate the gripper. -# """ -# -# def __init__(self, dt=1 / 50.0): -# self.dt = dt -# -# self.LEFT_FINGER_JOINT = 7 # Example index; update if needed. -# self.RIGHT_FINGER_JOINT = 9 # Example index; update if needed. -# self.GRIPPER_OPEN_POS = 0.7 # Adjust as needed. -# self.GRIPPER_CLOSED_POS = 0.0 # Adjust as needed. -# -# # End-effector link index as used in your URDF. -# self.END_EFFECTOR_INDEX = 7 -# -# # Load the Kinova Gen3 Lite URDF model. -# # Ensure the path "gen3lite_urdf/gen3_lite.urdf" exists in your directory -# self.__kinova_id = pb.loadURDF("gen3lite_urdf/gen3_lite.urdf", [0, 0, 0], useFixedBase=True) -# pb.resetBasePositionAndOrientation(self.__kinova_id, [0, 0, 0.0], [0, 0, 0, 1]) -# -# self.__n_joints = 7 # pb.getNumJoints(self.__kinova_id) - 5, where -5 for the gripper -# print(f'Found {self.__n_joints} active joints for the robot.') -# -# # Joint limits and rest/home poses. -# self.__lower_limits: List = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05] -# self.__upper_limits: List = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05] -# self.__joint_ranges: List = [5.8, 4, 5.8, 4, 5.8, 4, 6] -# self.__rest_poses: List = [0, 0, 0, 0, 0, 0, 0] -# self.__home_poses: List = [0, 0, 0.5 * math.pi, 0.5 * math.pi, 0.5 * math.pi, -math.pi * 0.5, 0] -# -# self.joint_ids = [pb.getJointInfo(self.__kinova_id, i) for i in range(self.__n_joints)] -# self.joint_ids = [j[0] for j in self.joint_ids if j[2] == pb.JOINT_REVOLUTE] -# -# # Initialize to rest position. -# for i in range(self.__n_joints): -# pb.resetJointState(self.__kinova_id, i, self.__rest_poses[i]) -# -# self.default_ori = list(pb.getQuaternionFromEuler([0, -math.pi, 0])) -# -# def set_to_home(self): -# """ -# Resets the arm to its predefined home position. -# """ -# for i in range(self.__n_joints): -# pb.resetJointState(self.__kinova_id, i, self.__home_poses[i]) -# -# def move_to_joint_positions(self, joints, max_steps=100): -# """ -# Move to target joint positions with position control. -# -# Args: -# joints (list): Target joint positions. -# max_steps (int): Maximum simulation steps to reach the target. -# """ -# for i in range(self.__n_joints): -# pb.setJointMotorControl2( -# bodyIndex=self.__kinova_id, -# jointIndex=i, -# controlMode=pb.POSITION_CONTROL, -# targetPosition=joints[i], -# force=500, -# positionGain=0.05, -# velocityGain=1 -# ) -# -# # Step the simulation for a short duration to allow movement. -# for _ in range(max_steps): -# pb.stepSimulation() -# time.sleep(self.dt) -# -# def move_to_cartesian(self, target_pos, target_ori, max_steps=240, error_threshold=0.01): -# """ -# Moves the arm using inverse kinematics and closed-loop control until the end effector -# reaches the desired position and orientation within a threshold. -# -# Args: -# target_pos (list or np.array): Desired end-effector position [x, y, z]. -# target_ori (list or np.array): Desired end-effector orientation (quaternion). -# max_steps (int): Maximum number of simulation steps to try. -# error_threshold (float): Acceptable Euclidean distance (in meters) between -# the current and target positions. -# """ -# -# # Calculate the inverse kinematics solution. -# jointPoses = pb.calculateInverseKinematics( -# self.__kinova_id, -# self.END_EFFECTOR_INDEX, -# target_pos, -# target_ori, -# lowerLimits=self.__lower_limits, -# upperLimits=self.__upper_limits, -# jointRanges=self.__joint_ranges, -# restPoses=self.__rest_poses, -# maxNumIterations=100 -# ) -# -# # Slice the IK solution so that only the controlled joints are used. -# jointPoses = jointPoses[:self.__n_joints] -# -# for step in range(max_steps): -# # Command each joint to the desired position. -# for i in range(self.__n_joints): -# pb.setJointMotorControl2( -# bodyIndex=self.__kinova_id, -# jointIndex=i, -# controlMode=pb.POSITION_CONTROL, -# targetPosition=jointPoses[i], -# force=500, -# positionGain=0.05, -# velocityGain=1 -# ) -# -# # Step the simulation. -# pb.stepSimulation() -# time.sleep(self.dt) -# -# # Get the current end-effector state. -# ee_state = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) -# current_pos = np.array(ee_state[0]) -# current_error = np.linalg.norm(np.array(target_pos) - current_pos) -# -# # If within threshold, break out. -# if current_error < error_threshold: -# print("Target reached within threshold.") -# break -# -# # Final achieved state. -# final_state = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) -# final_pos = final_state[0] -# final_ori = final_state[1] -# -# print("Target end-effector position:", target_pos) -# print("Final achieved end-effector position:", final_pos) -# -# def open_gripper(self): -# """ -# Opens the gripper. -# """ -# pb.setJointMotorControl2(self.__kinova_id, self.LEFT_FINGER_JOINT, pb.POSITION_CONTROL, -# targetPosition=self.GRIPPER_OPEN_POS, force=500) -# pb.setJointMotorControl2(self.__kinova_id, self.RIGHT_FINGER_JOINT, pb.POSITION_CONTROL, -# targetPosition=-self.GRIPPER_OPEN_POS, force=500) -# for _ in range(100): -# pb.stepSimulation() -# time.sleep(self.dt) -# -# print("Gripper opened.") -# -# def close_gripper(self): -# """ -# Closes the gripper. -# """ -# pb.setJointMotorControl2(self.__kinova_id, self.LEFT_FINGER_JOINT, pb.POSITION_CONTROL, -# targetPosition=self.GRIPPER_CLOSED_POS, force=500) -# pb.setJointMotorControl2(self.__kinova_id, self.RIGHT_FINGER_JOINT, pb.POSITION_CONTROL, -# targetPosition=self.GRIPPER_CLOSED_POS, force=500) -# for _ in range(100): -# pb.stepSimulation() -# time.sleep(self.dt) -# -# print("Gripper closed.") -# -# def check_collision(self, other_object_id): -# """ -# Determines if there is a collision between the Kinova Gen3 Lite robotic arm -# and another object based on their current positions in the PyBullet simulation. -# -# Args: -# other_object_id (int): The PyBullet body unique ID of the other object. -# -# Returns: -# bool: True if a collision is detected, False otherwise. -# """ -# # Ensure collision detection is up to date -# pb.performCollisionDetection() -# -# # Check for contact points between the arm and the specified object -# contact_points = pb.getContactPoints(bodyA=self.__kinova_id, bodyB=other_object_id) -# -# # Return True if any contact points exist -# return len(contact_points) > 0 -# -# -# def main(): -# """ -# Test the Gen3Lite Arm moving, gripper functionalities, and collision detection. -# """ -# -# # Initialize PyBullet simulation -# pb.connect(pb.GUI, options="--opengl2") -# pb.setGravity(0, 0, -9.8) -# -# # Create the controller -# controller = Gen3LiteArmController() -# pb.setTimeStep(controller.dt) -# -# # Test homing functionality -# print("\nTesting Gen3Lite Arm controller homing...") -# controller.move_to_cartesian([0.4, 0, 0.4], controller.default_ori) -# -# # --- COLLISION DETECTION TEST START --- -# print("\nTesting Collision Detection...") -# -# # 1. Create a dummy box object -# col_box_id = pb.createCollisionShape(pb.GEOM_BOX, halfExtents=[0.05, 0.05, 0.05]) -# -# # 2. Spawn the box far away (at x=1.0) where it shouldn't hit the arm -# box_id = pb.createMultiBody(baseMass=0, baseCollisionShapeIndex=col_box_id, basePosition=[1.0, 0, 0.5]) -# pb.stepSimulation() -# -# is_collision = controller.check_collision(box_id) -# print(f"Box at [1.0, 0, 0.5]. Collision detected? {is_collision} (Expected: False)") -# -# # 3. Move the box to where the arm currently is (approx [0.4, 0, 0.4]) -# print("Moving box to collide with arm...") -# pb.resetBasePositionAndOrientation(box_id, [0.4, 0, 0.4], [0, 0, 0, 1]) -# pb.stepSimulation() -# -# is_collision = controller.check_collision(box_id) -# print(f"Box at [0.4, 0, 0.4]. Collision detected? {is_collision} (Expected: True)") -# -# # Remove the box to continue other tests cleanly -# pb.removeBody(box_id) -# # --- COLLISION DETECTION TEST END --- -# -# controller.move_to_cartesian([-0.4, 0, 0.4], controller.default_ori) -# -# # Test gripper functionality -# print("\nTesting gripper open/close...") -# controller.open_gripper() -# controller.close_gripper() -# -# # Test the direct joint control -# print("\nTesting direct joint control...") -# home_ = [0, 0, 0, 0, 0, -math.pi * 0.5, 0] -# controller.move_to_joint_positions(home_) -# -# print("All tests completed.") -# pb.disconnect() -# -# -# if __name__ == "__main__": -# main() \ No newline at end of file From e976583ee19caf7dda4529a6d0e5818a527eb67f Mon Sep 17 00:00:00 2001 From: DavidMeger Date: Thu, 29 Jan 2026 09:23:36 -0500 Subject: [PATCH 2/7] Working towards a tree visualization. --- pybullet/arm_rrt.py | 116 ++++++++++++----- ...gen3lite_controller_collision_detection.py | 122 +++++++++++++++++- 2 files changed, 202 insertions(+), 36 deletions(-) diff --git a/pybullet/arm_rrt.py b/pybullet/arm_rrt.py index 4c292c3..a46699d 100644 --- a/pybullet/arm_rrt.py +++ b/pybullet/arm_rrt.py @@ -38,7 +38,7 @@ def __init__( rand_area=[], step_size=0.1, goal_sample_rate=0.1, - max_iter=500000 + max_iter=5 ): self.controller = gen3lite_controller_collision_detection.Gen3LiteArmController() self.controller.createBalloonMaze() @@ -50,17 +50,21 @@ def __init__( self.goal_sample_rate = goal_sample_rate self.max_iter = max_iter self.start_tree = Tree([self.start],cKDTree([self.start.point])) - self.goal_tree = Tree([self.goal],cKDTree([self.start.point])) + self.goal_tree = Tree([self.goal],cKDTree([self.goal.point])) - def add_node(self,tree): - rnd_point = self.sample() + def add_node(self,tree,target=None): + if target is None: + rnd_point = self.sample() + else: + rnd_point = target.point nearest_node = self.nearest_node(rnd_point,tree) new_node = self.steer(nearest_node, rnd_point) if self.collision_free(nearest_node.point, new_node.point): tree.add(new_node) - - return new_node + return new_node + else: + return None # ----------------------------- # Main planning loop @@ -71,25 +75,35 @@ def plan(self): for k in tqdm(range(self.max_iter)): if k % 2: - print("Adding to start") + #print("Adding to start") new_node = self.add_node(self.start_tree) while(new_node is not None): ret = self.add_node(self.goal_tree,new_node) if ret == None: break - if ret == self.TREES_CONNECT: + if self.reached_goal(ret,goal=new_node): print("Plan found!") + print(new_node.point,ret.point) + self.path_to_goal = self.extract_path(new_node,ret) + print(self.path_to_goal) + return True else: - print("Adding to goal tree") + #print("Adding to goal tree") new_node = self.add_node(self.goal_tree) - while(self.add_node(self.start_tree,new_node)): + while(new_node is not None): + ret = self.add_node(self.start_tree,new_node) + if ret == None: + break - if self.reached_goal(new_node): - self.path_to_goal = self.extract_path(new_node) - return True + if self.reached_goal(ret,goal=new_node): + print("Plan found!") + print(ret.point,new_node.point) + self.path_to_goal = self.extract_path(ret,new_node) + print(self.path_to_goal) + return True return False @@ -117,9 +131,13 @@ def nearest_node(self, point,tree): def steer(self, from_node, to_point): direction = to_point - from_node.point distance = np.linalg.norm(direction) - direction = direction / distance - new_point = from_node.point + self.step_size * direction + if distance < self.step_size: + new_point = to_point + else: + direction = direction / distance + new_point = from_node.point + self.step_size * direction + new_node = Node(new_point) new_node.parent = from_node return new_node @@ -133,29 +151,65 @@ def collision_free(self, p1, p2): # ----------------------------- # Goal check # ----------------------------- - def reached_goal(self, node): - return np.linalg.norm(node.point - self.goal.point) < self.step_size + def reached_goal(self, node,goal=None): + if goal is None: + goal = self.goal + + return np.linalg.norm(node.point - goal.point) < self.step_size and self.collision_free(node.point,goal.point) # ----------------------------- # Path extraction # ----------------------------- - def extract_path(self, node): - path = [self.goal.point] - while node is not None: - path.append(node.point) - node = node.parent - return path[::-1] + def extract_path(self, start_node,goal_node): + # Build the start-tree path backwards + start_tree_path = [] + while start_node is not None: + start_tree_path.append(start_node.point) + start_node = start_node.parent + + print("Start path",start_tree_path) + + # Build the goal-tree path forwards + goal_tree_path = [] + while goal_node is not None: + goal_tree_path.append(goal_node.point) + goal_node = goal_node.parent + + print("Goal path: ", goal_tree_path) + overall_path = start_tree_path[::-1] + overall_path.extend(goal_tree_path) + print("Overall path",overall_path) + return overall_path # ----------------------------- # Example Usage # ----------------------------- if __name__ == "__main__": - rrt = RRT() - - success = rrt.plan() - if success: - rrt.controller.execPath(rrt.path_to_goal) - print("Planning complete. Moving the robot to the goal.") - else: - print("Failed to find a path to the goal.") + import argparse + + parser = argparse.ArgumentParser( + prog='arm_rrt', + description='Plans and executes paths for arms around obstacles.') + parser.add_argument('--filename',default='rrt_path.npy') + parser.add_argument('-p', '--plan',action='store_true') + parser.add_argument('-e', '--exec',action='store_true') + args = parser.parse_args() + + if args.plan: + rrt = RRT() + success = rrt.plan() + + if success: + print("Tree planning reached the goal.") + np.save(args.filename,rrt.path_to_goal) + else: + print("Failed to find a path to the goal.") + + rrt.controller.visTree(rrt.start_tree,rgba_in=[0.5,0.0,0.5,1.0]) + rrt.controller.visTree(rrt.goal_tree,rgba_in=[0.902,0.106,0.714,1.0]) + + if args.exec: + path_to_goal = np.load(args.filename) + rrt = RRT() + rrt.controller.execPath(path_to_goal) \ No newline at end of file diff --git a/pybullet/gen3lite_controller_collision_detection.py b/pybullet/gen3lite_controller_collision_detection.py index 283f39a..fb73398 100644 --- a/pybullet/gen3lite_controller_collision_detection.py +++ b/pybullet/gen3lite_controller_collision_detection.py @@ -5,6 +5,82 @@ from enum import Enum import numpy as np +def draw_cylinder_between_points(p1, p2, radius=0.005, color=[0, 0, 1, 1]): + """ + Draws a cylinder in PyBullet between two 3D points. + """ + # 1. Calculate center position + center = [(p1[i] + p2[i]) / 2 for i in range(3)] + + # 2. Calculate distance (length of cylinder) + dist = math.sqrt(sum((p1[i] - p2[i])**2 for i in range(3))) + if dist == 0.0: + return None + + # 3. Calculate orientation (rotation from Y-axis to vector p2-p1) + # Default cylinder is aligned with Y-axis + v1 = [0, 1, 0] # Default orientation + v2 = [(p2[i] - p1[i]) / dist for i in range(3)] # Direction vector + + # Find axis of rotation (cross product) and angle (dot product) + axis = np.cross(v1, v2) + angle = math.acos(np.dot(v1, v2)) + + if np.linalg.norm(axis) < 1e-6: # Check if vectors are parallel + if v2[1] < 0: # If opposite direction + q = [1, 0, 0, 0] # 180 degrees + else: + q = [0, 0, 0, 1] # Identity + else: + axis = axis / np.linalg.norm(axis) + q = pb.getQuaternionFromAxisAngle(axis, angle) + + # 4. Create visual shape + visual_shape_id = pb.createVisualShape( + shapeType=pb.GEOM_CYLINDER, + radius=radius, + length=dist, + rgbaColor=color + ) + + # 5. Create multi-body to place in the scene + cylinder_id = pb.createMultiBody( + baseVisualShapeIndex=visual_shape_id, + basePosition=center, + baseOrientation=q + ) + + return cylinder_id + +def get_quaternion_from_two_points(start_point, end_point): + """ + Calculates a pybullet quaternion to orient an object's forward axis + (assumed to be the positive X-axis) from start_point to end_point. + """ + # Convert points to numpy arrays for easier vector math + start_point = np.array(start_point) + end_point = np.array(end_point) + + # Calculate the direction vector + direction_vector = end_point - start_point + + # Normalize the direction vector + length = np.linalg.norm(direction_vector) + if length == 0: + return [0, 0, 0, 1] # Return identity quaternion if points are the same + + unit_direction = direction_vector / length + + # Calculate Euler angles for alignment (assuming forward is positive X-axis) + # The math might need adjustment based on the object's default "forward" direction in its URDF/model + pitch = math.asin(unit_direction[2]) # Z-component + yaw = math.atan2(unit_direction[1], unit_direction[0]) # Y and X components + roll = 0 # No roll needed to point at the target + + # Convert Euler angles to a pybullet quaternion + quaternion = pb.getQuaternionFromEuler([roll, pitch, yaw]) + return quaternion + class Gen3LiteArmController(object): """ A controller for the Kinova Gen3 Lite robotic arm in PyBullet. @@ -41,7 +117,7 @@ def __init__(self, dt=1 / 50.0): self.__upper_limits: List = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05] self.__joint_ranges: List = [5.8, 4, 5.8, 4, 5.8, 4, 6] self.__rest_poses: List = [0, 0, 0, 0, 0, 0, 0] - self.__home_poses: List = [0, 0, -0.5 * math.pi, 0.5 * math.pi, 0.5 * math.pi, -math.pi * 0.5, 0] + self.__home_poses: List = [math.pi, 0, 0.5 * math.pi, 0.5 * math.pi, 0.5 * math.pi, -math.pi * 0.5, 0] self.joint_ids = [pb.getJointInfo(self.__kinova_id, i) for i in range(self.__n_joints)] self.joint_ids = [j[0] for j in self.joint_ids if j[2] == pb.JOINT_REVOLUTE] @@ -64,12 +140,12 @@ def getCurrentJointAngles(self): return angles def createBalloonMaze(self): - balloon_collision_id = pb.createCollisionShape(pb.GEOM_SPHERE, radius=0.1) - balloon_visual_id = pb.createVisualShape(pb.GEOM_SPHERE, radius=0.1,rgbaColor=[1.0,0.0,0.0,1.0]) + balloon_collision_id = pb.createCollisionShape(pb.GEOM_SPHERE, radius=0.11) + balloon_visual_id = pb.createVisualShape(pb.GEOM_SPHERE, radius=0.11,rgbaColor=[1.0,0.0,0.0,1.0]) for y in [-0.125, 0.125]: for z in [0.25, 0.5]: - box_id = pb.createMultiBody(baseMass=0, basePosition=[0.3, y, z],baseCollisionShapeIndex=balloon_collision_id, + box_id = pb.createMultiBody(baseMass=0, basePosition=[0.4, y, z],baseCollisionShapeIndex=balloon_collision_id, baseVisualShapeIndex=balloon_visual_id ) @@ -89,6 +165,40 @@ def createBalloonMaze(self): goal_visual_id = pb.createVisualShape(pb.GEOM_BOX, halfExtents=[0.05,0.05,0.05],rgbaColor=[0.0,0.0,1.0,1.0]) box_id = pb.createMultiBody(baseMass=0, basePosition=goal_state[0], baseVisualShapeIndex=goal_visual_id ) + def visTree(self,tree,rgba_in): + + pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0) + + for n in tree.node_list: + parent = n.parent + if not parent is None: + self.set_joint_positions(n.point) + n_pos = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) + + self.set_joint_positions(parent.point) + parent_pos = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) + + draw_cylinder_between_points(n_pos[0],parent_pos[0],color=rgba_in) + + #q = get_quaternion_from_two_points(n_pos[0],parent_pos[0]) + #height = np.linalg.norm(np.array(n_pos[0])-np.array(parent_pos[0])) + #branch_vis_id = pb.createVisualShape(pb.GEOM_CAPSULE, length=height,radius=0.005,rgbaColor=rgba_in) + #box_id = pb.createMultiBody(baseMass=0, basePosition=n_pos[0],baseOrientation=q, baseVisualShapeIndex=branch_vis_id ) + + self.set_joint_positions(self.__home_poses) + pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) + print("Finished plotting tree") + while True: + pb.stepSimulation() + time.sleep(1./240.) + + keys = pb.getKeyboardEvents() + + # Check if 'q' (ASCII 113) is pressed + if ord('q') in keys and keys[ord('q')] & pb.KEY_WAS_TRIGGERED: + print("Quit key pressed. Exiting...") + break + def set_to_home(self): """ Resets the arm to its predefined home position. @@ -101,7 +211,9 @@ def execPath(self,path): pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) for p in path: - self.move_to_joint_positions(p) + self.set_joint_positions(p) + time.sleep(0.25) + #self.move_to_joint_positions(p) def move_to_joint_positions(self, joints, max_steps=20): """ From 6e9ab62332368c3cae936e91becce21e10b3c19a Mon Sep 17 00:00:00 2001 From: DavidMeger Date: Thu, 29 Jan 2026 09:24:28 -0500 Subject: [PATCH 3/7] Updated gitignore --- .gitignore | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..2457d35 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +gpt_rrt.py +pybullet/gettingStarted.py +pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-313.pyc +pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-313.pyc From 9492e6cdcb91f5f4a1cd250ccf33443787c841cf Mon Sep 17 00:00:00 2001 From: DavidMeger Date: Thu, 29 Jan 2026 09:24:49 -0500 Subject: [PATCH 4/7] Git ignore. --- .gitignore | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.gitignore b/.gitignore index 2457d35..fd095a2 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,9 @@ gpt_rrt.py pybullet/gettingStarted.py pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-313.pyc pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-313.pyc +pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-313.pyc +pybullet/hardest_home.npy +.gitignore +pybullet/davespath.npy +.gitignore +pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-313.pyc From a6414f2c18383e6982d508aaa772f54c987906a4 Mon Sep 17 00:00:00 2001 From: David Meger Date: Thu, 29 Jan 2026 13:26:14 -0500 Subject: [PATCH 5/7] Final stage changes for Daves planning solution. From here, will strip down and deliver to students. --- .gitignore | 1 + ...troller_collision_detection.cpython-38.pyc | Bin 9638 -> 6720 bytes pybullet/arm_rrt.py | 85 +--- ...gen3lite_controller_collision_detection.py | 399 ++++-------------- pybullet/rrt_path.npy | Bin 0 -> 4384 bytes 5 files changed, 109 insertions(+), 376 deletions(-) create mode 100644 pybullet/rrt_path.npy diff --git a/.gitignore b/.gitignore index fd095a2..0ec87af 100644 --- a/.gitignore +++ b/.gitignore @@ -8,3 +8,4 @@ pybullet/hardest_home.npy pybullet/davespath.npy .gitignore pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-313.pyc +pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-38.pyc diff --git a/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-38.pyc b/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-38.pyc index 333e8db8ae663ea83a1347913fcdad45133fe15f..eb47e6b8352499500a279f6441de13c949e67bc5 100644 GIT binary patch literal 6720 zcmai3TWlQHd7j(u&R)2@cqNLoB99&4wvidfb{j`=yrg&wV~LVU%CQDA2FpE%!y%V5 z%QLf*$Sm_B8*$>ONCD>|L6DFyZuHQgL0^KRfQ0s?1@aIyFX=qB2nt6@5VS!MBu)|a z`_IhsW-^(@xz73j`OoG5&j0^=rKhK$;JLN)1#j$8MfoW;_CE$1qevlTOkrwMu~k`W zwkAv6)=_FrV=ZH6)MQN7&P^*!XU3ZfGq`z6xARP$SE`x&;G)b{)!4Y;g;9fA`yU;R zQKWDY8JAzeS5$oAl;oz`2FtMQn~I%bIcB2FvOFuG%&{UXp)^?!E2GS_Ue<@Q!1~z$ z$|8G+RZy1L33d`?4;zF$L-C1e?mvIQi};vW8xQ68dhDbS)OlQ-ZFxw&S3CVXsraEABAVFwct9J-P~u-@TDc*hyo#5f(%?d!I{+xLND?HKeXyT zvjX99KdL7k?a=dG2y~r`kXx&s&)4v#0nT!0i;Fshry04zYoR$JnZN&q%J-)KYxCV& zL&F|)AKDlbiOTScs-kq%w-rZ;)NL(NSM(iKrMkgX{InfiwlZ%kD@yt%i%~hI?r8E2 zwajj7C6BEoI@QI2VVT=HI2XXJ2>OD2lf@f3g5_L;xHA)DN;2=E13;UPgspdTcvGg$ zr;SI#w#&X=+W6fxDEsZVe~5KJK62o8}Y{JEv(%3AkDF(A+L0_A%*7lHFbUTZDsq^jzVj- zGQ>`^kFYZsG0YyhZIFWD;99c+o1nPg{02QAy|;PV#d&PvI67^?mOf4`hngeQT&Svc zsU7ku?mh$1wDP%6T<+(Px$iXJ|HGaC{ng$3e|+%ZflFtg^1a+WckTyg-ulI> z|5|fp`2*#iyCfg;UYDYFpH51bVl<`DM!OWFU5ZindHD>E-X~bOpGB5R@}IQ^Nz#3Y zA-#PFDI7-TDjnsGfh{#swz0G;S_enzZ?tt|Ub&-G^;l~y+9j7qpCjFw@t1=5QRhg- zb{r@I)YtJ?Rg1NeXJgg712hVGb4m2zLmzB3&Rx3_LT9eAI<)oLC7(|S532Z#b+Iw+ zHJjEIA58(h+qgEq%p0pgI|{FLm;ag826WrM*4kKb$X`H&JRE9$By47AiCC0W+0Ch{04PZ{cE)a~-;F3rp?ICGP)UAk~(VG`Ya z=#JGDxpGNXA)*t=opBZ7lFDO6XcF`r%91L1f8l+NVT)a3ZWr8#E?Y+kpm<4&gUB+wr} ze%y#=aDxk|^_mxj$B$|gn5U!cV|mGMIil{nJhUG>?u(dU~Scax~@v)!tliR~`%%Qy*muDm{xk0_z zUFWJU2phy8W&MZz!$LwOV>Cowj zAA*MX1Xzu?g@BzRkHVNFdl8=sr6H;sY5|+nr*3}i*m-}L{(!BLz-GDzPl9sfZO}bOXQy5w`;ym};1WJ&d&W`vfWu)%uRP9nSy=21O$$CEH zH)D1E@O;Yvhg{JuU(NjD~a$tYyjFAl{_m|MOqioAFMmxAS3r&0+?}SC! zQ74T+(TM_w6jriI&)TEM3e+8aV5LbCT5ZYVB~yY7d(R`j$@rYwwE z*I?P(`j*CWTRJmw1fSe8SRUo+tqevQtkBUrMknJ!89N#)-Zr+f>*v;=V+9=3F89(Q z?j`bt4sh>DIhX$o=Xck?x@#K4FlHy*Tt{&-*p%#!vW5CK(n>~dkFsr|o<}MGEX1HX#O5Nd z=ZkQ^;u6)m7l=JbEwD@svH9hCi_b_MuF#9bGT7@)E!Tgc&n8CfkMDLX)R)-d|5i); zep+*<{^_s3`^eNk*4)qhUE$m9fByHo?q7WU>7V@k_21OoFMsvFzw^!?zFTwOy8PnL z-u>}E)ZEq&=ij@k{Nk=VSnT`B(#3DwO&Q(+Z@ai$Z!S&Vi~yOTI7h3Zi%-)uN#n$4 zkX3sUoUSA{>cLsX#p%gQ7oGXbpPihW>|VP2<`?%SR2SRL$U7gfjo6@@R-7g71S7b4 zd33?^SNG6gJWMa2KxUhg`}s?+R}1ncu(5vfLf>cQ;!f!A_Fk7Z7wqp?1zv8%>ZYAt z3~o|~(ZI!E7wGGbygcl(tnZQ>(rocK1eiq%>82w@9o7t}wTYVcpkNr(v)9g4bWk?D?r0aNIgvJWSI{A zDxy_pY6tNaXls!f=%W$6X%8G~^k;!snYZ9@GOFcnBW|<)!us$ow=6U9gJTU%mRvg; zr2f`cPek*Biz~0wuWXg2Y(1TFVnV<>+b=muN3tZ}Ol9emS#eNFxU4GTgDwZ^J(5eW zls@6nOVTH@0c}dVvVXAtbMWbtE;BqtPykEie)Ka)3rAYn9*}Pz+ODv2y2>Yjd417I z)_Y6s^sQ#Uri%AEXmwd;%zusaoXX04jBi%rB{rbMvz`c>P`Q)3~ZYKf{6)aItoCoewlv1l1yhPTAYXAxuVi#P zC-jT;KmbZ!nY`+Jer(=Zn46iN2B=L``xEHN&>Do`jLSZZO}aEf_{g?N{Xm$Tup3ab zbMyu;V$~O~(5Es!xh-}fnVYPa;JTIfLvMq^AbWVBft2AxT|IUcozAD-K#rOw45Xr8kcM4MaQ5(L{o06H~+EX1jka1>`} zttrHlc*69C7xE-FOdMd4WM~RwNpU{G!G)=+nb;sh2tFWGj$nGqccL5Ol6WG+bhGU> z7#bb|553TFvN;e_TW|9jX7`bQ2vDjw5a*#lc!Dc4*v-`clJ36UBNx*lNu+)vutPM^ zLrHYdvlg#`{QF4BIm??mi~zMyuRxPFpEz274xMTIXCaZdTBgu~kC24p#0AG$3s}2J z^^)VPBbb!F$vF-S8jd5#{}fMCwrfvG_!ECcePqChE0kTOjKtihExVBcLAshl3Qr<~ zc4P}r0RwWTf!s8oFkdhWa!j>go88D`g60xV&wwhlAS`@$A;N6Nb~G8JA<^Jon_~c4loOAI3FLIcKugpy~rKo zwei4@cu=jny!q3Ng=fqvcdM+`fLqa;YrWz5!R?Yo{VeKe@n9v0ys}l|HOup@h0SXX z;D|;t0>|>27h2_NDGaST4{m$R4Xv6RtpzN!qM$Q?zzhU0hSqxE`B8Y$ss*=QQSJKd zlDoRUjJG4qZvjXn9epHebjnMNen9yyPhpu(Y^W7T0lz~|^ zt-HL0$TsI%8s)BqP$BKh}sEbnOJ!M^yU)HlYX!@3@=*(p42ZSky9(`z& z6JHE`5iMAzVh{!~LO$weS+vRjzQS^Upx?tdL>jCgWYNzC*^x>Lm@9$5h^YhR6ExQ-6`*`%dV9|cZWJRlT$$hl-4|BmCBI^3YFyAfA-x^-u$c4ib%h&JZ{g?eB9|Vx2bB) zL%Z7PakbOqYUNe&oVdC}5^yI;-SFc1(TD97|7f&KlkPl(nVKEN8x8|hl$LVy$d(!@ z+c>0kt))KHep}lxmXt>dGc@}*ey z9zp(vZgrIpVxa%o9KU?ydI(Ou!Ag(;H|BkJg46b0vgXRuUbSjn_fh2F=FJ=9Yi{{g z(1^ktoddd5--N{QZ`3zegly@4aC!m?*=p4bAq*VmMvxZBjnp^e%xkEP%0aDuWrP+^ zyOmTlrTNN%YZ-AI&-Wt78F{T5luOm{3Njr8Pi-e5TOzE_0f_A!-cnmoKc|#PSy!Pl z43yMUirm(M#{1Av&>`|ikhk++pPwl%JByQV&Cgz6o<#K!s$+Fs?3@HAsnaO{C_p8o zGo%&dmBP7LV^KUfJ2|!NOwAOhCl?)oE|W>*~h_mZ#GcoKInLg2erut6my(q?rNz~jU4E@_#{qb)vGkP zJK?T0DsOpVqg3^pXf`xH|CTdZ9J@9<3ChA5E`m4;IzNFU;m;5_Mu2#eA16Q*7MmT~ zA3!Pm5`dy6wWKP3dQ!`&c{OL8RL`RBh(4ss--tGhQeGWK+C2N&OtpuBNH&p78pj*v z09sIqc-2)65=bqA70#oUzd%6DQp{HXG2b*cX;B>2R|y5-Qq<zzMu! z4z%Km0tUuBCX|~N)RMZbp`^F8huW6jQn!tkPV7vIdn3~#J5Vc1c24k%=q`D>uOl_Z zkqSnLlO+(K&kBY(1Qh*B74)Nd%+5-32|!VGA6qH5{3$eU91{v z2FEg>HeWt;-B0)5vj?Os$+!`F@DVa3y70m(W!eLc^uxN|F2lgVPp`m6eV}h?EQx*k z!j{fbNKbDWXl0NI)6!coSz2mKW0?oWR$}89Hb%&L?V2Zpl~hY{5-nvr`B2$PB1L&U zAq*mA+eAK%H-lYAJ-#L77cq}4J3>F0L}D&tHb*fVQ;yz()qF~6nQRDk31Jo-D?X80 z|IbFDmy*21yhhkol0P2hw2>e-+SEABD=Ve(pc?Q;T3eq!?&#CUqusiXMmaHwIQl12 z6aQeeV8rGMB;6dW-8e_Nj?9ZsPzh+IhFV%H)!iANxp(6s`XD*CS6i9-i{JX*xv4)N zt$giIGJn|k%fI`u^3v~I`1s%6{>f8ZA`k*J$Q++I*5Ka#FqzmAMN7 z+&jrQJ3To+=PWI}KDjt)56ULHrW^OmsyWCc?^?h%W1|w3s+<@yJ_^Y%g*b^nX{g6! zQOtV&tsZM|3yp1440LV&?Lt~`h>e}8Rs*Q>o?-{D40dX73uy%_N3&Q_E=sT1L0j z=8FgOo7|6G*!J)m$*2@w!D}xe6A6h&hVo6A4w1I4gYIE85I3|)(&!{`4~XKjMFQSOH$fq#Jpra+x8vi0_Mhf ziKZ*B69{*d-8k2wndy?}$GO`sd;(a!RI;;}@LTN){)-r@Sm@u!Tk$b)fKc0W0iqY< zGMe&=GzbP@fzK?@%om;U`Qq~8{A`@3LrAF-8-BoRaiYX|34%6) z$sH%cs_WK;O;<=s!I_ksAE&!JDy+R&y~P(W%~;)UWN>m}*71g9Zz*OTMunkjc$0wQ z_upnN2l_YF(}t-UTJzGu41Wm2?fe*M+mV&~qh&Ixna&{pX-OwJu-V__P8O%-wS&KVA z0mt$wAe72D#owm4S%R|t^b*X*bSNqDS7Q{DX8wSt8%?ih*M>dR5PMNd_%x)18Mi{#t5SF%&MKdGuC*932Q0_$n!ccmd zat?uCB~T);N}xhOIMlP0TOr^QAk%@)%Qd^0= zM=%Y#LCA62bX{rL#@Y6X6K~mloq|APXJH3+P2jXFP_uTHMirJ_&%7J#NbhoqM_|g5 z|30P_z7C+|Nh6R>Ag$2O|5vgv4|Dh_ZShkxYpOYY0B-4VopeSLd8AtAP^0;&kq&q&wT_}_Z`6t-6I$$#r9cez&R$y_inz_hM&Fp=cwD~ zZ$1yH-9EPI69aTM{)cGs)W*vLXm9-U2XFiyEPK2q11OVt#|bqFCO8DR0)bz2@`kpB zxKuAn)zXE(Xp)D-Uqw@dB+@Q4m$>cP=kK*cQFgx&_dOTA6OZylfI{8aw_xFKJ)ybK z#K=W%77kt#xqtg`@9~Xvbf;G=jNCTi2Ew8e{*C$&Q9p8gMXhlCkfYj*RLc+~Ea=6l zWq7P_pf4g3bZ^j$7IeHkt^Q@iXJ6U3ia6g3E39(Y{RAjNGDa1MDVVs3su66SKX_?- zJMUo>+7+^J#i|yOq3abPXEfyWhqw{L(S23OA2|B3CsE(FZ#AYO;HD$UsNFKdsVGHO zEz@rqdSOH80Zjn!f+tIc~ zc-!NFUxV#%v5U3@x6j4J3{twsxwH%S1&I(&3C3OXqKIxbWf!d}+`qVT!gsvT?b$F0Y$a#~V=}oWW3bCp z1yic~Q;7A$gTs|}8}CHtN=QgoajgYQ1_3J%c1=HgUSv0M2er{~XP7-eL033yxZ_(8 z*U{2r)2puEW*Vu9`^0)3nh8QrHb_eJ0V~lR_CAW^o1lgN#4E`xZ9*$%)B(iNO}+WT zA!N3fq>BZW!@E_G{4ymPzlDTXDL+Wzd%uLM1}DxqPAy=#FhD-%IIwlA?Jp_EVL{n( zI7MCfR|rt3j=xEOh?T!ZfE+@;L7+k4HUVNX-XcH?*|j(!4@uW>l2RsK5j+F?q%s#w z-AtLocvG2YGgr+#eukM5Ez4-xeP~(KRdCG*$VAFLWlM>0dq&zAoZpU#AgnBWGU#%Z zC4{D?AoN$ZR1t)J3EX6?C-5f4T{?+J170KX6#T4~hOluKfnkF}(h;nPZP);WB}%iL z;LPLjCXMZk;MnXoLe|1AxQmv36spFk`BqZ+YsPk-TCsjeqJidL3!)eQcK{KWEY_&i zHWAeTldU#=mnCE%`n%)Jh|=1j-6-Mnh{e zXjmZy^~hlmz1D&|6!;5W`8S3R1tA(!qkm5T$HmTn8@e;V{ZIswmw$rjab54mRE*es zDYFc#pJuTKYdd0siztdO;`7MJNG}dfwlJQLN1Nxm-Mb73H?MOki#}J-j$b7(O5id; z`(FNsqn!Zt1brcL@CIJQ=Rxsoj5deG{qQc4!G(-**=0mG`vqkQn_JQ^-Iz+fH%&P* zX88;OLG~|GZl9?|)E#HwkYTlS-&sh-ncl+r9CaxI*r=_7_bKYk=c$%rrgqK?dpc+r zmk@){ZK3Dq-~feW?kR~R?BgNBY=@YKVVjYWkV846@%z*ROig_pr!Zifo%?UOpjCY5 z7pPG(+N|R=IN}8g*9njkiVf2IoODr~K(4+i{F68ttgK^Qa;Inz{!Icsv!=_?vH6;m dBUdnGC8Rqzvga{pnx{7KE#+HE(mZRb{|`!U2TA|{ diff --git a/pybullet/arm_rrt.py b/pybullet/arm_rrt.py index a46699d..7cb6ddd 100644 --- a/pybullet/arm_rrt.py +++ b/pybullet/arm_rrt.py @@ -1,21 +1,17 @@ import numpy as np from tqdm import tqdm from scipy.spatial import cKDTree -import matplotlib.pyplot as plt import random - +import argparse import gen3lite_controller_collision_detection -# ----------------------------- -# RRT Node -# ----------------------------- class Node: def __init__(self, point): self.point = np.array(point) self.parent = None class Tree: - def __init__(self,node_list,kdtree): + def __init__(self, node_list, kdtree): self.node_list = node_list self.kdtree = kdtree @@ -26,31 +22,18 @@ def add(self,new_point): data = [n.point for n in self.node_list] self.kdtree = cKDTree(data) -# ----------------------------- -# RRT Planner -# ----------------------------- class RRT: - def __init__( - self, - start=None, - goal=None, - obstacles=None, - rand_area=[], - step_size=0.1, - goal_sample_rate=0.1, - max_iter=5 - ): + def __init__(self, step_size=0.1,max_iter=50000): self.controller = gen3lite_controller_collision_detection.Gen3LiteArmController() - self.controller.createBalloonMaze() self.start = Node(self.controller.getCurrentJointAngles()) self.goal = Node(self.controller.goal) self.rand_ranges = self.controller.getRanges() self.step_size = step_size - self.goal_sample_rate = goal_sample_rate self.max_iter = max_iter self.start_tree = Tree([self.start],cKDTree([self.start.point])) self.goal_tree = Tree([self.goal],cKDTree([self.goal.point])) + self.path_to_goal = [] def add_node(self,tree,target=None): if target is None: @@ -66,16 +49,10 @@ def add_node(self,tree,target=None): else: return None - # ----------------------------- - # Main planning loop - # returns: True when a path has been found - # False if we have no path by the set number of iterations - # ----------------------------- def plan(self): for k in tqdm(range(self.max_iter)): if k % 2: - #print("Adding to start") new_node = self.add_node(self.start_tree) while(new_node is not None): @@ -83,14 +60,10 @@ def plan(self): if ret == None: break if self.reached_goal(ret,goal=new_node): - print("Plan found!") - print(new_node.point,ret.point) self.path_to_goal = self.extract_path(new_node,ret) - print(self.path_to_goal) return True else: - #print("Adding to goal tree") new_node = self.add_node(self.goal_tree) while(new_node is not None): @@ -99,35 +72,21 @@ def plan(self): break if self.reached_goal(ret,goal=new_node): - print("Plan found!") - print(ret.point,new_node.point) self.path_to_goal = self.extract_path(ret,new_node) - print(self.path_to_goal) return True return False - # ----------------------------- - # Sampling - # ----------------------------- def sample(self): - if random.random() < self.goal_sample_rate: - return self.goal.point point = [] for i in range(0,len(self.rand_ranges[0])): point.append(random.uniform(self.rand_ranges[0][i], self.rand_ranges[1][i])) return np.array(point) - # ----------------------------- - # Nearest node - # ----------------------------- - def nearest_node(self, point,tree): + def nearest_node(self, point, tree): _, idx = tree.kdtree.query(point) return tree.node_list[idx] - # ----------------------------- - # Steer - # ----------------------------- def steer(self, from_node, to_point): direction = to_point - from_node.point distance = np.linalg.norm(direction) @@ -142,50 +101,35 @@ def steer(self, from_node, to_point): new_node.parent = from_node return new_node - # ----------------------------- - # Collision checking - # ----------------------------- def collision_free(self, p1, p2): return self.controller.collision_free(p1,p2) - # ----------------------------- - # Goal check - # ----------------------------- def reached_goal(self, node,goal=None): if goal is None: goal = self.goal return np.linalg.norm(node.point - goal.point) < self.step_size and self.collision_free(node.point,goal.point) - # ----------------------------- - # Path extraction - # ----------------------------- def extract_path(self, start_node,goal_node): - # Build the start-tree path backwards + # Build the start-tree path, leaf to root (backwards) start_tree_path = [] while start_node is not None: start_tree_path.append(start_node.point) start_node = start_node.parent - - print("Start path",start_tree_path) - - # Build the goal-tree path forwards + + # Build the goal-tree path, leaf to root (forwards) goal_tree_path = [] while goal_node is not None: goal_tree_path.append(goal_node.point) goal_node = goal_node.parent - print("Goal path: ", goal_tree_path) + # First add the start path, reversing overall_path = start_tree_path[::-1] + # Add the goal path overall_path.extend(goal_tree_path) - print("Overall path",overall_path) return overall_path -# ----------------------------- -# Example Usage -# ----------------------------- -if __name__ == "__main__": - import argparse +if __name__ == "__main__": parser = argparse.ArgumentParser( prog='arm_rrt', @@ -205,8 +149,11 @@ def extract_path(self, start_node,goal_node): else: print("Failed to find a path to the goal.") - rrt.controller.visTree(rrt.start_tree,rgba_in=[0.5,0.0,0.5,1.0]) - rrt.controller.visTree(rrt.goal_tree,rgba_in=[0.902,0.106,0.714,1.0]) + rrt.controller.visTreesAndPaths( + [rrt.start_tree,rrt.goal_tree], + [rrt.path_to_goal], + rgbas_in=[[0.5,0.0,0.5,1.0],[0.902,0.106,0.714,1.0]] + ) if args.exec: path_to_goal = np.load(args.filename) diff --git a/pybullet/gen3lite_controller_collision_detection.py b/pybullet/gen3lite_controller_collision_detection.py index fb73398..b1ba3f9 100644 --- a/pybullet/gen3lite_controller_collision_detection.py +++ b/pybullet/gen3lite_controller_collision_detection.py @@ -1,133 +1,52 @@ import pybullet as pb import time import math -from typing import Dict, List, Optional -from enum import Enum +from typing import List import numpy as np -def draw_cylinder_between_points(p1, p2, radius=0.005, color=[0, 0, 1, 1]): - """ - Draws a cylinder in PyBullet between two 3D points. - """ - # 1. Calculate center position - center = [(p1[i] + p2[i]) / 2 for i in range(3)] - - # 2. Calculate distance (length of cylinder) - dist = math.sqrt(sum((p1[i] - p2[i])**2 for i in range(3))) - if dist == 0.0: - return None - - # 3. Calculate orientation (rotation from Y-axis to vector p2-p1) - # Default cylinder is aligned with Y-axis - v1 = [0, 1, 0] # Default orientation - v2 = [(p2[i] - p1[i]) / dist for i in range(3)] # Direction vector - - # Find axis of rotation (cross product) and angle (dot product) - axis = np.cross(v1, v2) - angle = math.acos(np.dot(v1, v2)) - - if np.linalg.norm(axis) < 1e-6: # Check if vectors are parallel - if v2[1] < 0: # If opposite direction - q = [1, 0, 0, 0] # 180 degrees - else: - q = [0, 0, 0, 1] # Identity - else: - axis = axis / np.linalg.norm(axis) - q = pb.getQuaternionFromAxisAngle(axis, angle) - - # 4. Create visual shape - visual_shape_id = pb.createVisualShape( - shapeType=pb.GEOM_CYLINDER, - radius=radius, - length=dist, - rgbaColor=color - ) - - # 5. Create multi-body to place in the scene - cylinder_id = pb.createMultiBody( - baseVisualShapeIndex=visual_shape_id, - basePosition=center, - baseOrientation=q - ) - - return cylinder_id - -def get_quaternion_from_two_points(start_point, end_point): - """ - Calculates a pybullet quaternion to orient an object's forward axis - (assumed to be the positive X-axis) from start_point to end_point. - """ - # Convert points to numpy arrays for easier vector math - start_point = np.array(start_point) - end_point = np.array(end_point) - - # Calculate the direction vector - direction_vector = end_point - start_point - - # Normalize the direction vector - length = np.linalg.norm(direction_vector) - if length == 0: - return [0, 0, 0, 1] # Return identity quaternion if points are the same - - unit_direction = direction_vector / length - - # Calculate Euler angles for alignment (assuming forward is positive X-axis) - # The math might need adjustment based on the object's default "forward" direction in its URDF/model - pitch = math.asin(unit_direction[2]) # Z-component - yaw = math.atan2(unit_direction[1], unit_direction[0]) # Y and X components - roll = 0 # No roll needed to point at the target - - # Convert Euler angles to a pybullet quaternion - quaternion = pb.getQuaternionFromEuler([roll, pitch, yaw]) - return quaternion - class Gen3LiteArmController(object): """ A controller for the Kinova Gen3 Lite robotic arm in PyBullet. - This class provides methods to control the arm's joints, move the end-effector - to desired positions and orientations using inverse kinematics, and operate the gripper. + This class provides methods to control the arm's joints and interact + with it surrounding environment through collision checking. """ - def __init__(self, dt=1 / 50.0): + self.dt = dt - - self.LEFT_FINGER_JOINT = 7 # Example index; update if needed. - self.RIGHT_FINGER_JOINT = 9 # Example index; update if needed. - self.GRIPPER_OPEN_POS = 0.7 # Adjust as needed. - self.GRIPPER_CLOSED_POS = 0.0 # Adjust as needed. - - # End-effector link index as used in your URDF. - self.END_EFFECTOR_INDEX = 7 - pb.connect(pb.GUI,) pb.setGravity(0, 0, -9.8) pb.setTimeStep(self.dt) + pb.configureDebugVisualizer(pb.COV_ENABLE_GUI, 0) + pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0) # Load the Kinova Gen3 Lite URDF model. # Ensure the path "gen3lite_urdf/gen3_lite.urdf" exists in your directory self.__kinova_id = pb.loadURDF("gen3lite_urdf/gen3_lite.urdf", [0, 0, 0], useFixedBase=True) pb.resetBasePositionAndOrientation(self.__kinova_id, [0, 0, 0.0], [0, 0, 0, 1]) + self.END_EFFECTOR_INDEX = 7 - self.__n_joints = 7 # pb.getNumJoints(self.__kinova_id) - 5, where -5 for the gripper - print(f'Found {self.__n_joints} active joints for the robot.') - - # Joint limits and rest/home poses. + self.__n_joints = 7 self.__lower_limits: List = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05] self.__upper_limits: List = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05] - self.__joint_ranges: List = [5.8, 4, 5.8, 4, 5.8, 4, 6] - self.__rest_poses: List = [0, 0, 0, 0, 0, 0, 0] self.__home_poses: List = [math.pi, 0, 0.5 * math.pi, 0.5 * math.pi, 0.5 * math.pi, -math.pi * 0.5, 0] self.joint_ids = [pb.getJointInfo(self.__kinova_id, i) for i in range(self.__n_joints)] self.joint_ids = [j[0] for j in self.joint_ids if j[2] == pb.JOINT_REVOLUTE] # Initialize to home position. - for i in range(self.__n_joints): - pb.resetJointState(self.__kinova_id, i, self.__home_poses[i]) - - self.default_ori = list(pb.getQuaternionFromEuler([0, -math.pi, 0])) - pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0) + self.setToHome() + + # This function creates obstacles and sets a reachable goal + self.createBalloonMaze() + + # Set the viewer's camera viewpoint + pb.resetDebugVisualizerCamera( + cameraDistance=1.5, + cameraYaw=-120, + cameraPitch=-10, + cameraTargetPosition=self.goalPosition + ) def getRanges(self): return (self.__lower_limits,self.__upper_limits) @@ -138,226 +57,125 @@ def getCurrentJointAngles(self): joint_state = pb.getJointState(self.__kinova_id,id) angles.append(joint_state[0]) return angles + + def setJointAngles(self, joint_angles): + for joint_index, q in enumerate(joint_angles): + pb.resetJointState(self.__kinova_id, joint_index, q) + + def setToHome(self): + self.setJointAngles(self.__home_poses) + + def execPath(self,path): + self.setJointAngles(self.__home_poses) + pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) + for p in path: + self.setJointAngles(p) + time.sleep(0.25) + def createBalloonMaze(self): - balloon_collision_id = pb.createCollisionShape(pb.GEOM_SPHERE, radius=0.11) - balloon_visual_id = pb.createVisualShape(pb.GEOM_SPHERE, radius=0.11,rgbaColor=[1.0,0.0,0.0,1.0]) + balloon_collision_id = pb.createCollisionShape(pb.GEOM_SPHERE, radius=0.07) + balloon_visual_id = pb.createVisualShape(pb.GEOM_SPHERE, radius=0.07,rgbaColor=[1.0,0.0,0.0,0.5]) + x = 0.4 for y in [-0.125, 0.125]: for z in [0.25, 0.5]: - box_id = pb.createMultiBody(baseMass=0, basePosition=[0.4, y, z],baseCollisionShapeIndex=balloon_collision_id, + box_id = pb.createMultiBody(baseMass=0, basePosition=[x, y, z],baseCollisionShapeIndex=balloon_collision_id, baseVisualShapeIndex=balloon_visual_id ) + x = 0 + balloon_collision_id = pb.createCollisionShape(pb.GEOM_SPHERE, radius=0.15) + balloon_visual_id = pb.createVisualShape(pb.GEOM_SPHERE, radius=0.15,rgbaColor=[1.0,0.0,0.0,0.5]) + for y in [-0.3, 0.3]: + for z in [0.2, 0.6]: + box_id = pb.createMultiBody(baseMass=0, basePosition=[x, y, z],baseCollisionShapeIndex=balloon_collision_id, + baseVisualShapeIndex=balloon_visual_id + ) + # This is a hard-coded sensible goal to try to reach, in the middle of the four balloons. self.goal = [0.1026325022237283, -0.2931188624740633, 1.2717083400432991, 0.048794139164578594, 0.07744723004754135, -0.8437927483158898, -0.024709326684397483] # Record where we were (usually home, but just to be safe...) curr = self.getCurrentJointAngles() # Move the arm to the goal, specified in joint angles - self.set_joint_positions(self.goal) + self.setJointAngles(self.goal) # Record the end effectors x,y,z position goal_state = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) + self.goalPosition = goal_state[0] # Move the arm back to where it began - self.set_joint_positions(curr) + self.setJointAngles(curr) # Now we can make a visual marker for the goal - goal_visual_id = pb.createVisualShape(pb.GEOM_BOX, halfExtents=[0.05,0.05,0.05],rgbaColor=[0.0,0.0,1.0,1.0]) + goal_visual_id = pb.createVisualShape(pb.GEOM_BOX, halfExtents=[0.05,0.05,0.05],rgbaColor=[0.0,0.0,1.0,0.5]) box_id = pb.createMultiBody(baseMass=0, basePosition=goal_state[0], baseVisualShapeIndex=goal_visual_id ) - def visTree(self,tree,rgba_in): + def visTreesAndPaths(self,trees,paths,rgbas_in): pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0) - for n in tree.node_list: - parent = n.parent - if not parent is None: - self.set_joint_positions(n.point) - n_pos = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) - - self.set_joint_positions(parent.point) - parent_pos = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) + for tree_idx in range(len(trees)): + tree = trees[tree_idx] + rgba_in = rgbas_in[tree_idx] + for n in tree.node_list: + parent = n.parent + if not parent is None: + self.setJointAngles(n.point) + n_pos = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) + + self.setJointAngles(parent.point) + parent_pos = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) + + point_vis_id = pb.createVisualShape(pb.GEOM_SPHERE, radius=0.005,rgbaColor=rgba_in) + node_sphere_id = pb.createMultiBody(baseMass=0, basePosition=n_pos[0], baseVisualShapeIndex=point_vis_id ) + + pb.addUserDebugLine(lineFromXYZ=n_pos[0],lineToXYZ=parent_pos[0],lineColorRGB=rgba_in[0:3],lineWidth=0.01,lifeTime=0) - draw_cylinder_between_points(n_pos[0],parent_pos[0],color=rgba_in) + point_vis_id = pb.createVisualShape(pb.GEOM_SPHERE, radius=0.01, + rgbaColor=[1.0, 1.0, 0.0, 0.5]) - #q = get_quaternion_from_two_points(n_pos[0],parent_pos[0]) - #height = np.linalg.norm(np.array(n_pos[0])-np.array(parent_pos[0])) - #branch_vis_id = pb.createVisualShape(pb.GEOM_CAPSULE, length=height,radius=0.005,rgbaColor=rgba_in) - #box_id = pb.createMultiBody(baseMass=0, basePosition=n_pos[0],baseOrientation=q, baseVisualShapeIndex=branch_vis_id ) + for path in paths: + for idx in range(len(path)): + + self.setJointAngles(path[idx]) + parent_pos = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) + + node_sphere_id = pb.createMultiBody(baseMass=0, basePosition=parent_pos[0], baseVisualShapeIndex=point_vis_id ) + + if idx+1 < len(path): + self.setJointAngles(path[idx+1]) + child_pos = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) + + pb.addUserDebugLine(lineFromXYZ=parent_pos[0],lineToXYZ=child_pos[0],lineColorRGB=[1.0, 1.0, 0.0],lineWidth=0.02,lifeTime=0) - self.set_joint_positions(self.__home_poses) + self.setToHome() pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) - print("Finished plotting tree") + print("Finished plotting tree. Press q to continue.") + while True: pb.stepSimulation() time.sleep(1./240.) keys = pb.getKeyboardEvents() - # Check if 'q' (ASCII 113) is pressed if ord('q') in keys and keys[ord('q')] & pb.KEY_WAS_TRIGGERED: - print("Quit key pressed. Exiting...") - break - - def set_to_home(self): - """ - Resets the arm to its predefined home position. - """ - for i in range(self.__n_joints): - pb.resetJointState(self.__kinova_id, i, self.__home_poses[i]) - - def execPath(self,path): - self.set_joint_positions(self.__home_poses) - - pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) - for p in path: - self.set_joint_positions(p) - time.sleep(0.25) - #self.move_to_joint_positions(p) - - def move_to_joint_positions(self, joints, max_steps=20): - """ - Move to target joint positions with position control. - - Args: - joints (list): Target joint positions. - max_steps (int): Maximum simulation steps to reach the target. - """ - for i in range(self.__n_joints): - pb.setJointMotorControl2( - bodyIndex=self.__kinova_id, - jointIndex=i, - controlMode=pb.POSITION_CONTROL, - targetPosition=joints[i], - force=2000, - positionGain=1.0, - velocityGain=1.0, - maxVelocity=0.3 - ) - - # Step the simulation for a short duration to allow movement. - for k in range(max_steps): - pb.stepSimulation() - curr = self.getCurrentJointAngles() - e = np.linalg.norm(np.array(joints) - np.array(curr)) - #print("Error at iter ", k, " is ", e) - #print("Target: ", joints) - #print("Current ", curr) - if e < 0.1: - break - - time.sleep(self.dt) - - def move_to_cartesian(self, target_pos, target_ori, max_steps=240, error_threshold=0.01): - """ - Moves the arm using inverse kinematics and closed-loop control until the end effector - reaches the desired position and orientation within a threshold. - - Args: - target_pos (list or np.array): Desired end-effector position [x, y, z]. - target_ori (list or np.array): Desired end-effector orientation (quaternion). - max_steps (int): Maximum number of simulation steps to try. - error_threshold (float): Acceptable Euclidean distance (in meters) between - the current and target positions. - """ - - # Calculate the inverse kinematics solution. - jointPoses = pb.calculateInverseKinematics( - self.__kinova_id, - self.END_EFFECTOR_INDEX, - target_pos, - target_ori, - lowerLimits=self.__lower_limits, - upperLimits=self.__upper_limits, - jointRanges=self.__joint_ranges, - restPoses=self.__rest_poses, - maxNumIterations=100 - ) - - # Slice the IK solution so that only the controlled joints are used. - jointPoses = jointPoses[:self.__n_joints] - - for step in range(max_steps): - # Command each joint to the desired position. - for i in range(self.__n_joints): - pb.setJointMotorControl2( - bodyIndex=self.__kinova_id, - jointIndex=i, - controlMode=pb.POSITION_CONTROL, - targetPosition=jointPoses[i], - force=500, - positionGain=0.05, - velocityGain=1 - ) - - # Step the simulation. - pb.stepSimulation() - time.sleep(self.dt) - - # Get the current end-effector state. - ee_state = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) - current_pos = np.array(ee_state[0]) - current_error = np.linalg.norm(np.array(target_pos) - current_pos) - - # If within threshold, break out. - if current_error < error_threshold: - print("Target reached within threshold.") + print("Q pressed. Continuing.") break - # Final achieved state. - final_state = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) - final_pos = final_state[0] - final_ori = final_state[1] - - print("Target end-effector position:", target_pos) - print("Final achieved end-effector position:", final_pos) - - def open_gripper(self): - """ - Opens the gripper. - """ - pb.setJointMotorControl2(self.__kinova_id, self.LEFT_FINGER_JOINT, pb.POSITION_CONTROL, - targetPosition=self.GRIPPER_OPEN_POS, force=500) - pb.setJointMotorControl2(self.__kinova_id, self.RIGHT_FINGER_JOINT, pb.POSITION_CONTROL, - targetPosition=-self.GRIPPER_OPEN_POS, force=500) - for _ in range(100): - pb.stepSimulation() - time.sleep(self.dt) - - print("Gripper opened.") - - def close_gripper(self): - """ - Closes the gripper. - """ - pb.setJointMotorControl2(self.__kinova_id, self.LEFT_FINGER_JOINT, pb.POSITION_CONTROL, - targetPosition=self.GRIPPER_CLOSED_POS, force=500) - pb.setJointMotorControl2(self.__kinova_id, self.RIGHT_FINGER_JOINT, pb.POSITION_CONTROL, - targetPosition=self.GRIPPER_CLOSED_POS, force=500) - for _ in range(100): - pb.stepSimulation() - time.sleep(self.dt) - - print("Gripper closed.") - - def set_joint_positions(self, joint_positions): - for joint_index, q in enumerate(joint_positions): - pb.resetJointState(self.__kinova_id, joint_index, q) - def collision_free(self,p1,p2): - self.set_joint_positions(p1) + self.setJointAngles(p1) if self.check_collision(): return False - self.set_joint_positions(p2) + self.setJointAngles(p2) if self.check_collision(): return False return True def check_collision(self): """ - Checks for collisions between the robot and ANY other body in the environment, - as well as self-collisions (robot links hitting each other). + Checks for collisions between the robot and ANY other body in the environment, as well as self-collisions + (robot links hitting each other). Returns: bool: True if any collision is detected, False otherwise. @@ -376,36 +194,3 @@ def check_collision(self): # If loop completes without returning, no collisions were found return False - -def main(): - """ - This is a dummy main function that won't be used for the core A2 planning but - gives you some idea for how to see the Gen3Lite Arm moving, gripper functionalities, and collision detection. - """ - controller = Gen3LiteArmController() - - # Test homing functionality - print("\nTesting Gen3Lite Arm controller homing...") - controller.move_to_cartesian([0.5, 0, 0.375], controller.default_ori) - #controller.set_joint_positions([0, 0, 0.5 * math.pi, 0.5 * math.pi, 0.5 * math.pi, -math.pi * 0.5, 0]) - - # --- COLLISION DETECTION TEST START --- - print("\nTesting Collision Detection...") - - for y in [-0.125, 0.125]: - for z in [0.25, 0.5]: - col_box_id = pb.createCollisionShape(pb.GEOM_SPHERE, radius=0.125) - box_id = pb.createMultiBody(baseMass=0, baseCollisionShapeIndex=col_box_id, basePosition=[0.4, y, z]) - - print(controller.getCurrentJointAngles()) - for i in range (10000): - pb.stepSimulation() - time.sleep(1./240.) - - is_collision = controller.check_collision() - print("Check collision returned: ",is_collision) - - pb.disconnect() - -if __name__ == "__main__": - main() diff --git a/pybullet/rrt_path.npy b/pybullet/rrt_path.npy new file mode 100644 index 0000000000000000000000000000000000000000..40d0a0ea9867925ade6248bed07640a7cf99de5d GIT binary patch literal 4384 zcmbVNX*d-8*G4L&$4==SMgEpPfBP3;sLJLYt30V?Rp~za2C0U}f%f4hg9$B*W zAp5?I8H1P^V@z3ky}i93|6l&+%kMhp`km`O=RWs!PS_da)B2|DYhGm^bGHw^}q9iaP(B8-%dttT#k@Ud#g zp}_@cZ(6JPmk$dSe&yZ4^&W#Jn#$`5X?#_COb>*kR$qwJauy4g%wjkcH5cH8Ca1T7 z+5+UpkYZa7u(0OCu}1A4cg&a)Sj!W-?l)cxX52I^UG`^z_Dw5tL^2cj^)y01y=KC5 z+qw<6i3`8+ywKpniJa!?wS?j#DZPfY1QhroV!U#d1uxy?o0hql5OV(U8|5GKV0Sdd zjXQh+zmqPfmYMq^v*F&QQ0=w9>wJ%Q{d;Cs)L1b5l#y+9a~^mts`Hv;h3I!WwK;I&gYdPusWAq4pGSycib~fBfPl z0{4A)E4eDfLC77uQ!ZeihxX^vXW1Vuz{QUKUDplh;2prZG@?j{cU!*k<`>W7aKo7r z@0ciTzmaJzYtK$d|26jlY#+Dt=P+A<6ym#$Uvg={7N)Vm=ot;Xr<&dMZRWA|7oX9X zB-eG)na)OVC{6nFzE&lkPS95`dccI7lg3w%)YCxoD-R>ioCdFOS+_NF9v{eVyiO>} zM2TH=p^gAHf@bCou{2^eD$55uXqC=`&q(^XX)z5vv^>RAzEk1UC~sIyI)@B9>3lxnew9z2&&4V9DZ%x5+0%zr{9TPKU z@|JHuYCz>k>(=s{E1=3c-_3XaD}EbtR&VxWfEO#lk&}%Mb!XnPzi^;H;nqJT!z`I- z@JQWDuJ>QuzQ0{9h`tPNO@pEWEgg8^ka@7^4hGz7I=?~iG#&mdtnw^-K?ds0*9~#6 znRvJ-;ChqCR}AIi+;g0B8H@xpCtEl7;F~S!LfcB`Ao*YS%kCX?V63epemh8p%d(yB zGyY8Mj<`Q`&$<&Y*sw>={<8?Vg0`bgM!nd|J^l9ds=i*{VIPVtrbAchWJcQn31&L^ zg6bQXSU=nrF+ACY?I#1-(_(%=MexHRPq}_fKJ2>OCP#+`sfFzIZ&v#=<~HZECP98- zi!sZUiCg@Szw_Ye#nW;kwpOm+p@A!`Ux959J#TTGH@UW|ueND#S3f!w&B_Ys&WwY5 zMCdkI87A5)<>qV>@5fr6_)ZB!7IgX~^$c|k;n|qr?16YHsCbVWk?rV^?63Lh>D_T4 z+%=>ax6NZkj}+!B4I)hKQty#pfIB}g-um1zj0Vw4!nOIcV7tG2=lOGVh?KtOQnNyY zb?1cg9zUE%u79+xQ+5nvabs?J*m@=`l#wWJDo1c>QLCf&B?(QzAAf(0N#do$by{WM z{m3yen>Xec@MEBlnuEKb_83av)}N}1q=Wyw&owzOA}&h?4pW~}Kx6KMohX9_=L3h- zinon{WQS-k{Kdcwfn8q1XNeedy4Tp#kp_Hu8;KmYq16BMp zQAu$WGG9jBjt!Z^m5-4j9=Axib!)#dQEvveC`gUusgrT##Fy{<6GR}bc}~2RMgv*X zBSv<`BjD_8s@im94rN31!gXB9$UitMXS#M85~kA3gXAYLHJO;P<2Qp+@S6o!%n8cqq1%#BehT*C39kT)p z8Z7LLyt%_*80v&#UYh(($3!02)coj4q)6^bE0P(913R?)M7~X;(JQ?m*Lwp{QEe?R zqfLWmuUm(za)%&f?&4)XDh(S46|%jPrm%nH*hBfgQTRC0YC6+3g=rT(=s{O{f#G~{ z<>fvaWE7;t9{Dr~Mqjt?DvYHeBbQXDpF55JCK?(gJRb&#ztCMbuUrI*$ci7fOy^@ji^Jzt$uOk5*6Rjr>@CRKBymRE{>mz&-{iaV;vPmD(zs| zp_S;oLWP@KrL7f+{V?((zEp@sMa>K6s;BE`@Wx5TlE^|AoD#S1aE+S54@n#{&62GU z^+e5?I8Fr}1B-@aMjxa&T0A-ao{Dz+{%9=en8nkLTatfTeudU9)>(a@S>$QIELtPd z1YeX7Xc{$BLGDScg4EhRurrj>yJJYjM)t8A)ecm&iuO%xd({jXI_fsUs#GjCD;}Mt z)WBNOu1(eXRN(&D`yyJT7i_Gml?Xp)acJt8Z90>R3UM9G_x9CbzuPzEQ3e$os-!O~ zdmtQ%keECWLj}sT*n-f`9$-pq*2J35;#^0x{RP!~%W{|6r>FCBm$7jk4sz=Yf2hl~z%bVBZ=7 znL!(YGVA;NNidw&{o$kU6nq+3!!vHw2f-O7pLa-6a27%@{hXP_>SDde;u)S;^iIc1 zn)4gJ@mJk4Qe%W$HB0aHouEK)x(iu?w+{qrgP`FP1!V=7_$T?O`1O{q$cFaU=#xGv zO6r_KjtsAp&%IO8l0 z-|!yiN!+zC6Oe0>gF$6>Vk_q+!1qZdpBk|j9FLMN=1-9^R#*I|KL0dYlokZ+77oII z5S0<1(n)lU^*K2wnTpvyAA0?-kYK;{+ay8nJ_yQuymBdf9IY1&-c#hJFlh0*nzmUw z-u{}(qis*YUEF(8+87C#_UhyPOTF}>022~%4Fos8@lj3<|FvdtI7z6jKSIG+bg`!jlkacEXkzMadaUoD35wfAV*e# ztZID{PHxh!5E&z3wCA#q_i#0Qc9%V~@BAp(-%j0Jt~&%1=2IqZ9wcPy*WC$wLq>}? zo6BAvPC}y|-75|fB%Jxd%~3wr0HLY_>Sp=FkTjOIcPqB*w!sCTC1-(mgP^!Ybi-dMz3@~*YD@kqo<8N>)!^cB zl%%rvP4OMahe|&wTuwxE^Ve8%t?mS)yY$U*mxf?-sI&#$pc}l`?KF4YL&BxB?Cn2N zh2CM5xvo_fcGvo_MUza|lc}24>||_Cw_+og&uV4$#uBjb$&KK&6oS z&CKJYSlpsbR6gW^PF}lxdjv=D=FF5wkk$x%ull~Nes3>?2c}rE>e`?*jQc?u7X^=8 zRN{SIJB-!qBNX@NzQIq6WpsQxj2xfEHg<9oVNfm4%#76q{Jf`j9GGbXRZfRn^{%UT zF0edktHUssX|jIs-wDIp`zFZTPD5DXESMUfM}lAr)-AoYLzD2?iCiZ6WuGl|>%u5H>)eH{caW&;|!Alb$`w4~Exw~La;9Qp!^-B7m3PUs4pH|#YT*Qs)5cg#iCtUDJXt0y>b>v<&nF4RvO@9-$dWboNQQ< z?i12Cpx_so%;v3)ZFsP$W!~#YDx_D4@qOw4iekCPbVhBcz(d;~wA5Ay4<-x+NtJIQ zT$+@>Tabb~PCn^!`J)*#Ib2T*#^pf6xYcvxy{%~HeokgEf(94W4@vCiuK~h8<@?u5 zU4X|GLxr%Jf)d}CbB7A*QO~7aI>4p?MoLZ agA*^m=7B@g*(6i8DHPkl-)(*R75)!gwGd7K literal 0 HcmV?d00001 From 0e3287d86f5fccbecfae1f515d03056cba63c411 Mon Sep 17 00:00:00 2001 From: David Meger Date: Fri, 30 Jan 2026 10:49:24 -0500 Subject: [PATCH 6/7] This commit syncs with the relased A2 code. Stripped planning from arm_rrt and moved it to rrt_solution. --- ...troller_collision_detection.cpython-38.pyc | Bin 6720 -> 7294 bytes pybullet/arm_rrt.py | 86 +++++---- ...oller_collision_detection.py => robots.py} | 0 pybullet/rrt_solution.py | 163 ++++++++++++++++++ 4 files changed, 202 insertions(+), 47 deletions(-) rename pybullet/{gen3lite_controller_collision_detection.py => robots.py} (100%) create mode 100644 pybullet/rrt_solution.py diff --git a/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-38.pyc b/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-38.pyc index eb47e6b8352499500a279f6441de13c949e67bc5..96e446ab41296c36aff6e73ef041d8433a388e36 100644 GIT binary patch delta 3165 zcmbtWO>7)V74GWpnd$lIo*$3x87FooP9T|;m9;m{5(Rc=?cn?o+e_j^FiUo}J=Grf zc-%9tZZEOZmKS1#wZc-^;;`K83& z*&3QjgsZOZL_wsY&l-ZTLg7uu~cujTDEJqolCsl|a41EUT{YLAgLi`OET?O)t~TQbK!C=B1+&wMo!I3u(l*rCRQX(gG2_X?e*n7LL`qg`)Cm{fcAcRd`Be`mB4W_$bX|e;onn# zL{i*{yi11pPa_X2xJ`@zE6Yp=Q8jMNUZ0t(-I}^Pd-KlR6sXgn4oH)wu&ZD_Sjq!C z#ovg|#|nW=J*M*yqN~Ka)@i%82Vj*Imcx;Kzyb|$tvTkh z6`IUq1KJI=TFrKBuU0dl=8o{cX@j{@d4S}UgrX8z{OW(+(+m7}5sg>kqvc~b_Bb+O z<@t36@rLEyGM$Fyj`H{8Yv0+IXF`V~>?Dp6UhB~5MjL|Rwr4^>;M76X>~t)LmQ}H8 zs5*_7x7v45?@K0p<$W5h-*KA>O?Dt|*jfitrLvLc#LJ$u;#^DWEj90XsWc1&+EAe|%O4Dop1s=p3x)BKav`OU*H zVHrulbP);A`>JEAel@WHs^n!IbpW;lN1|vsYV#A=j1LR^@Nkk)w z16n_W2=bn6ZWE7w_d_3JK*GeO!B|U7gDXd=swjYDE zHk)~6>}X(q4I0}(E`}}!W`+#NDo8BJ{|&=)HU#QW?En8*T?eda%vXHXk2D|@eVHb9 z)Q#v$7rW$4diU-(WYO1PGu%0s;07EZs!*3E>LMV25dwOMz(K;`rR)&-kXN#wlsg-`2S?6_Hvzs1?!CETK%``R-8YKT9@$8E zQq!QsPw-~$*rw3aWWOZZ($JRn6MkA8)i@m06hu{QCqkr^f3N%<>|`-rd63AUr%aQ3 zGSqSu<`UW-(VE}wrG~g4q}@KaF*061O+O@lW-0oJuyy~SUIaVi&BALED#1twjXWYN ze*#(;!I}*(<3{*!@+u+1ukqJ=W;4%+EQ|49_T0-AL!h4}c~;^>`LhFK2r^#V;*tr` z7yEF9zm@MhBM!gF36TzitGjBrp4GW+FR!*te44PAap+h0`}r}F;~(ZPk~yyQUYNja zA1F4xUyaun?H0wiOrS1VYwnR94Kk1w<{&A%6Ar~|#NCFW?j&F99ns+0%8;Bq#2@$m zd<0`YkUL{QFC8^vJbPHy7QohTf3?;3XF_y7Q#e6N+$mhh2^VjHiS9j`;ZF*u@<+m} z1|2@&9~UaR%;YO$=*_7SmMVDGk4zmO7>B+Dlr3Vdrw9s&&@pS6eL$-jARN Uw_%V!EtWH%xfIU8O9Q3<0oZ=Qx&QzG delta 2675 zcmZuzOKcm*8Q$4lE|*JkxqLq)%d{dt2q=jkvZJ_l=$GO;bu8D8+-3o1L1{+RN)$=Y zE)B;a%>r^^!-;_elT%R?(#1g!Z4nd!+Ea@5)D|d=-nzG*f;2@@^iZHdkmR3PS&|KN zFyBA(zxJP*fBxau)4!iG=aNZ7g5TQ4JMO7HADH)-=3z!88)3P0x^yA@Vb+vPYRQ)AVC2jmXSzIIweuo-W`1zeGCZ0${z{S8cR$wL@Lf z_8TFi=eg}>)otG()Faguw@p?-vfxdIs$D`Ge)xeVb-OTqK{)^Ho!qa^{UdVg!}@~E zcF84ln;nxs8S+54da)V3Cpi{IJDcF8{mgO}xFt?=($kc4ks5`6SysB7Vh_pQ~`dpKAcDs=kMk?3e0&G{L4L zYxNi~DFUd;)C6Liug#vn_=az#%QVSV1?2cB53rYg8JUlc1Tyxi z$riL8O0mCd4;6dDVQDL$cAVvo+x8vWXg7&xj|*r&cB3U%`t6O8|Nc+eI*C(e~OP{HYf*ZPvgZR>u8xX{r#J?;S zc9dO;kF&4zA5PWei{WlY=s33%TbcyL?bXo~2m-AEDcQy~m1f!9*sk~v-|W}13Yunr zj@_Bq52`gah>>=0Rphg9NT1`bxf@}A+Zf-y!<>6JAMfX>cC$siI{VaUyds>*^hIu5 z_Ctvzs_1!G&t-b^?SG1T#_EuYtbF9`MPPDLj@pE zeRq`v5wAr^ck?U*wcGHQ0@QsfuL7OiBny{7_9`nTj~wN>`S{1nmJVG6HVqPAT1L>J zXkqz*Jm6!6&zWcKA4&LGj#>Rl_g|>+&ZE=DZKWLGr zinRyIK*q_l(zSg96`Mdu2NAG*w&xG8zsCz9FM>Eb#5tUAM*ETz=}YU{hBScjI`rkp z27jWeBAhq8ZKRqN1W zd6>*9yO1hP+r{~YM?k6NdQg(DEH%31d>fOSL5>Tqx?Zo*+U9OnpFh^wA2J8nBlCOD z*JL}s)Mzcv-1MQ0d-OODK%w8^ANwwc69Bbjcxl*ddfW-p3zR@b*x@@!g{;?mE#E!e z!EXmDuM~8WJK(t;B;dMTaN8@QmWXRYN4Rh=fUTdKnSI?kJ^Q1Y0XJ`$J~0f`g&w80 z!M!*huRpda#y7-*92Y;8p>zTqU4-r(1dxygRYXNPGz-b!7^=!S+w(|H(dC3}AwxEl zD!R3QtH#jPGo3cuZJE*2jaI7z*z%{=gJ@k?foB z4utt5atHD!4*6q1E|?hg4UEbX`+08U#b_UNal9x!Q$GdzsZhJ05;l)=<)$TG6=^-Z z7~mK3G>st!k*^sc&93LLxfwwovfA{7mkOYMCGYTSI3Xh9zdOqo1aaVgS@gIU7=5|Ne?+S-dmVHr}M&D-> z#iMA7*~Q8e6?s_9YaS?jud<&OClkn$b8w&;p!?Wgiyuw$$0?AzhoB#RDq8|`;qi@e z7AYYg{br-|CxlG4QrV66uy-p*GrW@1^8mb@^KqK}t@2#%$(l~Dg5+Zs8>#Dv^7p)E zvbm8LhWog|0S;R?K&+V_<2;_KbdJND9Jr4i@rf^p$6NtH6uMUc(2=Sc`*lT+>MEeF o@71StgL0jkVe8$u=Xgoal + rnd_point = self.sample() + if self.collision_free(rnd_point, self.start.point): + new_start_node = Node(rnd_point,self.start) + self.start_tree.add(new_start_node) - for k in tqdm(range(self.max_iter)): - if k % 2: - new_node = self.add_node(self.start_tree) - - while(new_node is not None): - ret = self.add_node(self.goal_tree,new_node) - if ret == None: - break - if self.reached_goal(ret,goal=new_node): - self.path_to_goal = self.extract_path(new_node,ret) - return True - - else: - new_node = self.add_node(self.goal_tree) - - while(new_node is not None): - ret = self.add_node(self.start_tree,new_node) - if ret == None: - break - - if self.reached_goal(ret,goal=new_node): - self.path_to_goal = self.extract_path(ret,new_node) - return True - - return False + if self.collision_free(rnd_point, self.goal.point): + new_goal_node = Node(rnd_point,self.goal) + self.goal_tree.add(new_goal_node) + + self.path_to_goal = self.extract_path(new_start_node,new_goal_node) + return True def sample(self): point = [] @@ -135,12 +126,14 @@ def extract_path(self, start_node,goal_node): prog='arm_rrt', description='Plans and executes paths for arms around obstacles.') parser.add_argument('--filename',default='rrt_path.npy') + parser.add_argument('-e', '--environment',default='middle') parser.add_argument('-p', '--plan',action='store_true') - parser.add_argument('-e', '--exec',action='store_true') + parser.add_argument('-r', '--run',action='store_true') args = parser.parse_args() + rrt = RRT(env_name=args.environment) + if args.plan: - rrt = RRT() success = rrt.plan() if success: @@ -155,8 +148,7 @@ def extract_path(self, start_node,goal_node): rgbas_in=[[0.5,0.0,0.5,1.0],[0.902,0.106,0.714,1.0]] ) - if args.exec: + if args.run: path_to_goal = np.load(args.filename) - rrt = RRT() rrt.controller.execPath(path_to_goal) \ No newline at end of file diff --git a/pybullet/gen3lite_controller_collision_detection.py b/pybullet/robots.py similarity index 100% rename from pybullet/gen3lite_controller_collision_detection.py rename to pybullet/robots.py diff --git a/pybullet/rrt_solution.py b/pybullet/rrt_solution.py new file mode 100644 index 0000000..3aac4d9 --- /dev/null +++ b/pybullet/rrt_solution.py @@ -0,0 +1,163 @@ +import numpy as np +from tqdm import tqdm +from scipy.spatial import cKDTree +import random +import argparse +import gen3lite_controller_collision_detection + +class Node: + def __init__(self, point): + self.point = np.array(point) + self.parent = None + +class Tree: + def __init__(self, node_list, kdtree): + self.node_list = node_list + self.kdtree = kdtree + + def add(self,new_point): + self.node_list.append(new_point) + + if len(self.node_list) % 50 == 0: + data = [n.point for n in self.node_list] + self.kdtree = cKDTree(data) + +class RRT: + def __init__(self, step_size=0.1,max_iter=50000,env_name="Free"): + self.controller = gen3lite_controller_collision_detection.Gen3LiteArmController(env_name=env_name) + + self.start = Node(self.controller.getCurrentJointAngles()) + self.goal = Node(self.controller.goal_angles) + self.rand_ranges = self.controller.getRanges() + self.step_size = step_size + self.max_iter = max_iter + self.start_tree = Tree([self.start],cKDTree([self.start.point])) + self.goal_tree = Tree([self.goal],cKDTree([self.goal.point])) + self.path_to_goal = [] + + def add_node(self,tree,target=None): + if target is None: + rnd_point = self.sample() + else: + rnd_point = target.point + nearest_node = self.nearest_node(rnd_point,tree) + new_node = self.steer(nearest_node, rnd_point) + + if self.collision_free(nearest_node.point, new_node.point): + tree.add(new_node) + return new_node + else: + return None + + def plan(self): + + for k in tqdm(range(self.max_iter)): + if k % 2: + new_node = self.add_node(self.start_tree) + + while(new_node is not None): + ret = self.add_node(self.goal_tree,new_node) + if ret == None: + break + if self.reached_goal(ret,goal=new_node): + self.path_to_goal = self.extract_path(new_node,ret) + return True + + else: + new_node = self.add_node(self.goal_tree) + + while(new_node is not None): + ret = self.add_node(self.start_tree,new_node) + if ret == None: + break + + if self.reached_goal(ret,goal=new_node): + self.path_to_goal = self.extract_path(ret,new_node) + return True + + return False + + def sample(self): + point = [] + for i in range(0,len(self.rand_ranges[0])): + point.append(random.uniform(self.rand_ranges[0][i], self.rand_ranges[1][i])) + return np.array(point) + + def nearest_node(self, point, tree): + _, idx = tree.kdtree.query(point) + return tree.node_list[idx] + + def steer(self, from_node, to_point): + direction = to_point - from_node.point + distance = np.linalg.norm(direction) + + if distance < self.step_size: + new_point = to_point + else: + direction = direction / distance + new_point = from_node.point + self.step_size * direction + + new_node = Node(new_point) + new_node.parent = from_node + return new_node + + def collision_free(self, p1, p2): + return self.controller.collision_free(p1,p2) + + def reached_goal(self, node,goal=None): + if goal is None: + goal = self.goal + + return np.linalg.norm(node.point - goal.point) < self.step_size and self.collision_free(node.point,goal.point) + + def extract_path(self, start_node,goal_node): + # Build the start-tree path, leaf to root (backwards) + start_tree_path = [] + while start_node is not None: + start_tree_path.append(start_node.point) + start_node = start_node.parent + + # Build the goal-tree path, leaf to root (forwards) + goal_tree_path = [] + while goal_node is not None: + goal_tree_path.append(goal_node.point) + goal_node = goal_node.parent + + # First add the start path, reversing + overall_path = start_tree_path[::-1] + # Add the goal path + overall_path.extend(goal_tree_path) + return overall_path + +if __name__ == "__main__": + + parser = argparse.ArgumentParser( + prog='arm_rrt', + description='Plans and executes paths for arms around obstacles.') + parser.add_argument('--filename',default='rrt_path.npy') + parser.add_argument('-e', '--environment',default='middle') + parser.add_argument('-p', '--plan',action='store_true') + parser.add_argument('-r', '--run',action='store_true') + args = parser.parse_args() + + rrt = RRT(env_name=args.environment) + + if args.plan: + success = rrt.plan() + + if success: + print("Tree planning reached the goal.") + np.save(args.filename,rrt.path_to_goal) + else: + print("Failed to find a path to the goal.") + + rrt.controller.visTreesAndPaths( + [rrt.start_tree,rrt.goal_tree], + [rrt.path_to_goal], + rgbas_in=[[0.5,0.0,0.5,1.0],[0.902,0.106,0.714,1.0]] + ) + + if args.run: + path_to_goal = np.load(args.filename) + rrt.controller.execPath(path_to_goal) + \ No newline at end of file From 7b1ff3988096f1781270921c704e7b1c4fdb9fda Mon Sep 17 00:00:00 2001 From: David Meger Date: Fri, 30 Jan 2026 10:51:14 -0500 Subject: [PATCH 7/7] Updates to robots. --- pybullet/robots.py | 83 +++++++++++++++++++++++++++++----------------- 1 file changed, 52 insertions(+), 31 deletions(-) diff --git a/pybullet/robots.py b/pybullet/robots.py index b1ba3f9..da763cd 100644 --- a/pybullet/robots.py +++ b/pybullet/robots.py @@ -11,7 +11,7 @@ class Gen3LiteArmController(object): This class provides methods to control the arm's joints and interact with it surrounding environment through collision checking. """ - def __init__(self, dt=1 / 50.0): + def __init__(self, dt=1 / 50.0, env_name="Free"): self.dt = dt pb.connect(pb.GUI,) @@ -34,18 +34,15 @@ def __init__(self, dt=1 / 50.0): self.joint_ids = [pb.getJointInfo(self.__kinova_id, i) for i in range(self.__n_joints)] self.joint_ids = [j[0] for j in self.joint_ids if j[2] == pb.JOINT_REVOLUTE] - # Initialize to home position. - self.setToHome() - # This function creates obstacles and sets a reachable goal - self.createBalloonMaze() + self.createBalloonMaze(env_name) # Set the viewer's camera viewpoint pb.resetDebugVisualizerCamera( cameraDistance=1.5, cameraYaw=-120, cameraPitch=-10, - cameraTargetPosition=self.goalPosition + cameraTargetPosition=self.goal_position ) def getRanges(self): @@ -65,6 +62,18 @@ def setJointAngles(self, joint_angles): def setToHome(self): self.setJointAngles(self.__home_poses) + def fkine(self,angles): + # Record where we were (usually home, but just to be safe...) + curr = self.getCurrentJointAngles() + # Move the arm to the goal, specified in joint angles + self.setJointAngles(angles) + # Record the end effectors x,y,z position + state = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) + # Move the arm back to where it began + self.setJointAngles(curr) + + return state[0] + def execPath(self,path): self.setJointAngles(self.__home_poses) @@ -73,7 +82,34 @@ def execPath(self,path): self.setJointAngles(p) time.sleep(0.25) - def createBalloonMaze(self): + def createBalloonMaze(self,env_name): + if env_name == "Hardest": + self.createHardestMaze() + elif env_name == "Easiest": + self.createEasiestMaze() + elif env_name == "Free": + self.createFreeMaze() + + def createFreeMaze(self): + + # Initialize to home position. + self.setToHome() + + # This is a hard-coded sensible goal to try to reach + self.goal_angles = [0.1026325022237283, -0.2931188624740633, 1.2717083400432991, 0.048794139164578594, 0.07744723004754135, -0.8437927483158898, -0.024709326684397483] + + self.goal_position = self.fkine(self.goal_angles) + + # Make a visual marker for the goal + goal_visual_id = pb.createVisualShape(pb.GEOM_BOX, + halfExtents=[0.05,0.05,0.05], + rgbaColor=[0.0,0.0,1.0,0.5] + ) + + pb.createMultiBody(baseMass=0, basePosition=self.goal_position, baseVisualShapeIndex=goal_visual_id ) + + def createEasiestMaze(self): + self.createFreeMaze() balloon_collision_id = pb.createCollisionShape(pb.GEOM_SPHERE, radius=0.07) balloon_visual_id = pb.createVisualShape(pb.GEOM_SPHERE, radius=0.07,rgbaColor=[1.0,0.0,0.0,0.5]) @@ -84,31 +120,18 @@ def createBalloonMaze(self): baseVisualShapeIndex=balloon_visual_id ) - x = 0 + def createHardestMaze(self): + self.createEasiestMaze() + balloon_collision_id = pb.createCollisionShape(pb.GEOM_SPHERE, radius=0.15) balloon_visual_id = pb.createVisualShape(pb.GEOM_SPHERE, radius=0.15,rgbaColor=[1.0,0.0,0.0,0.5]) + + x = 0 for y in [-0.3, 0.3]: for z in [0.2, 0.6]: box_id = pb.createMultiBody(baseMass=0, basePosition=[x, y, z],baseCollisionShapeIndex=balloon_collision_id, baseVisualShapeIndex=balloon_visual_id ) - - # This is a hard-coded sensible goal to try to reach, in the middle of the four balloons. - self.goal = [0.1026325022237283, -0.2931188624740633, 1.2717083400432991, 0.048794139164578594, 0.07744723004754135, -0.8437927483158898, -0.024709326684397483] - - # Record where we were (usually home, but just to be safe...) - curr = self.getCurrentJointAngles() - # Move the arm to the goal, specified in joint angles - self.setJointAngles(self.goal) - # Record the end effectors x,y,z position - goal_state = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) - self.goalPosition = goal_state[0] - # Move the arm back to where it began - self.setJointAngles(curr) - - # Now we can make a visual marker for the goal - goal_visual_id = pb.createVisualShape(pb.GEOM_BOX, halfExtents=[0.05,0.05,0.05],rgbaColor=[0.0,0.0,1.0,0.5]) - box_id = pb.createMultiBody(baseMass=0, basePosition=goal_state[0], baseVisualShapeIndex=goal_visual_id ) def visTreesAndPaths(self,trees,paths,rgbas_in): @@ -137,16 +160,14 @@ def visTreesAndPaths(self,trees,paths,rgbas_in): for path in paths: for idx in range(len(path)): - self.setJointAngles(path[idx]) - parent_pos = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) + parent_pos = self.fkine(path[idx]) - node_sphere_id = pb.createMultiBody(baseMass=0, basePosition=parent_pos[0], baseVisualShapeIndex=point_vis_id ) + node_sphere_id = pb.createMultiBody(baseMass=0, basePosition=parent_pos, baseVisualShapeIndex=point_vis_id ) if idx+1 < len(path): - self.setJointAngles(path[idx+1]) - child_pos = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) + child_pos = self.fkine(path[idx+1]) - pb.addUserDebugLine(lineFromXYZ=parent_pos[0],lineToXYZ=child_pos[0],lineColorRGB=[1.0, 1.0, 0.0],lineWidth=0.02,lifeTime=0) + pb.addUserDebugLine(lineFromXYZ=parent_pos,lineToXYZ=child_pos,lineColorRGB=[1.0, 1.0, 0.0],lineWidth=0.02,lifeTime=0) self.setToHome() pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1)