From 4ccda7541002ba684455b2104f4d32bfeaf6f880 Mon Sep 17 00:00:00 2001 From: Rooholla-KhorramBakht Date: Wed, 12 Feb 2025 20:48:19 +0000 Subject: [PATCH] standard MPC tests added. --- .devcontainer/Dockerfile | 29 +- Go2Py/assets/d1_assets/stl/Link6-new.stl | Bin 0 -> 19584 bytes Go2Py/assets/mujoco/go2_with_arm_no_ef.xml | 292 + Go2Py/assets/urdf/go2_with_arm.urdf | 16 + Go2Py/sim/mujoco.py | 8 +- examples/09-crododdyl.ipynb | 12247 ++++++++++++++++++- 6 files changed, 12545 insertions(+), 47 deletions(-) create mode 100644 Go2Py/assets/d1_assets/stl/Link6-new.stl create mode 100644 Go2Py/assets/mujoco/go2_with_arm_no_ef.xml diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 4b7f439..c90d0ed 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -2,6 +2,7 @@ FROM ros:humble ENV DEBIAN_FRONTEND noninteractive ARG CONDA_VER=latest ARG OS_TYPE=x86_64 +SHELL ["/bin/bash", "-c"] RUN apt-get update && apt-get install -y -qq --no-install-recommends \ libglvnd-dev \ @@ -30,6 +31,12 @@ RUN apt-get update && apt-get install -y -qq --no-install-recommends \ RUN curl -s https://packagecloud.io/install/repositories/github/git-lfs/script.deb.sh | bash RUN apt-get install git-lfs + +RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /unitree_ros2/cyclonedds_ws/src && \ +git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\ +cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build + + # Install Miniconda RUN wget https://github.com/conda-forge/miniforge/releases/${CONDA_VER}/download/Miniforge3-Linux-${OS_TYPE}.sh -O ~/miniconda.sh \ && /bin/bash ~/miniconda.sh -b -p /opt/conda \ @@ -39,20 +46,22 @@ RUN wget https://github.com/conda-forge/miniforge/releases/${CONDA_VER}/download && echo "conda activate base" >> ~/.bashrc ENV PATH /opt/conda/bin:$PATH -SHELL ["conda", "run", "-n", "base", "/bin/bash", "-c"] + +RUN conda create -n go2py python==3.8.10 +SHELL ["conda", "run", "-n", "go2py", "/bin/bash", "-c"] + ENV CONDA_PREFIX /opt/conda -# Cheange the ROS2 RMW to CycloneDDS as instructed by Unitree -# RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /unitree_ros2/cyclonedds_ws/src && \ -# git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\ -# cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build +RUN echo "export CYCLONEDDS_HOME=/unitree_ros2/cyclonedds_ws/install/cyclonedds/" >> ~/.bashrc +ENV CYCLONEDDS_HOME "/unitree_ros2/cyclonedds_ws/install/cyclonedds" +RUN cd /root && git clone https://github.com/eclipse-cyclonedds/cyclonedds-python -b releases/0.10.x && cd cyclonedds-python && pip install . # Install Python dependencies -# RUN pip install torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu118 -# RUN pip install matplotlib opencv-python proxsuite scipy isort black -# RUN pip install warp-lang scikit-learn casadi -# RUN pip install onnx onnxruntime -# RUN pip install cyclonedds pygame pynput jupyter ipykernel +RUN pip install torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu118 +RUN pip install matplotlib opencv-python proxsuite scipy isort black +RUN pip install warp-lang scikit-learn casadi +RUN pip install onnx onnxruntime +RUN pip install pygame pynput jupyter ipykernel RUN pip install meshcat mujoco RUN conda install -y -c conda-forge \ pinocchio \ diff --git a/Go2Py/assets/d1_assets/stl/Link6-new.stl b/Go2Py/assets/d1_assets/stl/Link6-new.stl new file mode 100644 index 0000000000000000000000000000000000000000..98e2fe6c3710b5fc24c4b00ef4eb1a4a94a390c9 GIT binary patch literal 19584 zcmb_kXLwcBww<9jL!>JpK|&LVfl!j1eb!2l5~M}oNl;1%ElLaJ(v?U@g_{V3juC+X zMhR8OIcM+Sl_Eu|5Tr!}L=Y*`^_n-=4x2YtkblqbJK=nvYwWq^m}AaW_aXZK{cnaJ zJ}=3lKTCy^*&uvmxxx{-9XCcCKhoE$z6bt$ZkF?%NS0ab+Yvq4N-UBPp%Bp<%1wY^0f zn(cfS?7up@adpSI9G`R>?1xZ(Ii;(Qof-iYwawbXH{s&r?|o%62({F zV6ORhqE+F`x3bU=4UMeRk~95;@_aOVULyG;m*Y9|hP@?v@SMjqI}Wn~zDo(u-29Cv z=uvx{E3|>OiB@gw>su0GSgUU|l-Xk%d18*Y@-FNXuIUP6LliaeE8pe`QC5%Ub)@<~ zhwrwt5s20iTmF^cu6;hrTJ>=qiG4@3j>u3ZUivSHS? ziw&jLlfUmo^sf}*ZN0{DYmAV4?6>ZN_adz9T8(5Jv_h_Qgd#~XrMOzz-_M)VEc_`Y zV*H6N%^1pCh<|$p_JM(%8$Jq4{U`!)DcmQ4w=T8AnPs8L1g#J$wwX|rn=3W?)m4-Yb7yGOS^{v=(!p`%as~UN)WJ-5H%L<-lVfF3rs7j)&W{Z8>7}1vz*s5Sb#zThn~ruI`YN1urH?DvXkNwus^ zoP)^jc3STm7gOKPL1dcfY@Owz{on?chjS2Hp-OawO1^xhBw3#n4fIZ%Qsb!{L{$=f zJtxQ-SGASZh3ggifoL7^>w~`L_akds{R^s|y{mQNmQ7?KcO={`Jd?fni$Q`rG997BKodhq!_rns&%7JYb_qwD~wHkB%F9))(+_vWH_=mMQ#Kf)RePL5eS<`pC zDKi_s?AgA*xHqFn>!&K5!!*&h%nV?mMKGBfeM00uNN9O0%x@(BY>nl8g zRj+y!(}*T&Hs*o2@^*lg%GyOGroCFl7(JzgT_sSJM5m|yEUttUvBnH?No6WQJeF`XX*oNZX+z(E8)Ch;FNVsdZpWRbaJK8|y6TNxJG)t{MXx6U#rshYtmghWa z-8a}uM4yQMXIfWlSF-@CW3hyYnDMQ=F@Rq zqBY*xzK=&=G)Ii?pTCa!B**jLmhIfx9qwc;+V;KuUFRx=5g#&Q6eF-z!RqsW?Ec>9 zin0Bti{|p5`b*?TJ@@cqd%BI)wGNo-;~LUcB0m@#ovWH5X`)%HAI;Nkd&qGq(_EQ5 za_l%`eqi44s_0gjxW4R~`Qyyq8UlNWXrktmqRkt151XS4cpletgeS7lCYvjb1!}Z1 z(CS|Dw3%9 z0<#;#?5Y_6R<-6Et9g@F&9i3!$TX4r=MZa9@Ac*)wNnI9!(L%;iRRRQRpxD3YcApH zgT2CB!5$K=E0rROr*AYDbEQUI1$12M%Ivz_jtAyE(d`w6uldkr=AiMTWNySCS&7#^ zuw#xRgI4Eh|0fH52a2vS&kh-(sRYqFqF339X8eDam@Dg#&}?As((KWUYX0)f-ZV3@ zVQZ~2#5FB%M0c6tQ7cG2KX$s?i2f<7%$4njO7s>rrh6W!JZkR^dxfzf`fJ>2vG?RkbMVAiiTzD<<+!$2 z`N$sOFiu3q$UElrMT1mxBi{RUxw(W@ zf~}A@y;rK>I#qO1zqh^Fp7^KguAWi(t<_(>?soI^F&wm>QR{i%wK4B*SK?Cu%n7uo z+hBwYVMJv{;BzrobQ=q^`F3Y{N{BBwQXlkxH#UKHb_T*?&=TU8m| z%S*$e>`JX8jvWe-A61@e$0KY*JHz1zPd^^D0{q1NCfyzfTuQ}76mwCnCG-G+k3zadge6aDv4;V{`W(WD1LvPkJJ8zw! zv5QqgW11@8ND=+2cQ<3+9WJZfI_h$~MW1v(!g=Q-jCVj5@vINC2-(H_CmI}AQWopm z$o!Faaj@?hqstmeQ+~@*yB}Edi3aq(ApFX^%=f;J(?$dAq28;@htG+rE!&tas`Qok zTtw(LUY{8x&kYDQr|^C|>J@v4tP|Z!OEKf?)qAS$qDrvF*rP|1{g=m&3*F+q*<(J; zDmc^Eb1=WP`m2vP-i(2UgVuAf8=uwQ<+JC)nQvubZlgWjMwe=nWSt|8%|RR9%EISj z2J1FPib$)@pw%;u{O`cw`gqoTXNm%10SRf2ls-6%d^*~{lFD$h}Ux#M2c{(bDeZ)$;*=$m%w7Hk(SC~b!%FOj7^^VI@D+}x` zQSqc&qF!RIuY28z^5&+BE=p`-S2)ab%r2tUor_yWx%<9Pd3O*!M9T({N` z&&?QZAfw1MMwY8+(0kT`6;;e#`%}U(YN!%Df>9ig>z^O=sT~&dFyK*+hbC{{q4sC6 zheS8qtaL}#nCaWN^8NhLnPX7tRQ~AfjMN$nlsu`B&o^PFuUhBHPmSj)f(otGdCFWK zVZQC#>?>H|Fdi5uqK`iNLD2WTeWzRrT4hLTYr3f1>>X+k0ILMiyZ1{K8QO!anMi7;#=f@^idortCoq3@e z66yxqYFE3pXpd)0W7Ey)7Z>?f@rnic!3bhxi8gIoB(nDI_kCStltg||uR2%8kQZgY zmBW2Kc#OinC)BQG*b(G4j-F99=KIS*Ls$9E@MwUouxjgwMTN^*uP>kKQ~T|xIP@GD z;q{;#A^zG{&3AV3IL(j4g9f;gMvjh9vrP1fXmN0?=<`MsU)Mj!OKhdZAI5o}z1BG( zW`DHPSA6E@6^9~oF>{`zI0;rN7_=Cg?oesW2N7ku@2FC1fzyy;w}c57vOuJ##F)ySL9 z)tBDpa$cvk-q#9blp#79bok#9_DvqB^EOXO`E2o6%eRfqXY$7dmoJ(<~*0PCWkNy4G2yBJ+o{dndcC?H3o z2zC3*QM>8!qYJV5U*W=Ynduc~$ms;cC*hO?OB+a(5hi8^(1JP)Y==`FW zW%#2gcYEHs!?6}C99CDN(wXVz{($d{*}PMPHWEr4_aqfQ9iirMXpd(AJj+)u=*J=2=o~w3n=7)@tqA+%2WEgo*VoM!9UDF{ z29Fw}%^47ZGXtd$O1j)T)vac)*!Rf#{j>gASA}a1_H~`&vJp-ms4*20x~{gLFKwON zR@Ysf_uxW!hc3EW)1~GpIPMZn4EB@7 ze@<|#Q)JW+qIE>lvVV#5>%VjF6JsUv6F6~{iypOhsa(;y+M8I`if#Xa+s!+|=!crO zWq(=MrDm__6OlN!OGJJ+%zbV3U@d}i;0M|!+QHf#&DvF`8Tee(u5M$de|32|Wsdt( zzAci|&M@NAo4V8*5Isjmh!Q_}Sq@Ks&3&8iUug;lw2tuc+Te$U>)rDnuMM0m!kLVo zQL$qKL}LA)-9>UHYWaceqTcv!6tB-~ANs}Ztj`@AYDJfO=}niKWuiT=MvZiku*)bhlYJHCW5j83sV0QWTlDj?EUDT^uZ8#iW01>q_^0{j6o?3%&ScOY(K{6; z$;ADQMbL~S17{g%18wu_j&~-WD^yxII}=Z004(U95C1h(?l=8JSDqo`b5Z$v%&k$W z!auQ%pF3?hZHF2{oRhmQ)Q6oU!yDbX9!(L-BcP zu-Fp$wq8Yfzcs`^<*ACwyEuO?7UwUw7PTu5x6h-}jxVrlEpnyTs{wM0xV@y2_?0Uh zRyDP=>7aFl8glBj3==8`vG3@uo*%r+X`LSsD%36~*2E&SU-ibEPPVHZ)<>HwpTiZ} zK&H_ik?UwND?YueXwNqRF}4?GR`SMd+-lc$v`y47uc7?w<&k1QRufH$aSeiv!^OU^ zdxfebYRxv@U>j}N1}Xz>pl!aF@t>c>juAaXj_70WAv;k+c9C_WQajs<-zQG6GYX@I ziqX~n_~b`8CqGWzRp%{-?`PO`7i$brM*Zh3zh%8dugFdk`;PI@xf&8&QO@IAjcSZS ze$;NZ;}ecoynn@rg^WlmFsdP1NBHw8@~y|7W1Q6~VucfEKIYnv z@kngj&K210pxrBs4H2KKSszxOD%3eAGK%rg)xL^vCv4g^O(ee;tSJ%wK;DQN+%wJh zio7pU`P>=(KvkkoT;WE}Hc$O$xG2JBMaU?kb$96xkjrSrLS zOMdUi_xRk|LEy?8(L`(c-1#*|>^ncxzVG9#gwb;xiHLTcJ}1I%Efmuh_0TdZtjA4P zK#yFzCSrCGeb{oh`^1JcJAxRs>0^nCo8CaGBVp_S>NqRw6y-Yf7yX zF-aNXf3o^&b@#!tbXUO1JNAf!>LEJQIM`Y>G}%_ki~g^AvkM)y*EOgTR1d$+F*#VS zt~poS>eNUgKhe{Lk^JL1d$y#jWY6szzEE@sPY|K?HGT|G(H(!Rn|;h zoi0kM6U7SyvK)R8ts|H#-zw%x-Tgp*5UnHBily=HFNC_oqOlA1v?tN`i}#u7IiHDF zc;{}`x?em_oKfwlW1?yGU$tJ|x>l&UD6*?=1%;Sb?aF{m6BVjn*~%%qLQLi{3Tq;w z^>{4!`#1CJqo0e@yoZdvLNxY}@AZX!C04BXO5E!fCsD7c5?#A}NB&ooIFKQNA9Rp^ z4XJFz4gSL(mk^CLnCP`*MP->rYoDsrh}IFO`Hq)=opYk%<2zmsKbW;TS2<}?8lL*n zJp7`8yF;i%R4CDJpH2|#4ty=D%o`{*Z=s6n`O(6vEZ1x@#r1>DB|ca414e41YYW$z z4a@Bkozx1l$VN}jhT?`=b7E|Cm28+b#~goTw>V$AuT~Qgtt0$qcM>;-pAbve#%q2A zd>rb^tpA*$c37~+@EJ>9S!>poZQ{KGx(cdT-s?mYHAdvW`<~gmnpbq^(+q5dxJwFa}sx^ms(@J>&)@PusI41%8P+c%tXVBvV`z`};c|BRGVAAxMLarV9i+iavy%RN-c|`R!uQC+cN$fGxFwn= z_m%iuMCdl|t*mI}O}-@5d#R`r^aK4P`hGwq8I^ZdWL^!{GV0sKHI2+=l?=7o#_Uq_ ze4?=nC~Ak#$0W4DUYp%r9W5km%E-|AXh)@8>WeL|-B zR#?-A?hC8RNOf-sy~Qelyb;x2vBXSFE-A||Xs0O=6@#kedt`|f%}YZ|%l5o4O6Rh( z>XmA2#{;?2@}J7Ry;C&rSXr(g-&|7(`lS2e8&%yG{B>D*jr~9y_+0dfZ*oNT@OG$H zRkr7GP|XKJQlsn~M6`~G;P)U;)d-e19yE!-7+?hTc)Y^*uhPr6l3ms%8+f9GI>w4Y zv~}cSGoe~V89FS=e$(5DAhN6RMlJ90eWk)J<<^I@H5-^8XpiWtu3x)T*VdHp@}2F- z%tEfTQXOrTptq<6?$xl{rXN+Ym8jnCy%`#5s}ET~h4TBg-F`G*{N@E|@%Vv#N3@PO zRi}~Jc}`7vk>9St7^pXvqno^L_fYqI;BPzKpBOczdV3lDz-XdReCD-ljwm$#W%*!J znA|&OnybW=E_U@nZ!uT+#N^isqQVE&^o)}vzwDDE?#RQ z<%v3)=NLg`fhfI&Th1HSQ1*D`b{N_~-q0S=^a>fi=LR*DbGh!K`j9u&G{3#fzn5|8 zVN*M!)bC=9JJ-wR3VB1Oi5~wNiBxaP;&TzLBRu@3(^Y=ssR!Sz!RO+g31ph@urT5$ zMof*!|Ah@F&k?O7rp+(rj$6@O_Tl|@<@urpJ?)G_7EqN$A9&+^E!>72d99(wmHO?K zxXAJCCaKi>%+5uY=!FK8&0W9-GQB>{Cubz%N^IrC + + +