From db3122233f4d95709e66bf70fc43a4ed14bc22ea Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Mon, 19 Sep 2016 07:02:43 -0700 Subject: [PATCH 1/5] remove that odd triangle in the origin of samurai castle (VR) add rolling/spinning friction to cube, remove it from plane/samurai.urdf URDF2Bullet: support joint limits for revolute and prismatic, only if defined (if upper < lower, disable limit) add some profiling markers to improve performance --- data/cube_small.urdf | 1 - data/gripper/meshes/l_gripper_tip_scaled.stl | Bin 49984 -> 4234 bytes data/plane.urdf | 2 +- data/pr2_gripper.urdf | 25 ++-- data/samurai.urdf | 3 - data/samurai_monastry.obj | 8 -- data/sphere2.urdf | 5 + data/teddy_vhacd.urdf | 32 +++++ examples/ExampleBrowser/OpenGLGuiHelper.cpp | 2 + .../ImportURDFDemo/BulletUrdfImporter.cpp | 53 ++++++++- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 25 +++- .../Importers/ImportURDFDemo/UrdfParser.cpp | 2 +- .../Importers/ImportURDFDemo/UrdfParser.h | 2 +- examples/OpenGLWindow/GLPrimitiveRenderer.cpp | 5 +- examples/OpenGLWindow/SimpleOpenGL3App.cpp | 3 +- .../PhysicsServerCommandProcessor.cpp | 110 +++++++++++++----- .../SharedMemory/PhysicsServerExample.cpp | 7 +- .../StandaloneMain/hellovr_opengl_main.cpp | 100 +++++++++++----- .../btOverlappingPairCache.cpp | 4 +- .../btCollisionDispatcher.cpp | 5 +- .../btCompoundCompoundCollisionAlgorithm.cpp | 2 + .../btConvexConcaveCollisionAlgorithm.cpp | 5 +- .../btBvhTriangleMeshShape.cpp | 9 +- 23 files changed, 297 insertions(+), 113 deletions(-) create mode 100644 data/teddy_vhacd.urdf diff --git a/data/cube_small.urdf b/data/cube_small.urdf index 0bf03f5aa..299ded801 100644 --- a/data/cube_small.urdf +++ b/data/cube_small.urdf @@ -3,7 +3,6 @@ - diff --git a/data/gripper/meshes/l_gripper_tip_scaled.stl b/data/gripper/meshes/l_gripper_tip_scaled.stl index 94fb0246264762b9c9aba41800850b7de011ded3..e38df0ad9ace5d961f1285ddd6ac0f91eeacc140 100644 GIT binary patch literal 4234 zcmb7`X+Tuh6@V{@Cd7(HO(Y09fdtVgi`WA5mJ3utfMq%l8r`@qmC(*T((YMDtjs zuf%|AmObjKE zsCSR$P|*pqKDAXP%I8;cH{VFO?xM4Kj zE`O%b7NiY*ySSZ^PUeeMN9z%C*(l3j9JQg>v5rD5NW(rM^sS*>c4+pd7Q=3g571qe#U+Z0Nqd;TX;ysj|2H*ZU%J>Tp~gOPClGUwdVu7 zIS;!GsyD0@J&gO|%mh;?P%*6*xbzKC8$w;#QqZ26Rp8Tv;F3M?Cu_YR(QJ za6e|28?_Ay-R*e$Hp9MfK$IGPWy?H8tg zwV4bU$MXGiI)#qYJke-><)ZZcDsl7meZi;rlJf12&nes@xGSrwW^sd#Q^=xY->6YF zK3I))e;-nuD+`!igB69nL1@F93_AX=^Ta#jPRnqv;3QTxx{@N=X|~Tgtok@;l}Z~9 z<%+Fe7BV*x)`um^u4K^sD;Z=`&LRccg_VXhLN6>*smG1o;^LA40;7U(f5eIE!= z0tvW}2yMxoAtsll;s3;Vh)@D^>bmjTjH-LJ$fZKfV@&c?ar2C1+;Mxm2qiFEXbGXo zpNHVk^h2^`#uN;v?_66MSN64&X>j2^X5J7wXB~}QEAQ~*!(WJS5+U*agF5bb(kPzn zx}{FynW6pS@cd*Nu`ok`TF@Wd0)%W5les<5C2C!h!$S$&3HTDJ_v+sPWQ9|WeBM&R zPz(Bi(IO;l?lK3y*B}>m7%|)&NVCKOeU;WUBH^I*&K%skGuK+QJ92FJvcv6sbW9Ps z_1uPkyP;j3cI8?l@BfR0ZycMUg*AaR>%+UNf%oc_@Xt%SIY>YskVYt?{gg7aLXT%f z))TnraBJb4h>&NUg!g!3xwvPk84~cqL0Y}kN%6S*RHtZh{2YN=Gn-<$6Maib6F)(C zS)ytjdS+=ImzMoX0>4JoRD13)Fjdzw| z&*80QiID9Qp1N(T+_-wD3HpFJ!3Ys*%aw4%a0l;`6D30lB;anUwMxju=fYgXrvpz3 zj0E~)qiS(*7FWELjFtRtBD4jsPi=J^`E{WKk?xIF_oKbUO8l`W5)T?iW4Ni1fR@w? zcXCh=*G1yJJ!2RGYO#C1&OpW|9bu{qb7J#Y6H`gP@6yX-vm^>{pWEtMt#A7~+VbEI z-a)^gXlz&cIJVwW=jz@2uBOoV>E!M(OTY^UX@ulT3BO3VN51a+M1~Sbz;^|qQ9F&e z#q&G)O=^$~wctf%i3Yb;W$oAm>btUm`SV)vD3xgT<N|KKfezJNmp?0M z5#jp*3HX*Ebn9#)jkq5~+)7U~1ia^vM(B5oH}P}hB|7{hm&yE>Kj}!;<62Yl`bdGT zZr{j<^nZetCeS05gP{vw8Tc#EAVoGX0?|^N$N>j3p)=epji9GvGUq>a;LJIN@vu z-3E`yvp<&b+I1fhm^mb%C4_d&d8(}Ri=n&2KjWb-m=haS)rr3;5d|rBTaHl$Z9({(fbq+_r&;^)|!xRq(Ox}(W~)b#MZ0z>9eovmEi3D)aH)^`Ek&meD_v~&>l~$aPpz% z{zH^=`gJJuWUQVavHBiC-fpS-$E))>Q)sJ;edLv*R)L;CA}DPfxqCX6I7HNC{c$;W zGX1sg5L%Mil_Q{5dBOzJ@7h$7;n|ZB_GVGk^>+dKUg4oUYut|Px@|;E-o92r-+97d z++<3BA4V%uofN1AJ>-dAJ0j5aITEegz8zh&KvB7W+DwcSJ4y%VzgM05ol9D_Hj>`> zb`c*PTS9&v_{*{z^D6`Se3R(&uvQ!awYGR!ier;ElWbQ97RU70rubb+N1EDIpF#;F z;OG&Aed8MAlYSlP;5k1PNIt=H!F=5p(afR!4 zV)1&o#1r!RKmxy=(huFOI8Z`sm^#ocMw-e#iN!E~}rb5evel_xgsHN&oVI?xd-^(o8`)Pf!g zg5us1b&YbOs|NI-u%DqlNDBh}*?`8!CM!qqAZ|v%F}Az1IqA7UpHw7mQwc(_w*zf% zUSHXzucA-_2{@(&LFL&OcPiYg+*(tQ!W9RuU2W8!Vp|sn^4p{t8;L0wYhlN-8_JzN zm_i98_Ouekn%^BrS(?Cz^7It^YyA~sbm5j3_A^|g%5ONeb=b1ChY z56NAWP5e8xkak76lOWe*V&C~Si(~xoku*FtS6SY)N`dP~^X6knoAfN=v8{z<{4Sj2 z49+5B!n&|HZfZL#k$uc*oe4v@RTQp|XQEw5+Vj!G^7;;DW0~z`<-~+~G{9Ek)jwZ@vPQu>KXZ{o8_+fziNzMQz0+Ui@2$2J`U|yquTIKl^w>osI@Guwg?#`C*b*O768;QT93FOs>0$C6?S{%^~NHZZjex>4PPcS*r>%975i@UP@)jp)9Z@MP0!Cm=H zxuNLM<(9ff-@A;E|BO{%cxJ9`aw8Q(2_&i})fL}(%+|PD=!osUF`DQTYn6fraWr;l znyT`0IN4!xN?K}?BtC2sO-$TROWL}D;`%EKN#g2@ELU@1jnN$2v{va~!4ps`x1yFx zzi=Lz6mWqNo5miK>oxL2%HSCkN+9t;-COLrXgzsrdX^E05j`29`GY$VEw#f%u; zy)haRH3@%gasojKB#LY&ikWu#q{I3MMxb*xXxYPXyk*e^j(}QckB(CH8^3~B+l*wy z>M2F?jrsw2jMflC2_*P+*Jax|lzM5ga@!=B8@q74UUU=1+V{LksOvg5u9hykAiW3= zqeV|&a5D3GR1zV!=8)Tz%Z01})#KKnWziob4_S zi`Yw+eICVZ%x{kI!ST&#ZR-^Z)Pg%Co;Wum2D=AtQ*Kqw=bp8}??#KiEw_?>D?%km z3qon-K6ET#I34KtT!C4HS%BUOLYF%ClzZ({XH7tS(=L=WL*azII6yb2`;&D7P1daloCEAb3=}Xwy6lXvwtE+#U{Q zfhT?*?WmoXUYjN_3E&90dxbj=wzC|Wsl9uyIdyU!!tE>}0iU9Ra3&yA>r0!{XYvp( z4!C>eiB-3^Ylk22OsmUP+@2afk$K|T&+XbF6FSp{?M04&yJbiV!o_5bHa))wUH06W z+f&2+98W||P;0}Vy3zxgT{r?h&mqm$_72r@*;azKcZj60Z(*M|&(tHc7rM9!m-CMa z!bCRF51Fexv$pxX$su2!kuiRtSC||VKK4qGlZ=^b1M?*`C6T| zBd<>82&kp+IGLR3JBRcb<;IBIL{DmVzTrdX zI-OJ;%gYrgfy9mU@nm=2YI1znKt{NCx1xv5ENDusF}H7rdtaXDn$?854{b>^%8esTpdZpOYm1@pdb9-ty!g-=-M=U)vuA?&O??NsPm@AlR_J(C}eOfTrp014& zXiZQcnb%?isa)nGK@WKwc-3L0-=4uVBlxcZ_qCAbiJ9{(m0m`X^ly)2%69Lit|R=< zlg#6*q?Iejl8noTNZWPGCAilXgjEI>%I&+6G^FPtZqERTWjUisto=@M%OsBV$F2Jf z6#M+~bi?dhoL8`2p15DxRe5+aiEbM(LWb=^57+hXL+lo>C+0Vnu$|1>vvs83w^Qio zT_)?%N)b>!OUrHnA$?W~IK5lTH?yhf0KS}E<^35r=oerr}RBJ{zpqOJqN z=ta#-1SOEL+1pAy?zf6`Dbo=xI}B1kS~ij zliJ+Ua`^Y>q$!A$`VSz57KMz!M}Mjg z4Y8ze)4ed1K;lmAM&i|X14zcV^^9-~8sX;Q<4Zqh*5?SQCEuzq9<*^KEk0~yL?_=R zntjDX>5dF@j(}PZo%@pmvWk?C))6O8&QwJ{n?jqm(;_H=gngg>Wb>`AL>9I%V(1Su zl4zDlUscRPPyz|Z!+prReVs{Qy={!>YLcchij1Rc=d43e0tve@t;N9b&Lp#ej%c=)S8-)GNsqK?x*gw6zesmE6}{G}96H zPv-X6{4|PwGda!?P|JCZNSxZ8*OV^5$%vriYs7vjlWEP_3kXUe(QrX~amVAcnwm*E zBILv-amM*jdTnXui#vVb_ffj;(FWUG^Ci8u z6{~yRyDbOp^(ISvCTeVNm&>nu4koiY+*0Rmy~Awu`(;6=`nqUuPV&M~0tx6L8|}%v zmHM|crRcxW909f9`yfHk+D)b_kN8WmE|(DWDs$6VQr_*dT5rfr`EmRhGQcHG<9}J# z=N~@>Q?~_W&uWEFAhF8@sQDu27UK_udBcN7dPy&hnlubrfE@y=A z5^I!nE*6hE%M(!R=q`8Cu+4HZ-ZqyJj$?YDQ!kS6mMa?xlt7|`VIt{cmrY)Y`xx;# zum?KJi0x%O0kuNB2a$*DtL68k{fyXjtuyNLs}J62(V9XDB=UPEkaL^T$?gXVBkEo6 zj0Q4dMCKqGlIs>TF==!iX=$TR zQq#?MR4WV3rF)BiF{0YV1|Pg8V~@w80=3@%x~_^et1rF#TlEj2N}7qaGrOVqeUG#b zCVfQLH7~?-SM}uMlU>A&M~}o|#$TlOGkc14s(*@8#J|kOnFR?r>`5G&;cult2_!PG zjkx*FM{$?kTSiob1mn%ED$$uhQw7EWY3QvW_?L~xH#e1{+}KW>4cIPERJ)GBhAk`5 zJJac0nC2`>mCre5{IWVt|OWR;-ucG`gYbp0mjr9Ii7q>-u z){^wrK!)}N;r-4W^dme3|NX3RUNx99MddNGzSJ?{vDDi8$?=1+^`%vv@3J`NEsH}h z>xAQ7w~i@Lk`}f6So_#!(sZRt;=MX?n4$$M=c3Syq4dPP_A2{6&&2PW%A~@TKaS_w zKN08LERjx>jCSpx^<7-tvy9oeQGXh~nz~kwd^}Hq8HIg*yt09~@yau?@297Xh_nyE zi#KjZu6a9o0%-C5A$1soJ^c2d{WXdLC9vJ{s>dpm$5o>C@I7WDc1Az^;`v*&+WZMe zK&@z-C92R522$U9x?GiY#yB{_3TMpDs<+95}c=_KrKRKne5^_6_^H z;B!Y@&)63KJfbL20tw!$KViklZA>pb`(bSg38)2oRuE)Jf0_Mx^J-xb$_+Qo0 zqNcy!Ga`D-;Z4>zbp>TYJ$nliFk079^mz>`Ox{ zYrL<-jLv#d58v_WAb!}o$IWZ$8+n~SmU?a8Te;4}He%DI`sy8e)y%86Z9br| zH}3TJ<(o*`sGYc^)nc`cQpIUOS`hYzw7>?x^y%{5EijZod#BEosL}?SsA3-Ka`c7O=$W_jmAHd zcU|wL{@Cx8`s=Y*%m!YaDSt6erJY`ebHt?FBGm-xms+`7!O?=?bIuvPpEQly4Hqx`5kPyh8lHa>kRmvan~X}5-tISKR%w#1(2e%F!Ps$n#D z>}3RV1#Q5rv$unl2Dl>Ci0;mI;Ia!d3dfru7&QHfR_WKLZcljvYVp|}?XN{X9}!(z zR-dy0wP4=}g7=D}DF28bU8K3j#R0WoOoEWmDH_dQIDwjlYY^0eGm0nj_c|bLVjNw! zECWF;*v~w1a^gU2Q2(}a(C{@1j*k$>o^DtaWmhHto*XTfx9+N1+V_!=vYVq?U?{+6#IIROcYk!m@pcdbQ8Dl1(ot`sjPZI;}hO4c_ANn`dU#cH+ zqYsWs_IK?b8&pzf7PWklrG=7F`FmB%zJFA282(s>EwN`*lf&psn+RIaJPN^_z|qHh zm45RII_EW>t_|;lPL7ns(!XkT-~G>Is0C?3NNjoxg$6}ZQ1~TeO9NZJZ7_2J-oK2Uv4VR%bci%S}+UG4%w64 zEB~n1j(x_6MmyW6CSFLT^|#w{1k~bfl+Sx1HeCN)DZ3Je;mQfu;60=c+3@;{oB8XH z%tm&4d(wkiQd>JOj(}S4+SMa}Yt1kEQ~H4sp_?A7_bnYjJ^fm91k{>$$%I@e304mo zsCycGzMSLsGGY+@P|KQI6QPGZaiU*^+GS@1jazz?dqzPmn`yPkh*ne`wf+sW(W&n; z(fQG2+Vl50Zbrd1m?zfbo#OXX)2VS;8iMl<&PSd|yqh2veoLXl^N(@sE~K-O8F5JY zr7o8%nO8}l^)>nLXV9rL$8a`ayL-AeCKfTT)Xx{`*7iRrOKokPLYiuT*Nk-dchq13pW5V!maT%IZ%lwI*LRaP&a}w#4S(u42tPl!7bF-^nH2^3}r2 z#!}M#D%r2F1=-uGiR62{QVz+hP450^AQ^VoJ!{vQ7K&rGC*co7M38`52A#dtR%T|> z2XkEy{wb(bcYGU<&%arLpac>>%8kf`Qe&xqj_z42&Fn0$dmfAL9NfSWP|G=ckNRVjx-?!8r<{B(&Y7;uRng@%w8KshznnUpiv!v?e5Jmd zZ{siGiF{qIlI};Lb-N7lBt0JtwIX8%7X{jsipBju$vhFX>MlBWv;vji9)zJ5^pGc7 zzxau6xqm@+%~TjlAi>As^-#uA$r)r=R1ZT5jQpZ?q`0p7x@cJQn#J*X#7Nvz+=(uS z-bGO2*=D&YEh!Z>iz;Q$+3{kP<9iV`*3H@$%SC)Yu6&qSyDOSs+Ye72WXR1ZI9`J^zm82fY%D34-!dDg{jAZM4*qz~2m@{; z!v5z8uWzf7?_M9g`Se!=C6M5~(sQ%IUUl5CPo1p@<^*QrOkV&VJxlJK*F*%((VR9?0;6}z8YBtfrc zpS!1;>D@%C6Y-4mkbRZW5V=?5jw0dxMEs!dK(5ar%@gA`Z9}V~!|};&ixBK7*n>QA^IjkHeRM3YYciK3 zpcZ`Eu~U;aeb9~CvH078dED5A_IP5z*ORir=O|p)`2d$I_`HI%KoHXUH^W|~9(aCB zD|rLnB7U94#?Pj5`ReGC;+a~tq(c=q<>7|vvD&2E6UyJkmKW8>bLrbi-9EelDcyS{Ut|f3(vL|P7Gd$*w zJ6>_t9YF~s__btE---CU^=edTxs;0>M$3EU?COa#4C>-#LK=b+NI-jnU=lbOKk#dS zZA=mnrv-Aw6Elih;th-Y;z|4aAt-^_<>QF7*oWGTi^kRGn{#=GqYtiQY$UFphEpd7 zqpMR}A~+J^=!5+)2$`HDzA- zeR5;mQvVM(cHwyCiERry;lZ~>+^g)l3?-0&b5jtGX8GgMq6V$LagN&oz)=Z%RuGI{ z48*?;4RGksgIs^W9RN>cx)h>QJ7e*%v2ol}4fY!EmASA@$(lVFdnDK5PQ?2y8c6bY zXw`|%cmDbM>V0gnV$m%ICr0!}P|N*&=?vIX9S%eU;u=-d~$W z)Jot8s0BR~g#LftpdPhH(98V_5ws4q;28};u&!MT#hFZ|m+nynB`_w~k|12EorLup zIVzb67i8%B_%YdHx23t7?RQG#U8Re}5r*lS-}Q7SQXZ^Nzy=#8DL#KYB1k~3Z{Zo@ zh2a^RFE@3Oml(z1P98bRq?Wxn0&2l5us1A=Wc=VlL%N{kD1y;Kt*UcFMYYv7&CRk~ z%*K$>O>pJx?$oJA85cREd19@X9=5dXN&EKy!4Xia+3!~3blcsU2`_c0UanquMvGH@ zXoH<5|2bds-}5jq(>!s<_n3S(us1#G+<`lh0#Cj0MDnw7c>Kndii_V_1fzwy8gwyC z?65ycV|_^HmAauXj$U(A`P1(Xg1$qVC)$~+@U5+QAAJCA}dAXo1|MA z4jDAY@?;4+ zH4~p1FhKdx*-D1v6^_bPjkCprzs*Se!0L53H1-1kU;3Lm{GOxgDv0a}y&{=LO-5+dn8BULWOV6eM6v>yW zVl#3sf?6=JKVsX2wH#ys26_+=d5%x6bIUSc& z2P!}8ns5ZvDlV8KM!xJ%hRoHS7ri-Z0*>3eNxA-W6_;I@1)kW}^C#*)#DzNa{lmop zeTOGO1)=@wA^5YyTgA54c`my!3;bQOx5G-2{_5uRLP8e|wP3qEVV@C!XCGUwWIE@f zBQBvLYBP+u<>t%KcNmi(Y?DO1e9b9k)Tag*5>Sh`p^hj+v#gubyzDL*dIC?%^2Duc zP4T9}5AIP-}5gwpcGDkxZ?- zoY}bDt~oxl@35lp?}A~pFuU-KI9uD}X5#uit(C$~^$~0f+K_+D5r1T6k|!m)Q};z( zWAU*QLzU}8(-G8yk@LiU*Ffz3>X1^qlNLcO=pj#ZC=9~}^Oh@DU++gy0tw&Ep`z@U zMQF!W%qySU?eOuZM)a$TDHl16mM0ukUGdAj4@&Xrx)@3zF($`ZyzQ`%d^?oKY`i#` zkIroBOwS0NFw}x4`~M-bX5tFHPD*~WFEY$7jHyTFVsT3NGSc$mLT2N-c_KEn^i`fw zBLpRoXgPY0IRC*4(tdglBfc1p#wV6PP$tb-%H;~i#1r+R{c+mQ=gOp~%Mp}7;>w|5 z@pow+$?{&xYy`Tr#;8X}`W4?rPzy%O6a9a-z;E{0(~f8EAt-@_E{Fennb!7I*HE0?eIm7oL?bt){y@e2sK_i70vM%^BWZ_Tr&CO7nDs0B046WT&A zT+Ht2ujySSK?x-Evrni7PdZC%?bb0H+atT;-Bzyj`?oEeJ}GL%39-WX!1 zynQ2aedp84xHbc|@OCfk8<=%=d)amzF5dK2*&!U~2&e^fCr z4DInm_8eP$#>17q@C@eeJ42c$zLvRSI>?TiukhvwsHIMNqcSMh60_-r|UGmA%DD^8Ad>#)KGsl)h~uRh>V_-6ey! zrFp`jmJQD7jpc<-NH^dk4ByfrQ@&1DNqacKTmX+UWR6O3!&NaLMj$=w@M_=nq==@r}5sAA@SMm#k`tj*&W^Z zXOuuKNVD~d5!0BBxr_O61+^M>ZAhFfKCA1O=&D({GNLad@IN-7*2y*c#CU3%>efY? zGp{z(YK|PvnBi`Ny(!EUq4tRNri77iYZ-W9Qc-uav<3e%&i(s{JotJF1>j)!83}giC zHAwI_*sR3`mfe+~jDTYaYC+ronNhfu5hGc4p%%2k6FRS&GOv7@4NeQxGT`H=?cWnE zeUgOfuPG9&GXQ7s6B|9U-TRHoC2li|Jbr=$a?YQLSB$Baq2%OHRy!f$wW?vw_J3+8 zgxzb2x|PP`>Io?bYQfb2+7^W3r45nai7?#A_%4E4(085)3n)@H1r4KSzn&@Z?mWCx z&)-Q#{!y7m!I;lbDUNUIBEd(*xxbBVg?WXpbij&7DTOPHL~rV-*2m zWdca^#D)&Zbfsly?e0d6lnm`m5|+1)gzD!?kmd=)AIY?4*lO8SD$&B~5RiZ^3Bu;* zW9fpGca@IyW-2h+eg)G=2eVz|Q^*>Lx8dVpO1GEwqxC#CDzJh;hc!{;fYCwHbaa6P zTM~rj$4zPE;(pX-=LQA#8l-u`RGorDtWU|-)jtUIw`oBbd30|P8GCJ!{5~g)SXM73 zMlG}0U#Z$It(4shXVH5TeYm;?u>Jt_P!JkwRw#02JPp4$QHByo)NbZUER5EXNH1L_ zgrwpCx~7LA-DSQ?`(RKOsmR<;jy`E3!(73Z*j?m{Gw3tZG0NN&3mL`{;y8}HUb%wk zP0f*&eIev$!a{PlyY8>;V9y~qZdnU@xS&9U(L$OhoUBsl47~_#_~v_Z%*S9dxX6P< zF3gkbEDt07N=K94vscL3yMoAsB@v`a7u~xIzjtQT^T1ifwx`J5)rYt6Vf6w*$aCmI zTW+|i#5HfmRVjc3fAjv3`&wo5p3c<4vlCZ40k&IneGpk#dm(W@sq<=lJ15#2eNt?u z)Z=Wxc6nmjn;z8v;RhvXx*>)VNF1&mK$g#)PZnr&@8RZ(6Y151nM%f-jR4WY)k~pwLf2}q#$Dp_4*P>AysX&$66>kjV?*P(UW5J56YEwi zQCf{~rv*0}aMdqhT??2IL3nn$13L}hmP&_jASi(`!8#hO`d;e})Gx0st4wi=tH}Y0 zf_Z)rRJWeIRa`yZw29+ra3*~!I1&S7_3sk?(p7;r>dK|+Fb{iAUJCw%@Y=hzm;aqeQ1U8 zGOpUf-0R-t*$@X}JZrNID?JFp)AGN{)-GPOVfRg34G2iUY7(q=oAZ0cxSlur=Cz-z zGXV)JXFu}MLnL?XbYB#OREc(3(43x1$wyG@hQnwwx2X*Yu-PQ@M9D%QT7I(@MTepg zlt5yy$2emDv^%-^R#*4Kq}D`gQLa$}5BEb*>!5!KSsv1yERWwH^MuQlIO@MYTM54T zo0CAoct99wRP02eJau0I$oN?_KG{OK+GUdrwbo1vBYF7^NbjkIGEYRfBr8wpaO&Ar zkE`MVGuo?a5Q%u{POi+*Mi;E6Tg+tV&*8`1E2dKgL|!H<%S zMxnGuDpVes?d7ry^TQM4Xax0`x=!)llZ~JR5^#hH!a=Vjs`$Ou)*Nbqpcc#zPi+0P zS1Gn{NImYe+JKzW9HbAejvH*pd@ncSHp zpw?!MH)-m4L$g`Zom=u6YD>d+$+XstdfaG-vx_HgeeXyEzsU6UI-Y=94yL0?+@iA@ zPlHls<7I9LJu&!|5_M*e3}+P78u31uq^0fDOzEg2I=ckX#g9KKcG@G{{D3r1R94TR zO{ni&jPhjMpdZUTsf!&(+r*vnd<{#@xAI`C% zr(b0us0HougoTL8&|WSM=sQm= z{aL95?i)%CzG)DYKw|Nb{;Z0JR&&%}cS7jp$bs_E8xeH>xa$aN!Cdh~yVgCGh_@5y zuMroy+B>k~4Nuhjy;0ll)+l?d}MuH|(4 zf+D3LT12pt3e@7Os_Z-&O=Yi4<<;0RT%W_f;fVvU#?bk~5#`d)LkLPBami`~F_^nd z(`laW6w>|g18LjpuSx>@st>hb-|$4KeJko_W=`)ktIh8A{cl^4D3~{pq(!XK*jAP^ z8+QlQrcYkm(p5!uFw}w><%!Huhm|kUJ?OebD-5+@O&^}n%kQVmtlOW~e$axe2m}e( z5R!vZJzB4(}PCfZ_H ztp{rHHd=3KOUs8ZR%$l5V(1B^c_OI7OZ)e{G3}=B&%Nb@Z?oYWPj>$!W~kQik}(Yy z{3+Ceby#@fYsw7j=Zw(i&VmB}K8MwMV0}nd&+=Rpwg0&R*>vuzKrLA7oF_We*`Qv1 zHx(0i6RqKoMP$)(Yw7VYL59^2cKpa9CRHt^PrJUdcNtkeqg0bN&BB=zY!s*ks~_;h zxq6$)K20iZZfvWC5=c}w@FRbW%%p39UTp0C^yw(pc@Txo9F8kc3)YrUB`+WY^&3jJ z%N{VVY?B17d_NW6OSO|A0kwD=!@`?s8u$d_7lHZ|w%g{*0+Je3M+zNxTY|J8jO{v0 zRTC%TjscDw0c%T~$XZCU_Wuw)hwJK>T-}%=KAYMDH(1@BCx8~T$L_1mSXi{MuNsX@ zh@!AvSR*B-dLh}lutIF7f0fyAuifL=qglt$*FQV~wRo=vK1jiRJ2%%3UA3iUIxJsgCNiN-@IMSt;0}*hzc7s+c1Yt1SX* zz3{||Pm?gZKSFDsbCDyU*8F?dRm-vs#e`vxm{*-V%Tcy>wsvIPS+UG&A}8OC_11$s~%3F1QJ5dWL3*Pk5nhO+-8K{)6UqxZe1tfT~KRx@*}Esjd;g;T%ZCr|4on!*`lp;ww_IXFVl_*Z-Ai$67ZHWtE?Iyio1UZ(K?A$ z-0fvZwCKE4{M=xHYN7W_w(iz+4aOaBq-!4!|G?dFgYELfBEM)n?cg9S%G}ElP|L>n zfM}bzOJ!R5gxN4oor(we1Z(lOgWP>Jcz1~>9*jxEmcN~~(XHdTTU+qn9Z#Hbn2Ae| zOx7aPVH^Rq%vKbOpSxtKdY9_X(3=&^!d4Znv|C%X;qH>bTVgyhx^XJD+wP|QV7#2W z)d+8*@r0T^2OF7MYHh6!a0Jxq`}Kxs+-i+VHUPO!&0z0R`f95sV*M)fZ6zRHVR4C6KPM+F9b?p z^%LI4he|`d&exy5KU=}suykJ|RtDRuZ*_Yl!+I%#aH^?-hWSsS&dt0z3A6#dWfd?V z1mMNv6~(Sp%T=IB*DDaWPt;e9U-yiwc*BU+QP?unL78K)4Z(hfBN5Up@^xjkJ-U5M0fJhv4v*)NmEs@Cs%S{kQ z8nGo;-v@fd6Q`SX#GHAS9rNg3#_#I=Z=WI-T`nwg~6i#ZKO0{4S!dY<-W*A*-I$-bUW=EQQ*H?c(Y! z!Ic5hth#uvNG*oxQMdRiwEF5u5+XKs8+*1&t`a>-UWmC`8Td-}I5~ury?N^9bzfJx zrO(;}%JHlzw4X^m33s~+C2Y-3~8R2nO&iLwF{&+EoO5B)SB zLSt$+zCUfzVA`n`J!UE%ab3$>E34j?0DpHirufS8LTtI_KEfM)E$~F)7B11;7C-Dwjq^^x2fk_>MEY3I6l?nC@UvH^t zcAj?C@)TMfm&eT_*lRqorp7_}*fNnO&U8W06KDh4X16(hVr4Cfr{n8}a|G1lH$h-K7V80JY#aW;F+VYb$f{B>I(oMTWk^>SEAC_IpJx-?j5{<7mXL zB?yiTNWhi^p~sDIx_3_u`WAN>ZNC#od^%kf$Fu5mtLIN3+p9~&x7#Y^(wNDl_u|*$ zAHCdiQs{3CRA1-|)Lpp5t4C0&rTGV#Y)#O;NpFqzw%|W4~ z3*@&pbBOul_u_QB>+;BuxgHTAM!K>}Q(wR8-a2d88JSbX%aGNBm7G?C zt#e3q-BNL{_nm);Q0o|KGkq1ZwP=T6Tae(rYIG`=Mm1l6=1uH`pcagYC%n8T(h25y z=)m2j2ufh&e6GTljHEXYA4JAWt|2Ia*&XFLipVVT9>uzvPWgHP^b^*@&5Jv5aX@>~ ztK*21zaYJ?ugmVjQNwBNXaRrdmce-i?eWBsjXh|1O*=d;W+mqp^u#VIlqjo=q=zxO z{ceX9A{F1Z!V&fh5Nr$D;E8W7edzM-4e_KW-m>4gWKuf8Q2K6so$G(tPpnRA|2}kA zz74KKt2qK{Z8e-myiDs#?U3&4Qp-*%T8KL1&COoRPyz|)Evxp{pcP#lndDSPB&j^+vD+e;Pn zHy-P!jpO2gG*7t1=PIv<#^a^s=^O#I_&D6_Iw=kP67jLkT@cJ7%r1;c5Oz7H@>&$*X(HV#0>Zz_A0kvRE>>b&xZ_0t?fjBMR8o?ff{m&k1QM{8BCFUtd!lmxZXoX0^d*7>)Vh>tOY-KJNKIpO zdpM^v4oZvaFzn!d13@iV2a+e27UwFTSbg7p9YZjT7S?No9ty&z-)-rvl7stD}qV-CP$8+{(BPfA&IpK=I z&LL0lM-!ivpbq7K5L`J2tA>->7q5yNYQK?TeC))pX?xlz*cw|Ls^kc$1-)hWafHTn ze1wF1u0O^RPz%l#_BA}4Dqjwc#jECRMo;5;j@1-4oEEQ*WY9hk#5s={f{mX7 zjjKK@7mD_x%hFH^zsth!wf%Z8CvP<$#N>B37*TWelTu-7i9f#Rz!6Z(al;Do=0+`P z{V3fpRAi+&(Ul8dppSAx3blT`$s?|FKa1fZ_a&YPbGokh4710lJzG)uMH+g@6J-sX zE6s+?!tPBAxswC%3xEH{%Se;cR#KN*f0^`jK0l<9c;Uo!VkykiWW zvZN_TK&=-OSCYsi3u$!aCuZYBaFsH8SpeQ0G*5wAus?WWSG$*JyE+tKc$uw*Cj?+` z!j{-Gs$XBc^f)_@)5TGNzC)TPwp6#HN4xdG{)Jz)(7Hv_wM6&pDqo`BOD57f62yL8 zB{oA~PlJbM*7U{c0eJ7YVl9;TdFGK0BP^x6v-D)x65A)$|D_Tnu*e~b2WWSta`*%I@T3;#JZG0seITaGmUiy;nozKjxQI6-8 zGxq)Q`!GFj?85O1`=7n1PVG!5RJ6uJ<4!A33)){P&A{8is1Rsaf=0Ws)<7PNluPH}BEjSj~?=&>9q{WvX$&u$gC|o~acH#VI z6|D

ZWgqkJP)%MGpG_ddR+7ZV0B!zP&|vF6C;WC$OJ+ui`ES(qZb|$eI0GENmB! zCEkXIS1@fgb}KSN2Nb9U`sS+fh z4M?-^A1_v_hgYW33m@-s1oQ;bf-vl1Dz^7b(~#l%Tm?K>X{;q(EcUv4MZL64cSiW$ z^LuDdO*T2wJ{-fEiSXA80}8u}?F)+4(FMA1!qIiFBU3w1^4Mh}SMv;3M&pUEcV+xM zZn=1*s1t?~NX&Q?B3hW=Qro=Joy=^1&>f!$-#~u(wZTveR(s=#?Omeq;j`N`!!~LV z)PgnWctYJU5?2@%k}X%yA}E2x>bfc7f)Yp^+n6O* zw7;e9oUi**J!(Y~KG|kHv7aI#s0Hf|^2D}hDY)pHD3zQiTo1xty&9TXM ziChViL^*gTQIFaq!CEG;K8GOKJW9p8`WA~{z5j5;2X$|;=c4uGt?5~gW>Bxg$N^&`$N};~RD8 zJhqSOWUl(NDO7DX>)C()zir>?ns1kHvFFFCkW_qE+gE-V;;DsNFmhc>|4U3rOTuC4 zF3O>uzB2R#W`Xyrd0-O0SJGSQ9cUwI>jsMJuPh{qt1oi3KVgkM_TAuYDi#aP%o#i-!*^rOIe>^_DjW+ z=O;=nPv&q0)Z+WRXniWq%{?Mbn>$zwM+RKsc;dHv6n#`ngC5$a64(c@Z-)A1iZ#pn zkZ0|6zYTczLJAHUeqOuKsHF;O!P>7pp{r8BDupOB*xhg_fyDo>U{wU3 z5MHL@mf|8QD~In7I4WV^2twuUM6A9z8Cf@%v@ky~CO%iWbIeeP-4vYh`XGWeAYk2x z>Q%uc^Q|$-X`uV7XzB`c^mJV$?wEBL!JGGx=82OJ>L8;Tp*TG2F@iNRAOTxq-{^nV z$j9FYVUv(*j(}SHUHu+;wUKWm`*rRLeGIE=KzlslKB+>c$GmYudP9zYTJVNE+qVb5 zkT>odii;mN!tfqFw8sQQy?jb@5rVlvg`{l9A>?1Sr%D6880&JzQtxS%r6Bs^0)R|_SO*xf6J zq!23-kl&Wsa5n9U-knLp!#dW}LM=FhdBTqoZx}I%5l{k&(ua}c&)wD}{;&zNaYA8r zih3vFAw%5|)PggZCyI;hk$wJj>|VDYf)YsZqkY=ghRA$M6dtf=7lK-FZu7)Qk0$7| z#S|u&`_lA#g4Rex48uGV8PymS!dq|6XI!)qE*VJaZ4n4^8wN@KKA#= zp;UT_*vJ-}$|d+~z3`5^&c^@V?$=dlvS&4GQdrFzXaly(#}UvfmFf>Gl(XFLN$}Tt zp*`NlmCR*IM*jr7uG*78-(j?HEU>Dl0eh9@1<};%W-*s5s0BUzPc;(Sp2abY#R1nr z7!x0dt`<@+Ms&*=sfBk*AOTwvgjVb?iLUd5iT&Z_BGiI68DV^^0!>i@-P&!j(rNB6 z?%o}|g9ZDa{T;P8m2NXQqKX~qA^)rO-xY@^OwOg!mUV5k_kQ)0;4d4)K7jY#1;IRh z7OgpDgFdC#a0Kj4NDIQ7*0boMW_GB_c0VobXZWkraEuAU0?S$S{dHRuvo%Z$wcu}I z^F)_!skCxoy4L)5Z!U6p6HOQSf3v`DPEJpytr~UG8eJMBLkX;6-_d^wS?M3DzL@fi zt-IwcyC3)c5`TLS;_k#j0=C57!+lDnz0F3E^snP3c#{=on$K>UK@yEQF%WGY+LxQP zkbo_*{`i|pP3#7V`cD?IQv&~ccN&f{o;b5(CjB;k5K?~}s)cu#;LRtPX?Dt@#Vq=L zkS(&AJ3$L2kl_1c^ISu*h3x|EvPr4j`x1ChBjDA1G9&w*YR)U&4W7J(TD5VH4ce96 zc>>-AfiyeGF)mFR={y~;V^v3@`$wsc)>x@sU$A$2r5D`Bb{M4oO7vyD;z?p+{1J68 zLqGO3NG_{U$b_-D!_deO!4MBi3!r$a{@VpoT7WryoLExZ{7 z?-ljiXfN*hv`|$Mt_!oRj^FtXloP>NkE1MX09(YXkFqilrh``pAQ(y4m)*Bqeve+U7ukUOxVc zKr76T;>7Xf0F^3ch`fFdge7?Am~UkK0%Av3)q2&54j4K}92rPpO~$FympUtbs{7O3 zoAn6$9PbvrVEof5q)6mAj#MKZPRTfS@rmN2NBgC8hP|Ps#MZSpcP&rZzFol7}cpszNk)B1detb;mIRwm>*ipnJhKz&!AWR zQL06ID5@;2%Y212C&Koc)WieLsBwJ{flnuTniIFhc=ea^2SwZD>FlbI!2UOK``V9D zucRImZ>A&*EJ1?zU?poSHMXV~&9A+W-3QJR*#E{!8w)=v8I?onWA&x%sm0#J=TVZv z-D2rO-vQ#cniUmR0&^GdkBGqI%7w9gX!!%#-y>R6K9p@;_ zoa7lJZi}qyE>TWT9TLWJ;xKcL6BGK6Qy+L-7n2r$lra+pa{;#QJZ<(+)$E7`_UK^V zyI9rzUw`Tyr-mpJ@v2|((b#Or&ntty+%VWUy&mMYT@)U<$3-oAn?P=n>;fCViN)=Uf~$`lhjreY?`S*nGBKmd!8WI;+WO!)+!F z&hZeZDNYG*IbCy%+Gu+G{GlA%EkU#RtuRFnxhq$j6{l5o2{GLXFR;IzUx!j~=gs1J zg;z{qxW?Oy z*6#GBBZre1fmYRTjnN#JyV)Gi+D~4*(6AB3XeK(aJ%QykVDnU2lY@6RRBNl`njomd{TlzJ&Q+kY@C_D6yel;+Gj3aPmtPA&UxK4lj zzCcomNYluOr!roxp((07l=?MaC0cJSP;e|Efg{G)KX)2IwY3}( z*p}E1_imVaUyQ#vhB~>NXQKp1Mo_I$TH@~2wrf%L z9$d1^jYjVZqT_MR*+@k1B5f$PP9GMD7beq!o%bzR7hV;PF-h82cNE<&-6B$U9%KE1 zeZcShTEJ+Eb`H|Nm0u{RHbTFl*Nqu&Nd)y>v{;`J_zxpcw}iALt?nK}-h~tNeSe)$ z&b$F^XAN8hFw`1pewqBPK7`FYRu~RC;(-oM_@+pHkcs^|f^aRIC^CH277GD%xo5(Vx_tx0=rM zqDY{2)c9ks&oL2RZG-Na8N>2FFz164fkrmft3~ycn{UHp)W%?cpzgp>q|BR1P108> z1+xdSya~*@;Dm3`2r63eRyn<_C)3;@!F3sBTJ)y}JHJuO{?d)9`BCN1iSikq;5G-|Dvn=zBAcToGo2`87itnGtC9__xL8 z;H$c=-Dmr`mJ6$PQ5`%Rs`(q*C|H67SBu0~4OQQjtD;_>e^ti)bEG+uw6;*0;~c3@ zDLY<6E2L4cAxRb5b|#OtlH#Zfxx=#yNmy$ zIgt`ol@_)LQEf}Bv9o3IJXub}o~&RF?^2$2wjN428(@5ISd(R1eAigh<^}f1-It(z z+schkMaAGTEY^n+FHZO$cADuT%s1Z#&ynhvFh0!lgn6sa2|7^BWz|k%`TD3eiP^-B&#Wu09ugr;F z<|(FYW2|)G_6ino{RrF< z;4zuAvx*e|2BT<9QW>^yfN=&+l-w_?R=qux91<%t0;7>g8yV{>1J&Ozy38aW=|-F7LtU*@erQV@jwOtK;Or_%qr0Rj6S~Y$OFzCm zi3Iu$X=C0s{@eC(NDMWOxX-+c{^5l8vM74(e?nv)ZLi>Z8CMgy3YVm!C||m1YDshN z)=_ZPh*m*WXK6i^dbZ47_KaNX_WD$=S2sHUq>_SbOQbn*!|F`Qj$w3Z<#ibew8FKh z@#mJVnPPhKWa?OXDT_>?Rd}^=+PYSIZBNhHPdN_R-(SQIiZ}kS@vV$%Rg7hD;*iBG zve(a{Uw*l3K`UGdbK=VRSW;&M2(6Bzf*!>+Rbl%i?cFK|)1Uu6vpAvZMC$nFgm~1~ zi$y<>=EUiFVf6FRN_4DWp^SZiR_HflB^>ru%&Z2RvRi6BVhtLm!(`39uq&eX-*GlGgHT79b z`?LGNbrk<1^PXv?w8ahes;xuVItst%I8pM!Rn3SBrtNRs1ik^}-&bqOI@xw8O_KTB zAk*7X4Jr(yl;A1?OOWt!Pu5-<`?b#F{x;s6r*eN(+-grF=OyQKEWvjl)+9;)xTcD< z+oLIO&k=#dfrPbMjQ4}!W9I5qu`f;B-NRIO{}@`Jl_Sq?)jD*%rn)WQS zoL{fE_MD%T_I2spp#b{oR>}yps^E~SX%9bTujypZL<&xCN-4Q*Y5#y91o{X4*0Ajs z?eM%YrlL*u9xSt{1;u$bqjsg+n0K)aPQ)ekpl;5!sBxQMfh9v1bsUb3U&2?-SxmQE$3b%AY|T`FynR4e%h3q$lFLFbp&Ci=MXuSEumlP8p>e)iM;}TKEkm)}oY^yozT(6W9u+C(b1;p#AInBN z&O|tlB`IU*SMhh$Ai9;;RbUAc+*e+&ReJQmlN_5m3G_SC*iXh^pxocfV{0X-$+>U* zaHhu97Jf@eQs~V%-D$p6{Ug1d1+8$c#))B%7j~*zWK~OK8_O6#8rwG1329+|C9kaN zivpX4eJ4V`0VmF7CyUyvX41aI(`F=ajf-nh;}4j(V^1zNa$jAqHuOURzgQV9t&NJO zU(fUr;d=vQ9O3vifX}}qRsT7Lc4lOWqfHeV-{DALO@>0~>>!H1=0Mvn)n^1+@g5{^ zh5EGep%$|pWt`iwJx=5XD&*zhLzP!M%J^-71lDBiJ1==Jnk7Wg*wSPReq|udiS9w4 z#k%O>6g(-_f@2hEPL#xEi9VlV$hxrsYXfPVB_!!q9bd7tU=HmcJk^9F4%^_w*1T`Vtx`Mwx2~JcSESoZ@=8Hy?Y^O{)(-OEnV!P9lHCd0c;AL}Q2C8!VGpKEA^SHg)4kDi-j^Q;sy*Fmn5zFZqWW4AVB zAwQi9Ym%gMw<2gy&tl=FeUh;R&-THZ47Er=y8gH%LG@hHR7WekDo#|{_(A{u>^y2v x93Z22k-(ZH>1!twdCzV^_m|z3u>{XS!kP?K*t6ZD>({C3{8Uv#D;yb|_zxVm7n}e9 diff --git a/data/plane.urdf b/data/plane.urdf index 57b746104..b2c2d7659 100644 --- a/data/plane.urdf +++ b/data/plane.urdf @@ -1,6 +1,6 @@ - + diff --git a/data/pr2_gripper.urdf b/data/pr2_gripper.urdf index 6f9a5b496..47f8c8a39 100644 --- a/data/pr2_gripper.urdf +++ b/data/pr2_gripper.urdf @@ -17,7 +17,7 @@ - + @@ -41,12 +41,6 @@ - - - - - - @@ -61,6 +55,7 @@ + @@ -70,9 +65,9 @@ - + - + @@ -91,6 +86,7 @@ + @@ -98,12 +94,6 @@ - - - - - - @@ -118,6 +108,7 @@ + @@ -127,9 +118,9 @@ - + - + diff --git a/data/samurai.urdf b/data/samurai.urdf index f15d47fb6..0a01099cd 100644 --- a/data/samurai.urdf +++ b/data/samurai.urdf @@ -1,9 +1,6 @@ - - - diff --git a/data/samurai_monastry.obj b/data/samurai_monastry.obj index 75b2a7833..435aa18da 100644 --- a/data/samurai_monastry.obj +++ b/data/samurai_monastry.obj @@ -331416,11 +331416,3 @@ f 30335 30336 f 1329 1330 f 77715 77717 f 20619 20622 -o Cube -v -0.500000 -0.200000 -1.000000 -v 1.000000 -0.000000 0.000000 -v -0.500000 0.200000 1.000000 -usemtl Material -s off -f 86188 86189 86190 -f 86189 86188 86190 diff --git a/data/sphere2.urdf b/data/sphere2.urdf index cf7618d93..f00423887 100644 --- a/data/sphere2.urdf +++ b/data/sphere2.urdf @@ -1,6 +1,11 @@ + + + + + diff --git a/data/teddy_vhacd.urdf b/data/teddy_vhacd.urdf new file mode 100644 index 000000000..7df9e7919 --- /dev/null +++ b/data/teddy_vhacd.urdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index ecd1ed45d..ff499f0be 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -505,6 +505,8 @@ void OpenGLGuiHelper::autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWor void OpenGLGuiHelper::drawText3D( const char* txt, float posX, float posY, float posZ, float size) { + B3_PROFILE("OpenGLGuiHelper::drawText3D"); + btAssert(m_data->m_glApp); m_data->m_glApp->drawText3D(txt,posX,posY,posZ,size); } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index b2e528677..f61926486 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -14,7 +14,7 @@ subject to the following restrictions: #include "BulletUrdfImporter.h" #include "../../CommonInterfaces/CommonRenderInterface.h" - +#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" #include "URDFImporterInterface.h" #include "btBulletCollisionCommon.h" #include "../ImportObjDemo/LoadMeshFromObj.h" @@ -360,7 +360,45 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor return true; } +static btCollisionShape* createConvexHullFromShapes(std::vector& shapes) +{ + btCompoundShape* compound = new btCompoundShape(); + btTransform identity; + identity.setIdentity(); + for (int s = 0; s<(int)shapes.size(); s++) + { + btConvexHullShape* convexHull = new btConvexHullShape(); + tinyobj::shape_t& shape = shapes[s]; + int faceCount = shape.mesh.indices.size(); + + for (int f = 0; faddPoint(pt,false); + + pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], + shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], + shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); + convexHull->addPoint(pt, false); + + pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], + shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], + shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); + convexHull->addPoint(pt, false); + } + + convexHull->recalcLocalAabb(); + convexHull->optimizeConvexHull(); + compound->addChildShape(identity,convexHull); + } + + return compound; +} btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix) { @@ -467,7 +505,18 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co { case FILE_OBJ: { - glmesh = LoadMeshFromObj(fullPath,collisionPathPrefix); + if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) + { + glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix); + } + else + { + std::vector shapes; + std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix); + //create a convex hull for each shape, and store it in a btCompoundShape + shape = createConvexHullFromShapes(shapes); + return shape; + } break; } case FILE_STL: diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 8833dd15b..5478b5b7d 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -162,8 +162,6 @@ void ConvertURDF2BulletInternal( int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex); btRigidBody* parentRigidBody = 0; - //std::string name = u2b.getLinkName(urdfLinkIndex); - //b3Printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex); //b3Printf("mb link index = %d\n",mbLinkIndex); btTransform parentLocalInertialFrame; @@ -322,6 +320,14 @@ void ConvertURDF2BulletInternal( cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping; cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction; creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); + if (jointLowerLimit <= jointUpperLimit) + { + //std::string name = u2b.getLinkName(urdfLinkIndex); + //printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit); + + btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit); + world1->addMultiBodyConstraint(con); + } } else { @@ -343,8 +349,14 @@ void ConvertURDF2BulletInternal( -offsetInB.getOrigin(), disableParentCollision); creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); - btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody,mbLinkIndex,jointLowerLimit, jointUpperLimit); - world1->addMultiBodyConstraint(con); + if (jointLowerLimit <= jointUpperLimit) + { + //std::string name = u2b.getLinkName(urdfLinkIndex); + //printf("create btMultiBodyJointLimitConstraint for prismatic link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit,jointUpperLimit); + + btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit); + world1->addMultiBodyConstraint(con); + } //printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit); } else @@ -386,8 +398,9 @@ void ConvertURDF2BulletInternal( //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider col->setWorldTransform(tr); - - bool isDynamic = true; + + //base and fixed? -> static, otherwise flag as dynamic + bool isDynamic = (mbLinkIndex<0 && cache.m_bulletMultiBody->hasFixedBase())? false : true; short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index e1d9629a0..3abc77fa2 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -816,7 +816,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger) { joint.m_lowerLimit = 0.f; - joint.m_upperLimit = 0.f; + joint.m_upperLimit = -1.f; joint.m_effortLimit = 0.f; joint.m_velocityLimit = 0.f; joint.m_jointDamping = 0.f; diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 0ed995f33..9640f3666 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -138,7 +138,7 @@ struct UrdfJoint double m_jointFriction; UrdfJoint() :m_lowerLimit(0), - m_upperLimit(0), + m_upperLimit(-1), m_effortLimit(0), m_velocityLimit(0), m_jointDamping(0), diff --git a/examples/OpenGLWindow/GLPrimitiveRenderer.cpp b/examples/OpenGLWindow/GLPrimitiveRenderer.cpp index 60328514e..1acfeb940 100644 --- a/examples/OpenGLWindow/GLPrimitiveRenderer.cpp +++ b/examples/OpenGLWindow/GLPrimitiveRenderer.cpp @@ -1,7 +1,7 @@ #ifndef NO_OPENGL3 #include "GLPrimitiveRenderer.h" #include "GLPrimInternalData.h" - +#include "Bullet3Common/b3Logging.h" #include "LoadShader.h" #include @@ -205,7 +205,8 @@ void GLPrimitiveRenderer::drawRect(float x0, float y0, float x1, float y1, float void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVertex& v1,const PrimVertex& v2,const PrimVertex& v3,float viewMat[16],float projMat[16], bool useRGBA) { - + B3_PROFILE("GLPrimitiveRenderer::drawTexturedRect3D"); + assert(glGetError()==GL_NO_ERROR); diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index 25d283119..a7254f889 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -230,7 +230,7 @@ struct sth_stash* SimpleOpenGL3App::getFontStash() void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float worldPosY, float worldPosZ, float size1) { - + B3_PROFILE("SimpleOpenGL3App::drawText3D"); float viewMat[16]; float projMat[16]; CommonCameraInterface* cam = m_instancingRenderer->getActiveCamera(); @@ -273,6 +273,7 @@ void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float world bool measureOnly = false; float fontSize= 32;//64;//512;//128; + sth_draw_text(m_data->m_fontStash, m_data->m_droidRegular,fontSize,posX,posY, txt,&dx, this->m_instancingRenderer->getScreenWidth(),this->m_instancingRenderer->getScreenHeight(),measureOnly,m_window->getRetinaScale()); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0639eea64..332d737ed 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -28,6 +28,7 @@ #include "SharedMemoryCommands.h" btVector3 gLastPickPos(0, 0, 0); +bool gEnableRealTimeSimVR=false; struct UrdfLinkNameMapUtil { @@ -596,7 +597,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); - m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.05; + m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08; } void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies() @@ -2850,10 +2851,11 @@ double gDtInSec = 0.f; double gSubStep = 0.f; void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) { - if (m_data->m_allowRealTimeSimulation && m_data->m_guiHelper) + if ((gEnableRealTimeSimVR || m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper) { static btAlignedObjectArray gBufferServerToClient; - gBufferServerToClient.resize(32768); + gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + if (!m_data->m_hasGround) @@ -2864,7 +2866,8 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - + loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + if (m_data->m_gripperRigidbodyFixed == 0) { int bodyId = 0; @@ -2874,11 +2877,11 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) InteralBodyData* parentBody = m_data->getHandle(bodyId); if (parentBody->m_multiBody) { - parentBody->m_multiBody->setHasSelfCollision(1); - btVector3 pivotInParent(0, 0, 0); + parentBody->m_multiBody->setHasSelfCollision(0); + btVector3 pivotInParent(0.2, 0, 0); btMatrix3x3 frameInParent; - frameInParent.setRotation(btQuaternion(0, 0, 0, 1)); - + //frameInParent.setRotation(btQuaternion(0, 0, 0, 1)); + frameInParent.setIdentity(); btVector3 pivotInChild(0, 0, 0); btMatrix3x3 frameInChild; frameInChild.setIdentity(); @@ -2890,62 +2893,109 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) m_data->m_gripperMultiBody->setJointPos(0, 0); m_data->m_gripperMultiBody->setJointPos(2, 0); } - m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(1.); + m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(10000); btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld; world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed); } } - +#if 1 + for (int i = 0; i < 10; i++) + { + loadUrdf("cube.urdf", btVector3(3, -2, 0.5+i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + } + loadUrdf("sphere2.urdf", btVector3(-2, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("sphere2.urdf", btVector3(-2, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("sphere2.urdf", btVector3(-2, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("r2d2.urdf", btVector3(2, -2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("kuka_iiwa/model.urdf", btVector3(3, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("cube_small.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("husky/husky.urdf", btVector3(1, 1, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("cube_small.urdf", btVector3(0.2, 0.2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + + + // loadUrdf("cube_small.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); +#endif +//#define JENGA 1 +#ifdef JENGA + int jengaHeight = 17; + for (int j = 0; j < jengaHeight; j++) + { + for (int i = 0; i < 3; i++) + { + if (j & 1) + { + loadUrdf("jenga/jenga.urdf", btVector3(-0.5+0, 0.025*i, .0151*0.5 + .015*j), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + } + else + { + btQuaternion orn(btVector3(0, 0, 1), SIMD_HALF_PI); + loadUrdf("jenga/jenga.urdf", btVector3(-0.5 -1 / 3.*0.075 + 0.025*i, +1 / 3.*0.075,0.0151*0.5 + .015*j), orn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + } + } + } +#endif + + //loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("husky/husky.urdf", btVector3(5, 2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); m_data->m_huskyId = bodyId; } + loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true); + loadUrdf("teddy_vhacd.urdf", btVector3(1, 1, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + } if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody) { m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn)); m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos); - if (m_data->m_gripperMultiBody->getNumLinks() > 2) + for (int i = 0; i < m_data->m_gripperMultiBody->getNumLinks(); i++) { - for (int i = 0; i < 2; i++) + if (supportsJointMotor(m_data->m_gripperMultiBody, i)) { - if (supportsJointMotor(m_data->m_gripperMultiBody, i * 2)) + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i ).m_userPtr; + if (motor) { - btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i * 2).m_userPtr; - if (motor) + motor->setErp(0.1); + btScalar posTarget = 0.1 + (1 - btMin(0.75,gVRGripperAnalog)*1.5)*SIMD_HALF_PI*0.29; + btScalar maxPosTarget = 0.55; + + if (m_data->m_gripperMultiBody->getJointPos(i) < 0) { - motor->setErp(0.01); - - motor->setPositionTarget(0.1+(1-gVRGripperAnalog)*SIMD_HALF_PI*0.5, 1); - - - motor->setVelocityTarget(0, 0.1); - btScalar maxImp = 1550.*m_data->m_physicsDeltaTime; - motor->setMaxAppliedImpulse(maxImp); + m_data->m_gripperMultiBody->setJointPos(i,0); } + if (m_data->m_gripperMultiBody->getJointPos(i) > maxPosTarget) + { + m_data->m_gripperMultiBody->setJointPos(i, maxPosTarget); + } + + motor->setPositionTarget(posTarget, 1); + motor->setVelocityTarget(0, 0.5); + btScalar maxImp = 500*m_data->m_physicsDeltaTime; + motor->setMaxAppliedImpulse(maxImp); } } } } - int maxSteps = 3; - - int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps,m_data->m_physicsDeltaTime); + int maxSteps = m_data->m_numSimulationSubSteps+3; + if (m_data->m_numSimulationSubSteps) + { + gSubStep = m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps; + } + else + { + gSubStep = m_data->m_physicsDeltaTime; + } + + int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps, gSubStep); gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0; if (numSteps) { gNumSteps = numSteps; gDtInSec = dtInSec; - gSubStep = m_data->m_physicsDeltaTime; } } } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 837bb5c21..1ff054a0f 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -893,6 +893,8 @@ extern double gSubStep; void PhysicsServerExample::renderScene() { + B3_PROFILE("PhysicsServerExample::RenderScene"); + ///debug rendering //m_args[0].m_cs->lock(); @@ -934,6 +936,7 @@ void PhysicsServerExample::renderScene() if (gDebugRenderToggle) if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) { + B3_PROFILE("Draw Debug HUD"); //some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift) static int frameCount=0; @@ -1157,13 +1160,13 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt extern btVector3 gVRGripperPos; extern btQuaternion gVRGripperOrn; extern btScalar gVRGripperAnalog; - +extern bool gEnableRealTimeSimVR; void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis) { - + gEnableRealTimeSimVR = true; if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS) { diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index 32941de76..b0fd21d1a 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -1,4 +1,5 @@ #ifdef BT_ENABLE_VR +//#define BT_USE_CUSTOM_PROFILER //========= Copyright Valve Corporation ============// #include "../OpenGLWindow/SimpleOpenGL3App.h" @@ -701,11 +702,12 @@ bool CMainApplication::HandleInput() { glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); ///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync + //so it can (and likely will) cause crashes //add a special debug drawer that deals with this - //gDebugDrawFlags = btIDebugDraw::DBG_DrawWireframe;// :DBG_DrawContactPoints - //btIDebugDraw::DBG_DrawConstraintLimits+ - //btIDebugDraw::DBG_DrawConstraints - ; + // gDebugDrawFlags = btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints+ + // btIDebugDraw::DBG_DrawConstraintLimits+ + // btIDebugDraw::DBG_DrawConstraints + // ; } sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn); @@ -751,7 +753,7 @@ bool CMainApplication::HandleInput() } } - m_rbShowTrackedDevice[ unDevice ] = state.ulButtonPressed == 0; +// m_rbShowTrackedDevice[ unDevice ] = state.ulButtonPressed == 0; } sPrevStates[unDevice] = state; } @@ -769,6 +771,8 @@ void CMainApplication::RunMainLoop() while ( !bQuit && !m_app->m_window->requestedExit()) { + B3_PROFILE("main"); + bQuit = HandleInput(); RenderFrame(); @@ -812,9 +816,15 @@ void CMainApplication::RenderFrame() // for now as fast as possible if ( m_pHMD ) { - DrawControllers(); + { + B3_PROFILE("DrawControllers"); + DrawControllers(); + } RenderStereoTargets(); - RenderDistortion(); + { + B3_PROFILE("RenderDistortion"); + RenderDistortion(); + } vr::Texture_t leftEyeTexture = {(void*)leftEyeDesc.m_nResolveTextureId, vr::API_OpenGL, vr::ColorSpace_Gamma }; vr::VRCompositor()->Submit(vr::Eye_Left, &leftEyeTexture ); @@ -824,6 +834,7 @@ void CMainApplication::RenderFrame() if ( m_bVblank && m_bGlFinishHack ) { + B3_PROFILE("bGlFinishHack"); //$ HACKHACK. From gpuview profiling, it looks like there is a bug where two renders and a present // happen right before and after the vsync causing all kinds of jittering issues. This glFinish() // appears to clear that up. Temporary fix while I try to get nvidia to investigate this problem. @@ -833,6 +844,7 @@ void CMainApplication::RenderFrame() // SwapWindow { + B3_PROFILE("m_app->swapBuffer"); m_app->swapBuffer(); //SDL_GL_SwapWindow( m_pWindow ); @@ -840,6 +852,7 @@ void CMainApplication::RenderFrame() // Clear { + B3_PROFILE("glClearColor"); // We want to make sure the glFinish waits for the entire present to complete, not just the submission // of the command. So, we do a clear here right here so the glFinish will wait fully for the swap. glClearColor( 0, 0, 0, 1 ); @@ -849,6 +862,8 @@ void CMainApplication::RenderFrame() // Flush and wait for swap. if ( m_bVblank ) { + B3_PROFILE("glFlushglFinish"); + glFlush(); glFinish(); } @@ -856,13 +871,18 @@ void CMainApplication::RenderFrame() // Spew out the controller and pose count whenever they change. if ( m_iTrackedControllerCount != m_iTrackedControllerCount_Last || m_iValidPoseCount != m_iValidPoseCount_Last ) { + B3_PROFILE("debug pose"); + m_iValidPoseCount_Last = m_iValidPoseCount; m_iTrackedControllerCount_Last = m_iTrackedControllerCount; b3Printf( "PoseCount:%d(%s) Controllers:%d\n", m_iValidPoseCount, m_strPoseClasses.c_str(), m_iTrackedControllerCount ); } - UpdateHMDMatrixPose(); + { + B3_PROFILE("UpdateHMDMatrixPose"); + UpdateHMDMatrixPose(); + } } @@ -1565,6 +1585,8 @@ void CMainApplication::SetupDistortion() //----------------------------------------------------------------------------- void CMainApplication::RenderStereoTargets() { + B3_PROFILE("CMainApplication::RenderStereoTargets"); + sExample->stepSimulation(1./60.); glClearColor( 0.15f, 0.15f, 0.18f, 1.0f ); // nice background color, but not black @@ -1641,7 +1663,7 @@ void CMainApplication::RenderStereoTargets() { sExample->physicsDebugDraw(gDebugDrawFlags); } - + else { sExample->renderScene(); } @@ -1692,7 +1714,7 @@ void CMainApplication::RenderStereoTargets() { sExample->physicsDebugDraw(gDebugDrawFlags); } - + else { sExample->renderScene(); } @@ -1720,6 +1742,8 @@ void CMainApplication::RenderStereoTargets() //----------------------------------------------------------------------------- void CMainApplication::RenderScene( vr::Hmd_Eye nEye ) { + B3_PROFILE("RenderScene"); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glEnable(GL_DEPTH_TEST); @@ -1866,38 +1890,47 @@ Matrix4 CMainApplication::GetCurrentViewProjectionMatrix( vr::Hmd_Eye nEye ) //----------------------------------------------------------------------------- void CMainApplication::UpdateHMDMatrixPose() { - if ( !m_pHMD ) + if (!m_pHMD) return; - - vr::VRCompositor()->WaitGetPoses(m_rTrackedDevicePose, vr::k_unMaxTrackedDeviceCount, NULL, 0 ); + { + B3_PROFILE("WaitGetPoses"); + vr::VRCompositor()->WaitGetPoses(m_rTrackedDevicePose, vr::k_unMaxTrackedDeviceCount, NULL, 0); + } m_iValidPoseCount = 0; m_strPoseClasses = ""; - for ( int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice ) { - if ( m_rTrackedDevicePose[nDevice].bPoseIsValid ) + B3_PROFILE("for loop"); + + for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice) { - m_iValidPoseCount++; - m_rmat4DevicePose[nDevice] = ConvertSteamVRMatrixToMatrix4( m_rTrackedDevicePose[nDevice].mDeviceToAbsoluteTracking ); - if (m_rDevClassChar[nDevice]==0) + if (m_rTrackedDevicePose[nDevice].bPoseIsValid) { - switch (m_pHMD->GetTrackedDeviceClass(nDevice)) + m_iValidPoseCount++; + m_rmat4DevicePose[nDevice] = ConvertSteamVRMatrixToMatrix4(m_rTrackedDevicePose[nDevice].mDeviceToAbsoluteTracking); + if (m_rDevClassChar[nDevice] == 0) { - case vr::TrackedDeviceClass_Controller: m_rDevClassChar[nDevice] = 'C'; break; - case vr::TrackedDeviceClass_HMD: m_rDevClassChar[nDevice] = 'H'; break; - case vr::TrackedDeviceClass_Invalid: m_rDevClassChar[nDevice] = 'I'; break; - case vr::TrackedDeviceClass_Other: m_rDevClassChar[nDevice] = 'O'; break; - case vr::TrackedDeviceClass_TrackingReference: m_rDevClassChar[nDevice] = 'T'; break; - default: m_rDevClassChar[nDevice] = '?'; break; + switch (m_pHMD->GetTrackedDeviceClass(nDevice)) + { + case vr::TrackedDeviceClass_Controller: m_rDevClassChar[nDevice] = 'C'; break; + case vr::TrackedDeviceClass_HMD: m_rDevClassChar[nDevice] = 'H'; break; + case vr::TrackedDeviceClass_Invalid: m_rDevClassChar[nDevice] = 'I'; break; + case vr::TrackedDeviceClass_Other: m_rDevClassChar[nDevice] = 'O'; break; + case vr::TrackedDeviceClass_TrackingReference: m_rDevClassChar[nDevice] = 'T'; break; + default: m_rDevClassChar[nDevice] = '?'; break; + } } + m_strPoseClasses += m_rDevClassChar[nDevice]; } - m_strPoseClasses += m_rDevClassChar[nDevice]; } } - - if ( m_rTrackedDevicePose[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid ) { - m_mat4HMDPose = m_rmat4DevicePose[vr::k_unTrackedDeviceIndex_Hmd].invert(); + B3_PROFILE("m_mat4HMDPose invert"); + + if (m_rTrackedDevicePose[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid) + { + m_mat4HMDPose = m_rmat4DevicePose[vr::k_unTrackedDeviceIndex_Hmd].invert(); + } } } @@ -2145,6 +2178,11 @@ void CGLRenderModel::Draw() //----------------------------------------------------------------------------- int main(int argc, char *argv[]) { +#ifdef BT_USE_CUSTOM_PROFILER + //b3SetCustomEnterProfileZoneFunc(...); + //b3SetCustomLeaveProfileZoneFunc(...); +#endif + CMainApplication *pMainApplication = new CMainApplication( argc, argv ); if (!pMainApplication->BInit()) @@ -2157,6 +2195,10 @@ int main(int argc, char *argv[]) pMainApplication->Shutdown(); +#ifdef BT_USE_CUSTOM_PROFILER +//... +#endif + return 0; } #endif //BT_ENABLE_VR \ No newline at end of file diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index ad69fcbd7..55ebf06f1 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -372,10 +372,10 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro return userData; } //#include - +#include "LinearMath/btQuickprof.h" void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) { - + BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs"); int i; // printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size()); diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 3b6913c0e..d41e98c32 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -16,7 +16,7 @@ subject to the following restrictions: #include "btCollisionDispatcher.h" - +#include "LinearMath/btQuickprof.h" #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" @@ -227,6 +227,8 @@ public: virtual bool processOverlap(btBroadphasePair& pair) { + BT_PROFILE("btCollisionDispatcher::processOverlap"); + (*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo); return false; @@ -249,7 +251,6 @@ void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pa - //by default, Bullet will use this near callback void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo) { diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp index 1d64d84b8..ab2632ee3 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp @@ -15,6 +15,7 @@ subject to the following restrictions: */ #include "btCompoundCompoundCollisionAlgorithm.h" +#include "LinearMath/btQuickprof.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/BroadphaseCollision/btDbvt.h" @@ -124,6 +125,7 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1) { + BT_PROFILE("btCompoundCompoundLeafCallback::Process"); m_numOverlapPairs++; diff --git a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 912a52855..322b1288d 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -15,6 +15,7 @@ subject to the following restrictions: #include "btConvexConcaveCollisionAlgorithm.h" +#include "LinearMath/btQuickprof.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -79,6 +80,7 @@ void btConvexTriangleCallback::clearCache() void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) { + BT_PROFILE("btConvexTriangleCallback::processTriangle"); if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax)) { @@ -184,7 +186,7 @@ void btConvexConcaveCollisionAlgorithm::clearCache() void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { - + BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision"); const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap; const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap; @@ -265,6 +267,7 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { + BT_PROFILE("processTriangle"); (void)partId; (void)triangleIndex; //do a swept sphere for now diff --git a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index ace4cfa26..0940da1a4 100644 --- a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -245,16 +245,18 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co btStridingMeshInterface* m_meshInterface; btTriangleCallback* m_callback; btVector3 m_triangle[3]; - + int m_numOverlap; MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) :m_meshInterface(meshInterface), - m_callback(callback) + m_callback(callback), + m_numOverlap(0) { } virtual void processNode(int nodeSubPart, int nodeTriangleIndex) { + m_numOverlap++; const unsigned char *vertexbase; int numverts; PHY_ScalarType type; @@ -321,8 +323,7 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); m_bvh->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); - - + #endif//DISABLE_BVH From e488c724968f62a2ed52f5020fd08737729711a5 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 19 Sep 2016 10:19:51 -0700 Subject: [PATCH 2/5] fix build issue --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 332d737ed..e7d61d473 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2958,7 +2958,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) if (motor) { motor->setErp(0.1); - btScalar posTarget = 0.1 + (1 - btMin(0.75,gVRGripperAnalog)*1.5)*SIMD_HALF_PI*0.29; + btScalar posTarget = 0.1 + (1 - btMin(btScalar(0.75),gVRGripperAnalog)*btScalar(1.5))*SIMD_HALF_PI*0.29; btScalar maxPosTarget = 0.55; if (m_data->m_gripperMultiBody->getJointPos(i) < 0) From a3aa8ef7f144fa1c300ec6a3003456812827fd7b Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Mon, 19 Sep 2016 14:31:47 -0700 Subject: [PATCH 3/5] tune gripper parameters for VR demo --- data/l_finger_collision.stl | Bin 0 -> 21334 bytes data/pr2_gripper.urdf | 38 ++++++++++++------ .../PhysicsServerCommandProcessor.cpp | 10 ++--- .../StandaloneMain/hellovr_opengl_main.cpp | 8 ++-- 4 files changed, 34 insertions(+), 22 deletions(-) create mode 100644 data/l_finger_collision.stl diff --git a/data/l_finger_collision.stl b/data/l_finger_collision.stl new file mode 100644 index 0000000000000000000000000000000000000000..b12b2dd9ae6da9a07d8851fd1c4e3475ce8b27a9 GIT binary patch literal 21334 zcmb8XXIND^7d9NbVjCNZioJu1q6qArYztz=UU5`ZR76DuWI#|uu(K-9$I8jMK*E*CfY| zb7e(%yKcg%$6zz7-HGhqiRHh34qHKjm1uupG;sS2;Q%xh0SKqA!|Di-9PB7d&V!I1wErQ zsiAXYE*;G0YmT#hq*YGa6L{aua8Wh?AaB&bR)6(OqvLNK_L`ormDH#!t*R!iO7m#6 z+YFpdH$PaVWtVl;tACk8YZqJkKMt_g#vwJTOAY6-cinnOjlGXnX>&@rny9U6+}WDI zOoUzRXGO`4`Lk)+X05#U@9;DaH(5q&cS+9n?By;s zqPOkgZTpN9Z7;3RuJ2q%SC19hi;vbZd3qwPJF>H7+KM`6D@zKk{Bg47;Eh^R<8%5t zo?9S9tcqkDIS?0-2+3PvGWW?FDM}ViG@I97N+0!Io_*&|V;y}$Btp7xozKS<3KbrG z-C1nb5<0EKob0FP>gYDy6lvkYqjhvdBIJF(?fi4#cu{a`yoUDf^G%@>mW5i19%!h~ zdbo_{^FN$DPio6As^J25>A?i!^Nwa5CCI08xCJNiBD^I+%aPNwik*9wnA<~ek#z|Sa)U$&q!%3I*cAk-wocPT@2W7 z?tT$Kb6;j@RmLAP?{6PTA340ySY0kh@XM%*#;=XydCwj#1xmbr8Ab0rc%?2m&d93T&^^qHR=oL-_ohp!y8U@v={VgX(~Aa%UdgsS6Y~8-te7@qocUYw z*+wX_Kg(#KznJbtBX$kACvSDE>JD-i4keeN&*wxL$MU@>U z8%r#7hxp{3RKK;}XN`q^Znwqz&p~zdYHoryuTw)y8{JxJjF>Tqm-`k-KSV|u);1q3 ztG{&5zT}y!r#y4B%*=kF`;eP@-w$goW7l5RFE6<+HHO~KZz+^k%li0KtbvmLx!kh1 z-a&of(QEqRQ>Dz;p#j{KCZq7^H1GQC+$lxcf@qF`OOMbREIis~nt>28a*(1eCl zwRLShq*Zad%X5cDJ&m&IMaAbabuCF=xt6H3d&*nlwAF)d+fz!e2W7OPlv zNU(TQWez_V7)(ESt<~N=UuX7w986=EC2Ae~la-YdAwfkqGqLX6IZ6;$wJNxYV7q$;iDg%O6wi?l zkqG(cQ>=JDvyndhT8_5vc|mh=lUTh$N{(5fse}2TSXavuGspBVTENU{9b?&a#`X=C zC^bP`EqR^(nw-k;E5fh#Wb{ePsUnB;6aV{aqf#Tp{phLu#P9=Fmbb6`ltv$_8KW(Pny)Noo%uZ&L_uch; zu}$ftfSvNadfqO-xu|GK4dh1diwE4EJW}?$IE!^6Kb|kyabFQ`Y5&696 z?x>AYqrj8^W{p43(%uhMI1m?+2ss_qkneaKET-Sw$LF+k(FcssXpfg`P2b}lboGuJ zG~eEI6Rjo0=d}ZSHesl-ZSn(-SZE0%5%M+PWOm?5u;CoER?)y#Du?^naPCtyp8J0Y zQD$nKMMpKdXECR4wftS`njG^^{(tby&$Y#uO;rq(3|?B>qLq4KS+Vr4UfDa-@^Z~) zi#hk6)NmK2#Di*!_|j)>6%A~q&bt@tc=8H4(PD33YKE4gwDh7umVRa8%;VD-?KCmK zV%y^c_HIr8UO)PO&x90g?aY_`6C*}Uy5Wuz`?HL23X<%r_y=@-;i_WKVqYi;3$;LLT4!nRQ&|E*d^)C{VKYV;mhEcT^(-el`(_kd^OO z@XZn7Vrs&4Z`?uP-T?O^ggiNtz?aF@tDiPoLkaG25Q&hbOAGRXcd}WNx1){4CyMKD zbWhr9<3{~rtw~y=3of+5?_2*9YlE#l#Nw;PjV~9zS~1pQbl&mwjdpfm6S~BGj~u~q z4Y~{W_%cSnJ9ACcz*fjj$X~m5jB7qJ^tW&GHxD-j3d$A5AFU4vQ1s3Bs=yvs_S;Y6et?c4d7 z_xpfE)4u9+kU0lJx}FGQ*{P8tDgMNMyTtYtx%3X9d{nw-d;WV`2+eyiT{}`TMryp< z_#_I1^6noLz(*wuSfL`mghA#2aj|A45 z#q^IB&m9+7kOOg5&s%#0u`Ru#1X<~jjc+3Qgh+%mx<8tA&Ko6sQ=jPb$_%AljxNy1 zs0{0tH@gw7PH@=YJ zIH<4KV7Y5WiBWMJb@SY;ZO@l%{t+8ON#+LaOxZ*^9#Nx;v&_kpMXOI6b(Ac46-PTS zI2<10^|JV-3dx6XU`**Lmk)hX?9R0HkKp*R{_{j?V2b3i*gAdb$cm!tK_ zGgy~v(Y>{V&&M`mjk(lY;Q(AjlIxP>8b+V8-lE>BnH)VwpAbo|6Q@VAN}VFbtpe%F z+kj($NQC%(9>ajlN^r-b?hQKqGe-QhA&gy+*-n($cN1qf z=9sN|9k!fbdQkVecUf}eObrm{;tsJfn-VxmoO+$K)USI;KO!|k>iJtLSNx!V<>#bE zf4>Q0M5X7{eNr+*3HpTA60+gN6@GuZMvHX_5-33nk(-c0j~6mW*NT=gmnJG2*b2D` z*|+`{Thqc%)U0utw=0xR`+D}Xblg%&$vPHuN~QignB|V6r}X1U&zi>llTPB^`hPe| z-t9}H^Cwic^jg`(L?l9%4;#o!1~`ZrwYv$-2x10O&8khEwSzZF7$^SO6-!Zqc{oHO zq~^B@qJZHi-Zgk+!ME?rl{h;6?MdzIlg}3WJRjjbNzU!RP3s^o+;#rh_*uoye5ufV*I+xJa0J}p{v=;NW}fC%YPumX2Ia-Y|%I8+2r-%M-%k*Y1Y z)4_BN%b=$Z-Sg#B~!mKTF)=Y z@@Lv=U3X|pi=PXyR9kP$K$cpdu^z=Ec>5yZ21;%FS|&YK1UxTkMadEuK}mspTET30 z1&NRai+T#b+CPbfjb~(EdJ#wKzc{Y#T$tg7>jZqqQC}zH=(0Te z*Uz1eU4f(MgEjB8E1fMmWVUtmkC5cM2eZF=XIjrzRXGq>waV@5Eq&jqKGxR@R1Iu( z@8(9@{^M#b@63+U^Bb?NtWc>ceCU{910|uuHq*mzS7^<>+Unf{H_|DCthU{~)Y!W- z-7=|8ICo5pG*E)s?0Vx5&_1hnXk&WVG*()sGd}1i*7Hudfs!-b570axf7isUPI`Eu zG@7~gsowo*52?{Se?|8BVHQ1?KgK|bAu~`;G6QujV5k13_HnJ-ySlXDLDOcH8ElsK zc*V{hA7Y>+bXRx1$n>W4fzTj9GvNFT>J^!Eds z+54g66+h5+%pefb{z7@SF;61h9TcPVF7_Mt2qB(j%UZws%-~;Z{Gz4%JmVL|bM#jAqst$^ zTjmWfEEc8oQ|2qQMAazb9H2+Anby4HVnz67FM8kMUbZb;hj|9v6%!Kiusy9)F3P&2 zMXW#x)-%|%cs3+L%6vQ1>_?%+RDlxA4k9-pp$|5jU*;b&pWRV8P+QeF@q`+uuQo9H zznEpk`2$aTuonoqnkRubdD%&<>t9G<{J@BV)fa>exv-SitL`H{eJw2jSBd(FBxksx z%lN~B!^FY%`#EBvwyJUckH)O+5jSDxx+@%5@1d?TzPWTX3Kq;`{9R%fA0D`tPF$0R zRw?GI%xXB>5z^az5zGHdV^vyCFi^63(pH+7bzX1r#7nQeX+0eo@=|}{)KbnR@9R*O z+O!ZG*ey!Yz*fjDS28PpX9?jW#p6M9^)1IX(Z*(|R%y##Z>(X#9wDT3<`nU0&PuDV z*CH!Q!a}ytMm}yEF=6Lul#ofxzHpU?jSL?6Oy{Cx%jPjGg10V zq20KGw68L4c?SD)Zo5wM-Hi*(aR2|_T>~>|uoE)vtEeCOk_-1PWeqs>AnOaPuWS;` zi`(Dh9~xEWd)vQ{e^+ES3ARFRLg@R8diAG;xW0a}c+_tv?Y}Ea|7T8Z9Y+)AM_G$g zy1emn4x*^^*_L){D<-3wC(vh(yS&?o~X)+^c8F6Hfu&Dp`Z;GuqHO?Z#S0 zmbIO27xee@9$M{_*T?U%0wt#}>Ga+C9+oO{e-3>mH9}o-yxQ$P=<)488rTZC33*$u zrZLU*5e+g&Qslr~2JTp89LzY(=64<`M4B(deLwDFTfh@=M3Q;Y8q@d^FE=r^Vj}}>$GsEospM|^ez>)0m(#pfc(8$zdWU9Hw^9M# z;gvO|E=SJo2aZ|YPhaK};zlYOxOYZwLh6NO@v?~_BI!-4hBF*S9Nha5(sxlJkM$cR z0=~^-D8Z@@h$Zriy6-ws^y#{kMZ5d$M~Q6~jg_mZJoF0sSLn zW5uPiDsY(S)Oo++Ir?PNwpVo!(z0tDZ4@3W%J1r+_<^<~Hz6;I@8mD?ju&IT6j5qp z5LeZ>_p&cLAa1bM-G>;sD#pD5u6+slHp9Uv&-xl8C(Sc4LSp2sQz?O_MHHvYx;2#} zSZHx~R-*lJ7VvDi!hyJmM943H`j|UvIvdwYcT`RkalMPy67uM7aaOQoM`KzIC*#&@ z4_a(fO}aa2jo$rvL;X~MMk{0_>FW95l|03bFsF{j%k6X7%9S28aa(mdYIUX_KcWQ< zT2qs*abB$>5+VLM>lx4A(unU<#z1?~TSOvc_>{uzXif0Q2anXM3N^ahq?>zr$32u-LkB>=ET)2#&(%cTs=}O zboiY=-9&YiKvXjkNv^MWIKLG0i^yq{O)*;H+>Wt?kUaGhsZXEDg0}c*MG0nc5J_5< zQApIVb~4_5TF&vT8G9GUpOE6qyNH@i%FEiTp&ClCIst1N2=VxBC!h9goRL%_HX9{1 zo3GYN|Kmnu52u=kE+uK-J2#=_-=xd8z5S+k{Hbp21?LE{OtunEq>ZC3A=Kxf<2lw|>Rq z&-bz%KRv`4d@+r#oLb1T@wU5;d@6^?{>rw@EhhZCxGEfoi%7EG=jwJ|`c!98L;K7y z%3zGb8dyTgPK;sC9D`X-g$V+)*_dg@u_2^-Z_YdQ4>W4+isLB3-o+{#$x*$o@ZF!^ zs6Vv0Kn-k#+_D0D!%^$76FvBoPEq3I;#sur;!ED!dwVEV#cIXx>Sl@7=7XMFhs~ZW zP_lAe3f&d_D*Mu_HYV2c5>kbvTGvFpq4x%=8rTZCWexojccbt`4>7)WNey=?7(Xy3 z$}g(p&&E?WnrEJUqEs7VE(5b)gv^=ZZLA#JSmYEQqwItb7xUMIBu;K)xEX~-!#Wm@ z`##(cqP2vasn_4Q?;pXF=e*)rLk(7$HjZtwHga*WE~E3i4I_>IUe|ffihfpm2^TU&#C_&p*&%uV`4R8rqH-WEA`vqFw@OC49t(M&F-#!Vf*#Xo zQpbDRsB-g6v{tT=S5z|YoT?-K9{+?R7HX>+^}h!isU1I9JNez>$3hR$yqV!z$cv`t z!7c}B+$vdTncLAkKkX20|Kya`yDOD`yf_hMe7rG;f9Q5nkp#CoOdC(!u6>ld$+Ag@ zXp=Xw+S_?ebeSFe6l*+8?P6B;47DH@wnA<~9(-xSMple58Xc&Vja73PKX3+@EB(VR ztY6C*W8bz<%31`Y^Zhp-^v3p@G+!0l*~A;mG*&)htP!?!KEt&LMtxjy5OQF)J9D@k zZ8&(>v?3O2s~T7Ly0iTGVvH8e8tW(l4bzU4w~8&}H8pvzwf@~GW66e|9Bfx=2UX9X z>|rc$X0#De=Zd=>%jO~S+5i88qjljT?6;+pjbgukQrMAC<+vE`!g7Yj7{6x>--Qx8 zu8q(B{DY&@(z=HCtX3kZ(KQR^_w2h=TfGO?fev(m;ua~cAIe4><9Ci&`?nKqN@!K_4=I0S2tHGaE ztoXx9KzrWDmIIQN9FzC5N%x0~FJ)(Nl)ygPoNO6K_fOcarFKt|8jH2{Y*&J>7%z-w8REK@6YmGp$a{oe(60CJrzw>UR53%sReT7feqa5cC^a-t%Rrjtr>`99u z;^vZAj#eR`%8}smndOY~5esKuQaBJ7kqEhP?hY$o;Ai3KAFpU&E0v>Vi;2dXTT}Uw z)Z!dxPMoQ+4xJF@q#HBWHxy@G>T7tyhcgb&M6yq#q0@T^GPbC#dw zWGpz+S*%%kP?`I%6><~udy=0Ryd#U>fBQT5YX+a0$)_aZP=>k^c zV&w=S9V(8bu^$rold@q3O7P?dkqB{1T)=}fM+j@VO&lvZU<7rnnIWXzCWG~k>};K1 zCtBb-A$RHq`f}0l`wJv=QttT?vS;o!OZicI%(%Z}1>OLJGf1T}myq>tA6U7*m#yBt z2PyTt_#R_~Bc!!^RT_A>r=I6ztYQ`NVfC-9{YY`9wF~=;DW?kvl;HQFa`cI-ZzMUU zTB{sj0;4|0U3^0cxm>)qk^F6y9+2Ed(ZE)4rXy#D(B@062i_MmZanOz+$+Kx3Yg21 zS?3GRMo@(sEG4Y1!hyJOjwd-jJ{ixypN=#_mQWr2z*Z{9=nCcZVF_2Y+|RMf%@@qD zsq6L$AyIVg?h&l{-;v600$2K2_b%_PLgCli8KF{fyPeUUPgKu-{aTrzy2qVu}3h zuc?z1tI!hF53R8$8{S|iU2=7jKuNu82k6BLceS+}+AFIwLSoBKXTRP~u_hml5V&sZ z+VcP{mM~GPF|*TuDpG2D-KH^v{rD@ND1mh=xNgUySPIX@7a+FA~Sdk+ZTB7<o?5IY>)RMEgz$SrR{ zlyT+O^lCz1+ge~`!JRPHe#v_3+Yie!o~t6n8(NZeL@C~@a1_ndx~}*4HOM* zh1`S$fBVE;dNmVaLHU#$BY3kMYxW7blbgY}J@yq(eTo<;!3c*jQP#JUhrAhWB=yx$^gSiGWHt(LQR!M{S3tO4Fd#@aYSPJSz5qz1L)FOGK?D8YWio|bp@`&Ter zm%g%iXUPgTsI$XXn6V&abW3oZ}{n%FC9pVtHzN;@gLB z2HyIX=QxA9ih+B|3CH$s3J2mM5+Rd{jT6O(XslRhB1bG-$)L6JE!i6(tQ+RAOS39i z5DT?cjny?nS))Vag=>LH%(niV^5X`eQmt-3?fY?)_2-_xY{ub6qIy6x10`79rq+>N z_w2>SjPnsL{*@FBY=zwN_Zi+-GO9Q5=hx!f7+BYZwHOfFbwcWQd&|5_Ru%<{)i6*3 zu~x^`naneUy<$)2HWb|heo{2B6>`fvSmC}#;^zz=e!qyo2!Jyju6^a5hJk-F-%@Az zDr+Z!5?ryWwFZ;2B3OF00>b@7cclUXqXBZu`RdjucHv5AQM>LAjuPZkIjXx4Fkaox zCmP+VV#V(c^P*~&bHFB9Gq}*tx~_Cy9VM6-#nVOkwl5DguBUY4`MyLem9qFfA~zu| zB4UlL)8nk8FPBs}5LeYW-K@Xxom|MM-DjOQW|uMJiCI-bPTXn2cikLKdu)j|P=a;H zSj9}p_P>4+!z#tH+i|}(K?znB+yDRLcIL)%Lq%%(W>)FrdS=HxDDQ3JeNMdpCGSnl z2^BYTQ`wp~rxXsvMI=HT>yH*=r=DP|bNW;C9DTBD+p4i;Rq-Q#(Qm|77F+uSM|RBV zBN8E_0|tvxl|QjP@d5i03oSt;LeA-f#iZ4r*d^_uqJgbcjz^E5GqJdkD4Ey8z&s_M zC}QQ1JXdpG#NOmbhFx;hL9u8|X^?)iki47HjU*Sv0}&h3)gt&)PQlS()3>CzZqbP%_)!D@gd9 zUdYg1j2ZTfj;%&Re&@Tluo3V3iU#GbFvutAXrXG=-?g8yGK+_Z{NypkJvnNt8t3~| zVq*$U7UP~*QEr2wg{oCe@?U0A?SB^E-mhSghf(gws5yW#v46953ww(JRaPh?h`5L( zBj@C^?DFltBB;VXhFIv6>c^fB!7Sx?q&RhAkQF631}et}=i}_k{vjftWe!6PY=zvi zR%=l~Ef)e|)%s}eu>1(~JVeCX?gz>#ieui~+c)J^O_RGHCL zu?qPRN#;vJ+OTCMzVm~#`~B~3hW*?L#{+k)^48YqrbeNtylg~fGlBbMJaxczkUT%X z(u_}^;x3w9cQbHS!-`Lw=LzW&ILsP&y(ufXG1@>0R@NYr{C%IVtwhCrRmAt6-8kM? z!n!533Msc%busLbuQ*@1k%L8Q$`DahlE;ZPw{b-0XV; zTjt;XY`eXTNI!n9XK>s&b5XH3_)9wfYS^?@je1ExiQ=@USX18EzfImn{UqvP$oHe$nTozt~^yq!;F1@s?@lb8&RY;Zs_A$p$ih6kgLqw3$;% zJpHtMKT42K<>=F^s;KvKdr{|J<$tSH<*9{^e27FyugXd6@zs)I??fMg_uBE+FW!*< zQSoZ4{IosU*=@J8?9aC9So`yj6O%l@Mv8M!!g<<;I5&u7%5O42c366zvnWz5EjE!) zy5iN;j)fdHTtp%yg+z$++ZOQe1#bVVVXOGX+dPOQ>lvb!@>O*v8oNdxcE?dmmm_GA zBUo$x{D1G5*^q3%^%5yuH-+)hiAnqI?05R~P{tXN5epGD{%>#P)?yzgvtU^od|Nr4fgdD*Vr6=;$Zx<;V*h=N- z6FAsd=kSVMD_wx$+>ZG~jERJN2+?jW7!Ragz<-y48vj zT>IksTCU;tmN#5#b}{BZPt{R^5eKa$q@;U*@!;({_WoTfD@xFIjG}}rdo#j_OS{F2 zI?LaUftqdPLnJv+M|!puNscu|^%@tm?fqVS)_LWQJ66XN aa=VtFcwEy-IF;VMA0_{{@56>f$o~PK-3EdH literal 0 HcmV?d00001 diff --git a/data/pr2_gripper.urdf b/data/pr2_gripper.urdf index 47f8c8a39..1ec2cf3ff 100644 --- a/data/pr2_gripper.urdf +++ b/data/pr2_gripper.urdf @@ -17,7 +17,7 @@ - + @@ -32,7 +32,7 @@ - + @@ -41,8 +41,14 @@ + + + + + + - + @@ -54,7 +60,7 @@ - + @@ -65,12 +71,12 @@ - + - + - + @@ -85,8 +91,8 @@ - - + + @@ -94,6 +100,12 @@ + + + + + + @@ -107,8 +119,8 @@ - - + + @@ -118,9 +130,9 @@ - + - + diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 332d737ed..57cb56185 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -557,7 +557,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor() m_data = new PhysicsServerCommandProcessorInternalData(); createEmptyDynamicsWorld(); - m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0; + m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001; } @@ -596,8 +596,8 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer(); - m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); - m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08; + m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10)); + m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.04; } void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies() @@ -2957,7 +2957,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i ).m_userPtr; if (motor) { - motor->setErp(0.1); + motor->setErp(0.2); btScalar posTarget = 0.1 + (1 - btMin(0.75,gVRGripperAnalog)*1.5)*SIMD_HALF_PI*0.29; btScalar maxPosTarget = 0.55; @@ -2972,7 +2972,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) motor->setPositionTarget(posTarget, 1); motor->setVelocityTarget(0, 0.5); - btScalar maxImp = 500*m_data->m_physicsDeltaTime; + btScalar maxImp = 1*m_data->m_physicsDeltaTime; motor->setMaxAppliedImpulse(maxImp); } } diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index b0fd21d1a..485ee71ca 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -704,10 +704,10 @@ bool CMainApplication::HandleInput() ///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync //so it can (and likely will) cause crashes //add a special debug drawer that deals with this - // gDebugDrawFlags = btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints+ - // btIDebugDraw::DBG_DrawConstraintLimits+ - // btIDebugDraw::DBG_DrawConstraints - // ; + //gDebugDrawFlags = btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints+ + //btIDebugDraw::DBG_DrawConstraintLimits+ + //btIDebugDraw::DBG_DrawConstraints + //; } sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn); From a4165adddcb3fe905e33e413ba5a4619f288d266 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Mon, 19 Sep 2016 14:40:01 -0700 Subject: [PATCH 4/5] more tweak in pr2_gripper.urdf (VR), remove some collision meshes (until interaction issues are resolved) --- data/pr2_gripper.urdf | 16 +++------------- 1 file changed, 3 insertions(+), 13 deletions(-) diff --git a/data/pr2_gripper.urdf b/data/pr2_gripper.urdf index 1ec2cf3ff..01ab8233b 100644 --- a/data/pr2_gripper.urdf +++ b/data/pr2_gripper.urdf @@ -41,12 +41,7 @@ - - - - - - + @@ -76,7 +71,7 @@ - + @@ -100,12 +95,7 @@ - - - - - - + From bb23f915781e0541934c3debbdc3cc7a94be7475 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Mon, 19 Sep 2016 14:40:17 -0700 Subject: [PATCH 5/5] disable some logging method --- examples/OpenGLWindow/GLPrimitiveRenderer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/OpenGLWindow/GLPrimitiveRenderer.cpp b/examples/OpenGLWindow/GLPrimitiveRenderer.cpp index 1acfeb940..49cdbaed1 100644 --- a/examples/OpenGLWindow/GLPrimitiveRenderer.cpp +++ b/examples/OpenGLWindow/GLPrimitiveRenderer.cpp @@ -1,7 +1,7 @@ #ifndef NO_OPENGL3 #include "GLPrimitiveRenderer.h" #include "GLPrimInternalData.h" -#include "Bullet3Common/b3Logging.h" +//#include "Bullet3Common/b3Logging.h" #include "LoadShader.h" #include @@ -205,7 +205,7 @@ void GLPrimitiveRenderer::drawRect(float x0, float y0, float x1, float y1, float void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVertex& v1,const PrimVertex& v2,const PrimVertex& v3,float viewMat[16],float projMat[16], bool useRGBA) { - B3_PROFILE("GLPrimitiveRenderer::drawTexturedRect3D"); + //B3_PROFILE("GLPrimitiveRenderer::drawTexturedRect3D"); assert(glGetError()==GL_NO_ERROR);