From 66d223d972da3426eac9cba4198d9a8052f0f11a Mon Sep 17 00:00:00 2001 From: wty-yy <993660140@qq.com> Date: Mon, 2 Feb 2026 15:50:04 +0800 Subject: [PATCH] Add new terrains --- README.md | 4 +- README_zh.md | 4 +- deploy/deploy_mujoco/configs/go2.yaml | 4 +- resources/robots/go2/cross_slope.xml | 43 + resources/robots/go2/cross_stairs.xml | 95 +++ resources/robots/go2/imgs/label_1.png | Bin 0 -> 2201 bytes resources/robots/go2/imgs/label_10.png | Bin 0 -> 2655 bytes resources/robots/go2/imgs/label_4.png | Bin 0 -> 2423 bytes resources/robots/go2/imgs/label_7.png | Bin 0 -> 2272 bytes resources/robots/go2/race_track.xml | 18 +- resources/robots/go2/race_track_generator.py | 808 ------------------- resources/robots/go2/stairs.xml | 2 +- 12 files changed, 155 insertions(+), 823 deletions(-) create mode 100644 resources/robots/go2/cross_slope.xml create mode 100644 resources/robots/go2/cross_stairs.xml create mode 100644 resources/robots/go2/imgs/label_1.png create mode 100644 resources/robots/go2/imgs/label_10.png create mode 100644 resources/robots/go2/imgs/label_4.png create mode 100644 resources/robots/go2/imgs/label_7.png delete mode 100644 resources/robots/go2/race_track_generator.py diff --git a/README.md b/README.md index 44379e6..fc75ae7 100644 --- a/README.md +++ b/README.md @@ -94,7 +94,7 @@ python deploy/deploy_mujoco/deploy_go2.py Connect an Xbox-compatible gamepad to enable teleoperation; otherwise, the agent keeps a default forward command. - **Swap the policy**: The default checkpoint is `deploy/pre_train/go2/go2_cts_150k.pt`. Replace `policy_path` in the YAML config with your own `logs/{experiment_name}/exported/policies/policy.pt`. -- **Swap terrains**: Default terrain is `resources/robots/go2/stairs.xml`. Alternatives include `flat.xml` and `race_track.xml`. Generate new terrains with [terrain_generator.py](resources/robots/go2/terrain_generator.py) (see also [unitree_mujoco/terrain_tool](https://github.com/unitreerobotics/unitree_mujoco/tree/main/terrain_tool)). +- **Swap terrains**: Default terrain is `resources/robots/go2/stairs.xml`. Alternatives include `flat.xml`, `race_track.xml`, `cross_stairs.xml`, and `cross_slope.xml`. Generate new terrains with [windigal - mujoco_terrains](https://github.com/windigal/mujoco_terrains) #### Results @@ -159,7 +159,7 @@ Related publications implemented in this repo: - [CTS: Concurrent Teacher-Student Reinforcement Learning for Legged Locomotion](https://arxiv.org/pdf/2405.10830) Contributors: -- [@windigal](https://github.com/windigal): CTS algorithm reproduction, video editing +- [@windigal](https://github.com/windigal): CTS algorithm reproduction, terrain generation, video editing - [@wertyuilife2](https://github.com/wertyuilife2): CTS algorithm reproduction --- diff --git a/README_zh.md b/README_zh.md index b200dab..2b1606c 100644 --- a/README_zh.md +++ b/README_zh.md @@ -94,7 +94,7 @@ python deploy/deploy_mujoco/deploy_go2.py 如果有xbox协议的手柄接入主机，自动切换为手柄控制，否则只会保持默认指令前进。 - 替换网络模型：默认模型位于 `deploy/pre_train/go2/go2_cts_150k.pt`；自己训练模型保存于`logs/{experiment_name}/exported/policies/policy.pt`，只需替换 yaml 配置文件中 `policy_path`。 -- 替换环境地形：默认地形为 `resources/robots/go2/stairs.xml`，其他可选地形，平地 `flat.xml`，赛道 `race_track.xml`，地形使用[terrain_generator.py](resources/robots/go2/terrain_generator.py)生成，参考[unitree_mujoco/terrain_tool](https://github.com/unitreerobotics/unitree_mujoco/tree/main/terrain_tool)。 +- 替换环境地形：默认地形为 `resources/robots/go2/stairs.xml`，其他可选地形，平地 `flat.xml`，赛道 `race_track.xml`，交叉楼梯和斜坡`cross_stairs`/`cross_slope`，地形使用[windigal - mujoco_terrains](https://github.com/windigal/mujoco_terrains)生成 #### 运行效果 @@ -157,7 +157,7 @@ python deploy_real_go2.py eth0 - [CTS: Concurrent Teacher-Student Reinforcement Learning for Legged Locomotion](https://arxiv.org/pdf/2405.10830) 贡献者： -- [@windigal](https://github.com/windigal)：复现CTS算法，剪辑视频 +- [@windigal](https://github.com/windigal)：复现CTS算法，生成地形，剪辑视频 - [@wertyuilife2](https://github.com/wertyuilife2)：复现CTS算法 --- diff --git a/deploy/deploy_mujoco/configs/go2.yaml b/deploy/deploy_mujoco/configs/go2.yaml index 3093373..fb47b0e 100644 --- a/deploy/deploy_mujoco/configs/go2.yaml +++ b/deploy/deploy_mujoco/configs/go2.yaml @@ -2,8 +2,10 @@ policy_path: "{LEGGED_GYM_ROOT_DIR}/deploy/pre_train/go2/go2_moe_cts_137000_0.63 # policy_path: "{LEGGED_GYM_ROOT_DIR}/deploy/pre_train/go2/go2_cts_150k.pt" # xml_path: "{LEGGED_GYM_ROOT_DIR}/resources/robots/go2/flat.xml" -# xml_path: "{LEGGED_GYM_ROOT_DIR}/resources/robots/go2/race_track.xml" # change go2 init pos to pos="-5 2 0.445" in resources/robots/go2/go2.xml +# xml_path: "{LEGGED_GYM_ROOT_DIR}/resources/robots/go2/race_track.xml" xml_path: "{LEGGED_GYM_ROOT_DIR}/resources/robots/go2/stairs.xml" +# xml_path: "{LEGGED_GYM_ROOT_DIR}/resources/robots/go2/cross_slope.xml" +# xml_path: "{LEGGED_GYM_ROOT_DIR}/resources/robots/go2/cross_stairs.xml" # Total simulation time simulation_duration: 60000000.0 diff --git a/resources/robots/go2/cross_slope.xml b/resources/robots/go2/cross_slope.xml new file mode 100644 index 0000000..f5cf284 --- /dev/null +++ b/resources/robots/go2/cross_slope.xml @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/resources/robots/go2/cross_stairs.xml b/resources/robots/go2/cross_stairs.xml new file mode 100644 index 0000000..ec1e85e --- /dev/null +++ b/resources/robots/go2/cross_stairs.xml @@ -0,0 +1,95 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/resources/robots/go2/imgs/label_1.png b/resources/robots/go2/imgs/label_1.png new file mode 100644 index 0000000000000000000000000000000000000000..4fc8ee77d0b216abb0a79b86f14b79d4a6ef3577 GIT binary patch literal 2201 zcmds1_ct4k9*vQT5wvy?ZH+$jYXo28sR&}G5u=eBQEFBMMO8~_WAAEEGhZ6CM(rwE zqgA_J)ru`e)E>2JJn!Fl_ndp~xxakQx#!&bi9@51U=9ce0001+niyg(^wkA5Kr8?N zQ^cXZ%!OeJ#2^g-m4jj{7lzdXZV3kfs#7>mTz~)oGZk%NV|0FgzQ4cU-{0Tc+dDNi zMWfM*i;MH}^4`6BcYJ)@*Vo6x!^6hL=I-u3Jw07kR#sC}^XJc>v$M0ymoH!3R8dj+ z{rfiv1j6I-dwY9zb#)dN791QL>gwtT2L~Y`A>!iVEG#TrTU&45ywTU!=j7xZ8ykD{ z=n(`0VPY8B?d&JT+Su60%gf8k%34`jrKF?`4-eyTI1-68 zGc$8?a^mXh%EZLv;^NZQ)+Qhz0Efe&Q0V&ldUA5Io}ONLdHIhYKTJ(c!C){iFE1L6 zHZwD;sHn)y%me~~Zf-2n!3tU@-CV@q&Va*RNkE z5C~OORV5`Qot>TL=H}z$grNdRJ?ZWT4`x%TwEM0E32=s@7&znojZ3L z8ygc76C)xboS{4DF%$}wnwskA>B-H_&0sJB0sEz^O zU|?W$bhM_ z?yi)Sl#r0n?c2BW^Ye3Za*U0QhlYlnoSd?v(|Mu1iw#&1qw33>pHh5`}aZxDaOD# z-Kbr9W0-ICfs5Nxf0NXl`ff!Y0?Q8!^R0E|Zp@6&(|FvO%E>}T;*}|}kM-|ep7deK z_hMlYtX zJ>}cFKoU$vG|HWZW>aO2U|M0%775f$oR{b-EDq$xO4!N&9ln|^idt~WF%J?T=!e(Q ze6AkYgBeZ6DYO~-k^ys*2Q3Bo4tNJ?R?c zlAgSUhJgo-&TTmRe`l4gdm#(-YIBal$b(A#rnGi6SXVoDe#zg#QkL%YZ1eh5Pb}$* zB?&lTJ3v$ zg^GfJIOT^BttdHj7@%d0)nr-~JMRxbetiqTlSZlIq)ZcY=Kc87D@( zEO@5kjhft2ZG|4F)Rzv7jhr3QhX=7`dYpOuflw(23Oy~T`}uxhudt!gMX zE>C_`8_F08=zW-pQ#sBWQ%_mj^yJF3^Y>P4Tj4D4k?hqhlb}&186UBiHZg%*)~CAc z-MEu7>G-O8jfKv)mQ&WjQvJ>~5%G|!ppVekMKhjIC-F|~KVJ77yE0reA+E1XypW5N zWkwb2BFlH-A|}hDku^0bQ)O**FVX0zT)iyWj-u@ltLB?)ecHkReB1ND5~0%+3c#|!>M$E?jrYTalvTX0y5V2-iHi(s znbx^$`Pb|sdTi#b5Qlgc$;!~39ihnPd+OZbW@Jv_%_93;#qz&G!iuaAt#aD>KZ1vP z6F&fsMKawM>-~2t9l@sq7FhB4f$DEf3Q?*hY(LEE-uXSF1{l7MHaI84rpR-rEj-O8 zmB&e82!&;kOOM8&q<7#o_fGF2oqHv`z_3u#^~ZL{9i&eQ)Fa@^XU8SKZsD?vcLgvt LLK#*X;G_NvD8ZG4 literal 0 HcmV?d00001 diff --git a/resources/robots/go2/imgs/label_10.png b/resources/robots/go2/imgs/label_10.png new file mode 100644 index 0000000000000000000000000000000000000000..191908bdbf73d7471e876569ba8dc9c6b1c1f213 GIT binary patch literal 2655 zcmd^A_cI%g!w#``Q>|4ac8ySqpe-UAGqp5U=r z&_}=kxc+;8Zw%Qh6&)1-pzH(fg%vpfK)Rx};KR^HX@864y3nG!Y zyu7@)xVW~qc6fMLUteEcUESW^etv#FJUk46K!k*Ze0_blwzgVYS~@#B`}_O9eEDK+ zZT;lQll%AYOG`^Xd-g0OB&4RM=HI`6S65e9ELKEBgoA?vfj}G`9d&nilat@%11u~o zdV70czkV$!D5$Bad2({Hx3`y=n5e9*eCN&`YHDgyQc?;E3MM8dQBhHMclVi@ne_B@ z1_p+no*pVHDoabtj~+2HGD=BF9UmX>@9%&9{F#rBkBp4$)~#D+W@eLFJHclh={1It(~5phQr~;#>PcOMFj-~uCA{3 z_V(M`+j@F>baZsw+}v;8ywTU!udAzbbaa%EkYHh9p`@gI`0$~Zm)FqHP;hW?KtMov zc=*W3NJ&YFy1IH~WTcUiQG9&7i;GKaY;09k6%-2f^z%?*#o&(6+{jg866%YXm={oT8F8X6i}T3QVa z4ULVBN=i!a-@pI!=Z~bMLwll07L8lk)+e5zy<)I0BFEf3{i+}lcBOaVWz|P z{My01pa$(`K=0C(3BM}Ys^#5?vbA&ywH+^S;`mJAwCO+zHdtrmVPt;F--bvD7eM>g zKhM13)6vt(Ly-+EhSp;(oiVHGRb>wI{UuKB`2oM)uzGr!-YCvvLiQi{zOY^ru%L0&jsCC z!m4jz;vs=j`(BW$!2~e1I!w+FZ!8)sgNZCYQ&Pq`KMH~HN#RFAYa>tvo)OfF1Eobf z1T^@a@@)l5G|FAh|D=*iD={dglf>bhH6gRzBl2sk^=DxvY&??<^}%|zY#EbDbvr%A%=m2mNfa^}i5=%&&+kR89fFM3N26S~qC7AwD@%ST^bJ?oVKB-ZAw){*Q{IzalcF=)a z+5!Ieo7s{{M~Bvi_BR8v+yRe|qXe@Yo2@+HHerF~9MhI7EnI${lW=?d(0}lG8J4tv z^#Ecj)I4{^%)w@-Xmu&PvGQ~|e|ijaF5U& zM4lONzdkFX75F-)de~*R>b+h>2F@A838S>ACi;S&L587D`>QTV519JVg|&rbGO z3@Zwz9j#8agT2@)KAkgzQ8Nhj1u#&-9T68R9N$qz_A-rVqv>(iUW8XF^h43m9@k!(-A2^ZcE`y7<}Wm>-%T@b8`#y ziEhlSWd%O$>z2c|Z+MB6MnRns628dK%d?qh*Iv$EXlpf1K`~G z59O^T`dlE&i z2erD&qy)y+dLxpS(+YSJN@rXWeI;^TXpw1t>$-K(Vp=Vp+9+wFaB5K-mPDM?VPSnR zKB2iAJr*y0Y57u=H}}`mI&K6Gqh(#<4+9?yG4?UN()t{9MorK zGk6lr?CbPOE`QK3c|GO=Y|GMBjWC1`sX$e$RANN1#MmwxMHW_WOhB~B%LOd!+`D+) zraFmZ!HscdC#kZ9X(z&GE_3F7RoTDEk!QA94adtP^tKfgm9%5(TIu5fx40j@-6e2% a&RI;*<$OZV`p(}R1VBR#3CF=KG5-VZNQcb; literal 0 HcmV?d00001 diff --git a/resources/robots/go2/imgs/label_4.png b/resources/robots/go2/imgs/label_4.png new file mode 100644 index 0000000000000000000000000000000000000000..b80ad7c1bad501058cec78b10ae0a91e8589d0a7 GIT binary patch literal 2423 zcmd^9^;Z*)1KdbKM~LK*Zlpy*Vk4*294Ik524i%{kj_zpq=bbq!hv)nB_#q%ZJfwydULWyEIg;P$MV+P?L7`)PWiRpqPgnn(1C#T%4bu@9ysY`0-=VS9Uf zb#=9~v$MImxv8nCr>E!e@bKl!m%O~ZA|fI}LP9DkDn&&_dwY9>gM%F%9Y;q;RaI4T za&llWn3I!JO-=3BuU`iT2g%9Fm!a3MUr$d@uc@g~P*6}(Qrg+kQ+&CQjRlw4d~jE;^*B9Sl{ zOh-q@z`(%S*}1E$Yhq$zU|?W;e0+3t^wXzL$H&KS-n_wLu@4_U)YjI1@Zf=uk555C z!PwZCmzNg}4GjYWgP)(@?Ch+Jj7&vEg}uG~ty{M!DJf}bX;oEK!@|OdM53#!tGv9t zw6t_;YO1WPtcHfh&d$!ypFgv+vl$r~`S|$q^YaM=!p)mEM@B}jT)A@N#tjDt2MP)b zdV2alfBt;?_RYx1==JN@Gcz**0RiIT;)#igQ&UrVdU|YZY%gBCkdl(p)z$U%^b8CP zeEasTrKM$cb+v?qgrT9~*RNlfmX^%S%>4cR5eS5+sAyJJR(N>0latf>`ufn&P;hW? zOG`^=Xz10eS0f`MZ{NN>IXT(e+lxY>%FD}5O--LZeF}%eYin!G&CR*FxtW-lLPA1t zINaU4cN-fU1qB6dZEfGZduL^3)!p4qB9Xp-{|*9yGBY#({{0&f5#i|QSXo(l=gyt7 zvaTIh&d#o>scCI(U0huJ`SWLEW8>lB z;b+gDrKP2voSe|n(LH+fXnua)-QB&uzCI`@$j!~It*tFCE{=gP(Utb@C!6+&! zLLd+*6w1uZY-eW|6&00~lvG+;+Sk{YmzQT_V^ddGr>v~}`0?Y*`L$2wk6y~(0EDsW z<<$T00O;8a`f^zSoe#{?9{^zZ_&)*%yvv*c0BXK_I#4qU{Y?vK>W0`g-Jg87m=Fgl zw6ufFEl}wmwi=gg$>9(hM6pEq*8B7EQ_khUAXeX$U=Am3!s|En-d**Yc#-TZIrkox z2v3%L?wdSL#b042Zh4^lbo1DX{Lbz!oP|gz1Xb&o?5FkR0c`}o<58~Qt7}*YilT#( zhmb9b|l?6W>1Y4f=3FJu0ew!HkLcAsRvTwkrIPw7Z#f&wXfTvZ6g|qM#wMQ1Y2K<@4XM zz!a;beBm6R%|Z7&E;c#>{*jo_dz*&%ApM3x``axQ@Ig8AhLq_Ia*7|vSoxc&p^?6w z9%GC`n1vIr@*rabhpN*HXaIM{5@D{}a1Pb^xEXfXLbcn#poYcAa}99lbDm0F1tSu% zLL8XKh<22Raa{LL7cVQue^aq2C`7+T9aIwyeJIiKH$`jz2tw&UJ*h{DH`nWlnqh^M zN9`RX?-VXtFk-$EeY#@jioAJdpDiG-?hF?FWC;k_;h#xTqmv!HjrtSqA|3z|U21sc zk|BVbh-7^=$<<=5Sw#Wy66`XktabM~2>0s6EeC!&8Cnybn>7bwm-<(aBwC4x4&I18 z)AMeA@-+A#&E1s(Y-7AlLaQ=>sIekqxe97Z_9XS05Zj^R9prdbuN2*%SJ)Ck(;~^w z!F|d#>PX~M>NZ!*OEAqY-3<3YjL3^zX_j4QtrcifjH1;OCnE{12ENj<{11PkJcMYA ziyEj^Nkl6H>l)Z~9aoYR)qOooUNi<%_6Re|Nfby17Uwv#n+o)*_3>7)qE;{%F+UJ| z$xKea-h*X4fAUpQ?Aw@)l(`}m3rre^`koNG0u|j}u1l-3p57qeysMGVOK@?DnOd_$ zQYCfd^Sas_?Ggdkh@n-MaX3;cPO%y#KE{$HJ<#8{2%Zdi=bHRw%$xOWe+GaNixi7u z@Ruhfn|@d*6x61MsH91=v#qjYL{p3s_5nj}+MqeEd;Qos&wWrsT< z4Jw%o3gcg0;lVP`Hv2C1+fm|nb8wUAtJT@)TE!*XNc`WCX-RL0n$sy-nP=;MXU7)f?DofD7KR0Kvx1mAy_Hm;uL1nPA@i+4J3hWZ zL7IfvkGMbbsl25SmL8<`Jv*G>RfrH84&LUb$RI16~;OVu>TwP!nMdnm1HZAECW z3Cg@gMPcX<#Lr zu>Ol+kaWap$R?nlH9m8>!XsNl`d6xd)nAdF*a-8b>&zW@6bt+3_Vs*)L9> zy+MwivBMDrV0|&)(?!6Tx$}J2NH&}vo&G$~xU)2;vhvWD!idgc=y~gm4|q-{I@)MI zEsCI&c2_Dp*(j_~sqC1AtidO&pT(lGhKZ^HoqQYPD}Co=%RP-kUgvkK$Y^P4xxBo5c6PS6w>LL8H#j)><;$1;{{E?{srB{s-@ku<|Nebq zVq$4&>FDT)gM&j-QnI0;VPj*XzP>&TObaJZP5n53j6C=|N6x#{5G(9zM6o}T{s z^Jf1z6x&vhlM>`_79N{nC z-Qq&yRwN@+k+>nYC%~eRBbhLP%)CA#4+~_Ike5E04)%K#qhMBnu0Zo7%)$3{Y6We> z68MZPJiSVEP2U_vcu$3RQq>c9d}~g0ETU>W9Ln3b$YEW1f|cfWDY~mI?9g1pEcBV8 z_^S(+u|v?V-m-K#DwR-zGsh98fTRqwP$-06VAz9YK(@M4rV2w#va@`J5}%J%aDsxo z?#ORNiwe;|i-isutr_%Dg)i)@>hA)^hpWt~%E!=BjAo_Yb77_4ZaE=Aw@TqBtIT;Q z<9r<#qGSK2eLuRu%ZZVm!|yw8lnk%u-y7sg{8g~uLMrjM*8v#+*G@R6*#i|q#K8~;Y5Ctit+hr!CR)B5VBaqaa%wt zHmQF|7lWD1mEn?nY9d5JPAxK+sYUogvZOdBr;1~mVm053@inW+8KGPNNHV_hUTwXE z78vgiJ)M=O+lt&xMh;^2;skW?`UN`_dba&Wvza(fs}AApsrbpaTLQ^ny9Ag&B+SN< z0)5Pbsed5wSCMq^deiLX-OYq>>@TbPLU&ZNMDc)(q3GV3unmWWUCj2frY{D(6d zhQSXOD5-HttGE8FRGahi8xc=-MMy`s2$J5VYcCu=867{g<+i0tRA;BOsr|>pKM0rt zfwFSPvKG7}PY6rlfo;_j3EFO>Gw0cNe?{%Nf>=+G1R2d5?$( z3b>strYirfqJmfXc%~Y>ObcC&=IXBMccxt#e5=@*3Y(-0)4QZNVy;Dq< zCH>fhqwgVwCVD$-?1imlsL}wpm7$EwGo;;ZPhm6-v-No>A zsu??xE}=W;xaF6g7nj-)`C@!{WGB1O%-e6q&n86is814-p0;;?V3exV$OfjaTZcin z>g&`ncjxZ!oU@LNaMNF1^;a*+w@d^LrsJ|_4j<+3W*x!vrF|hoT9Q@b;oyA^?N19K zzng@-|3X+`s+k|jMPbPXZ|F^5VKDsh{etk@+)?CG{s1-Id+qpybWTh?>uG&!OVODYW9K7=`1^J1R>>PSJZE|D - + - + - + - + - + - - + + - + - + \ No newline at end of file diff --git a/resources/robots/go2/race_track_generator.py b/resources/robots/go2/race_track_generator.py deleted file mode 100644 index e84c22c..0000000 --- a/resources/robots/go2/race_track_generator.py +++ /dev/null @@ -1,808 +0,0 @@ -import xml.etree.ElementTree as xml_et -from pathlib import Path -import numpy as np -import cv2 -import noise -import os - -ROBOT = "go2" -INPUT_SCENE_PATH = os.path.join(os.path.dirname(__file__), "flat.xml") -OUTPUT_SCENE_PATH = os.path.join(os.path.dirname(__file__), "race_track_tmp.xml") -PATH_DIR = Path(__file__).parent.absolute() -# zyx euler angle to quaternion -def euler_to_quat(roll, pitch, yaw): - cx = np.cos(roll / 2) - sx = np.sin(roll / 2) - cy = np.cos(pitch / 2) - sy = np.sin(pitch / 2) - cz = np.cos(yaw / 2) - sz = np.sin(yaw / 2) - - return np.array( - [ - cx * cy * cz + sx * sy * sz, - sx * cy * cz - cx * sy * sz, - cx * sy * cz + sx * cy * sz, - cx * cy * sz - sx * sy * cz, - ], - dtype=np.float64, - ) - - -# zyx euler angle to rotation matrix -def euler_to_rot(roll, pitch, yaw): - rot_x = np.array( - [ - [1, 0, 0], - [0, np.cos(roll), -np.sin(roll)], - [0, np.sin(roll), np.cos(roll)], - ], - dtype=np.float64, - ) - - rot_y = np.array( - [ - [np.cos(pitch), 0, np.sin(pitch)], - [0, 1, 0], - [-np.sin(pitch), 0, np.cos(pitch)], - ], - dtype=np.float64, - ) - rot_z = np.array( - [ - [np.cos(yaw), -np.sin(yaw), 0], - [np.sin(yaw), np.cos(yaw), 0], - [0, 0, 1], - ], - dtype=np.float64, - ) - return rot_z @ rot_y @ rot_x - - -# 2d rotate -def rot2d(x, y, yaw): - nx = x * np.cos(yaw) - y * np.sin(yaw) - ny = x * np.sin(yaw) + y * np.cos(yaw) - return nx, ny - - -# 3d rotate -def rot3d(pos, euler): - R = euler_to_rot(euler[0], euler[1], euler[2]) - return R @ pos - - -def list_to_str(vec): - return " ".join(str(s) for s in vec) - - -class TerrainGenerator: - - def __init__(self) -> None: - self.scene = xml_et.parse(INPUT_SCENE_PATH) - self.root = self.scene.getroot() - self.worldbody = self.root.find("worldbody") - self.asset = self.root.find("asset") - self._add_wood_material() - self._add_sponge_material() - - def _add_wood_material(self): - # 1. 添加纹理 (Texture) - # 这里使用内置的 "flat" 类型配上棕色，模拟一种简单的木头颜色。 - # 如果你有真实的木纹图片(比如 wood.png)，请将 type="2d" builtin="flat" - # 改为 type="2d" file="../wood.png" - tex = xml_et.SubElement(self.asset, "texture") - tex.attrib["name"] = "wood_tex" - tex.attrib["type"] = "2d" - tex.attrib["file"] = "./assets/wood.png" - tex.attrib["rgb1"] = "0.6 0.4 0.2" # 棕色 (RGB) - tex.attrib["width"] = "512" - tex.attrib["height"] = "512" - - # 2. 添加材质 (Material) - mat = xml_et.SubElement(self.asset, "material") - mat.attrib["name"] = "wood_mat" # 材质名称，后面 AddBox 要用 - mat.attrib["texture"] = "wood_tex" # 关联上面的纹理 - mat.attrib["specular"] = "0.2" # 木头反光度较低 - mat.attrib["shininess"] = "0.1" # 亮度较低 - mat.attrib["rgba"] = "1 1 1 1" - - def _add_sponge_material(self): - # 1. 添加纹理 (Texture) - # 因为没有图片文件，我们使用 builtin="flat" 来生成纯色纹理 - tex = xml_et.SubElement(self.asset, "texture") - tex.attrib["name"] = "sponge_tex" - tex.attrib["type"] = "2d" - tex.attrib["builtin"] = "flat" # 使用内置平面纹理，不需要 file 路径 - # tex.attrib["rgb1"] = "1.0 0.7 0.7" # 设置颜色：粉色 (参考图片颜色) - tex.attrib["rgb1"] = "0.90196 0.83922 0.56471" # 设置颜色：黄色 - tex.attrib["width"] = "512" - tex.attrib["height"] = "512" - - # 2. 添加材质 (Material) - mat = xml_et.SubElement(self.asset, "material") - mat.attrib["name"] = "mat_sponge" # 材质名称，AddBox 中调用这个名字 - mat.attrib["texture"] = "sponge_tex" - - # 海绵的关键视觉特性：不反光、不油亮 - mat.attrib["specular"] = "0.1" # 几乎没有镜面反射 (相比木头的0.2要低很多) - mat.attrib["shininess"] = "0.1" # 几乎没有光泽 - mat.attrib["rgba"] = "1 0.7 0.7 1" # 叠加颜色，保持原样 - - # Add Box to scene - def AddBox(self, - position=[1.0, 0.0, 0.0], - euler=[0.0, 0.0, 0.0], - size=[0.1, 0.1, 0.1], - sponge=False): - geo = xml_et.SubElement(self.worldbody, "geom") - geo.attrib["pos"] = list_to_str(position) - geo.attrib["type"] = "box" - geo.attrib["size"] = list_to_str( - 0.5 * np.array(size)) # half size of box for mujoco - quat = euler_to_quat(euler[0], euler[1], euler[2]) - geo.attrib["quat"] = list_to_str(quat) - # === 修改部分开始 === - if sponge: - # 1. 视觉：使用海绵材质 (假设你在 asset 中定义的名字叫 mat_sponge) - geo.attrib["material"] = "mat_sponge" - # 2. 物理：solref 时间常数越大越软 (0.02 比较软, 默认约 0.002) - geo.attrib["solref"] = "0.03 1" - geo.attrib["priority"] = "1" - geo.attrib["solmix"] = "1" - # 3. 摩擦：海绵通常摩擦力较大 (可选) - geo.attrib["friction"] = "1.2 0.005 0.0001" - else: - geo.attrib["material"] = "wood_mat" - geo.attrib["friction"] = "0.5 0.005 0.0001" - - def AddGeometry(self, - position=[1.0, 0.0, 0.0], - euler=[0.0, 0.0, 0.0], - size=[0.1, 0.1],geo_type="box"): - - # geo_type supports "plane", "sphere", "capsule", "ellipsoid", "cylinder", "box" - geo = xml_et.SubElement(self.worldbody, "geom") - geo.attrib["pos"] = list_to_str(position) - geo.attrib["type"] = geo_type - geo.attrib["size"] = list_to_str( - 0.5 * np.array(size)) # half size of box for mujoco - quat = euler_to_quat(euler[0], euler[1], euler[2]) - geo.attrib["quat"] = list_to_str(quat) - geo.attrib["material"] = "wood_mat" - - def AddStairs(self, - init_pos=[1.0, 0.0, 0.0], - yaw=0.0, - width=0.2, - height=0.15, - length=1.5, - stair_nums=10): - - local_pos = [0.0, 0.0, -0.5 * height] - for i in range(stair_nums): - local_pos[0] += width - local_pos[2] += height - x, y = rot2d(local_pos[0], local_pos[1], yaw) - self.AddBox([x + init_pos[0], y + init_pos[1], local_pos[2]], - [0.0, 0.0, yaw], [width, length, height]) - - - def AddDownStairs(self, - init_pos=[1.0, 0.0, 0.0], - yaw=0.0, - width=0.3, - height=0.15, - length=1.5, - stair_nums=10): - - # 从上向下生成台阶：第一个台阶中心在 +0.5*height，随后每步降低 height - local_pos = [0.0, 0.0, 0.5 * height] - for i in range(stair_nums): - local_pos[0] += width - local_pos[2] -= height - x, y = rot2d(local_pos[0], local_pos[1], yaw) - self.AddBox([x + init_pos[0], y + init_pos[1], local_pos[2]], - [0.0, 0.0, yaw], [width, length, height]) - - - def AddStairsSeries(self, - init_pos=[1.0, 0.0, 0.0], - yaw=0.0, - width=0.3, - length=1.5, - stair_nums_up=6, - stair_nums_down=6, - start_height=0.05, - step_inc=0.03, - max_height=0.2, - flat_length=0.5): - """生成一系列台阶：先上台阶（stair_nums_up），顶部有一段平地（flat_length），再下台阶（stair_nums_down）。 - 每完成一对上/下台阶后，单步高度增加 step_inc，直到达到 max_height。 - - 参数说明： - - init_pos: 底层起点（列表），序列沿局部 x 方向展开 - - yaw: 台阶朝向 - - width: 每级台阶在 x 方向的深度（步幅） - - length: 台阶在 y 方向的宽度（和 AddStairs 一致） - - stair_nums_up/down: 上/下台阶的级数 - - start_height: 第一对台阶的每级高度 - - step_inc: 每对增加的高度 - - max_height: 最大每级高度（包含） - - flat_length: 顶部平地长度（沿 x） - """ - - # 保持 init_pos 不变（上/下台阶成对结束后回到同一基准高度） - base_pos = np.array(init_pos, dtype=float) - height = start_height - - # 平台长度至少为 1.0 米 - platform_length = max(flat_length, 1.0) - - # 迭代每一对台阶直到高度超限 - while height <= max_height + 1e-8: - # --- 上台阶 --- - # 上台阶第 i 级中心 z = base_z + (-0.5 + i) * height, i = 1..stair_nums_up - for i in range(1, stair_nums_up + 1): - center_x_local = i * width - center_z = base_pos[2] + (-0.5 + i) * height - x, y = rot2d(center_x_local, 0.0, yaw) - self.AddBox([x + base_pos[0], y + base_pos[1], center_z], - [0.0, 0.0, yaw], [width, length, height]) - - # 顶部平地：放在最后一级之后，平台长度至少 platform_length - last_up_center_x = stair_nums_up * width - last_up_top_surface = base_pos[2] + stair_nums_up * height # 顶部平面高度 - flat_thickness = height # 平地厚度，使用与台阶同高度以保证接触 - flat_center_local_x = last_up_center_x + width / 2.0 + platform_length / 2.0 - flat_center_z = last_up_top_surface + flat_thickness / 2.0 - x, y = rot2d(flat_center_local_x, 0.0, yaw) - # 尺寸：在 x 方向用 platform_length, y 用 length, z 用 flat_thickness - self.AddBox([x + base_pos[0], y + base_pos[1], flat_center_z], - [0.0, 0.0, yaw], [platform_length, length, flat_thickness]) - - # --- 下台阶 --- - # 平地末端 x - flat_end_x = last_up_center_x + width / 2.0 + platform_length - for j in range(1, stair_nums_down + 1): - center_x_local = flat_end_x + width / 2.0 + (j - 1) * width - # 第 j 级下台阶的中心 z = last_up_top_surface - 0.5*height - (j-1)*height - center_z = last_up_top_surface - 0.5 * height - (j - 1) * height - x, y = rot2d(center_x_local, 0.0, yaw) - self.AddBox([x + base_pos[0], y + base_pos[1], center_z], - [0.0, 0.0, yaw], [width, length, height]) - - # 下台阶之后也添加一段平地（连接到下一组上台阶），长度至少 platform_length - seq_end_local_x = flat_end_x + stair_nums_down * width # 这是最后一个下台阶的前缘 x - post_flat_center_local_x = seq_end_local_x + platform_length / 2.0 - # 该平地应与下一组上台阶的起始高度对齐：其顶面与 base_z + height 对齐 - post_flat_center_z = base_pos[2] + height / 2.0 - x, y = rot2d(post_flat_center_local_x, 0.0, yaw) - self.AddBox([x + base_pos[0], y + base_pos[1], post_flat_center_z], - [0.0, 0.0, yaw], [platform_length, length, flat_thickness]) - - # 为下一对台阶准备：把 base_pos 在 x 方向平移到当前序列末端（post flat 末端），保持 z 不变 - seq_total_end_local_x = seq_end_local_x + platform_length - # 计算下一组基准位移，使下一组上台阶第一级的前缘与当前 post-flat 的末端无缝对接 - # base_shift_local_x 为相对于当前 base 的局部 x 偏移 - overlap = 1e-3 # 以米为单位，微小重叠以避免可视缝隙 - base_shift_local_x = seq_total_end_local_x - width / 2.0 - overlap - dx, dy = rot2d(base_shift_local_x, 0.0, yaw) - base_pos[0] = base_pos[0] + dx - base_pos[1] = base_pos[1] + dy - - # 增加单级高度 - height = round(height + step_inc, 8) - - - def AddSuspendStairs(self, - init_pos=[1.0, 0.0, 0.0], - yaw=1.0, - width=0.2, - height=0.15, - length=1.5, - gap=0.1, - stair_nums=10): - - local_pos = [0.0, 0.0, -0.5 * height] - for i in range(stair_nums): - local_pos[0] += width - local_pos[2] += height - x, y = rot2d(local_pos[0], local_pos[1], yaw) - self.AddBox([x + init_pos[0], y + init_pos[1], local_pos[2]], - [0.0, 0.0, yaw], - [width, length, abs(height - gap)]) - - def AddRoughGround(self, - init_pos=[1.0, 0.0, 0.0], - euler=[0.0, -0.0, 0.0], - nums=[10, 10], - box_size=[0.5, 0.5, 0.5], - box_euler=[0.0, 0.0, 0.0], - separation=[0.2, 0.2], - box_size_rand=[0.05, 0.05, 0.05], - box_euler_rand=[0.2, 0.2, 0.2], - separation_rand=[0.05, 0.05]): - - local_pos = [0.0, 0.0, -0.5 * box_size[2]] - new_separation = np.array(separation) + np.array( - separation_rand) * np.random.uniform(-1.0, 1.0, 2) - for i in range(nums[0]): - local_pos[0] += new_separation[0] - local_pos[1] = 0.0 - for j in range(nums[1]): - new_box_size = np.array(box_size) + np.array( - box_size_rand) * np.random.uniform(-1.0, 1.0, 3) - new_box_euler = np.array(box_euler) + np.array( - box_euler_rand) * np.random.uniform(-1.0, 1.0, 3) - new_separation = np.array(separation) + np.array( - separation_rand) * np.random.uniform(-1.0, 1.0, 2) - - local_pos[1] += new_separation[1] - pos = rot3d(local_pos, euler) + np.array(init_pos) - self.AddBox(pos, new_box_euler, new_box_size) - - def AddPerlinHeighField( - self, - position=[1.0, 0.0, 0.0], # position - euler=[0.0, -0.0, 0.0], # attitude - size=[1.0, 1.0], # width and length - height_scale=0.2, # max height - negative_height=0.2, # height in the negative direction of z axis - image_width=128, # height field image size - img_height=128, - smooth=100.0, # smooth scale - perlin_octaves=6, # perlin noise parameter - perlin_persistence=0.5, - perlin_lacunarity=2.0, - output_hfield_image="height_field.png"): - - # Generating height field based on perlin noise - terrain_image = np.zeros((img_height, image_width), dtype=np.uint8) - for y in range(image_width): - for x in range(image_width): - # Perlin noise - noise_value = noise.pnoise2(x / smooth, - y / smooth, - octaves=perlin_octaves, - persistence=perlin_persistence, - lacunarity=perlin_lacunarity) - terrain_image[y, x] = int((noise_value + 1) / 2 * 255) - - cv2.imwrite(str(PATH_DIR / "assets" / output_hfield_image), terrain_image) - - hfield = xml_et.SubElement(self.asset, "hfield") - hfield.attrib["name"] = "perlin_hfield" - hfield.attrib["size"] = list_to_str( - [size[0] / 2.0, size[1] / 2.0, height_scale, negative_height]) - hfield.attrib["file"] = "../" + output_hfield_image - - geo = xml_et.SubElement(self.worldbody, "geom") - geo.attrib["type"] = "hfield" - geo.attrib["hfield"] = "perlin_hfield" - geo.attrib["pos"] = list_to_str(position) - quat = euler_to_quat(euler[0], euler[1], euler[2]) - geo.attrib["quat"] = list_to_str(quat) - - def AddHeighFieldFromImage( - self, - position=[1.0, 0.0, 0.0], # position - euler=[0.0, -0.0, 0.0], # attitude - size=[2.0, 1.6], # width and length - height_scale=0.02, # max height - negative_height=0.1, # height in the negative direction of z axis - input_img=None, - output_hfield_image="height_field.png", - image_scale=[1.0, 1.0], # reduce image resolution - invert_gray=False): - - input_image = cv2.imread(input_img) # 替换为你的图像文件路径 - - width = int(input_image.shape[1] * image_scale[0]) - height = int(input_image.shape[0] * image_scale[1]) - resized_image = cv2.resize(input_image, (width, height), - interpolation=cv2.INTER_AREA) - terrain_image = cv2.cvtColor(resized_image, cv2.COLOR_BGR2GRAY) - if invert_gray: - terrain_image = 255 - position - cv2.imwrite(str(PATH_DIR / "assets" / output_hfield_image), terrain_image) - - hfield = xml_et.SubElement(self.asset, "hfield") - hfield.attrib["name"] = "image_hfield" - hfield.attrib["size"] = list_to_str( - [size[0] / 2.0, size[1] / 2.0, height_scale, negative_height]) - hfield.attrib["file"] = "../" + output_hfield_image - - geo = xml_et.SubElement(self.worldbody, "geom") - geo.attrib["type"] = "hfield" - geo.attrib["hfield"] = "image_hfield" - geo.attrib["pos"] = list_to_str(position) - quat = euler_to_quat(euler[0], euler[1], euler[2]) - geo.attrib["quat"] = list_to_str(quat) - - def Save(self): - self.scene.write(OUTPUT_SCENE_PATH) - - def AddSlope(self, - position=[1.0, 0.0, 0.0], - yaw=0.0, - length=0.575, - width=0.55, - height=0.15, - thickness=0, - add_baffle=True, - add_baffle_height=False, - sponge=False): - """ - 生成带后背板和两侧挡板的斜坡。 - 侧挡板上沿与斜坡面对齐。 - """ - if not thickness: - thickness = 0.05 # 斜坡面板厚度 - side_thickness = 0.05 # 侧板厚度 - - # =========================== - # 1. 计算公共几何参数 - # =========================== - - # 计算坡度角和斜边长 - angle = np.arctan2(height, length) - ramp_len = np.sqrt(length**2 + height**2) - - # 法线向量 (nx, nz) - nx = np.sin(angle) - nz = -np.cos(angle) - - # 斜坡面板的几何中心 (局部坐标) - # 这里的逻辑是将面板中心沿着法线向下偏移厚度的一半，保证上表面对齐理想斜面 - mid_x = length / 2.0 - mid_z = height / 2.0 - lx = mid_x + nx * (thickness / 2.0) - lz = mid_z + nz * (thickness / 2.0) - - # 统一的旋转角度 (Pitch: -angle, Yaw: yaw) - final_euler = [0.0, -angle, yaw] - - # =========================== - # 2. 生成主斜坡面 (Ramp) - # =========================== - gx, gy = rot2d(lx, 0, yaw) - ramp_pos = [position[0] + gx, position[1] + gy, position[2] + lz] - self.AddBox(ramp_pos, final_euler, [ramp_len, width, thickness],sponge=sponge) - - # =========================== - # 3. 生成垂直背板 (Back Wall) - # =========================== - # 位于斜坡末端，高度为 height - if add_baffle: - back_lx = length + (thickness / 2.0) - if add_baffle_height: - back_lz = height - height *= 2 - else: - back_lz = height / 2.0 - bgx, bgy = rot2d(back_lx, 0, yaw) - back_pos = [position[0] + bgx, position[1] + bgy, position[2] + back_lz] - # 背板竖直放置，只受Yaw影响 - self.AddBox(back_pos, [0, 0, yaw], [thickness, width, height]) - - # 生成两侧挡板 (Side Walls) - side_h = height / 2 - - # 计算侧板的中心 Z 坐标 (side_lz) - # 目标：侧板的上表面 Z = 斜坡的上表面 Z - # 斜坡上表面 Z (局部) = lz + thickness/2 - # 侧板上表面 Z (局部) = side_lz + side_h/2 - # 等式：lz + thickness/2 = side_lz + side_h/2 - # 解得： - side_lz = lz + (thickness / 2.0) - (side_h / 2.0) - if add_baffle_height: - side_lz += side_h - # side_h *= 2 - # lx += height * 2 / length / 2 - # 侧板的 X 坐标与斜坡中心一致 (lx) - - # 侧板的 Y 偏移量 - # 放在斜坡宽度的两侧：(斜坡宽/2) + (侧板厚/2) - y_shift = (width / 2.0) + (side_thickness / 2.0) - - # 生成左右两个侧板 - for sign in [-1, 1]: # -1:左侧, 1:右侧 - if add_baffle_height and sign == 1: continue - local_y = sign * y_shift - - # 将 (lx, local_y) 旋转 Yaw 角到全局 - sgx, sgy = rot2d(lx, local_y, yaw) - - side_pos = [ - position[0] + sgx, - position[1] + sgy, - position[2] + side_lz - ] - - # 侧板的旋转角度与斜坡完全一致，这样上边缘才会平行 - self.AddBox(side_pos, final_euler, [ramp_len, side_thickness, side_h]) - - def AddSlopeGroup(self, position=[0.0, 0.0, 0.0], yaw=0.0, add_baffle=True, add_baffle_height=False,): - - L = 0.6 # 坡长 (爬升方向) - W = 0.6 # 坡宽 (侧向) - H = 0.164 - - p1_local = [-L / 2, -W] - yaw1 = np.pi / 2 - - p2_local = [-L, W/2] - yaw2 = 0 - - p3_local = [L, -W/2] - yaw3 = np.pi - - p4_local = [L / 2, W] - yaw4 = np.pi * 3 / 2 - - blocks = [ - (p1_local, yaw1), - (p2_local, yaw2), - (p3_local, yaw3), - (p4_local, yaw4) - ] - - for pos_local, local_yaw in blocks: - off_x, off_y = rot2d(pos_local[0], pos_local[1], yaw) - - abs_pos = [ - position[0] + off_x, - position[1] + off_y, - position[2] - ] - - abs_yaw = yaw + local_yaw - - self.AddSlope(position=abs_pos, - yaw=abs_yaw, - length=L, - width=W, - height=H, - add_baffle=add_baffle, - add_baffle_height=add_baffle_height) - - def AddBlockyHeightField( - self, - position=[1.0, 0.0, 0.0], - euler=[0.0, -0.0, 0.0], - size=[1.0, 1.0], - height_scale=0.2, # 高度差幅度 - negative_height=0.1, - image_width=128, - img_height=128, - smooth=50.0, # 注意：如果要完全随机，把这个数改得很小（如 2.0） - pixels_per_block=16, # <--- 新参数：决定方块的大小 - output_hfield_image="height_field.png"): - - # 1. 准备图像数据 - terrain_image = np.zeros((img_height, image_width), dtype=np.uint8) - - # 2. 预先生成一个随机种子偏移，保证每次地形不一样 - seed_offset_x = np.random.randint(0, 10000) - seed_offset_y = np.random.randint(0, 10000) - - for y in range(img_height): - for x in range(image_width): - - # === 核心修改开始 === - # 这里的整除逻辑 (//) 是制造“方块感”的关键 - # 它将坐标强制归整，例如 x=0到15 都会变成 0，x=16到31 都会变成 16 - # 这样这 16 个像素取到的噪声值就是一模一样的，形成平坦的台阶 - block_x = (x // pixels_per_block) * pixels_per_block - block_y = (y // pixels_per_block) * pixels_per_block - - # 使用归整后的 block_x, block_y 来生成噪声 - noise_value = noise.pnoise2((block_x + seed_offset_x) / smooth, - (block_y + seed_offset_y) / smooth, - octaves=1, # 减少细节，让方块表面平整 - persistence=0.5, - lacunarity=2.0) - # === 核心修改结束 === - - # 映射到 0-255 - terrain_image[y, x] = int((noise_value + 1) / 2 * 255) - - hfield = xml_et.SubElement(self.asset, "hfield") - hfield.attrib["name"] = "perlin_hfield" - hfield.attrib["size"] = list_to_str( - [size[0] / 2.0, size[1] / 2.0, height_scale, negative_height]) - hfield.attrib["file"] = output_hfield_image - - geo = xml_et.SubElement(self.worldbody, "geom") - geo.attrib["type"] = "hfield" - geo.attrib["hfield"] = "perlin_hfield" - geo.attrib["pos"] = list_to_str(position) - quat = euler_to_quat(euler[0], euler[1], euler[2]) - geo.attrib["quat"] = list_to_str(quat) - - def unit1_sponge(self): - self.AddSlope(position=[0.0, 1.2, 0.0], yaw=np.pi, length=2.32, width=1.2, height=0.6, add_baffle_height=True) - self.AddSlope(position=[-2.32, 0.0, 0.0], yaw=0, length=2.32, width=1.2, height=0.6, add_baffle_height=True) - - self.AddBox(position=[0.6, -0.6, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[-2.92, -0.6, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[0.6, 1.8, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[-2.92, 1.8, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[-3.52, 0.0, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - - self.AddBox(position=[-2.92, 0.6, 0.025], size=[1.2, 2.4, 0.05], sponge=True) - self.AddBox(position=[0.6, 0.6, 0.025], size=[1.2, 2.4, 0.05], sponge=True) - - self.AddSlope(position=[0.0, 1.2, 0.05], yaw=np.pi, length=2.32, width=1.2, thickness=0.05, height=0.6, sponge=True, add_baffle=False) - self.AddSlope(position=[-2.32, 0.0, 0.05], yaw=0, length=2.32, width=1.2, thickness=0.05, height=0.6, sponge=True, add_baffle=False) - - - - def unit2_slopes(self): - self.AddBox(position=[1.225, 0.6, 0.0], size=[2.4, 0.05, 0.1], euler=[0, 0, np.pi / 2]) - self.AddSlopeGroup(position=[1.85, 0.0, 0.0], yaw=0.0, add_baffle=True) - self.AddSlopeGroup(position=[1.85, 1.2, 0.0], yaw=0.0, add_baffle=True) - self.AddBox(position=[2.475, 0.6, 0.0], size=[2.4, 0.05, 0.1], euler=[0, 0, np.pi / 2]) - self.AddSlopeGroup(position=[3.1, 0.0, 0.0], yaw=0.0, add_baffle=True) - self.AddSlopeGroup(position=[3.1, 1.2, 0.0], yaw=0.0, add_baffle=True) - self.AddSlopeGroup(position=[4.3, 0.0, 0.0], yaw=0.0, add_baffle=True) - self.AddSlopeGroup(position=[4.3, 1.2, 0.0], yaw=0.0, add_baffle=True) - self.AddBox(position=[3.7, 0.575, 0.0], size=[2.4, 0.05, 0.1]) - self.AddBox(position=[3.7, 0.625, 0.0], size=[2.4, 0.05, 0.1]) - self.AddBox(position=[4.925, 0.6, 0.0], size=[2.4, 0.05, 0.1], euler=[0, 0, np.pi / 2]) - self.AddSlopeGroup(position=[5.55, 0.0, 0.0], yaw=0.0, add_baffle=True) - self.AddSlopeGroup(position=[5.55, 1.2, 0.0], yaw=0.0, add_baffle=True) - self.AddBox(position=[6.175, 0.6, 0.0], size=[2.4, 0.05, 0.1], euler=[0, 0, np.pi / 2]) - self.AddBox(position=[1.9, 1.8, 0.3], size=[1.4, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[3.2, 1.8, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[4.4, 1.8, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[5.6, 1.8, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - - self.AddBox(position=[1.9, -0.6, 0.3], size=[1.4, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[3.2, -0.6, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[4.4, -0.6, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[5.6, -0.6, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - - self.AddBox(position=[1.2, 0.0, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - self.AddBox(position=[2.5, 1.2, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - self.AddBox(position=[4.95, 0.0, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - self.AddBox(position=[6.2, 1.2, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - - - def unit3_stairs(self): - self.AddBox(position = [6.8, 1.2, 0.1], size = [1.2, 1.2, 0.2]) - self.AddBox(position = [8.0, 0.0, 0.1], size = [1.2, 1.2, 0.2]) - self.AddBox(position = [9.2, 1.2, 0.1], size = [1.2, 1.2, 0.2]) - self.AddBox(position = [10.4, 0.0, 0.1], size = [1.2, 1.2, 0.2]) - - self.AddBox(position=[6.8, 1.8, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[8.0, 1.8, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[9.2, 1.8, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[10.4, 1.8, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - - self.AddBox(position=[6.8, -0.6, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[8.0, -0.6, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[9.2, -0.6, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[10.4, -0.6, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - - self.AddBox(position=[11.0, 1.2, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - - def unit4_diagonal(self): - self.AddSlope(position=[12.8, 1.8, 0.0], yaw=np.pi / 2, length=2.32, width=1.2, height=0.6, add_baffle_height=True) - self.AddSlope(position=[12.8, 1.8, 0.05], yaw=np.pi / 2, length=2.32/48, width=1.2, height=0.6/48, thickness=0.05, add_baffle=False) - self.AddSlope(position=[12.8, 4.07, 0.6375], yaw=np.pi / 2, length=2.32/48, width=1.2, height=0.6/48, thickness=0.05, add_baffle=False) - self.AddSlope(position=[12.225, 1.8, 0.05], yaw=np.pi / 2, length=2.32, width=0.05, height=0.6, thickness=0.05, add_baffle=False) - self.AddSlope(position=[13.375, 1.8, 0.05], yaw=np.pi / 2, length=2.32, width=0.05, height=0.6, thickness=0.05, add_baffle=False) - self.AddBox(position=[12.8, 3.54, 0.475], size=[1.64, 0.05, 0.05], euler=[0.0, -0.1779, 0.7685]) - self.AddBox(position=[12.8, 2.38, 0.175], size=[1.64, 0.05, 0.05], euler=[0.0, 0.1779, -0.7685]) - - self.AddSlope(position=[11.6, 4.12, 0.0], yaw=-np.pi / 2, length=2.32, width=1.2, height=0.6, add_baffle_height=True) - self.AddSlope(position=[11.6, 4.12, 0.05], yaw=-np.pi / 2, length=2.32/48, width=1.2, height=0.6/48, thickness=0.05, add_baffle=False) - self.AddSlope(position=[11.6, 1.85, 0.6375], yaw=-np.pi / 2, length=2.32/48, width=1.2, height=0.6/48, thickness=0.05, add_baffle=False) - self.AddSlope(position=[12.175, 4.12, 0.05], yaw=-np.pi / 2, length=2.32, width=0.05, height=0.6, thickness=0.05, add_baffle=False) - self.AddSlope(position=[11.025, 4.12, 0.05], yaw=-np.pi / 2, length=2.32, width=0.05, height=0.6, thickness=0.05, add_baffle=False) - self.AddBox(position=[11.6, 3.54, 0.175], size=[1.64, 0.05, 0.05], euler=[0.0, -0.1779, -0.7685]) - self.AddBox(position=[11.6, 2.38, 0.475], size=[1.64, 0.05, 0.05], euler=[0.0, 0.1779, 0.7685]) - - self.AddBox(position=[11.6, -0.6, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[12.2, 0.0, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - self.AddBox(position=[12.8, 0.6, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[13.4, 1.2, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - - self.AddBox(position=[12.2, 0.625, 0.025], size=[2.4, 0.05, 0.05]) - self.AddBox(position=[12.2, 1.775, 0.025], size=[2.4, 0.05, 0.05]) - self.AddBox(position=[11.025, 1.2, 0.025], size=[1.2, 0.05, 0.05], euler=[0, 0, np.pi / 2]) - self.AddBox(position=[13.375, 1.2, 0.025], size=[1.2, 0.05, 0.05], euler=[0, 0, np.pi / 2]) - self.AddBox(position=[11.6, 1.2, 0.025], size=[1.64, 0.05, 0.05], euler=[0, 0, -np.pi / 4]) - self.AddBox(position=[12.8, 1.2, 0.025], size=[1.64, 0.05, 0.05], euler=[0, 0, np.pi / 4]) - - self.AddBox(position=[12.2, 4.145, 0.025], size=[2.4, 0.05, 0.05]) - self.AddBox(position=[12.2, 5.295, 0.025], size=[2.4, 0.05, 0.05]) - self.AddBox(position=[11.025, 4.72, 0.025], size=[1.2, 0.05, 0.05], euler=[0, 0, np.pi / 2]) - self.AddBox(position=[13.375, 4.72, 0.025], size=[1.2, 0.05, 0.05], euler=[0, 0, np.pi / 2]) - self.AddBox(position=[11.6, 4.72, 0.025], size=[1.64, 0.05, 0.05], euler=[0, 0, np.pi / 4]) - self.AddBox(position=[12.8, 4.72, 0.025], size=[1.64, 0.05, 0.05], euler=[0, 0, -np.pi / 4]) - - self.AddBox(position=[11.0, 4.72, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - self.AddBox(position=[13.4, 4.72, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - self.AddBox(position=[11.6, 5.32, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - - self.AddBox(position=[11.6, 0.0, 0.0], size=[1.2, 1.2, 0.03]) - self.AddBox(position=[12.2, 1.2, 0.0], size=[2.4, 1.2, 0.03]) - self.AddBox(position=[12.2, 4.72, 0.0], size=[2.4, 1.2, 0.03]) - - - def unit5_sandstone(self): - - self.AddBox(position=[11.0, 5.92, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - self.AddBox(position=[11.0, 7.12, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - self.AddBox(position=[11.0, 8.32, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - self.AddBox(position=[11.0, 9.52, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - self.AddBox(position=[13.4, 5.92, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - self.AddBox(position=[13.4, 7.12, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - self.AddBox(position=[13.4, 8.32, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - self.AddBox(position=[13.4, 9.52, 0.3], size=[0.6, 1.2, 0.03], euler=[0, np.pi / 2, 0]) - - self.AddBox(position=[12.8, 6.52, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[12.8, 10.12, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - self.AddBox(position=[11.6, 8.92, 0.3], size=[1.2, 0.6, 0.03], euler=[np.pi / 2, 0, 0]) - - self.AddBox(position=[12.2, 6.545, 0.05], size=[2.4, 0.05, 0.05]) - self.AddBox(position=[12.2, 8.895, 0.05], size=[2.4, 0.05, 0.05]) - self.AddBox(position=[11.025, 7.72, 0.05], size=[2.4, 0.05, 0.05], euler=[0, 0, np.pi / 2]) - self.AddBox(position=[13.375, 7.72, 0.05], size=[2.4, 0.05, 0.05], euler=[0, 0, np.pi / 2]) - self.AddBox(position=[12.2, 7.72, 0.05], size=[3.28, 0.05, 0.05], euler=[0, 0, -np.pi / 4]) - self.AddBox(position=[12.2, 7.72, 0.05], size=[3.28, 0.05, 0.05], euler=[0, 0, np.pi / 4]) - self.AddBox(position=[11.6, 7.12, 0.05], size=[1.64, 0.05, 0.05], euler=[0, 0, -np.pi / 4]) - self.AddBox(position=[12.8, 7.12, 0.05], size=[1.64, 0.05, 0.05], euler=[0, 0, np.pi / 4]) - self.AddBox(position=[11.6, 8.32, 0.05], size=[1.64, 0.05, 0.05], euler=[0, 0, np.pi / 4]) - self.AddBox(position=[12.8, 8.32, 0.05], size=[1.64, 0.05, 0.05], euler=[0, 0, -np.pi / 4]) - - self.AddBlockyHeightField(position=[12.2, 7.72, -0.00], size=[2.4, 4.8], height_scale=0.08) - - -if __name__ == "__main__": - tg = TerrainGenerator() - - # # Box obstacle - # tg.AddBox(position=[0.55 / 2 + 2, 0, 0.075], size=[np.sqrt(0.55 * 0.55 + 0.15*0.15), 0.1, 0.01], euler=[np.pi / 2, 0, 0]) - # tg.AddBox(position=[3.5, 0, 0.075], size=[1.5, 0.5, 0.01], euler=[0, 0, 0]) - - # # Geometry obstacle - # # geo_type supports "plane", "sphere", "capsule", "ellipsoid", "cylinder", "box" - # tg.AddGeometry(position=[1.5, 0.0, 0.25], euler=[0, 0, 0.0], size=[1.0,0.5,0.5],geo_type="cylinder") - - # # Slope - # tg.AddBox(position=[2.0, 2.0, 0.5], - # euler=[0.0, -0.5, 0.0], - # size=[3, 1.5, 0.1]) - - # # Stairs - # tg.AddStairs(init_pos=[1.0, 4.0, 0.0], yaw=0.0) - - # # Suspend stairs - # tg.AddSuspendStairs(init_pos=[1.0, 6.0, 0.0], yaw=0.0) - - # # Rough ground - # tg.AddRoughGround(init_pos=[-2.5, 5.0, 0.0], - # euler=[0, 0, 0.0], - # nums=[10, 8]) - - # # Perlin heigh field - # tg.AddPerlinHeighField(position=[-1.5, 4.0, 0.0], size=[2.0, 1.5]) - - # # Heigh field from image - # tg.AddHeighFieldFromImage(position=[-1.5, 2.0, 0.0], - # euler=[0, 0, -1.57], - # size=[2.0,2.0], - # input_img="./unitree_robot.jpeg", - # image_scale=[1.0, 1.0], - # output_hfield_image="unitree_hfield.png") - tg.unit1_sponge() - tg.unit2_slopes() - tg.unit3_stairs() - tg.unit4_diagonal() - tg.unit5_sandstone() - - tg.Save() diff --git a/resources/robots/go2/stairs.xml b/resources/robots/go2/stairs.xml index 80d0cd0..6fb6c1a 100644 --- a/resources/robots/go2/stairs.xml +++ b/resources/robots/go2/stairs.xml @@ -1,5 +1,5 @@ - +