From 60712989dc3d9f5bb37834584ea2c403ad656718 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 30 May 2025 10:09:34 -0300 Subject: [PATCH 01/78] =?UTF-8?q?=E2=9C=A8=20Add=20mujoco=20models?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Pedro de Santi --- mujoco/model/car.xml | 72 ++++++++++++++++++++++++++++ mujoco/model/maze.stl | Bin 0 -> 45484 bytes mujoco/model/robot.xml | 104 +++++++++++++++++++++++++++++++++++++++++ 3 files changed, 176 insertions(+) create mode 100644 mujoco/model/car.xml create mode 100644 mujoco/model/maze.stl create mode 100644 mujoco/model/robot.xml diff --git a/mujoco/model/car.xml b/mujoco/model/car.xml new file mode 100644 index 0000000..0d4ea67 --- /dev/null +++ b/mujoco/model/car.xml @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mujoco/model/maze.stl b/mujoco/model/maze.stl new file mode 100644 index 0000000000000000000000000000000000000000..638a4293d557decb0ac123e2be72e7c79e24a9f7 GIT binary patch literal 45484 zcmb_lPsnH4QT<`GqoW8K1Q!ztn(moMM8X6z{!33L9T-s*m0k!cv>6d$k`O``(FnaT zO%|C^O#B03l8Bj^n7ENPs1P%=PzKv9gdi@0(e4>=K-`GfCLnE{I(4e*{&`>b!nf#G zU)_7^)V;TE-TUr)-`DSZ&lA7)`0L(v<=21ZO|Sd^xIXf#Q@gG{f9K9amrke09zC6| zTsmF7oig>PYWj8w5hV>lbm;AN^?AXHhW)Rl^j<;9Cpc<9IjsxFmizKmVfTE&prSqt7h$hpJulsTxK6p zzaju`94Sjr+o#k0FTUwQayVD#3l7(>2zZDZ8-(-q-o10@+&dUo4Pi*?WobpIBsh{! zdh@Eql{jvOU5!8VxNe%aD}p86!_lkzU;H!cBV5Ln5#Bk>uIo_;94mq)#UZp1^2)~J z+2nxD#+Mwy#g-SsY#kxotDn67o@z&qTH|oL=;^R@S`iaR@(E!sSj?VImoI;l&BYkC ztG8N|Ag71}`V|4y5d!Fs{nHbg=Qj43&zK(_0U3H(S`jLlJ(S*D$hayFuy;OjK))iO zIzqrh&OIVCs||?Tl@ndRB4!TpNiVNH_t^^%CdcV(VHf?@I80A(@NXgT(*eT0x_tRV zimQuv?p!w;n0F1~GW4{B=$H{KSv?d&?fAt{J^Vm&Jo26INV#*E9ypF*Axw3IFf
    S+la zx~@LoOXOAYS-r`t(@7k-OYR)9sCEd!`wk(F^vdS>HJ=}y12XkxaYl?Bg)Ri6A1Myl2>S&wtiWoTxO+Ef?(ViS{u|6w4 z%BTzdicrZ8A@5MfkTxEF747I8Z?QZNJuY%kE`&m9yULo;ST??M?I=LUvMzmS47OU4MHsk8!p5Z*YnN+nfjWw ztY3(cqtJ!m{P?k-{<#a{_}@DZSzHyLIG}fLeElfI>2!op%hM~{wHe zm=PmKp{XygY(0NK9G;~cpRy;jxAl>?qY$Ul5n}Pm*7MJMJ34|g^kGSju)D>uBSwxw z7lP6LLr;DD)8gQ$bv|;acV~pgd__3lYk6 z!1hxicMk4uG#<_r97hPY{KfbFS3Q6Dj%(y_bnFkwu_BO{%Bzw?Z_c!K1jp(0rAyp* z248TveuF@Z_6T}qyS`^phf%`u=m?jiN64`vfG&=bLl4^??f6+^;P)7hfg`wPL?Cwv zaNFASOP_K3#$_$@xEeSNNxghn5fcZv!W-SotK@j(d%~{e`OaZ__%kC+b%b!PSAFXn zwJ*7dn!YdT2=UA+-@k*|sadf1;4Q0)=)%Jw|32aZTQx1xUvLA!irro18m9uaCe z5mtN=3?IDl=JR|CH4Z|2T{kla!xb^&^eRHF(ZJU5Y8vP z+JT>I-r0Fo=SvQrW3e5DfOLcax91SAj**Pk4hYzd@$hrA6)|&&FZ6b!SMPu2Eq2Bv zj{B;2-L5#GUlGt9A=q-%z-3&Xi81CKVX{^O*Rys)MH9DG)r z5wLQE0LSIezwrI2T_0pzh?Z6o9EC0fd-rdD>JeLW$-#Yk;|ojLzt0HJJ#D?XBIw~;pZr~| z=g;T7YY0b2U(wQv7&(ehdiCxrANXRe=XkPH?}9Hm@J47wKy`#b`|Pw)az!rb2)7G8E}ZPFh$s<9qJ=QqwSFjH`MvQe zU&Vb!z!qxn5b(-&OL`uxd^I2pnSBK-D`MslpY*%&@VSvKFAmMcbv%v`Y#X?ESr6#> z9Q)qjAe!%p#4#gm9xMnvy@cNGt#|c&j%+jVkwdw0M!?7(v3O;9S97osj*fXQ4tR*m z`mC2M9-;~|>(CByV9X_>mMnQJtO!^+Km-SB;IbZg;nl{9{Z&Iic0KBt+%6%aL|!F2 z^mc1e5**%(oi8}lyE7ujc7s^G(iuSKfUI){ai}RenHV_=y^AYxfL(IukXIE~Vn0Hx zUU8?w@el{l)K~9TJ9joU+0s>cOO9bi6~f?02;}+GkH62J{ry6p;Z3x)qa)mI zyrFh^MNAxtrXI#?Jd(q+Lh(^=Rb9U#RI)+1S76~~5$P+VwWD)DcD>aKjv1kn;7GLe z^2*-o+j$9AUyQlcmfShwtv*{`2yh-CobNx*e=OrF`>G?{E<6lNrxg(;@+#3ns2w~} zjn?`%EFpIe*RKe(b%b#Acm3cuwsQuoMTo0g=6B=MIJkaAOdN@p-dw1CiQ9@XmkfkB z;yxqH7D{#q=X>A%U(WO0Tt7Mja^!YeSrHRQqC=17_&(82R4*h4z8cVK&^hAkg&6@O z2MD0;i<)P%9ho^h!sN`N&aomUjzotZHy;ngl{j#h3=hSj`;4%)=m5blI{xUdzb)Ir zQ!{@bksNL}PP)jkB1Vqlqu!bk?eH%f-Im1_#v3SmYL5cKLp|NXschrQ>F z_XZ6CH1%QWv?4~1;uAvcu%EqGnXmj?9L~qi5hq49Ka$-_x`(NUxBjIT>>HQGu+GRD z!gV?$!yR8AjR^cSB9g<=v`tKL+_YR$e3VfK`W2y)9RfOg8?LO}aE9h6m%ptV&@(Mp|Jp3!b&KDf~ zI&ei`Wkv zyb$Pt14IKuw21VY69;s%pnD*NuphIeqf5_dcbL}<-zDRo7HE7%P&;@igziyAnQr2U z(%nsni32}wZQD9%=ZUv1Ho;+0-N!%1-1TOFwhIBHdxVcaitUQlD_(P-V0>m@S-Y~S z5JQR34M9%_jyS*Ia}y#ZzMt@xM;{W9Ua=OH4RH#+pGFTHgRNFedcO~dcqXqCL9|?_ zYVe9rcfdqntn+YUjrO-U}Q}cHMwj_6dftULJa#fbeQ4X*&+- zE7PeRD#2Z{*V1VEu5a+Fm2^bPvDN~!#??^EZCjgGj#e@thLw8osv(B;TzcPycLd+Z zlHE8sHdZ_3wH-O;diQVWd`EAx_ju>v`BZ$d6>qKmN!y<_fx!6h5GdhE$B5>s^W6|$ zOWv*=twbC>@;4j}p=cjDo}Sy02rKEcOZwv(m2Z2{yO?)v29JH!O9s1vSUEaEGHSgL z2)_@#JCt<0l5xK~!fK_53qtt=Z(mck9Y7+m;}|#;)%eCE5%}IC_Ka&JrY*gfNADsd zX_v`&^kC6i*h&)J5s-Nip;+>$&j@ednzS!cCWN;GXStXYIIX43IlAR&i25!A=o^P# zSxK`iZ&!}W&bQ2#vNmM&5je0Gcdy2hj*z?{+y=*hD>3EdTUP^lVkI3>@;ri>IW0m` z#z-n+{64g!l?(`tzu%7#8LypV8S`XENw+Kc-Vk2vFxnN>h=yiYW4=ZS+k`Q&D^9$8 zj}kpn=e+CufESu8H{+xTQG$r!6*Ee+)=L^fD{4G*0<%F3wc-hB<;Yr=WkN=)HJa>! z!7KUhGe=$_OT=m0IvztwL&%ax>cFu@*xU%cd3VrPED;B0lVr=4agBX7BCJ-7;D$rF zU4}5XaYe*z#(XJJ#PH45T@XE%GAh^Ht}hI&q$7^o(QWlwA@i#qHZuCEmvqEo?`D?q z-Mu|`dS4+54>MdVNwhuL=e{dX4>H;ZgnbQ|9QTrU2fNCyOQvQNVAL#*RLkTV_lK`>Vx_3qH`gI(B0ug_yXAe6hjRv?xqFO5fEk$p5X zV&)Ol7@(HWM4)Dj=Ga%=mR302Gv_ZHy`&>@T{#ocYsr%DvxgDvB^?p+W?!;FcrEQE zR*qiM?S_8Gf#+5}Ve`Ah#~#J+`6wUfTXAa&SI(itPo9X-Y5;T$#}>iWpmVHyhMebS zKV^GLrEMGY<;1I(bVPr{KYG;>y_WQAU-goXNO>8LEdsSPgZ)HgjFQL(Rx;XEp2u_H z5hC+SDTcXN_xMAd~b-V zb+;&yR~&Qh$P2BU$bO?OfJx0?tnzDk6QV!Pt~ zimznUXvgH91bXnwIk>(W0xPJmqA+F|Nphd(Bcj(bS(bE%7xYRRzsqhzB-a)pwAYH0 zuGA$$ryD5AQgG>POuU6SafSCj+4S46;zY~w42aA2I%vJ*+$FW$5=XF?G(>us<<(c> z(1};@`Cbe1%7L91OKRTx5&toycF%! zutZt8qotcUMuh&Z1#Hg<#ir+sSpR(Q;5nv#Hyqd)!Cul3%tZBD5PQT>D=cktN$hRB zcI=EG=e|mFF})f}8iHrmsMhvAWA?%9)vVPW&No>t4;yBy_?q0$`XUlH2&+|SJ7JA0 z#$3BPYhl)cU1jZsBWs~ghFUo5vuo+As%5d72+Y4gWQNEw? z{T7{i7pH)@?9?$iEZP%+$mbP^%o(>Ui((E&TyaM`AbN?igx|4y#ihI2QTg?zd4)4w zyZjs`YW6dk$kg1aVh(Cf`+dk`ZYO7y-eYRF#B9L*3Tm#iwj-{E?^Y`~)*01Hx?RaD zB3o`_kut_YW1~Adge{k5MrZ_e-y(9HJI503$&M1)!(H;dA-vY%{7|%G=5)In|COU7 zhFY3Y%lr@mG29U$*D92~yFsv)IAZ0g-py#XSlvbYCU#p?cZB4Ao*2=-L0GMfm=zKG zC*NV@UJCQvX0Bbe^T?L>l8(ss89FZIl5Q(J#4}E|W9h4*q#=^y-Vi$Fggf(?SD&Cn z9A-D$G;<7yR;%|!;AUQJQO#70w-Odv^ z2YZQjEp{&t&q55f!cy)jvK{J0lyrpT4I*26%UT_C!DFd<0ey^1>v=Ef2+5q)cSD@j zyJNpMyM1TiKFO;A(Q2iBiL0TcA#z`GZwS=Nth7X8FA;~^jl8mOjJ8^>u(TP$p`;;{ zyOuq*IA|*nR!b)mE5}$;2%cXeH0FJKNAz0K>xrhKq?dF=%6Yb^G2bGrR_>DWq)>=n z((R^x;ZTY05p=fxUGpr_^_(BO5=Uvy%Y@xGh+gZS_KE!oNHM+hiqFny^-wFKaz$t! zpkzRF52G!S8M>`rD{|$IgHID)(h;#oy$hn(3J%M3%--3%+8cVw1~Gg&z)an3WuC|L z4p+MkM=$A!T$>C%_Ekp=wUk35%Vs;&7r0CAvq<)FSEepqN2I(&d)3$JIW#)cH?fV; z#9<}fZpq84CXxYh);g$saLlM)(h!-a@+t2ugg=dCEX*7gS5~6@ z2fGntdX_7%21Kuwa{IzGp4Id$Yxz|XgSIKg#G5T2N;)Dq)R(JQ-iBUF9H(Ujdr3#A z9ruK&H@xPH#R5FS6>%jE?t@*`)P0Lc4^T@rD~^Lh?LgiN*|T@@Er?#SLo6#!`EIs^ z_F9Ktb$@q=m7^mhqt*+-q5HmfcL*Pe`p4d4`S}#zT5&bdJ1=|swo7Ly!U&-=60ANQ zT?mfFD<634h2l78;{m%fhw1H_5wLZHz}c+*_L+Wf8-I5&>~_RCJPEsP-dDuTA#~`| zEBGf4tSpjor;Hk04I&B?!IB-Kkf>ulaMAzjp7l@Xpj~_+jjL81{3HZR#8GJKRTFny zuoYtva_7*P!?rH6vxMl5*m;HBhAnWgoVgv&LyB7Je z(qNoc#K=(|hTd-U$})ug{|5Y04zo#e=YSr#8Nm#(L$KvN`|x*kF+Ylfc5k|E$}3{z zD73Ou^Xl2+z}qs|B_CxV_!ceO5eSv+5NP?eD}VS5+7B-J3A>%+q0EWYuZWpLXzA4s z`)xq``_y1XPW1D=Xh(7AWQXl21U%d$=+&?O`==g29dI$yMh>8BUSQw8B7iQAlBvhP zoV7pjrcdPmZvdN}PaL|>2+p+~0({`m1td;YIM0RPou@70m=PlfYN@8~^olrg@JYM) zbk0P*T3!gfom>&*_{?AaO^yd`#Mf=nCg^0F_Z2a6uohb`B%|cZKmU>T_hdeNiKXOI zP3`n>f??2X+YRUVQKGs2w&Q$VV1e4MDrm)6(g*B1Vov3xSvgoL6x8idKB&Mh&iq zGeRXBM0<`xE&Hp$@f{Iu6<@8GT60mq5Rg$)h{Y@GBVNg2=aeZMIyvGjydtreY!S}= ziGTZMtsmBo$R(Y_W#)9gU0f07;ev=S2IQ5Ghy9&Y{M`?qgLcPht;M6rFYw)z$Fem*+~Xk9Pz|gaW$0e5YGMNOFyc4ZtEbT0rR|Z zm|PKx6R{Nm??%MHjpB8F*n5J`mmKl_U`CjcBZRm7$(Q~|?JzDp8(TX%!tKU0v&$=D z;z%_0_HVZ1>(7aU(NKIkk)nP@C`MO=-j9F>7a|hRNxt8)ekX!<`TQgf)-S}!Q65S! zulVwsZD}Eb_xGDtYcYc-O;TO;fwj=Ux=WsbzXXjWEK;I0zhtn3l=>R+GuJ&Rdf z*eAw*ry+o*9(6FwRs>7rRq+Xdv4)H*Ic%4ja_5NM>x?ky5yHLde>JjUaZF3& zU*@hDoe|viao0b#gL4A2bL1ljXYGnW59|@-_~`3jIqyCW>tG>hmunQ|6)|!YANAe= zmeX}{CF5Hwt!me;pU9!NSv5b#k`+O({4dEl1@cwIm7kIqU&fW|SA@YQ#K^7Q#orxF z4qIhXHgt0M>a!x?;sL_By$SvgdLVr72)7%36&%6{fu9>k%GBfExp$Fk@f%ykS1pHr zML>0caIfs|s(l}DL~7M<2uH`B!Q~YJ@QjF-yMFX4IDG%s`GO}2Et_O(N^eJgh~oQwDi^v&sQIR;y*uK99T1ouQ+aUJSGHe9UUiKxt;5GMm8ZjH+)gvh0I+TYs8Ms?UF;t8KL&92zuC`wzZG$9F*^~agwDKp_17{ z>Bn}!7IGqLQ|=tvVfJ>A{Rlzy^*8>6^6tf&cg5l8oafN52=(&pq4a8p{pI%ui^FQ? zJU6?^L9f}4LcrD$0$$Z+NI!>0Uv-4Zu18IB2qQ$4q*p>0f?nB^Oa2{tKj-Ou!J+e? z8G*62lc5b_T7h?CsSCArAS4a|WDS zAg&4lx%O3ji#8+h^N1L@t(0DU?uUQ-GvYumO70x$-5J5S+98}fT;eF!nt?OQSaJL< zES@_e-X=sZSrIFTJaK=ccjq<*MPzFsdS7+)j^kX6#}&I{PHRo1teglQww%3ZBk1!1 zdlSh*>q{Xa$}UPeg0h|en4Xt7xH|hf;VVBP#`al~{XQai)`wc@q07;}%#HG@PN4eR z@Pw?F`8EKKorOTHqntRFlwJCdjJj|%TfLU# zeCto&(h(_pZ0|S*gw@grt{lB& zw5vXxz3K?X80<=4JATFPSkmoA?0Wz8cI*(an{UkgL}WxDnyp8@q}#>ZLr-JI9V4n% za``Qf|Gk)ujFBIRWS746u$jO`8wvUq$5&Z<7$VnT0V-q*wsqryW^~d-7(jawXcSf zj*z^JAbSt96D_y%KA))=&1){Ny*rdN#K6q~%x8mctNYF#Svl12ENO_tb_|H2R`mMP z@|xRt?-;quYDr$_V7`g-{*PH}T-p!;=&P|8qaE(MvNqp__jYg$=x?tTvOgpCzEUp|!631bit0QKP zjwl(m6bqQ6-K$>G5h-tGRM{G9aRhhsyh=Jk(X-2N9Z|D3XS1I`kL`%~@%`rpvBp(9 zJx{w@ZD5YbCU1a@M^v1Ff)(Cp$J-t4@} ztPg*4-kE*Lo>zK1wg}V`V%hZ#C936!+tH3T&*R(@k%*H!?47YTNVw(6|cq2 zK$&@kGl??n2*Do3k9x-fj%(HqtfJ7z9pd0seT#!?`o4%V{>2pt*|59#c*l64+RPz8 zu~U-lyR_(4?w}l-9PGU#1owCYBKMlu7h-+o8ENglq35NRYyY%IY`pS02pd3RM%iU! z&&+@yq6eU6?|Nk5M}66EvOP7XZGW0Hon1r>B|`giX87NqIh|ub__?kb>m2=ga71`X zw@aB3>Q2pGH3VugKPrw##JTJrW^FVPeMS9_qnB{5aF+FXv*Vzxs+B!*mzEbo^Kml= zH3oi<70laljqO{;TyuiaP>9;$&ry>8x~&)wkKvG)8O1eRZ%oKR9c8ZAy{~Fcz;DhE zpQ+itdDgP;VMDw7knfIgf7Qx{UfPiy9pSQOuzFeZL%z=$N4>5)eAPT>pY+Vh8GI!o z^hS(iA$m!SV4w3YgJF+|%-~rRYdfmV9GNrn)Bx3xcLTA+Rb?0SEPL16l;bgTRPUl> zMN}?UJ1P><{}qX;7Z;i|<`OU2AeMG?TPa&DWrp;+aWFHWWJDbGRktg2GXb9BG zRX|*c=q0je_Zqpg5WQB(+-Lg$M81mo;m=FT1{e>@yPOyZtD*V9yY`P^rM0|D21JaU zbmdj;Qq9&-ij#v4;!8jogcs3mWey=M-eCmj)T&L5*0IXZ&1$f0=& zzea?Ybh{yE9yk->wfya3{WX%t(Mtxqc$=wR6r}X3Bl=e;q2IK8DCu@7Z$@x~KrM@o zc;hpV$5_(r#y)rD7!a+N^mb?F$d(T!4N*JeIofv!&L+;*l|z}qOLC8n$iEfd%p3z^ zs3i`q*g1l^-?5U8NLjnIvk~}z0oX^ij}T%g8SKjcnPWgi#=(=M^y>~_Ea`SBSEif2 zqOGb`mge<*EZHD3FRvW!q+rOX?Ibl;gYezXTa&LL*1W5X+)pN0OFOcaqnD^1`fWEn zrCy(+y`JKAZme1wx#RT%&r9OKm$DIoo4)ELg%I~{1UrJBu$J0~^@>+yQL*AK+4t1f zSkrsDBa#&*J48PnjFq!&CE9BpdX;A+bsEK|$)ny?pZK0iXydro-YvUAaLoS?o(|q1 literal 0 HcmV?d00001 diff --git a/mujoco/model/robot.xml b/mujoco/model/robot.xml new file mode 100644 index 0000000..2b957a4 --- /dev/null +++ b/mujoco/model/robot.xml @@ -0,0 +1,104 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 58617a36f9118dee28ac88d40769d329a3a58049 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sat, 31 May 2025 17:08:00 +0200 Subject: [PATCH 02/78] =?UTF-8?q?=F0=9F=94=A7=20Update=20robot=20model:=20?= =?UTF-8?q?enhance=20grid=20material=20properties,=20adjust=20lidar=20posi?= =?UTF-8?q?tions,=20and=20correct=20wheel=20orientations?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/model/robot.xml | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/mujoco/model/robot.xml b/mujoco/model/robot.xml index 2b957a4..1797830 100644 --- a/mujoco/model/robot.xml +++ b/mujoco/model/robot.xml @@ -3,7 +3,7 @@ - + @@ -43,26 +43,26 @@ - + - - - - + + + + - + - + @@ -82,8 +82,8 @@ - - + + From fbc75930474514e06a39aacd47d3f38734361e48 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sat, 31 May 2025 22:53:37 +0200 Subject: [PATCH 03/78] =?UTF-8?q?=E2=9C=A8=20Add=20maze=20generation=20scr?= =?UTF-8?q?ipt:=20parse=20maze=20files=20and=20generate=20MuJoCo=20XML=20f?= =?UTF-8?q?or=20walls?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/gen_maze.py | 217 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 217 insertions(+) create mode 100755 mujoco/gen_maze.py diff --git a/mujoco/gen_maze.py b/mujoco/gen_maze.py new file mode 100755 index 0000000..b8fdbed --- /dev/null +++ b/mujoco/gen_maze.py @@ -0,0 +1,217 @@ +#!/usr/bin/env python3 + +import sys +import argparse +from enum import StrEnum +from pathlib import Path + +# Example maze format: +# o---o---o---o---o +# | G | | +# o o o o---o +# | | | +# o---o---o---o o +# | | +# o o---o---o o +# | S | | +# o---o---o---o---o + + +CELL_CHAR_WIDTH = 4 +CELL_CHAR_HEIGHT = 2 + +class MazeSymbol(StrEnum): + POST = 'o' + WALL_HORIZONTAL = '---' + WALL_HORIZONTAL_EMPTY = ' ' + WALL_VERTICAL = '|' + WALL_VERTICAL_EMPTY = '' + +# order: +# Read Horizantal walls +# Read Vertical walls +# and so on + +# array>> + +class MazeParser: + def __init__(self, cell_size: float = 0.18, wall_thickness: float = 0.012, wall_height: float = 0.05): + self.cell_size = cell_size + self.wall_thickness = wall_thickness + self.wall_height = wall_height + self.width = 0 + self.height = 0 + + def parse_maze_file(self, maze_file: str) -> list[str]: + """Parse the maze file and return lines.""" + + with open(maze_file, 'r') as f: + lines = [line.rstrip() for line in f.readlines()] + + if not lines: + raise ValueError("Empty maze file") + + self.maze_width = (len(lines[0]) - 1) // CELL_CHAR_WIDTH + self.maze_height = (len(lines) - 1) // CELL_CHAR_HEIGHT + + print(f"Detected maze dimensions: {self.maze_width}x{self.maze_height}") + + return lines + + def extract_horizontal_walls(self, lines: list[str]) -> list[list[bool]]: + """Extract horizontal wall information from the maze lines.""" + horizontal_walls = [] + for i in range(0, len(lines), 2): + row = [] + for j in range(0, len(lines[i]), 4): + if lines[i][j + 1:j + 4] == MazeSymbol.WALL_HORIZONTAL: + row.append(True) + else: + row.append(False) + horizontal_walls.append(row) + return horizontal_walls + + + def extract_vertical_walls(self, lines: list[str]) -> list[list[bool]]: + """Extract vertical wall information from the maze lines.""" + vertical_walls = [] + for i in range(1, len(lines), 2): + row = [] + for j in range(0, len(lines[i]), 4): + if lines[i][j] == MazeSymbol.WALL_VERTICAL: + row.append(True) + else: + row.append(False) + vertical_walls.append(row) + return vertical_walls + + def generate_posts_xml(self) -> list[str]: + xml = [] + + for row in range(self.maze_height + 1): + for col in range(self.maze_width + 1): + x = col * self.cell_size + y = row * self.cell_size + xml.append( + f' ' + ) + + return xml + + def generate_horizontal_walls_xml(self, horizontal_walls: list[list[bool]]) -> list[str]: + xml = [] + + inverted_horizontal_walls = horizontal_walls[::-1] # Invert the order for MuJoCo + + for row in range(len(inverted_horizontal_walls)): + for col in range(len(inverted_horizontal_walls[row])): + if inverted_horizontal_walls[row][col]: + x = col * self.cell_size + self.cell_size / 2 + y = row * self.cell_size + xml.append( + f' ' + ) + + return xml + + def generate_vertical_walls_xml(self, vertical_walls: list[list[bool]]) -> list[str]: + xml = [] + + # Invert the vertical walls to match MuJoCo's coordinate system + inverted_vertical_walls = vertical_walls[::-1] # Invert the order for MuJoCo + + for row in range(len(inverted_vertical_walls)): + for col in range(len(inverted_vertical_walls[row])): + if inverted_vertical_walls[row][col]: + x = col * self.cell_size + y = row * self.cell_size + self.cell_size / 2 + xml.append( + f' ' + ) + + return xml + + def generate_xml(self, horizontal_walls: list[list[bool]], vertical_walls: list[list[bool]]) -> str: + """Generate the MuJoCo XML for the maze walls.""" + xml_lines = [] + + xml_lines.append('') + + xml_lines.extend(self.generate_posts_xml()) + xml_lines.extend(self.generate_horizontal_walls_xml(horizontal_walls)) + xml_lines.extend(self.generate_vertical_walls_xml(vertical_walls)) + + xml_lines.append('') + + return '\n'.join(xml_lines) + +def main(): + parser = argparse.ArgumentParser(description="Generate MuJoCo XML maze walls from text maze file") + parser.add_argument("maze_file", help="Path to the maze text file") + parser.add_argument("-o", "--output", help="Output XML file (default: maze.xml)") + parser.add_argument("--cell-size", type=float, default=0.18, help="Cell size in meters (default: 0.18)") + parser.add_argument("--wall-thickness", type=float, default=0.0126, help="Wall thickness in meters (default: 0.0126)") + + args = parser.parse_args() + + if not Path(args.maze_file).exists(): + print(f"Error: Maze file '{args.maze_file}' not found") + sys.exit(1) + + output_file = args.output or "maze.xml" + + try: + maze_parser = MazeParser(args.cell_size, args.wall_thickness) + + # Parse the maze file + lines = maze_parser.parse_maze_file(args.maze_file) + print(f"Parsed maze with {len(lines)} lines.") + + # Extract walls + horizontal_walls = maze_parser.extract_horizontal_walls(lines) + print(f"Found {len(horizontal_walls)} horizontal wall rows.") + vertical_walls = maze_parser.extract_vertical_walls(lines) + print(f"Found {len(vertical_walls)} vertical wall rows.") + + + + for i in range(len(horizontal_walls)): + for j in range(len(horizontal_walls[i])): + print("0", end="") + if horizontal_walls[i][j]: + print(MazeSymbol.WALL_HORIZONTAL, end="") + else: + print(MazeSymbol.WALL_HORIZONTAL_EMPTY, end="") + print() + + if i < len(vertical_walls): + for j in range(len(vertical_walls[i])): + if vertical_walls[i][j]: + print(MazeSymbol.WALL_VERTICAL, end=" ") + else: + print(MazeSymbol.WALL_HORIZONTAL_EMPTY, end=" ") + print() + + # Generate XML + xml_content = maze_parser.generate_xml(horizontal_walls, vertical_walls) + with open(output_file, 'w') as f: + f.write(xml_content) + + print(f"Generated XML maze walls saved to '{output_file}'") + + except ValueError as e: + print(f"Error parsing maze file: {e}") + sys.exit(1) + + +if __name__ == "__main__": + main() From ea3ad7e98ebe4faafbbd7cfb39b906150c4edee4 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sat, 31 May 2025 23:45:33 +0200 Subject: [PATCH 04/78] =?UTF-8?q?=F0=9F=94=A7=20Update=20gen=5Fmaze=20to?= =?UTF-8?q?=20add=20default=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/gen_maze.py | 64 +++++++++++++++++++++++----------------------- 1 file changed, 32 insertions(+), 32 deletions(-) diff --git a/mujoco/gen_maze.py b/mujoco/gen_maze.py index b8fdbed..66668c7 100755 --- a/mujoco/gen_maze.py +++ b/mujoco/gen_maze.py @@ -5,18 +5,6 @@ from enum import StrEnum from pathlib import Path -# Example maze format: -# o---o---o---o---o -# | G | | -# o o o o---o -# | | | -# o---o---o---o o -# | | -# o o---o---o o -# | S | | -# o---o---o---o---o - - CELL_CHAR_WIDTH = 4 CELL_CHAR_HEIGHT = 2 @@ -27,13 +15,6 @@ class MazeSymbol(StrEnum): WALL_VERTICAL = '|' WALL_VERTICAL_EMPTY = '' -# order: -# Read Horizantal walls -# Read Vertical walls -# and so on - -# array>> - class MazeParser: def __init__(self, cell_size: float = 0.18, wall_thickness: float = 0.012, wall_height: float = 0.05): self.cell_size = cell_size @@ -94,9 +75,7 @@ def generate_posts_xml(self) -> list[str]: y = row * self.cell_size xml.append( f' ' + f'pos="{x:.6f} {y:.6f} {self.wall_height / 2:.6f}"/>' ) return xml @@ -113,9 +92,7 @@ def generate_horizontal_walls_xml(self, horizontal_walls: list[list[bool]]) -> l y = row * self.cell_size xml.append( f' ' + f'pos="{x:.6f} {y:.6f} {self.wall_height / 2:.6f}"/>' ) return xml @@ -131,25 +108,48 @@ def generate_vertical_walls_xml(self, vertical_walls: list[list[bool]]) -> list[ if inverted_vertical_walls[row][col]: x = col * self.cell_size y = row * self.cell_size + self.cell_size / 2 + xml.append( - f' ' + f' ' ) - return xml + def add_header(self) -> str: + xml = [] + xml.append('') + xml.append(f'') + xml.append(f'') + return '\n'.join(xml) + + def generate_default_class(self) -> str: + xml = [] + + xml.append('') + xml.append(' ') + xml.append(f' ') + xml.append(' ') + xml.append(' ') + xml.append(f' ') + xml.append(' ') + xml.append(' ') + xml.append(f' ') + xml.append(' ') + xml.append('') + + return '\n'.join(xml) + def generate_xml(self, horizontal_walls: list[list[bool]], vertical_walls: list[list[bool]]) -> str: """Generate the MuJoCo XML for the maze walls.""" xml_lines = [] - xml_lines.append('') + xml_lines.append(self.add_header()) + xml_lines.append('') + xml_lines.append(self.generate_default_class()) xml_lines.extend(self.generate_posts_xml()) xml_lines.extend(self.generate_horizontal_walls_xml(horizontal_walls)) xml_lines.extend(self.generate_vertical_walls_xml(vertical_walls)) - xml_lines.append('') return '\n'.join(xml_lines) From fa9585c0aba77ba377b24e78a1e4efd479ae0dac Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sat, 31 May 2025 23:50:19 +0200 Subject: [PATCH 05/78] =?UTF-8?q?=F0=9F=94=A7=20Refactor=20XML=20generatio?= =?UTF-8?q?n:=20add=20default=20classes=20for=20walls=20and=20update=20geo?= =?UTF-8?q?m=20attributes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/gen_maze.py | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/mujoco/gen_maze.py b/mujoco/gen_maze.py index 66668c7..cbac869 100755 --- a/mujoco/gen_maze.py +++ b/mujoco/gen_maze.py @@ -74,7 +74,7 @@ def generate_posts_xml(self) -> list[str]: x = col * self.cell_size y = row * self.cell_size xml.append( - f' ' ) @@ -91,7 +91,7 @@ def generate_horizontal_walls_xml(self, horizontal_walls: list[list[bool]]) -> l x = col * self.cell_size + self.cell_size / 2 y = row * self.cell_size xml.append( - f' ' ) @@ -110,7 +110,7 @@ def generate_vertical_walls_xml(self, vertical_walls: list[list[bool]]) -> list[ y = row * self.cell_size + self.cell_size / 2 xml.append( - f' ' ) return xml @@ -125,17 +125,17 @@ def add_header(self) -> str: def generate_default_class(self) -> str: xml = [] - xml.append('') - xml.append(' ') - xml.append(f' ') + xml.append(' ') + xml.append(' ') + xml.append(f' ') + xml.append(' ') + xml.append(' ') + xml.append(f' ') + xml.append(' ') + xml.append(' ') + xml.append(f' ') + xml.append(' ') xml.append(' ') - xml.append(' ') - xml.append(f' ') - xml.append(' ') - xml.append(' ') - xml.append(f' ') - xml.append(' ') - xml.append('') return '\n'.join(xml) @@ -147,9 +147,11 @@ def generate_xml(self, horizontal_walls: list[list[bool]], vertical_walls: list[ xml_lines.append('') xml_lines.append(self.generate_default_class()) + xml_lines.append(' ') xml_lines.extend(self.generate_posts_xml()) xml_lines.extend(self.generate_horizontal_walls_xml(horizontal_walls)) xml_lines.extend(self.generate_vertical_walls_xml(vertical_walls)) + xml_lines.append(' ') xml_lines.append('') return '\n'.join(xml_lines) From 889fbe11d0978482de8ed9edbbada77cf3e7573a Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sat, 31 May 2025 23:50:42 +0200 Subject: [PATCH 06/78] =?UTF-8?q?=F0=9F=94=A7=20Update=20robot=20model:=20?= =?UTF-8?q?refine=20asset=20textures,=20adjust=20mesh=20definitions,=20and?= =?UTF-8?q?=20correct=20body=20names?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/model/robot.xml | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/mujoco/model/robot.xml b/mujoco/model/robot.xml index 1797830..f41b480 100644 --- a/mujoco/model/robot.xml +++ b/mujoco/model/robot.xml @@ -2,15 +2,12 @@ - - + + - - + - - - + "/> - @@ -41,11 +37,12 @@ - + + + - - + @@ -54,7 +51,7 @@ - + From 0de980785de36e98f7607c5e451f5adc7ed5cf79 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sun, 1 Jun 2025 00:33:09 +0200 Subject: [PATCH 07/78] =?UTF-8?q?=F0=9F=94=A7=20Enhance=20maze=20generatio?= =?UTF-8?q?n:=20improve=20XML=20formatting,=20add=20asset=20generation,=20?= =?UTF-8?q?and=20refactor=20default=20class=20definitions?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/gen_maze.py | 55 +++++++++++++++++----------------------------- 1 file changed, 20 insertions(+), 35 deletions(-) diff --git a/mujoco/gen_maze.py b/mujoco/gen_maze.py index cbac869..c8744c7 100755 --- a/mujoco/gen_maze.py +++ b/mujoco/gen_maze.py @@ -74,7 +74,7 @@ def generate_posts_xml(self) -> list[str]: x = col * self.cell_size y = row * self.cell_size xml.append( - f' ' ) @@ -91,7 +91,7 @@ def generate_horizontal_walls_xml(self, horizontal_walls: list[list[bool]]) -> l x = col * self.cell_size + self.cell_size / 2 y = row * self.cell_size xml.append( - f' ' ) @@ -110,7 +110,7 @@ def generate_vertical_walls_xml(self, vertical_walls: list[list[bool]]) -> list[ y = row * self.cell_size + self.cell_size / 2 xml.append( - f' ' ) return xml @@ -122,20 +122,31 @@ def add_header(self) -> str: xml.append(f'') return '\n'.join(xml) + def generate_assets(self) -> str: + xml = [] + xml.append(' ') + xml.append(' ') + xml.append(' ') + xml.append(' \n') + return '\n'.join(xml) + def generate_default_class(self) -> str: xml = [] + wall_parameters = 'type="box" material="walls" zaxis="0 1 0"' + wall_size = self.cell_size - self.wall_thickness + xml.append(' ') xml.append(' ') - xml.append(f' ') + xml.append(f' ') xml.append(' ') xml.append(' ') - xml.append(f' ') + xml.append(f' ') xml.append(' ') xml.append(' ') - xml.append(f' ') + xml.append(f' ') xml.append(' ') - xml.append(' ') + xml.append(' \n') return '\n'.join(xml) @@ -146,6 +157,7 @@ def generate_xml(self, horizontal_walls: list[list[bool]], vertical_walls: list[ xml_lines.append(self.add_header()) xml_lines.append('') + xml_lines.append(self.generate_assets()) xml_lines.append(self.generate_default_class()) xml_lines.append(' ') xml_lines.extend(self.generate_posts_xml()) @@ -173,38 +185,11 @@ def main(): try: maze_parser = MazeParser(args.cell_size, args.wall_thickness) - - # Parse the maze file lines = maze_parser.parse_maze_file(args.maze_file) - print(f"Parsed maze with {len(lines)} lines.") - - # Extract walls horizontal_walls = maze_parser.extract_horizontal_walls(lines) - print(f"Found {len(horizontal_walls)} horizontal wall rows.") vertical_walls = maze_parser.extract_vertical_walls(lines) - print(f"Found {len(vertical_walls)} vertical wall rows.") - - - - for i in range(len(horizontal_walls)): - for j in range(len(horizontal_walls[i])): - print("0", end="") - if horizontal_walls[i][j]: - print(MazeSymbol.WALL_HORIZONTAL, end="") - else: - print(MazeSymbol.WALL_HORIZONTAL_EMPTY, end="") - print() - - if i < len(vertical_walls): - for j in range(len(vertical_walls[i])): - if vertical_walls[i][j]: - print(MazeSymbol.WALL_VERTICAL, end=" ") - else: - print(MazeSymbol.WALL_HORIZONTAL_EMPTY, end=" ") - print() - - # Generate XML xml_content = maze_parser.generate_xml(horizontal_walls, vertical_walls) + with open(output_file, 'w') as f: f.write(xml_content) From a26ff1d192168b225f9ea5dcd5d97fdb3314bc6f Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sun, 1 Jun 2025 00:43:16 +0200 Subject: [PATCH 08/78] =?UTF-8?q?=E2=9C=A8=20Add=20maze=20generation=20fun?= =?UTF-8?q?ctionality:=20implement=20maze=20parsing,=20wall=20extraction,?= =?UTF-8?q?=20and=20XML=20generation=20for=20MuJoCo?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/model/gen_maze.py | 205 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 205 insertions(+) create mode 100755 mujoco/model/gen_maze.py diff --git a/mujoco/model/gen_maze.py b/mujoco/model/gen_maze.py new file mode 100755 index 0000000..54db130 --- /dev/null +++ b/mujoco/model/gen_maze.py @@ -0,0 +1,205 @@ +#!/usr/bin/env python3 + +import sys +import argparse +from enum import StrEnum +from pathlib import Path + +CELL_CHAR_WIDTH = 4 +CELL_CHAR_HEIGHT = 2 + +class MazeSymbol(StrEnum): + POST = 'o' + WALL_HORIZONTAL = '---' + WALL_HORIZONTAL_EMPTY = ' ' + WALL_VERTICAL = '|' + WALL_VERTICAL_EMPTY = '' + +class MazeParser: + def __init__(self, cell_size: float = 0.18, wall_thickness: float = 0.012, wall_height: float = 0.05): + self.cell_size = cell_size + self.wall_thickness = wall_thickness + self.wall_height = wall_height + self.width = 0 + self.height = 0 + + def parse_maze_file(self, maze_file: str) -> list[str]: + """Parse the maze file and return lines.""" + + with open(maze_file, 'r') as f: + lines = [line.rstrip() for line in f.readlines()] + + if not lines: + raise ValueError("Empty maze file") + + self.maze_width = (len(lines[0]) - 1) // CELL_CHAR_WIDTH + self.maze_height = (len(lines) - 1) // CELL_CHAR_HEIGHT + + print(f"Detected maze dimensions: {self.maze_width}x{self.maze_height}") + + return lines + + def extract_horizontal_walls(self, lines: list[str]) -> list[list[bool]]: + """Extract horizontal wall information from the maze lines.""" + horizontal_walls = [] + for i in range(0, len(lines), 2): + row = [] + for j in range(0, len(lines[i]), 4): + if lines[i][j + 1:j + 4] == MazeSymbol.WALL_HORIZONTAL: + row.append(True) + else: + row.append(False) + horizontal_walls.append(row) + return horizontal_walls + + + def extract_vertical_walls(self, lines: list[str]) -> list[list[bool]]: + """Extract vertical wall information from the maze lines.""" + vertical_walls = [] + for i in range(1, len(lines), 2): + row = [] + for j in range(0, len(lines[i]), 4): + if lines[i][j] == MazeSymbol.WALL_VERTICAL: + row.append(True) + else: + row.append(False) + vertical_walls.append(row) + return vertical_walls + + def generate_posts_xml(self) -> list[str]: + xml = [] + + for row in range(self.maze_height + 1): + for col in range(self.maze_width + 1): + x = col * self.cell_size + y = row * self.cell_size + xml.append( + f' ' + ) + + return xml + + def generate_horizontal_walls_xml(self, horizontal_walls: list[list[bool]]) -> list[str]: + xml = [] + + inverted_horizontal_walls = horizontal_walls[::-1] # Invert the order for MuJoCo + + for row in range(len(inverted_horizontal_walls)): + for col in range(len(inverted_horizontal_walls[row])): + if inverted_horizontal_walls[row][col]: + x = col * self.cell_size + self.cell_size / 2 + y = row * self.cell_size + xml.append( + f' ' + ) + + return xml + + def generate_vertical_walls_xml(self, vertical_walls: list[list[bool]]) -> list[str]: + xml = [] + + # Invert the vertical walls to match MuJoCo's coordinate system + inverted_vertical_walls = vertical_walls[::-1] # Invert the order for MuJoCo + + for row in range(len(inverted_vertical_walls)): + for col in range(len(inverted_vertical_walls[row])): + if inverted_vertical_walls[row][col]: + x = col * self.cell_size + y = row * self.cell_size + self.cell_size / 2 + + xml.append( + f' ' + ) + return xml + + def add_header(self) -> str: + xml = [] + xml.append('') + xml.append(f'') + xml.append(f'') + return '\n'.join(xml) + + def generate_assets(self) -> str: + xml = [] + xml.append(' ') + xml.append(' ') + xml.append(' ') + xml.append(' \n') + return '\n'.join(xml) + + def generate_default_class(self) -> str: + xml = [] + + wall_parameters = 'type="box" material="walls" zaxis="0 1 0"' + wall_size = self.cell_size - self.wall_thickness + + xml.append(' ') + xml.append(' ') + xml.append(f' ') + xml.append(' ') + xml.append(' ') + xml.append(f' ') + xml.append(' ') + xml.append(' ') + xml.append(f' ') + xml.append(' ') + xml.append(' \n') + + return '\n'.join(xml) + + def generate_xml(self, horizontal_walls: list[list[bool]], vertical_walls: list[list[bool]]) -> str: + """Generate the MuJoCo XML for the maze walls.""" + xml_lines = [] + + xml_lines.append(self.add_header()) + + xml_lines.append('') + xml_lines.append(self.generate_assets()) + xml_lines.append(self.generate_default_class()) + xml_lines.append(' ') + xml_lines.extend(self.generate_posts_xml()) + xml_lines.extend(self.generate_horizontal_walls_xml(horizontal_walls)) + xml_lines.extend(self.generate_vertical_walls_xml(vertical_walls)) + xml_lines.append(' ') + xml_lines.append('') + + return '\n'.join(xml_lines) + +def main(): + parser = argparse.ArgumentParser(description="Generate MuJoCo XML maze walls from text maze file") + parser.add_argument("maze_file", help="Path to the maze text file") + parser.add_argument("-o", "--output", help="Output XML file (default: maze.xml)") + parser.add_argument("--cell-size", type=float, default=0.18, help="Cell size in meters (default: 0.18)") + parser.add_argument("--wall-thickness", type=float, default=0.0126, help="Wall thickness in meters (default: 0.0126)") + parser.add_argument("--wall-height", type=float, default=0.05, help="Wall height in meters (default: 0.05)") + + args = parser.parse_args() + + if not Path(args.maze_file).exists(): + print(f"Error: Maze file '{args.maze_file}' not found") + sys.exit(1) + + output_file = args.output or "maze.xml" + + try: + maze_parser = MazeParser(args.cell_size, args.wall_thickness) + lines = maze_parser.parse_maze_file(args.maze_file) + horizontal_walls = maze_parser.extract_horizontal_walls(lines) + vertical_walls = maze_parser.extract_vertical_walls(lines) + xml_content = maze_parser.generate_xml(horizontal_walls, vertical_walls) + + with open(output_file, 'w') as f: + f.write(xml_content) + + print(f"Generated XML maze walls saved to '{output_file}'") + + except ValueError as e: + print(f"Error parsing maze file: {e}") + sys.exit(1) + + +if __name__ == "__main__": + main() From e9786a658abfd71cfe7a8d20bc1dad78ecb215d6 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sun, 1 Jun 2025 00:53:48 +0200 Subject: [PATCH 09/78] =?UTF-8?q?=F0=9F=94=A5=20=20Remove=20unused=20maze?= =?UTF-8?q?=20generation=20files:=20delete=20gen=5Fmaze.py,=20car.xml,=20a?= =?UTF-8?q?nd=20maze.stl?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/gen_maze.py | 204 ------------------------------------------ mujoco/model/car.xml | 72 --------------- mujoco/model/maze.stl | Bin 45484 -> 0 bytes 3 files changed, 276 deletions(-) delete mode 100755 mujoco/gen_maze.py delete mode 100644 mujoco/model/car.xml delete mode 100644 mujoco/model/maze.stl diff --git a/mujoco/gen_maze.py b/mujoco/gen_maze.py deleted file mode 100755 index c8744c7..0000000 --- a/mujoco/gen_maze.py +++ /dev/null @@ -1,204 +0,0 @@ -#!/usr/bin/env python3 - -import sys -import argparse -from enum import StrEnum -from pathlib import Path - -CELL_CHAR_WIDTH = 4 -CELL_CHAR_HEIGHT = 2 - -class MazeSymbol(StrEnum): - POST = 'o' - WALL_HORIZONTAL = '---' - WALL_HORIZONTAL_EMPTY = ' ' - WALL_VERTICAL = '|' - WALL_VERTICAL_EMPTY = '' - -class MazeParser: - def __init__(self, cell_size: float = 0.18, wall_thickness: float = 0.012, wall_height: float = 0.05): - self.cell_size = cell_size - self.wall_thickness = wall_thickness - self.wall_height = wall_height - self.width = 0 - self.height = 0 - - def parse_maze_file(self, maze_file: str) -> list[str]: - """Parse the maze file and return lines.""" - - with open(maze_file, 'r') as f: - lines = [line.rstrip() for line in f.readlines()] - - if not lines: - raise ValueError("Empty maze file") - - self.maze_width = (len(lines[0]) - 1) // CELL_CHAR_WIDTH - self.maze_height = (len(lines) - 1) // CELL_CHAR_HEIGHT - - print(f"Detected maze dimensions: {self.maze_width}x{self.maze_height}") - - return lines - - def extract_horizontal_walls(self, lines: list[str]) -> list[list[bool]]: - """Extract horizontal wall information from the maze lines.""" - horizontal_walls = [] - for i in range(0, len(lines), 2): - row = [] - for j in range(0, len(lines[i]), 4): - if lines[i][j + 1:j + 4] == MazeSymbol.WALL_HORIZONTAL: - row.append(True) - else: - row.append(False) - horizontal_walls.append(row) - return horizontal_walls - - - def extract_vertical_walls(self, lines: list[str]) -> list[list[bool]]: - """Extract vertical wall information from the maze lines.""" - vertical_walls = [] - for i in range(1, len(lines), 2): - row = [] - for j in range(0, len(lines[i]), 4): - if lines[i][j] == MazeSymbol.WALL_VERTICAL: - row.append(True) - else: - row.append(False) - vertical_walls.append(row) - return vertical_walls - - def generate_posts_xml(self) -> list[str]: - xml = [] - - for row in range(self.maze_height + 1): - for col in range(self.maze_width + 1): - x = col * self.cell_size - y = row * self.cell_size - xml.append( - f' ' - ) - - return xml - - def generate_horizontal_walls_xml(self, horizontal_walls: list[list[bool]]) -> list[str]: - xml = [] - - inverted_horizontal_walls = horizontal_walls[::-1] # Invert the order for MuJoCo - - for row in range(len(inverted_horizontal_walls)): - for col in range(len(inverted_horizontal_walls[row])): - if inverted_horizontal_walls[row][col]: - x = col * self.cell_size + self.cell_size / 2 - y = row * self.cell_size - xml.append( - f' ' - ) - - return xml - - def generate_vertical_walls_xml(self, vertical_walls: list[list[bool]]) -> list[str]: - xml = [] - - # Invert the vertical walls to match MuJoCo's coordinate system - inverted_vertical_walls = vertical_walls[::-1] # Invert the order for MuJoCo - - for row in range(len(inverted_vertical_walls)): - for col in range(len(inverted_vertical_walls[row])): - if inverted_vertical_walls[row][col]: - x = col * self.cell_size - y = row * self.cell_size + self.cell_size / 2 - - xml.append( - f' ' - ) - return xml - - def add_header(self) -> str: - xml = [] - xml.append('') - xml.append(f'') - xml.append(f'') - return '\n'.join(xml) - - def generate_assets(self) -> str: - xml = [] - xml.append(' ') - xml.append(' ') - xml.append(' ') - xml.append(' \n') - return '\n'.join(xml) - - def generate_default_class(self) -> str: - xml = [] - - wall_parameters = 'type="box" material="walls" zaxis="0 1 0"' - wall_size = self.cell_size - self.wall_thickness - - xml.append(' ') - xml.append(' ') - xml.append(f' ') - xml.append(' ') - xml.append(' ') - xml.append(f' ') - xml.append(' ') - xml.append(' ') - xml.append(f' ') - xml.append(' ') - xml.append(' \n') - - return '\n'.join(xml) - - def generate_xml(self, horizontal_walls: list[list[bool]], vertical_walls: list[list[bool]]) -> str: - """Generate the MuJoCo XML for the maze walls.""" - xml_lines = [] - - xml_lines.append(self.add_header()) - - xml_lines.append('') - xml_lines.append(self.generate_assets()) - xml_lines.append(self.generate_default_class()) - xml_lines.append(' ') - xml_lines.extend(self.generate_posts_xml()) - xml_lines.extend(self.generate_horizontal_walls_xml(horizontal_walls)) - xml_lines.extend(self.generate_vertical_walls_xml(vertical_walls)) - xml_lines.append(' ') - xml_lines.append('') - - return '\n'.join(xml_lines) - -def main(): - parser = argparse.ArgumentParser(description="Generate MuJoCo XML maze walls from text maze file") - parser.add_argument("maze_file", help="Path to the maze text file") - parser.add_argument("-o", "--output", help="Output XML file (default: maze.xml)") - parser.add_argument("--cell-size", type=float, default=0.18, help="Cell size in meters (default: 0.18)") - parser.add_argument("--wall-thickness", type=float, default=0.0126, help="Wall thickness in meters (default: 0.0126)") - - args = parser.parse_args() - - if not Path(args.maze_file).exists(): - print(f"Error: Maze file '{args.maze_file}' not found") - sys.exit(1) - - output_file = args.output or "maze.xml" - - try: - maze_parser = MazeParser(args.cell_size, args.wall_thickness) - lines = maze_parser.parse_maze_file(args.maze_file) - horizontal_walls = maze_parser.extract_horizontal_walls(lines) - vertical_walls = maze_parser.extract_vertical_walls(lines) - xml_content = maze_parser.generate_xml(horizontal_walls, vertical_walls) - - with open(output_file, 'w') as f: - f.write(xml_content) - - print(f"Generated XML maze walls saved to '{output_file}'") - - except ValueError as e: - print(f"Error parsing maze file: {e}") - sys.exit(1) - - -if __name__ == "__main__": - main() diff --git a/mujoco/model/car.xml b/mujoco/model/car.xml deleted file mode 100644 index 0d4ea67..0000000 --- a/mujoco/model/car.xml +++ /dev/null @@ -1,72 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mujoco/model/maze.stl b/mujoco/model/maze.stl deleted file mode 100644 index 638a4293d557decb0ac123e2be72e7c79e24a9f7..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 45484 zcmb_lPsnH4QT<`GqoW8K1Q!ztn(moMM8X6z{!33L9T-s*m0k!cv>6d$k`O``(FnaT zO%|C^O#B03l8Bj^n7ENPs1P%=PzKv9gdi@0(e4>=K-`GfCLnE{I(4e*{&`>b!nf#G zU)_7^)V;TE-TUr)-`DSZ&lA7)`0L(v<=21ZO|Sd^xIXf#Q@gG{f9K9amrke09zC6| zTsmF7oig>PYWj8w5hV>lbm;AN^?AXHhW)Rl^j<;9Cpc<9IjsxFmizKmVfTE&prSqt7h$hpJulsTxK6p zzaju`94Sjr+o#k0FTUwQayVD#3l7(>2zZDZ8-(-q-o10@+&dUo4Pi*?WobpIBsh{! zdh@Eql{jvOU5!8VxNe%aD}p86!_lkzU;H!cBV5Ln5#Bk>uIo_;94mq)#UZp1^2)~J z+2nxD#+Mwy#g-SsY#kxotDn67o@z&qTH|oL=;^R@S`iaR@(E!sSj?VImoI;l&BYkC ztG8N|Ag71}`V|4y5d!Fs{nHbg=Qj43&zK(_0U3H(S`jLlJ(S*D$hayFuy;OjK))iO zIzqrh&OIVCs||?Tl@ndRB4!TpNiVNH_t^^%CdcV(VHf?@I80A(@NXgT(*eT0x_tRV zimQuv?p!w;n0F1~GW4{B=$H{KSv?d&?fAt{J^Vm&Jo26INV#*E9ypF*Axw3IFf
      S+la zx~@LoOXOAYS-r`t(@7k-OYR)9sCEd!`wk(F^vdS>HJ=}y12XkxaYl?Bg)Ri6A1Myl2>S&wtiWoTxO+Ef?(ViS{u|6w4 z%BTzdicrZ8A@5MfkTxEF747I8Z?QZNJuY%kE`&m9yULo;ST??M?I=LUvMzmS47OU4MHsk8!p5Z*YnN+nfjWw ztY3(cqtJ!m{P?k-{<#a{_}@DZSzHyLIG}fLeElfI>2!op%hM~{wHe zm=PmKp{XygY(0NK9G;~cpRy;jxAl>?qY$Ul5n}Pm*7MJMJ34|g^kGSju)D>uBSwxw z7lP6LLr;DD)8gQ$bv|;acV~pgd__3lYk6 z!1hxicMk4uG#<_r97hPY{KfbFS3Q6Dj%(y_bnFkwu_BO{%Bzw?Z_c!K1jp(0rAyp* z248TveuF@Z_6T}qyS`^phf%`u=m?jiN64`vfG&=bLl4^??f6+^;P)7hfg`wPL?Cwv zaNFASOP_K3#$_$@xEeSNNxghn5fcZv!W-SotK@j(d%~{e`OaZ__%kC+b%b!PSAFXn zwJ*7dn!YdT2=UA+-@k*|sadf1;4Q0)=)%Jw|32aZTQx1xUvLA!irro18m9uaCe z5mtN=3?IDl=JR|CH4Z|2T{kla!xb^&^eRHF(ZJU5Y8vP z+JT>I-r0Fo=SvQrW3e5DfOLcax91SAj**Pk4hYzd@$hrA6)|&&FZ6b!SMPu2Eq2Bv zj{B;2-L5#GUlGt9A=q-%z-3&Xi81CKVX{^O*Rys)MH9DG)r z5wLQE0LSIezwrI2T_0pzh?Z6o9EC0fd-rdD>JeLW$-#Yk;|ojLzt0HJJ#D?XBIw~;pZr~| z=g;T7YY0b2U(wQv7&(ehdiCxrANXRe=XkPH?}9Hm@J47wKy`#b`|Pw)az!rb2)7G8E}ZPFh$s<9qJ=QqwSFjH`MvQe zU&Vb!z!qxn5b(-&OL`uxd^I2pnSBK-D`MslpY*%&@VSvKFAmMcbv%v`Y#X?ESr6#> z9Q)qjAe!%p#4#gm9xMnvy@cNGt#|c&j%+jVkwdw0M!?7(v3O;9S97osj*fXQ4tR*m z`mC2M9-;~|>(CByV9X_>mMnQJtO!^+Km-SB;IbZg;nl{9{Z&Iic0KBt+%6%aL|!F2 z^mc1e5**%(oi8}lyE7ujc7s^G(iuSKfUI){ai}RenHV_=y^AYxfL(IukXIE~Vn0Hx zUU8?w@el{l)K~9TJ9joU+0s>cOO9bi6~f?02;}+GkH62J{ry6p;Z3x)qa)mI zyrFh^MNAxtrXI#?Jd(q+Lh(^=Rb9U#RI)+1S76~~5$P+VwWD)DcD>aKjv1kn;7GLe z^2*-o+j$9AUyQlcmfShwtv*{`2yh-CobNx*e=OrF`>G?{E<6lNrxg(;@+#3ns2w~} zjn?`%EFpIe*RKe(b%b#Acm3cuwsQuoMTo0g=6B=MIJkaAOdN@p-dw1CiQ9@XmkfkB z;yxqH7D{#q=X>A%U(WO0Tt7Mja^!YeSrHRQqC=17_&(82R4*h4z8cVK&^hAkg&6@O z2MD0;i<)P%9ho^h!sN`N&aomUjzotZHy;ngl{j#h3=hSj`;4%)=m5blI{xUdzb)Ir zQ!{@bksNL}PP)jkB1Vqlqu!bk?eH%f-Im1_#v3SmYL5cKLp|NXschrQ>F z_XZ6CH1%QWv?4~1;uAvcu%EqGnXmj?9L~qi5hq49Ka$-_x`(NUxBjIT>>HQGu+GRD z!gV?$!yR8AjR^cSB9g<=v`tKL+_YR$e3VfK`W2y)9RfOg8?LO}aE9h6m%ptV&@(Mp|Jp3!b&KDf~ zI&ei`Wkv zyb$Pt14IKuw21VY69;s%pnD*NuphIeqf5_dcbL}<-zDRo7HE7%P&;@igziyAnQr2U z(%nsni32}wZQD9%=ZUv1Ho;+0-N!%1-1TOFwhIBHdxVcaitUQlD_(P-V0>m@S-Y~S z5JQR34M9%_jyS*Ia}y#ZzMt@xM;{W9Ua=OH4RH#+pGFTHgRNFedcO~dcqXqCL9|?_ zYVe9rcfdqntn+YUjrO-U}Q}cHMwj_6dftULJa#fbeQ4X*&+- zE7PeRD#2Z{*V1VEu5a+Fm2^bPvDN~!#??^EZCjgGj#e@thLw8osv(B;TzcPycLd+Z zlHE8sHdZ_3wH-O;diQVWd`EAx_ju>v`BZ$d6>qKmN!y<_fx!6h5GdhE$B5>s^W6|$ zOWv*=twbC>@;4j}p=cjDo}Sy02rKEcOZwv(m2Z2{yO?)v29JH!O9s1vSUEaEGHSgL z2)_@#JCt<0l5xK~!fK_53qtt=Z(mck9Y7+m;}|#;)%eCE5%}IC_Ka&JrY*gfNADsd zX_v`&^kC6i*h&)J5s-Nip;+>$&j@ednzS!cCWN;GXStXYIIX43IlAR&i25!A=o^P# zSxK`iZ&!}W&bQ2#vNmM&5je0Gcdy2hj*z?{+y=*hD>3EdTUP^lVkI3>@;ri>IW0m` z#z-n+{64g!l?(`tzu%7#8LypV8S`XENw+Kc-Vk2vFxnN>h=yiYW4=ZS+k`Q&D^9$8 zj}kpn=e+CufESu8H{+xTQG$r!6*Ee+)=L^fD{4G*0<%F3wc-hB<;Yr=WkN=)HJa>! z!7KUhGe=$_OT=m0IvztwL&%ax>cFu@*xU%cd3VrPED;B0lVr=4agBX7BCJ-7;D$rF zU4}5XaYe*z#(XJJ#PH45T@XE%GAh^Ht}hI&q$7^o(QWlwA@i#qHZuCEmvqEo?`D?q z-Mu|`dS4+54>MdVNwhuL=e{dX4>H;ZgnbQ|9QTrU2fNCyOQvQNVAL#*RLkTV_lK`>Vx_3qH`gI(B0ug_yXAe6hjRv?xqFO5fEk$p5X zV&)Ol7@(HWM4)Dj=Ga%=mR302Gv_ZHy`&>@T{#ocYsr%DvxgDvB^?p+W?!;FcrEQE zR*qiM?S_8Gf#+5}Ve`Ah#~#J+`6wUfTXAa&SI(itPo9X-Y5;T$#}>iWpmVHyhMebS zKV^GLrEMGY<;1I(bVPr{KYG;>y_WQAU-goXNO>8LEdsSPgZ)HgjFQL(Rx;XEp2u_H z5hC+SDTcXN_xMAd~b-V zb+;&yR~&Qh$P2BU$bO?OfJx0?tnzDk6QV!Pt~ zimznUXvgH91bXnwIk>(W0xPJmqA+F|Nphd(Bcj(bS(bE%7xYRRzsqhzB-a)pwAYH0 zuGA$$ryD5AQgG>POuU6SafSCj+4S46;zY~w42aA2I%vJ*+$FW$5=XF?G(>us<<(c> z(1};@`Cbe1%7L91OKRTx5&toycF%! zutZt8qotcUMuh&Z1#Hg<#ir+sSpR(Q;5nv#Hyqd)!Cul3%tZBD5PQT>D=cktN$hRB zcI=EG=e|mFF})f}8iHrmsMhvAWA?%9)vVPW&No>t4;yBy_?q0$`XUlH2&+|SJ7JA0 z#$3BPYhl)cU1jZsBWs~ghFUo5vuo+As%5d72+Y4gWQNEw? z{T7{i7pH)@?9?$iEZP%+$mbP^%o(>Ui((E&TyaM`AbN?igx|4y#ihI2QTg?zd4)4w zyZjs`YW6dk$kg1aVh(Cf`+dk`ZYO7y-eYRF#B9L*3Tm#iwj-{E?^Y`~)*01Hx?RaD zB3o`_kut_YW1~Adge{k5MrZ_e-y(9HJI503$&M1)!(H;dA-vY%{7|%G=5)In|COU7 zhFY3Y%lr@mG29U$*D92~yFsv)IAZ0g-py#XSlvbYCU#p?cZB4Ao*2=-L0GMfm=zKG zC*NV@UJCQvX0Bbe^T?L>l8(ss89FZIl5Q(J#4}E|W9h4*q#=^y-Vi$Fggf(?SD&Cn z9A-D$G;<7yR;%|!;AUQJQO#70w-Odv^ z2YZQjEp{&t&q55f!cy)jvK{J0lyrpT4I*26%UT_C!DFd<0ey^1>v=Ef2+5q)cSD@j zyJNpMyM1TiKFO;A(Q2iBiL0TcA#z`GZwS=Nth7X8FA;~^jl8mOjJ8^>u(TP$p`;;{ zyOuq*IA|*nR!b)mE5}$;2%cXeH0FJKNAz0K>xrhKq?dF=%6Yb^G2bGrR_>DWq)>=n z((R^x;ZTY05p=fxUGpr_^_(BO5=Uvy%Y@xGh+gZS_KE!oNHM+hiqFny^-wFKaz$t! zpkzRF52G!S8M>`rD{|$IgHID)(h;#oy$hn(3J%M3%--3%+8cVw1~Gg&z)an3WuC|L z4p+MkM=$A!T$>C%_Ekp=wUk35%Vs;&7r0CAvq<)FSEepqN2I(&d)3$JIW#)cH?fV; z#9<}fZpq84CXxYh);g$saLlM)(h!-a@+t2ugg=dCEX*7gS5~6@ z2fGntdX_7%21Kuwa{IzGp4Id$Yxz|XgSIKg#G5T2N;)Dq)R(JQ-iBUF9H(Ujdr3#A z9ruK&H@xPH#R5FS6>%jE?t@*`)P0Lc4^T@rD~^Lh?LgiN*|T@@Er?#SLo6#!`EIs^ z_F9Ktb$@q=m7^mhqt*+-q5HmfcL*Pe`p4d4`S}#zT5&bdJ1=|swo7Ly!U&-=60ANQ zT?mfFD<634h2l78;{m%fhw1H_5wLZHz}c+*_L+Wf8-I5&>~_RCJPEsP-dDuTA#~`| zEBGf4tSpjor;Hk04I&B?!IB-Kkf>ulaMAzjp7l@Xpj~_+jjL81{3HZR#8GJKRTFny zuoYtva_7*P!?rH6vxMl5*m;HBhAnWgoVgv&LyB7Je z(qNoc#K=(|hTd-U$})ug{|5Y04zo#e=YSr#8Nm#(L$KvN`|x*kF+Ylfc5k|E$}3{z zD73Ou^Xl2+z}qs|B_CxV_!ceO5eSv+5NP?eD}VS5+7B-J3A>%+q0EWYuZWpLXzA4s z`)xq``_y1XPW1D=Xh(7AWQXl21U%d$=+&?O`==g29dI$yMh>8BUSQw8B7iQAlBvhP zoV7pjrcdPmZvdN}PaL|>2+p+~0({`m1td;YIM0RPou@70m=PlfYN@8~^olrg@JYM) zbk0P*T3!gfom>&*_{?AaO^yd`#Mf=nCg^0F_Z2a6uohb`B%|cZKmU>T_hdeNiKXOI zP3`n>f??2X+YRUVQKGs2w&Q$VV1e4MDrm)6(g*B1Vov3xSvgoL6x8idKB&Mh&iq zGeRXBM0<`xE&Hp$@f{Iu6<@8GT60mq5Rg$)h{Y@GBVNg2=aeZMIyvGjydtreY!S}= ziGTZMtsmBo$R(Y_W#)9gU0f07;ev=S2IQ5Ghy9&Y{M`?qgLcPht;M6rFYw)z$Fem*+~Xk9Pz|gaW$0e5YGMNOFyc4ZtEbT0rR|Z zm|PKx6R{Nm??%MHjpB8F*n5J`mmKl_U`CjcBZRm7$(Q~|?JzDp8(TX%!tKU0v&$=D z;z%_0_HVZ1>(7aU(NKIkk)nP@C`MO=-j9F>7a|hRNxt8)ekX!<`TQgf)-S}!Q65S! zulVwsZD}Eb_xGDtYcYc-O;TO;fwj=Ux=WsbzXXjWEK;I0zhtn3l=>R+GuJ&Rdf z*eAw*ry+o*9(6FwRs>7rRq+Xdv4)H*Ic%4ja_5NM>x?ky5yHLde>JjUaZF3& zU*@hDoe|viao0b#gL4A2bL1ljXYGnW59|@-_~`3jIqyCW>tG>hmunQ|6)|!YANAe= zmeX}{CF5Hwt!me;pU9!NSv5b#k`+O({4dEl1@cwIm7kIqU&fW|SA@YQ#K^7Q#orxF z4qIhXHgt0M>a!x?;sL_By$SvgdLVr72)7%36&%6{fu9>k%GBfExp$Fk@f%ykS1pHr zML>0caIfs|s(l}DL~7M<2uH`B!Q~YJ@QjF-yMFX4IDG%s`GO}2Et_O(N^eJgh~oQwDi^v&sQIR;y*uK99T1ouQ+aUJSGHe9UUiKxt;5GMm8ZjH+)gvh0I+TYs8Ms?UF;t8KL&92zuC`wzZG$9F*^~agwDKp_17{ z>Bn}!7IGqLQ|=tvVfJ>A{Rlzy^*8>6^6tf&cg5l8oafN52=(&pq4a8p{pI%ui^FQ? zJU6?^L9f}4LcrD$0$$Z+NI!>0Uv-4Zu18IB2qQ$4q*p>0f?nB^Oa2{tKj-Ou!J+e? z8G*62lc5b_T7h?CsSCArAS4a|WDS zAg&4lx%O3ji#8+h^N1L@t(0DU?uUQ-GvYumO70x$-5J5S+98}fT;eF!nt?OQSaJL< zES@_e-X=sZSrIFTJaK=ccjq<*MPzFsdS7+)j^kX6#}&I{PHRo1teglQww%3ZBk1!1 zdlSh*>q{Xa$}UPeg0h|en4Xt7xH|hf;VVBP#`al~{XQai)`wc@q07;}%#HG@PN4eR z@Pw?F`8EKKorOTHqntRFlwJCdjJj|%TfLU# zeCto&(h(_pZ0|S*gw@grt{lB& zw5vXxz3K?X80<=4JATFPSkmoA?0Wz8cI*(an{UkgL}WxDnyp8@q}#>ZLr-JI9V4n% za``Qf|Gk)ujFBIRWS746u$jO`8wvUq$5&Z<7$VnT0V-q*wsqryW^~d-7(jawXcSf zj*z^JAbSt96D_y%KA))=&1){Ny*rdN#K6q~%x8mctNYF#Svl12ENO_tb_|H2R`mMP z@|xRt?-;quYDr$_V7`g-{*PH}T-p!;=&P|8qaE(MvNqp__jYg$=x?tTvOgpCzEUp|!631bit0QKP zjwl(m6bqQ6-K$>G5h-tGRM{G9aRhhsyh=Jk(X-2N9Z|D3XS1I`kL`%~@%`rpvBp(9 zJx{w@ZD5YbCU1a@M^v1Ff)(Cp$J-t4@} ztPg*4-kE*Lo>zK1wg}V`V%hZ#C936!+tH3T&*R(@k%*H!?47YTNVw(6|cq2 zK$&@kGl??n2*Do3k9x-fj%(HqtfJ7z9pd0seT#!?`o4%V{>2pt*|59#c*l64+RPz8 zu~U-lyR_(4?w}l-9PGU#1owCYBKMlu7h-+o8ENglq35NRYyY%IY`pS02pd3RM%iU! z&&+@yq6eU6?|Nk5M}66EvOP7XZGW0Hon1r>B|`giX87NqIh|ub__?kb>m2=ga71`X zw@aB3>Q2pGH3VugKPrw##JTJrW^FVPeMS9_qnB{5aF+FXv*Vzxs+B!*mzEbo^Kml= zH3oi<70laljqO{;TyuiaP>9;$&ry>8x~&)wkKvG)8O1eRZ%oKR9c8ZAy{~Fcz;DhE zpQ+itdDgP;VMDw7knfIgf7Qx{UfPiy9pSQOuzFeZL%z=$N4>5)eAPT>pY+Vh8GI!o z^hS(iA$m!SV4w3YgJF+|%-~rRYdfmV9GNrn)Bx3xcLTA+Rb?0SEPL16l;bgTRPUl> zMN}?UJ1P><{}qX;7Z;i|<`OU2AeMG?TPa&DWrp;+aWFHWWJDbGRktg2GXb9BG zRX|*c=q0je_Zqpg5WQB(+-Lg$M81mo;m=FT1{e>@yPOyZtD*V9yY`P^rM0|D21JaU zbmdj;Qq9&-ij#v4;!8jogcs3mWey=M-eCmj)T&L5*0IXZ&1$f0=& zzea?Ybh{yE9yk->wfya3{WX%t(Mtxqc$=wR6r}X3Bl=e;q2IK8DCu@7Z$@x~KrM@o zc;hpV$5_(r#y)rD7!a+N^mb?F$d(T!4N*JeIofv!&L+;*l|z}qOLC8n$iEfd%p3z^ zs3i`q*g1l^-?5U8NLjnIvk~}z0oX^ij}T%g8SKjcnPWgi#=(=M^y>~_Ea`SBSEif2 zqOGb`mge<*EZHD3FRvW!q+rOX?Ibl;gYezXTa&LL*1W5X+)pN0OFOcaqnD^1`fWEn zrCy(+y`JKAZme1wx#RT%&r9OKm$DIoo4)ELg%I~{1UrJBu$J0~^@>+yQL*AK+4t1f zSkrsDBa#&*J48PnjFq!&CE9BpdX;A+bsEK|$)ny?pZK0iXydro-YvUAaLoS?o(|q1 From 8bf146a04bfd3f4c44528393108f69a02da949f7 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sun, 1 Jun 2025 00:54:15 +0200 Subject: [PATCH 10/78] =?UTF-8?q?=F0=9F=94=A7=20Fix=20model=20file=20path?= =?UTF-8?q?=20in=20robot.xml:=20update=20maze=20model=20reference=20to=20c?= =?UTF-8?q?orrect=20location?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/model/robot.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mujoco/model/robot.xml b/mujoco/model/robot.xml index f41b480..e66df26 100644 --- a/mujoco/model/robot.xml +++ b/mujoco/model/robot.xml @@ -5,7 +5,7 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From bd5e56db013dc50bf82d00d1fecc9fab7136f0da Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sun, 1 Jun 2025 21:19:27 +0200 Subject: [PATCH 12/78] =?UTF-8?q?=F0=9F=94=A7=20Fix=20XML=20header=20forma?= =?UTF-8?q?tting:=20add=20newline=20for=20improved=20readability=20in=20ge?= =?UTF-8?q?nerated=20maze=20XML?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/model/gen_maze.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mujoco/model/gen_maze.py b/mujoco/model/gen_maze.py index 54db130..8df75f4 100755 --- a/mujoco/model/gen_maze.py +++ b/mujoco/model/gen_maze.py @@ -119,7 +119,7 @@ def add_header(self) -> str: xml = [] xml.append('') xml.append(f'') - xml.append(f'') + xml.append(f'\n') return '\n'.join(xml) def generate_assets(self) -> str: From 1f8645519bce0727cea84345fb8ab5f1a29984e0 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sun, 1 Jun 2025 21:30:17 +0200 Subject: [PATCH 13/78] =?UTF-8?q?=F0=9F=94=A7=20Clean=20up=20MazeParser:?= =?UTF-8?q?=20remove=20unnecessary=20docstrings=20and=20whitespace=20for?= =?UTF-8?q?=20improved=20readability?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/model/gen_maze.py | 21 ++------------------- 1 file changed, 2 insertions(+), 19 deletions(-) diff --git a/mujoco/model/gen_maze.py b/mujoco/model/gen_maze.py index 8df75f4..fe664b9 100755 --- a/mujoco/model/gen_maze.py +++ b/mujoco/model/gen_maze.py @@ -24,8 +24,6 @@ def __init__(self, cell_size: float = 0.18, wall_thickness: float = 0.012, wall_ self.height = 0 def parse_maze_file(self, maze_file: str) -> list[str]: - """Parse the maze file and return lines.""" - with open(maze_file, 'r') as f: lines = [line.rstrip() for line in f.readlines()] @@ -36,11 +34,9 @@ def parse_maze_file(self, maze_file: str) -> list[str]: self.maze_height = (len(lines) - 1) // CELL_CHAR_HEIGHT print(f"Detected maze dimensions: {self.maze_width}x{self.maze_height}") - return lines def extract_horizontal_walls(self, lines: list[str]) -> list[list[bool]]: - """Extract horizontal wall information from the maze lines.""" horizontal_walls = [] for i in range(0, len(lines), 2): row = [] @@ -54,7 +50,6 @@ def extract_horizontal_walls(self, lines: list[str]) -> list[list[bool]]: def extract_vertical_walls(self, lines: list[str]) -> list[list[bool]]: - """Extract vertical wall information from the maze lines.""" vertical_walls = [] for i in range(1, len(lines), 2): row = [] @@ -77,13 +72,11 @@ def generate_posts_xml(self) -> list[str]: f' ' ) - return xml def generate_horizontal_walls_xml(self, horizontal_walls: list[list[bool]]) -> list[str]: xml = [] - - inverted_horizontal_walls = horizontal_walls[::-1] # Invert the order for MuJoCo + inverted_horizontal_walls = horizontal_walls[::-1] for row in range(len(inverted_horizontal_walls)): for col in range(len(inverted_horizontal_walls[row])): @@ -94,21 +87,17 @@ def generate_horizontal_walls_xml(self, horizontal_walls: list[list[bool]]) -> l f' ' ) - return xml def generate_vertical_walls_xml(self, vertical_walls: list[list[bool]]) -> list[str]: xml = [] - - # Invert the vertical walls to match MuJoCo's coordinate system - inverted_vertical_walls = vertical_walls[::-1] # Invert the order for MuJoCo + inverted_vertical_walls = vertical_walls[::-1] for row in range(len(inverted_vertical_walls)): for col in range(len(inverted_vertical_walls[row])): if inverted_vertical_walls[row][col]: x = col * self.cell_size y = row * self.cell_size + self.cell_size / 2 - xml.append( f' ' @@ -132,7 +121,6 @@ def generate_assets(self) -> str: def generate_default_class(self) -> str: xml = [] - wall_parameters = 'type="box" material="walls" zaxis="0 1 0"' wall_size = self.cell_size - self.wall_thickness @@ -147,15 +135,11 @@ def generate_default_class(self) -> str: xml.append(f' ') xml.append(' ') xml.append(' \n') - return '\n'.join(xml) def generate_xml(self, horizontal_walls: list[list[bool]], vertical_walls: list[list[bool]]) -> str: - """Generate the MuJoCo XML for the maze walls.""" xml_lines = [] - xml_lines.append(self.add_header()) - xml_lines.append('') xml_lines.append(self.generate_assets()) xml_lines.append(self.generate_default_class()) @@ -165,7 +149,6 @@ def generate_xml(self, horizontal_walls: list[list[bool]], vertical_walls: list[ xml_lines.extend(self.generate_vertical_walls_xml(vertical_walls)) xml_lines.append(' ') xml_lines.append('') - return '\n'.join(xml_lines) def main(): From 4972b71067cd7bfa1a6f4b4d1da225be3236e81a Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Mon, 2 Jun 2025 00:57:00 +0200 Subject: [PATCH 14/78] =?UTF-8?q?=F0=9F=94=A7=20Enhance=20MazeParser:=20ad?= =?UTF-8?q?d=20functionality=20to=20find=20start=20and=20goal=20positions?= =?UTF-8?q?=20in=20maze=20files?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/model/gen_maze.py | 96 ++++++++++++++++++++++++++++++++++++---- 1 file changed, 88 insertions(+), 8 deletions(-) diff --git a/mujoco/model/gen_maze.py b/mujoco/model/gen_maze.py index fe664b9..fef6941 100755 --- a/mujoco/model/gen_maze.py +++ b/mujoco/model/gen_maze.py @@ -14,6 +14,8 @@ class MazeSymbol(StrEnum): WALL_HORIZONTAL_EMPTY = ' ' WALL_VERTICAL = '|' WALL_VERTICAL_EMPTY = '' + START = 'S' + GOAL = 'G' class MazeParser: def __init__(self, cell_size: float = 0.18, wall_thickness: float = 0.012, wall_height: float = 0.05): @@ -22,6 +24,8 @@ def __init__(self, cell_size: float = 0.18, wall_thickness: float = 0.012, wall_ self.wall_height = wall_height self.width = 0 self.height = 0 + self.start = None + self.goals = [] def parse_maze_file(self, maze_file: str) -> list[str]: with open(maze_file, 'r') as f: @@ -61,6 +65,62 @@ def extract_vertical_walls(self, lines: list[str]) -> list[list[bool]]: vertical_walls.append(row) return vertical_walls + def find_start_and_goals(self, lines: list[str]) -> tuple[tuple[int, int], list[tuple[int, int]]]: + start = None + goals = [] + + row = 0 + col = 0 + + inverted_lines = lines[::-1] + + for i in range(1, len(inverted_lines), 2): + for j in range(2, len(inverted_lines[i]), 4): + if inverted_lines[i][j] == MazeSymbol.START: + if start is not None: + raise ValueError("Multiple start points found") + start = (row, col) + elif inverted_lines[i][j] == MazeSymbol.GOAL: + goals.append((row, col)) + col += 1 + row += 1 + col = 0 + + self.start = start + self.goals = goals + + return start, goals + + def is_start_or_goal(self, row: int, col: int) -> bool: + if (row, col) == self.start: + return True + if (row, col) in self.goals: + return True + return False + + def is_post_on_start_cell(self, row: int, col: int) -> bool: + if self.is_start_or_goal(row, col) or \ + self.is_start_or_goal(row - 1, col) or \ + self.is_start_or_goal(row, col - 1) or \ + self.is_start_or_goal(row - 1, col - 1): + return True + + return False + + def is_horizontal_wall_on_start_cell(self, row: int, col: int) -> bool: + if self.is_start_or_goal(row - 1, col) or \ + self.is_start_or_goal(row, col): + return True + + return False + + def is_vertical_wall_on_start_cell(self, row: int, col: int) -> bool: + if self.is_start_or_goal(row, col) or \ + self.is_start_or_goal(row, col - 1): + return True + + return False + def generate_posts_xml(self) -> list[str]: xml = [] @@ -68,8 +128,10 @@ def generate_posts_xml(self) -> list[str]: for col in range(self.maze_width + 1): x = col * self.cell_size y = row * self.cell_size + + class_name = 'post_w' if self.is_post_on_start_cell(row, col) else 'post' xml.append( - f' ' ) return xml @@ -83,8 +145,10 @@ def generate_horizontal_walls_xml(self, horizontal_walls: list[list[bool]]) -> l if inverted_horizontal_walls[row][col]: x = col * self.cell_size + self.cell_size / 2 y = row * self.cell_size + + class_name = 'hwall_w' if self.is_horizontal_wall_on_start_cell(row, col) else 'hwall' xml.append( - f' ' ) return xml @@ -98,8 +162,10 @@ def generate_vertical_walls_xml(self, vertical_walls: list[list[bool]]) -> list[ if inverted_vertical_walls[row][col]: x = col * self.cell_size y = row * self.cell_size + self.cell_size / 2 + + class_name = 'vwall_w' if self.is_vertical_wall_on_start_cell(row, col) else 'vwall' xml.append( - f' ' ) return xml @@ -121,18 +187,30 @@ def generate_assets(self) -> str: def generate_default_class(self) -> str: xml = [] - wall_parameters = 'type="box" material="walls" zaxis="0 1 0"' - wall_size = self.cell_size - self.wall_thickness + red_wall_parameters = 'type="box" zaxis="0 1 0" material="walls"' + white_wall_parameters = 'type="box" zaxis="0 1 0" rgba="1 1 1 1"' + length = (self.cell_size - self.wall_thickness) / 2 + height = self.wall_height / 2 + width = self.wall_thickness / 2 xml.append(' ') xml.append(' ') - xml.append(f' ') + xml.append(f' ') + xml.append(' ') + xml.append(' ') + xml.append(f' ') xml.append(' ') xml.append(' ') - xml.append(f' ') + xml.append(f' ') + xml.append(' ') + xml.append(' ') + xml.append(f' ') xml.append(' ') xml.append(' ') - xml.append(f' ') + xml.append(f' ') + xml.append(' ') + xml.append(' ') + xml.append(f' ') xml.append(' ') xml.append(' \n') return '\n'.join(xml) @@ -172,6 +250,8 @@ def main(): lines = maze_parser.parse_maze_file(args.maze_file) horizontal_walls = maze_parser.extract_horizontal_walls(lines) vertical_walls = maze_parser.extract_vertical_walls(lines) + start, goals = maze_parser.find_start_and_goals(lines) + print(f"Start point: {start}, Goals: {goals}") xml_content = maze_parser.generate_xml(horizontal_walls, vertical_walls) with open(output_file, 'w') as f: From 0617a9da6c092eb69157894a8ba20b59b9341453 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 3 Jun 2025 00:58:18 -0300 Subject: [PATCH 15/78] =?UTF-8?q?=E2=9A=97=EF=B8=8F=20Add=20dc=20motor=20a?= =?UTF-8?q?ctuator=20to=20mujoco=20model?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/model/robot.xml | 18 +++++------------- 1 file changed, 5 insertions(+), 13 deletions(-) diff --git a/mujoco/model/robot.xml b/mujoco/model/robot.xml index e66df26..569ee30 100644 --- a/mujoco/model/robot.xml +++ b/mujoco/model/robot.xml @@ -34,6 +34,9 @@ + + + @@ -67,20 +70,9 @@ - - - - - - - - - - - - - + + From 39502e7b567e809dff99ac5e852fa2c677ae3985 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 3 Jun 2025 07:57:27 -0300 Subject: [PATCH 16/78] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Fix=20mujoco=20model?= =?UTF-8?q?=20motor=20constants?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/model/robot.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mujoco/model/robot.xml b/mujoco/model/robot.xml index 569ee30..da6c19f 100644 --- a/mujoco/model/robot.xml +++ b/mujoco/model/robot.xml @@ -35,7 +35,7 @@ - + From 702ad7c6cae31be3898708bde1bfc82e5a054c25 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 4 Jun 2025 17:09:54 +0200 Subject: [PATCH 17/78] =?UTF-8?q?=F0=9F=94=A7=20Update=20mujoco=20model:?= =?UTF-8?q?=20add=20ROS2=20plugin=20configuration=20and=20fix=20dc=5Fmotor?= =?UTF-8?q?=20general=20attributes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/model/robot.xml | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/mujoco/model/robot.xml b/mujoco/model/robot.xml index da6c19f..0b34163 100644 --- a/mujoco/model/robot.xml +++ b/mujoco/model/robot.xml @@ -1,6 +1,15 @@ + + + + + + + + + @@ -35,7 +44,7 @@ - + From b20d373e60c44bbdd9f97611d9feefc69206a4da Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 4 Jun 2025 17:11:27 +0200 Subject: [PATCH 18/78] =?UTF-8?q?=F0=9F=94=A7=20Update=20mujoco=20maze=20m?= =?UTF-8?q?odel?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/model/maze.xml | 62 +++++++++++++++++++++++++------------------ 1 file changed, 36 insertions(+), 26 deletions(-) diff --git a/mujoco/model/maze.xml b/mujoco/model/maze.xml index 17b0178..f89ea2a 100644 --- a/mujoco/model/maze.xml +++ b/mujoco/model/maze.xml @@ -1,6 +1,7 @@ + @@ -9,19 +10,28 @@ - + + + + - + + + + - + + + + - - + + @@ -37,8 +47,8 @@ - - + + @@ -146,9 +156,9 @@ - - - + + + @@ -163,9 +173,9 @@ - - - + + + @@ -180,9 +190,9 @@ - - - + + + @@ -309,7 +319,7 @@ - + @@ -363,8 +373,8 @@ - - + + @@ -375,8 +385,8 @@ - - + + @@ -446,8 +456,8 @@ - - + + @@ -507,7 +517,7 @@ - + @@ -516,8 +526,8 @@ - - + + From 5abe80725ce2ef68b375ae504892e58f9edfbdc2 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Fri, 30 May 2025 01:20:50 +0200 Subject: [PATCH 19/78] =?UTF-8?q?=F0=9F=94=A7=20Fix=20sensor=20tag=20forma?= =?UTF-8?q?tting=20for=20GPU=20Lidar=20sensors=20in=20robot.sdf?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- gazebo/models/robot.sdf | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gazebo/models/robot.sdf b/gazebo/models/robot.sdf index 86b69e4..82b1604 100644 --- a/gazebo/models/robot.sdf +++ b/gazebo/models/robot.sdf @@ -92,7 +92,7 @@ true imu - " + 0 0 0 0 0 0 lidar_0 100 @@ -120,7 +120,7 @@ 1 true - " + 0 0 0 0 0 0 lidar_1 100 @@ -148,7 +148,7 @@ 1 true - " + 0 0 0 0 0 0 lidar_2 100 @@ -176,7 +176,7 @@ 1 true - " + 0 0 0 0 0 0 lidar_3 100 From 9262e468d6c528440206b06c5130b4487a3fc3e2 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Fri, 30 May 2025 14:35:45 +0200 Subject: [PATCH 20/78] =?UTF-8?q?=F0=9F=94=A7=20Update=20launch=20and=20ta?= =?UTF-8?q?sk=20configurations=20for=20ROS=20simulation=20debugging?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/launch.json | 24 +++++++++++++----------- .vscode/tasks.json | 12 ++++++------ 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 060245a..526b128 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -1,21 +1,23 @@ { - // For information on how to use the debug with ROS see - // https://github.com/ms-iot/vscode-ros/blob/master/doc/debug-support.md "version": "0.2.0", "configurations": [ { - // Note: Install GDB to use this configuration - "name": "ROS: Attach", - "type": "ros", - "request": "attach" + "name": "ROS Launch", + "request": "launch", + "type": "cppdbg", + "preLaunchTask": "ROS Launch Micras Simulation", + "serverLaunchTimeout": 10000, + "miDebuggerServerAddress": "localhost:3000", + "cwd": "${workspaceFolder}", + "program": "${workspaceFolder}/../../install/micras_simulation/lib/micras_simulation/${input:args}", }, { - "name": "ROS: Launch", - "type": "ros", + "name": "ROS Attach", "request": "launch", - "target": "${workspaceFolder}/launch/include/node.launch.xml", - "preLaunchTask": "ROS Launch World", - "arguments": ["name:=${input:args}"] + "type": "cppdbg", + "miDebuggerServerAddress": "localhost:3000", + "cwd": "${workspaceFolder}", + "program": "${workspaceFolder}/../../install/micras_simulation/lib/micras_simulation/${input:args}", } ], "inputs": [ diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 694909a..a9da3a2 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -2,15 +2,15 @@ "version": "2.0.0", "tasks": [ { - "label": "ROS Launch World", + "label": "ROS Launch Micras Simulation", "type": "ros", - "command": "ros2 launch micras_simulation world.launch.xml", + "command": "ros2 launch micras_simulation micras.launch.xml", "isBackground": true, "problemMatcher": [ { "pattern": [ { - "regexp": ".", + "regexp": "", "file": 1, "location": 2, "message": 3 @@ -18,11 +18,11 @@ ], "background": { "activeOnStart": true, - "beginsPattern": ".", - "endsPattern": ".", + "beginsPattern": "\\[INFO\\] \\[launch\\]", + "endsPattern": "\\[gdbserver-\\d+\\] Listening on port" } } ] - }, + } ] } From 9fac80687a6fc8d7e53c2241b9e3e40b1a6917dd Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Fri, 30 May 2025 17:32:24 +0200 Subject: [PATCH 21/78] =?UTF-8?q?=F0=9F=94=A7=20Add=20launch-prefix=20for?= =?UTF-8?q?=20gdbserver=20in=20micras=5Fnode=20configuration?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- launch/include/node.launch.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/launch/include/node.launch.xml b/launch/include/node.launch.xml index 38c8db1..c480ca8 100644 --- a/launch/include/node.launch.xml +++ b/launch/include/node.launch.xml @@ -1,5 +1,7 @@ - + From c30857a32da233dac88b1d3a0680b97600509295 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Fri, 30 May 2025 18:05:52 +0200 Subject: [PATCH 22/78] =?UTF-8?q?=F0=9F=94=A7=20Update=20debugger=20config?= =?UTF-8?q?uration=20and=20launch=20command=20for=20ROS=20simulation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/launch.json | 4 ++++ .vscode/tasks.json | 4 ++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 526b128..b1c49cb 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -8,6 +8,8 @@ "preLaunchTask": "ROS Launch Micras Simulation", "serverLaunchTimeout": 10000, "miDebuggerServerAddress": "localhost:3000", + "miDebuggerPath": "gdb", + "miDebuggerArgs": "-x ${workspaceFolder}/scripts/pause_gazebo.py", "cwd": "${workspaceFolder}", "program": "${workspaceFolder}/../../install/micras_simulation/lib/micras_simulation/${input:args}", }, @@ -15,6 +17,8 @@ "name": "ROS Attach", "request": "launch", "type": "cppdbg", + "miDebuggerPath": "gdb", + "miDebuggerArgs": "-x ${workspaceFolder}/scripts/pause_gazebo.py", "miDebuggerServerAddress": "localhost:3000", "cwd": "${workspaceFolder}", "program": "${workspaceFolder}/../../install/micras_simulation/lib/micras_simulation/${input:args}", diff --git a/.vscode/tasks.json b/.vscode/tasks.json index a9da3a2..9d10116 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -4,7 +4,7 @@ { "label": "ROS Launch Micras Simulation", "type": "ros", - "command": "ros2 launch micras_simulation micras.launch.xml", + "command": "bash -c 'source /opt/ros/jazzy/setup.bash && source ${workspaceFolder}/../../install/setup.bash && ros2 launch micras_simulation micras.launch.xml'", "isBackground": true, "problemMatcher": [ { @@ -19,7 +19,7 @@ "background": { "activeOnStart": true, "beginsPattern": "\\[INFO\\] \\[launch\\]", - "endsPattern": "\\[gdbserver-\\d+\\] Listening on port" + "endsPattern": "Listening on port" } } ] From 9da023f847ab25dfb2f43146aa24b0f2bea88221 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Fri, 30 May 2025 18:06:02 +0200 Subject: [PATCH 23/78] =?UTF-8?q?=F0=9F=94=A7=20Add=20script=20to=20pause?= =?UTF-8?q?=20and=20continue=20Gazebo=20simulation=20using=20GDB=20events?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- scripts/pause_gazebo.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) create mode 100644 scripts/pause_gazebo.py diff --git a/scripts/pause_gazebo.py b/scripts/pause_gazebo.py new file mode 100644 index 0000000..d377d2c --- /dev/null +++ b/scripts/pause_gazebo.py @@ -0,0 +1,20 @@ +import gdb +import os +# gz service -s /world/maze/control --reqtype gz.msgs.WorldControl --reptype gz.msgs.Boolean --req 'pause: true' + +def set_gazebo_paused(pause): + pause_command = "true" if pause else "false" + command = f"gz service -s /world/maze/control --reqtype gz.msgs.WorldControl --reptype gz.msgs.Boolean --req 'pause: {pause_command}'" + print(f"Executing command: {command}") + os.system(command) + +def stop(ev): + print("Stopping Gazebo...") + set_gazebo_paused(True) + +def cont(ev): + print("Continuing Gazebo...") + set_gazebo_paused(False) + +gdb.events.stop.connect(stop) +gdb.events.cont.connect(cont) From c20d2455b9cc3e5d1b96b6a5902a90cb3db6ce8d Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Fri, 30 May 2025 18:07:38 +0200 Subject: [PATCH 24/78] =?UTF-8?q?=F0=9F=94=A7=20Fix=20maintainer=20name=20?= =?UTF-8?q?formatting=20in=20package.xml?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index fb8cf6b..0f52035 100644 --- a/package.xml +++ b/package.xml @@ -6,7 +6,7 @@ TODO: Package description santi - + cosme TODO: License declaration From 5107412fd10e2928ad575cfca449d53d96ff835c Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Fri, 30 May 2025 23:18:34 +0200 Subject: [PATCH 25/78] =?UTF-8?q?=F0=9F=94=A7=20Refactor=20launch=20files?= =?UTF-8?q?=20to=20use=20dynamic=20debug=20argument=20for=20gdbserver=20co?= =?UTF-8?q?nfiguration?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- launch/include/node.launch.xml | 5 ++++- launch/micras.launch.xml | 2 ++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/launch/include/node.launch.xml b/launch/include/node.launch.xml index c480ca8..4158461 100644 --- a/launch/include/node.launch.xml +++ b/launch/include/node.launch.xml @@ -1,7 +1,10 @@ + + + + launch-prefix="$(var prefix_string)"/> diff --git a/launch/micras.launch.xml b/launch/micras.launch.xml index 08720f8..7875bdf 100644 --- a/launch/micras.launch.xml +++ b/launch/micras.launch.xml @@ -1,9 +1,11 @@ + + From 82242fa39ef94758caf852e6064be9725e72f6dd Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Fri, 30 May 2025 23:20:15 +0200 Subject: [PATCH 26/78] =?UTF-8?q?=F0=9F=94=A7=20Update=20subproject=20comm?= =?UTF-8?q?it=20reference=20in=20MicrasFirmware?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MicrasFirmware | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MicrasFirmware b/MicrasFirmware index 4ec774f..99c94c3 160000 --- a/MicrasFirmware +++ b/MicrasFirmware @@ -1 +1 @@ -Subproject commit 4ec774f1731b21c3567d50950207498f33d783af +Subproject commit 99c94c3a582b27ecef159ee990282bf7211945aa From 9a497b5431f8798d75b86f42503b3a5d42a77f66 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 4 Jun 2025 19:30:44 +0200 Subject: [PATCH 27/78] =?UTF-8?q?=F0=9F=9A=9A=20Rename=20models=20folder?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/{model => models}/gen_maze.py | 0 mujoco/{model => models}/maze.txt | 0 mujoco/{model => models}/maze.xml | 0 mujoco/{model => models}/robot.xml | 0 4 files changed, 0 insertions(+), 0 deletions(-) rename mujoco/{model => models}/gen_maze.py (100%) rename mujoco/{model => models}/maze.txt (100%) rename mujoco/{model => models}/maze.xml (100%) rename mujoco/{model => models}/robot.xml (100%) diff --git a/mujoco/model/gen_maze.py b/mujoco/models/gen_maze.py similarity index 100% rename from mujoco/model/gen_maze.py rename to mujoco/models/gen_maze.py diff --git a/mujoco/model/maze.txt b/mujoco/models/maze.txt similarity index 100% rename from mujoco/model/maze.txt rename to mujoco/models/maze.txt diff --git a/mujoco/model/maze.xml b/mujoco/models/maze.xml similarity index 100% rename from mujoco/model/maze.xml rename to mujoco/models/maze.xml diff --git a/mujoco/model/robot.xml b/mujoco/models/robot.xml similarity index 100% rename from mujoco/model/robot.xml rename to mujoco/models/robot.xml From 7e4360e2faadbddde12d347e503bcc52219ded29 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 4 Jun 2025 19:31:09 +0200 Subject: [PATCH 28/78] =?UTF-8?q?=F0=9F=94=A7=20Update=20constants?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- config/constants.hpp | 89 ++++++++++++++++++++++---------------------- 1 file changed, 45 insertions(+), 44 deletions(-) diff --git a/config/constants.hpp b/config/constants.hpp index d75e318..7dcb2a7 100644 --- a/config/constants.hpp +++ b/config/constants.hpp @@ -15,19 +15,19 @@ #include "micras/nav/speed_controller.hpp" namespace micras { -/***************************************** +/*************** * Constants - *****************************************/ + ***************/ constexpr uint8_t maze_width{16}; constexpr uint8_t maze_height{16}; constexpr float cell_size{0.18}; constexpr uint32_t loop_time_us{1042}; -constexpr float wall_thickness{0.0126F}; -constexpr float start_offset{0.04F + wall_thickness / 2.0F}; -constexpr float exploration_speed{0.5F}; -constexpr float max_linear_acceleration{1.0F}; -constexpr float max_angular_acceleration{200.0F}; +constexpr float wall_thickness{0.012F}; +constexpr float start_offset{0.05F + wall_thickness / 2.0F}; +constexpr float max_angular_acceleration{400.0F}; +constexpr float crash_acceleration{1000000.0F}; +constexpr float fan_speed{100.0F}; constexpr core::WallSensorsIndex wall_sensors_index{ .left_front = 0, @@ -36,37 +36,36 @@ constexpr core::WallSensorsIndex wall_sensors_index{ .right_front = 3, }; -/***************************************** +/*************** * Template Instantiations - *****************************************/ + ***************/ namespace nav { using Maze = TMaze; } // namespace nav -/***************************************** +/*************** * Configurations - *****************************************/ + ***************/ const nav::ActionQueuer::Config action_queuer_config{ .cell_size = cell_size, .start_offset = start_offset, + .curve_safety_margin = 0.0375F + 0.015F, .exploring = { - .max_linear_speed = exploration_speed, - .max_linear_acceleration = max_linear_acceleration, - .max_linear_deceleration = max_linear_acceleration, - .curve_radius = cell_size / 2.0F, - .max_centrifugal_acceleration = 2.78F, - .max_angular_acceleration = max_angular_acceleration, + .max_linear_speed = 0.2F, + .max_linear_acceleration = 9.0F, + .max_linear_deceleration = 9.0F, + .max_centrifugal_acceleration = 2.0F, + .max_angular_acceleration = 300.0F, }, .solving = { - .max_linear_speed = exploration_speed, - .max_linear_acceleration = max_linear_acceleration, - .max_linear_deceleration = max_linear_acceleration, - .curve_radius = cell_size / 2.0F, - .max_centrifugal_acceleration = 1.0F, + .max_linear_speed = 5.0F, + .max_linear_acceleration = 12.0F, + .max_linear_deceleration = 20.0F, + .max_centrifugal_acceleration = 40.0F, .max_angular_acceleration = max_angular_acceleration, }, }; @@ -74,17 +73,19 @@ const nav::ActionQueuer::Config action_queuer_config{ const nav::FollowWall::Config follow_wall_config{ .pid = { - .kp = 0.5F, + .kp = 30.0F, .ki = 0.0F, - .kd = 0.0F, + .kd = 0.008F, .setpoint = 0.0F, .saturation = 1.0F, .max_integral = -1.0F, }, - .max_linear_speed = 0.1F, - .post_threshold = 16.5F, + .wall_sensor_index = wall_sensors_index, + .max_angular_acceleration = max_angular_acceleration, .cell_size = cell_size, - .post_clearance = 0.2F * cell_size, + .post_threshold = 4.0F, + .post_reference = 0.44F * cell_size, + .post_clearance = 0.035F, }; const nav::Maze::Config maze_config{ @@ -95,48 +96,48 @@ const nav::Maze::Config maze_config{ {maze_width / 2, (maze_height - 1) / 2}, {(maze_width - 1) / 2, (maze_height - 1) / 2}, }}, + .cost_margin = 1.2F, + .action_queuer_config = action_queuer_config, }; const nav::Odometry::Config odometry_config{ - .linear_cutoff_frequency = 5.0F, + .linear_cutoff_frequency = 50.0F, .wheel_radius = 0.0112F, - .initial_pose = {{0.0F, 0.0F}, 0.0F}, + .initial_pose = {{cell_size / 2.0f, start_offset}, std::numbers::pi_v / 2.0f}, }; const nav::SpeedController::Config speed_controller_config{ - .max_linear_acceleration = max_linear_acceleration, - .max_angular_acceleration = max_angular_acceleration, .linear_pid = { .kp = 10.0F, - .ki = 1.0F, + .ki = 10.0F, .kd = 0.0F, .setpoint = 0.0F, - .saturation = 40.0F, + .saturation = 20.0F, .max_integral = -1.0F, }, .angular_pid = { - .kp = 2.0F, - .ki = 1.0F, + .kp = 0.7F, + .ki = 5.0F, .kd = 0.0F, .setpoint = 0.0F, - .saturation = 40.0F, + .saturation = 20.0F, .max_integral = -1.0F, }, .left_feed_forward = { - .linear_speed = 12.706F, - .linear_acceleration = 2.796F, - .angular_speed = -0.971F, - .angular_acceleration = -0.0258F, + .linear_speed = 16.4F, + .linear_acceleration = 3.98F, + .angular_speed = -0.55F, + .angular_acceleration = -0.143F, }, .right_feed_forward = { - .linear_speed = 13.319F, - .linear_acceleration = 2.878F, - .angular_speed = 0.901F, - .angular_acceleration = -0.0244F, + .linear_speed = 16.4F, + .linear_acceleration = 3.98F, + .angular_speed = +0.55F, + .angular_acceleration = +0.143F, }, }; } // namespace micras From f90266081b4fcef6524a934560cd12a19b4ced97 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 4 Jun 2025 19:32:02 +0200 Subject: [PATCH 29/78] =?UTF-8?q?=F0=9F=94=A7=20Add=20Mujoco=20launch=20fi?= =?UTF-8?q?les=20for=20simulation=20and=20node=20integration?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- launch/include/mujoco_world.launch.xml | 3 +++ launch/mujoco_micras.launch.xml | 11 +++++++++++ 2 files changed, 14 insertions(+) create mode 100644 launch/include/mujoco_world.launch.xml create mode 100644 launch/mujoco_micras.launch.xml diff --git a/launch/include/mujoco_world.launch.xml b/launch/include/mujoco_world.launch.xml new file mode 100644 index 0000000..28826c7 --- /dev/null +++ b/launch/include/mujoco_world.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/launch/mujoco_micras.launch.xml b/launch/mujoco_micras.launch.xml new file mode 100644 index 0000000..9709ec8 --- /dev/null +++ b/launch/mujoco_micras.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + From 4c3b0dbeb1f6ae13b5cad7485bedb20f5536e652 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 4 Jun 2025 19:32:19 +0200 Subject: [PATCH 30/78] =?UTF-8?q?=F0=9F=94=A7=20Add=20installation=20path?= =?UTF-8?q?=20for=20mujoco=20directory=20in=20CMakeLists.txt?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4410350..dbfa3d4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -86,6 +86,10 @@ install(DIRECTORY gazebo PATTERN gazebo/micras_plugin EXCLUDE ) +install(DIRECTORY mujoco + DESTINATION share/${PROJECT_NAME} +) + install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) From 00d0f1ac5c538b777b4b5a84f9801b4ace00d50a Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 4 Jun 2025 20:56:44 +0200 Subject: [PATCH 31/78] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20=20Refactor=20wall?= =?UTF-8?q?=20sensor=20configuration=20and=20update=20message=20types?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- config/target.hpp | 44 +++++++++++++++------------ include/micras/proxy/wall_sensors.hpp | 29 ++++++++---------- src/proxy/wall_sensors.cpp | 35 ++++++++++++--------- 3 files changed, 58 insertions(+), 50 deletions(-) diff --git a/config/target.hpp b/config/target.hpp index 1890f19..dc4801e 100644 --- a/config/target.hpp +++ b/config/target.hpp @@ -26,6 +26,8 @@ #include "micras/proxy/storage.hpp" #include "micras/proxy/torque_sensors.hpp" +#include "micras/proxy/debug_bridge.hpp" + // clang-format off namespace micras { /***************************************** @@ -93,12 +95,12 @@ const proxy::Buzzer::Config buzzer_config { const proxy::RotarySensor::Config rotary_sensor_left_config { micras_node, // node - "rotary_sensor_left" // topic + "left_encoder" // topic }; const proxy::RotarySensor::Config rotary_sensor_right_config { micras_node, // node - "rotary_sensor_right" // topic + "right_encoder" // topic }; const proxy::TorqueSensors::Config torque_sensors_config { @@ -121,28 +123,30 @@ const proxy::TorqueSensors::Config torque_sensors_config { }; const proxy::WallSensors::Config wall_sensors_config { - micras_node, // node - { - "wall_sensor_0", - "wall_sensor_1", - "wall_sensor_2", - "wall_sensor_3" - }, // topic_array - 0.3F, // max_distance - 4095, // max_reading - 5.0F, // filter_cutoff - { - 0.413F, - 0.161F, - 0.177F, - 0.230F, - }, // base_readings - 0.5F // uncertainty + .node = micras_node, + .topic_array = { + "lidar_0", + "lidar_1", + "lidar_2", + "lidar_3" + }, + .uncertainty = 0.5F, + .base_readings = { + 0.0666F, + 0.1608F, + 0.1608F, + 0.0666F, + }, + .max_sensor_reading = 0.6F, + .min_sensor_reading = 0.01F, + .max_sensor_distance = 0.18F * 2, + .filter_cutoff = 20.0F }; const proxy::Imu::Config imu_config { micras_node, // node - "imu" // topic + "gyro", // topic + "accelerometer" // topic }; const proxy::Battery::Config battery_config { diff --git a/include/micras/proxy/wall_sensors.hpp b/include/micras/proxy/wall_sensors.hpp index 999ced3..47b7bbe 100644 --- a/include/micras/proxy/wall_sensors.hpp +++ b/include/micras/proxy/wall_sensors.hpp @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include "micras/core/butterworth_filter.hpp" @@ -25,11 +25,12 @@ class TWallSensors { struct Config { std::shared_ptr& node; std::array topic_array; - float max_distance; - uint16_t max_reading; - float filter_cutoff; - std::array base_readings; float uncertainty; + std::array base_readings; + float max_sensor_reading; + float min_sensor_reading; + float max_sensor_distance; + float filter_cutoff; }; /** @@ -96,7 +97,7 @@ class TWallSensors { /** * @brief Array of subscribers for distance sensors. */ - std::array::SharedPtr, num_of_sensors> subscribers; + std::array::SharedPtr, num_of_sensors> subscribers; /** * @brief Array of distances measured by the sensors. @@ -106,12 +107,7 @@ class TWallSensors { /** * @brief Maximum distance detectable by the sensor. */ - const float max_distance; - - /** - * @brief Maximum ADC reading. - */ - const uint16_t max_reading; + const float max_sensor_reading; /** * @brief LED state. @@ -145,10 +141,6 @@ class TWallSensors { /** * @brief Buffer to store the ADC values. - * - * - * - * */ std::array buffer; @@ -166,6 +158,11 @@ class TWallSensors { * @brief Ratio of the base reading to still consider as seeing a wall. */ float uncertainty; + + /** + * @brief Constant value for the wall sensor. + */ + float constant; }; } // namespace micras::proxy diff --git a/src/proxy/wall_sensors.cpp b/src/proxy/wall_sensors.cpp index 67bcf6a..e96d65d 100644 --- a/src/proxy/wall_sensors.cpp +++ b/src/proxy/wall_sensors.cpp @@ -11,22 +11,22 @@ namespace micras::proxy { template TWallSensors::TWallSensors(const Config& config) : - max_distance{config.max_distance}, - max_reading{config.max_reading}, leds_on{false}, + max_sensor_reading{config.max_sensor_reading}, filters{core::make_array(config.filter_cutoff)}, base_readings{config.base_readings}, - uncertainty{config.uncertainty} { + uncertainty{config.uncertainty}, + constant{static_cast( + -std::pow(config.max_sensor_distance, 2) * + std::log(1.0f - config.min_sensor_reading / config.max_sensor_reading) + )}, +{ this->turn_off(); for (uint8_t i = 0; i < num_of_sensors; i++) { - this->subscribers[i] = config.node->template create_subscription( + this->subscribers[i] = config.node->template create_subscription( config.topic_array[i], 1, - [this, i](const sensor_msgs::msg::LaserScan& msg) { - if (!msg.ranges.empty()) { - this->readings[i] = this->leds_on ? msg.ranges[0] : 0; - } - } + [this, i](const example_interfaces::msg::Float64& msg) { this->readings[i] = this->leds_on ? msg.data : 0; } ); } } @@ -61,12 +61,19 @@ float TWallSensors::get_reading(uint8_t sensor_index) const { template float TWallSensors::get_adc_reading(uint8_t sensor_index) const { - return this->readings.at(sensor_index) / this->max_distance * this->max_reading; + float raw_reading = this->readings.at(sensor_index); + if (raw_reading < 0.0F) { + raw_reading = this->max_sensor_reading; + } else if (raw_reading == 0.0F) { + raw_reading = this->max_sensor_reading; + } else { + raw_reading = this->readings.at(sensor_index); + } + + float intensity = 1 / std::pow(raw_reading, 2.0F); + float reading = this->max_sensor_reading * (1 - std::exp(-this->constant * intensity)); - // return static_cast(std::abs(this->buffer.at(sensor_index) - this->buffer.at(sensor_index + - // num_of_sensors)) - // ) / - // this->adc.get_max_reading(); + return reading; } template From 7a48d15e71b103d6eb156b39ef5a8aa15d608c68 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 4 Jun 2025 20:58:32 +0200 Subject: [PATCH 32/78] =?UTF-8?q?=F0=9F=94=A8=20=20Update=20rotary=20senso?= =?UTF-8?q?r=20configuration=20to=20use=20Float64=20message=20type?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- config/target.hpp | 8 ++++---- include/micras/proxy/rotary_sensor.hpp | 10 +++++----- src/proxy/rotary_sensor.cpp | 13 +++---------- 3 files changed, 12 insertions(+), 19 deletions(-) diff --git a/config/target.hpp b/config/target.hpp index dc4801e..b26dc9d 100644 --- a/config/target.hpp +++ b/config/target.hpp @@ -94,13 +94,13 @@ const proxy::Buzzer::Config buzzer_config { *****************************************/ const proxy::RotarySensor::Config rotary_sensor_left_config { - micras_node, // node - "left_encoder" // topic + .node = micras_node, + .topic = "left_encoder" }; const proxy::RotarySensor::Config rotary_sensor_right_config { - micras_node, // node - "right_encoder" // topic + .node = micras_node, + .topic = "right_encoder" }; const proxy::TorqueSensors::Config torque_sensors_config { diff --git a/include/micras/proxy/rotary_sensor.hpp b/include/micras/proxy/rotary_sensor.hpp index 36d23df..6fe36fd 100644 --- a/include/micras/proxy/rotary_sensor.hpp +++ b/include/micras/proxy/rotary_sensor.hpp @@ -7,7 +7,7 @@ #include #include -#include +#include namespace micras::proxy { /** @@ -39,14 +39,14 @@ class RotarySensor { private: /** - * @brief Subscriber para os dados do sensor rotativo. + * @brief Subscription to the rotary sensor topic. */ - rclcpp::Subscription::SharedPtr subscriber; + rclcpp::Subscription::SharedPtr subscriber; /** - * @brief Dados do sensor rotativo recebidos. + * @brief Received data from the rotary sensor. */ - sensor_msgs::msg::JointState data; + double data{0.0}; }; } // namespace micras::proxy diff --git a/src/proxy/rotary_sensor.cpp b/src/proxy/rotary_sensor.cpp index adef0cf..b4ba932 100644 --- a/src/proxy/rotary_sensor.cpp +++ b/src/proxy/rotary_sensor.cpp @@ -6,19 +6,12 @@ namespace micras::proxy { RotarySensor::RotarySensor(const Config& config) { - this->subscriber = config.node->create_subscription( - config.topic, 1, - [this](const sensor_msgs::msg::JointState& msg) { - if (!msg.position.empty()) { - this->data = msg; - } - } + this->subscriber = config.node->create_subscription( + config.topic, 1, [this](const example_interfaces::msg::Float64& msg) { this->data = msg; } ); - - this->data.position.resize(1, 0.0); } float RotarySensor::get_position() const { - return this->data.position[0]; + return static_cast(this->data); } } // namespace micras::proxy From 908ad3aa309d75f1d836b53b918846b835b19860 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 4 Jun 2025 21:10:14 +0200 Subject: [PATCH 33/78] =?UTF-8?q?=F0=9F=94=A7=20Update=20configuration=20s?= =?UTF-8?q?tructures=20to=20use=20named=20initializers=20for=20clarity?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- config/target.hpp | 58 +++++++++++++++++++++++------------------------ 1 file changed, 29 insertions(+), 29 deletions(-) diff --git a/config/target.hpp b/config/target.hpp index b26dc9d..9514255 100644 --- a/config/target.hpp +++ b/config/target.hpp @@ -57,36 +57,36 @@ const proxy::Storage::Config maze_storage_config { *****************************************/ const proxy::Led::Config led_config { - micras_node, // node - "led" // topic + .node = micras_node, + .topic = "led" }; const proxy::Argb::Config argb_config { - micras_node, // node - { + .node = micras_node, + .topic_array = { "rgb_0", "rgb_1" - }, // topic_array + } }; const proxy::Button::Config button_config { - micras_node, // node - "button" // topic + .node = micras_node, + .topic = "button" }; const proxy::DipSwitch::Config dip_switch_config { - micras_node, // node - { + .node = micras_node, + .topic_array = { "dip_switch_0", "dip_switch_1", "dip_switch_2", "dip_switch_3" - }, // topic_array + }, }; const proxy::Buzzer::Config buzzer_config { - micras_node, // node - "buzzer" // topic + .node = micras_node, + .topic = "buzzer" }; /***************************************** @@ -104,8 +104,8 @@ const proxy::RotarySensor::Config rotary_sensor_right_config { }; const proxy::TorqueSensors::Config torque_sensors_config { - micras_node, // node - { + .node = micras_node, + .wheel_pairs_topics = { { { "torque_flw", @@ -116,10 +116,10 @@ const proxy::TorqueSensors::Config torque_sensors_config { "torque_rrw" } // front_topic, rear_topic } - }, // wheel_pairs_topics - 0.04F * 20, // shunt_resistor - 0.5F, // max_torque - 3.3F // reference_voltage + }, + .shunt_resistor = 0.04F * 20, + .max_torque = 3.0F, + .reference_voltage = 3.3F }; const proxy::WallSensors::Config wall_sensors_config { @@ -144,16 +144,16 @@ const proxy::WallSensors::Config wall_sensors_config { }; const proxy::Imu::Config imu_config { - micras_node, // node - "gyro", // topic - "accelerometer" // topic + .node = micras_node, + .gyro_topic = "gyro", + .accelerometer_topic = "accelerometer" }; const proxy::Battery::Config battery_config { - micras_node, // node - "battery", // topic - 9.9F, // max_voltage - 4095 // max_reading + .node = micras_node, + .topic = "battery", + .max_voltage = 9.9F, + .max_reading = 4095 }; @@ -162,13 +162,13 @@ const proxy::Battery::Config battery_config { *****************************************/ const proxy::Fan::Config fan_config { - micras_node, // node - "fan" // topic + .node = micras_node, + .topic = "fan" }; const proxy::Locomotion::Config locomotion_config { - micras_node, // node - "cmd_vel" // topic + .node = micras_node, + .topic = "cmd_vel" }; } // namespace micras From de48275511b615481ba93cc9a1ab6a195239d9d5 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 4 Jun 2025 21:10:53 +0200 Subject: [PATCH 34/78] =?UTF-8?q?=F0=9F=94=A7=20Update=20IMU=20data=20hand?= =?UTF-8?q?ling=20to=20use=20Float64MultiArray=20for=20gyro=20and=20accele?= =?UTF-8?q?rometer=20topics?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/micras/proxy/imu.hpp | 11 +++++++---- src/proxy/imu.cpp | 21 +++++++++++++-------- 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/include/micras/proxy/imu.hpp b/include/micras/proxy/imu.hpp index a11c9f5..42f048b 100644 --- a/include/micras/proxy/imu.hpp +++ b/include/micras/proxy/imu.hpp @@ -9,7 +9,7 @@ #include #include #include -#include +#include namespace micras::proxy { /** @@ -22,7 +22,8 @@ class Imu { */ struct Config { std::shared_ptr& node; - std::string topic; + std::string gyro_topic; + std::string accelerometer_topic; }; /** @@ -78,12 +79,14 @@ class Imu { /** * @brief Subscriber para os dados do IMU. */ - rclcpp::Subscription::SharedPtr subscriber; + rclcpp::Subscription::SharedPtr gyro_subscriber; + rclcpp::Subscription::SharedPtr accelerometer_subscriber; /** * @brief Dados do IMU recebidos. */ - sensor_msgs::msg::Imu data; + example_interfaces::msg::Float64MultiArray gyro_data; + example_interfaces::msg::Float64MultiArray accelerometer_data; /** * @brief Current angular velocity on each axis. diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index bb1d555..0b96125 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -8,18 +8,23 @@ namespace micras::proxy { Imu::Imu(const Config& config) { - this->subscriber = config.node->create_subscription( - config.topic, 1, [this](const sensor_msgs::msg::Imu& msg) { this->data = msg; } + this->gyro_subscriber = config.node->create_subscription( + config.gyro_topic, 1, [this](const example_interfaces::msg::Float64MultiArray& msg) { this->gyro_data = msg; } + ); + + this->accelerometer_subscriber = config.node->create_subscription( + config.accelerometer_topic, 1, + [this](const example_interfaces::msg::Float64MultiArray& msg) { this->accelerometer_data = msg; } ); } void Imu::update() { - this->angular_velocity[0] = this->data.angular_velocity.x; - this->angular_velocity[1] = this->data.angular_velocity.y; - this->angular_velocity[2] = this->data.angular_velocity.z; - this->linear_acceleration[0] = this->data.linear_acceleration.x; - this->linear_acceleration[1] = this->data.linear_acceleration.y; - this->linear_acceleration[2] = this->data.linear_acceleration.z; + this->angular_velocity[0] = this->gyro_data.data[0]; + this->angular_velocity[1] = this->gyro_data.data[1]; + this->angular_velocity[2] = this->gyro_data.data[2]; + this->linear_acceleration[0] = this->accelerometer_data.data[0]; + this->linear_acceleration[1] = this->accelerometer_data.data[1]; + this->linear_acceleration[2] = this->accelerometer_data.data[2]; } float Imu::get_angular_velocity(Axis axis) const { From 23ef03d2b54c450150bca45548eadfe03037c98b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 4 Jun 2025 18:52:29 -0300 Subject: [PATCH 35/78] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Improve=20simulation?= =?UTF-8?q?=20models?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mujoco/models/robot.xml | 32 +++++++++++++++----------------- 1 file changed, 15 insertions(+), 17 deletions(-) diff --git a/mujoco/models/robot.xml b/mujoco/models/robot.xml index 0b34163..451bd99 100644 --- a/mujoco/models/robot.xml +++ b/mujoco/models/robot.xml @@ -1,11 +1,11 @@ - +