From dcab0e2b1f4c370680c6bdb7b3e8fec0f5f2c8a4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 6 Aug 2015 00:35:03 -0700 Subject: [PATCH 1/3] use multibody by default when using FILE/Open URDF (instead of maximal coordinate rigid body+typed constraints) --- data/slope.bullet | Bin 77316 -> 79960 bytes .../ExampleBrowser/OpenGLExampleBrowser.cpp | 3 +-- .../Importers/ImportURDFDemo/UrdfParser.cpp | 4 ++-- examples/OpenGLWindow/stb_image_write.h | 5 ++++- examples/Tutorial/Tutorial.cpp | 20 +++++++----------- 5 files changed, 15 insertions(+), 17 deletions(-) diff --git a/data/slope.bullet b/data/slope.bullet index 035cc6a6b00e8479e3a8ddcbcfb1cf3266352c01..ef8d1340865ca3cd370cf0ee9697b822a7c49a79 100644 GIT binary patch delta 8335 zcmaKx3vg7`9me-2Yusc*T*y1Yuz@s&kQX$81vMdQfCvbwyip)Z#fTbEDVUiTlhIcW%p~xpQYMTvNSaQlP$d$=udXm&=vXJ95iS0j#9<-81bZk?J=t8qACF2epv}u0EmKe?7f{LTuFy>AYumKDZ+XMPf-SF$L0Wf6buDIYCs;@60<)8)dpW3jjIX=z<{6<>7=4p9@J z8^1Xf+o|TSo@}$wecAM7mYadh65~5M&HN>EjJ-aPnQMG${!&))p*5we*j7fBTr;D;<#3NVky&jQIqyj~=Q=7979EL#lrC!D1JRj&7^PABF0j!?UxVq%2wACSVrf3u9O#cWn+=ZAtbHTcZ1=i=>COzAHgA<_PtP&O z3yREM?=`=*FiXfPYILMD)n3fYnGvvfp(!$!A) zqBJQsD)C=RYM~O4l%~Z(CH@98f2`9&V?a=v5(}}nv&)=@@PnWP=&1IL<@kKv9|$50RW!6=ufN z6y^;(s0#>w5Bv6h8e1@H#L*O zZpOIrjfs5GZ9piEiCc(O*3+GdL#F~Xr7MVISfPBf^V*LGL1{`HLuzaw%p@NezV?;a zqBJI!ndb?Z3$RVM$79NuR+;l2x5;{@#gRm$W5OUZym+w|aS&6w_>NFQ7geDZrTZW* zVonz|#wSrVC`yy!qV9DKCVP6wcqY^$0Z-U|uK_`6N-Wgq3z*ZZ0;bF7F?k(TroU*W zsVJ^V7)Q0JMCoecEM|8`t22)3Kv0?zcach)86QUzK~b6%gZ0l!9!HZvQkoWX_0MWB z*Y9%1(bqvxni6xdx7X%4Ey)QIr77_iLu~MFC&l?Br717=MZ84ni}_*qYyaJk5G$;3 z3~Q6ojMBK6sl*>JX>9>>^n%CCY^yLsL$j?G(_ENuk5sWiZVGBpx{g?jx&6-tAstYh z%uyN?cd`E$7KPo9p89m^a6nB3LTOBFba2TfW?FOH9BCRbN~7YXtcrjcoF6ca6&^FW z-9F{GVeWQRpmY`S6%+aFy>=q6^yZ-(k&%@%a7u$>DT1e0I4|>^ASg|VrI_O%t_nrs zH8xf?0--b}o*JrUUQIQ%bvGx?D^8p#UB!!i5sxt}PpyuQK9vrKO-Cb2!{RW)Cm)O- z^qN6YniNlweE*@aBd%INP?{1$kvjTtIB7(E12m;8h_@K>M;{4V<>vnM{NXuMD;P?1 zVl2k`p~pgTW|{$t(xjNFdtLI7nhBE9v^a{f{?6mhkeUU8(v&!gv3~H$pkwH213_s@ z48>@B^Qo}=^Xr_bgxqf&2_LfaEjAOM3yjjJxQZvR@7d(8x*Ig5D~PKY zbg%T-H&IDxrQ^A79zaUNVlBdZx5bZt^FdLX6l*cy_ineJPH)Vqc0iN{#Z^r7ojXGA zrre%m!&my7Kq!rgt=QA&b~@wj9uSnK#8#Y&@9PPeb4xttbWh@W)dEzYbQLf5MO?;Y z@7|3k*aWllg=j@-T0F*J-moV=s1|{uG%3C!`NVgfLA4kJr75u#BYj-M8``PHf7)3<#w$aTKu!z8|(qOzD^j3Evbf z2T$o5VlLKL`I^%u_ky4_CEg;n{PpNFMl5{p143y`%*8xk{v&77tpGu3%8PvwGcnB< z>`y#NUx`+fro~L0q~{$Am|Z{J9QU}l3M8dzF%%Ph&Ku5&dItzfQ(`D4`s|+vlN#!N z(3GwqhGNRK9B@v~9{@pVN_<4B@$L97qlJ?@axIBP*rniAu%t0nJ7@2l8Lf=7T*8WXoL zv`c=+7l!hp;875iro=4_>7w_7akHeyKv9|$w=iZ$9ZUYMpbIpmD~N4aA?t+mO0EMz zX-aIvp!WX3X}0wsC{2lL7|t1ga;DDXASg|Fu`l8pMqJ9D6T9XKw4yXEt|2|t7rl@% z*E|V?(wMl0*tJv6@$1_lC{2lLNDY3VT=NtVN@L<0rtx6E(={7FP?{3gFpWPSh@V?; z1Vw35T*D;(>~G0mYi|Ng=?dZ+rtv>MQqNvb1EDk~wqYD!IPEmRW)PI7#5T;Rb7#Wt zu7TU$9nPpNKq!rQu`l8qVrS2W-HX3U@EYJ5G@~>w#$gbj{Um73h|b*O5DQ^_n%Kbq3!`q=?Y>brq-#yI|r++ASg|Vkw~5VH0s*eq-PrtN@L<8 z=JCl3$zAjuXi8TQ7crWTU38|%b`X@N#6;}wkxOB#$NXzDCR@?}RCyi@r8%(?v-!y7 zpf%CK?Eph*&Wn8!A2FT7p9QU_6F#WDfOeGT#YjxpgID4w(K|s=niLz6{KXf}5PA^= zr75uyv-!ZklTV_*1Detm#74~NHwW!Gh<+~H1(4FPxQQ9~%8WPzpr+=_PRpTtj+!&A6~%v@aMNx z=jw1x$BmSOz3tf3;d&YW2Jn}I_XBSHd7uDVw`02l|N7CMe?DxtyUtra>#*xp%yE{w zu3EoN_1MSw#K}rqU7glN*Q?fx)_Ir5I%9QOTPssuPC4#A`U2X^|{s`T2X2p$owBiT~G1= delta 7738 zcmb7}3s6*57{||r-DO#J5d~I6Ob|2_HANyaUu;|-iBA+Y@qwa|PeLP8OGG41w1G29 zk11bM&NPlwWav25s5MSA=~Px&lTCw8W;0IXSVK+YG=1lO2VK7IJ%@JZu#ak1Na1dkS^*GVO&nG&>>&}lV=7J2w zjg#Y#Ovwo+=|>PlPKDO_5d!dB9M8)l?Pt~R1hh&3`n!xxoG}&Je<~!nGS-Sx;X@J{ zh=mp(28dY}$K9V8F-g{Kym-I!2Jcv}XNL+F+=Oa9;EgP3orNR$IoF?->Ucw7AU_lc zT5WG|Fh3aReeZR3bPvAElbR?_5l2qp;_ZXS^4O>||L-(C$!Xl&s4)4U=w>`kTx@2e zKLG}i%zY67KEpqZukn>w1L#Fa58pn{OESN~JXc%UQFYoywen2NINH8ByYsu%8 z@hxhsH^E{63-KL^vRXxQCa*{q3mVV&1z)#jB#9t?t5RccS!rhQKJwN_i1Z;yz(S7n zk;u}dEIuQtXO~sGm5j5POwd?BP2SR1&SG`S5PrfGvzWjaw_&xu1ab4jwI;vNPa+D5 zq!7}>(~3;q)?bni66r^%D1L62$#0|~rIN@4g!J#$$wvvi$J;61^-$ zRu5k&a11ct%cUAwj3pRsDBz;kmrM0JsR^=lmuf!CN$;%CR1=9ae30N6V55=cvYOqd zGJzm|%R_^!6w=5i2y7~=yCIDb!N4`ZJagA6r13aGL?qQqqkvEtM~f@f(l7}n;waWq zxy3|6VHB;ZlC_-HcM`!citgB?k;PX^ICkTR3wE0C@$!;^5 zN)U{q=c^^6+E5A!icz$!Mj?%91QAhGFOBJh!Wi07rS z&?I$hx0%c%2u9GF*JL%thBBX!7(w|PvY5k$vVc$`g6gI5B%v^Vwj9(f*g}Gd_^FXb z2|+M=wjI?}T0}4rJ>iGf+X`tcCJ4sQj$;zBbBrYf5%Cj#ZM`cIm18U=C`Qou;|gh% z5(FdYfD?LYEF&aF&{6NHrLmk)7(qkLSWmU7lo1jm=}!dr9AqOwF^(30D^WX%JWCL~ zLb~#bL=+OKA|yuAs_!M~Ad%+?g;z-Hen3j)Ae#t@vGl-?vYyjgKTj}>rAL3#NMtj? zFqSsAYb3ITU>Hk3xvr4NR)SzGz5KJRWG9gq2!gTn+Ak7ON#sR>Vl3_WRnDTFM79wG zuaJ6fNkk!$YC>W(4gMxc2Z?Ma6kZ`6bQ>v^L~00$@ih02tmh<=9R$NGq!a(pV6_Cp zh+5pC!FCc1BkIb(BxdI#bp*kPTKTs^A}tlrpae8cXti$;mDF`_o& rr0XJEW0bIZI7b=O$$-BlaE7z2#BY?d<3?-58l#n!oq9jZIOhKsv0^2r diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 8ff0a7bf3..37a602c93 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -266,9 +266,8 @@ void openFileDemo(const char* filename) s_parameterInterface->removeAllParameters(); - CommonExampleOptions options(s_guiHelper,0); + CommonExampleOptions options(s_guiHelper,1); options.m_fileName = filename; - char fullPath[1024]; sprintf(fullPath, "%s", filename); b3FileUtils::toLower(fullPath); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 2e42525d5..cb3ed0816 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -794,8 +794,8 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF vis.m_localMaterial = **mat; } else { - logger->reportError("Cannot find material with name:"); - logger->reportError(vis.m_materialName.c_str()); + //logger->reportError("Cannot find material with name:"); + //logger->reportError(vis.m_materialName.c_str()); } } } diff --git a/examples/OpenGLWindow/stb_image_write.h b/examples/OpenGLWindow/stb_image_write.h index 41b703388..1a51a939f 100644 --- a/examples/OpenGLWindow/stb_image_write.h +++ b/examples/OpenGLWindow/stb_image_write.h @@ -493,7 +493,10 @@ int stbi_write_png(char const *filename, int x, int y, int comp, const void *dat unsigned char *png = stbi_write_png_to_mem((unsigned char *) data, stride_bytes, x, y, comp, &len); if (!png) return 0; f = fopen(filename, "wb"); - if (!f) { free(png); return 0; } + if (!f) { + free(png); + return 0; + } fwrite(png, 1, len, f); fclose(f); free(png); diff --git a/examples/Tutorial/Tutorial.cpp b/examples/Tutorial/Tutorial.cpp index b924f682a..946139fa6 100644 --- a/examples/Tutorial/Tutorial.cpp +++ b/examples/Tutorial/Tutorial.cpp @@ -61,6 +61,9 @@ struct LWRightBody //Exponential map //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia + //btQuaternion q_w = [ sin(|w|*dt/2) * w/|w| , cos(|w|*dt/2)] + //btQuaternion q_new = q_w * q_old; + btVector3 axis; btScalar fAngle = m_angularVelocity.length(); //limit the angular motion @@ -88,6 +91,7 @@ struct LWRightBody predictedOrn.normalize(); m_worldPose.m_worldOrientation = predictedOrn; } + } @@ -99,13 +103,9 @@ struct LWRightBody -///quick demo showing the right-handed coordinate system and positive rotations around each axis class Tutorial : public CommonExampleInterface { CommonGraphicsApp* m_app; - float m_x; - float m_y; - float m_z; int m_tutorialIndex; LWRightBody* m_body; @@ -114,14 +114,14 @@ public: Tutorial(CommonGraphicsApp* app, int tutorialIndex) :m_app(app), - m_x(0), - m_y(0), - m_z(0), m_tutorialIndex(tutorialIndex) { m_app->setUpAxis(2); - + switch (m_tutorialIndex) + { + + } { int boxId = m_app->registerCubeShape(100,100,1); @@ -186,10 +186,6 @@ public: } virtual void stepSimulation(float deltaTime) { - m_x+=0.01f; - m_y+=0.01f; - m_z+=0.01f; - //m_body->m_worldPose.m_worldPosition+= btVector3(0,0.01,0); //m_body->m_linearVelocity=btVector3(0,0.1,0); m_body->m_angularVelocity =btVector3(0,0.1,0); m_body->integrateTransform(deltaTime); From 46fae61c69736fbed8e2c1a006dcf356721ff0b7 Mon Sep 17 00:00:00 2001 From: = <=> Date: Thu, 6 Aug 2015 11:59:31 -0700 Subject: [PATCH 2/3] fixes in shared memory: only allow server to create and initialize shared memory, client will report failure intercept signals to cleanup shared memory in standalone app, thanks to Roland Philippsen. --- examples/ExampleBrowser/main.cpp | 3 + .../RollingFrictionDemo.cpp | 4 + examples/SharedMemory/PhysicsClient.cpp | 37 +++--- examples/SharedMemory/PhysicsClient.h | 4 +- examples/SharedMemory/PhysicsClientC_API.cpp | 5 +- examples/SharedMemory/PhysicsClientC_API.h | 3 +- .../SharedMemory/PhysicsClientExample.cpp | 110 +++++++++++++----- examples/SharedMemory/PhysicsServer.cpp | 47 ++++---- examples/SharedMemory/PhysicsServer.h | 2 +- .../SharedMemory/PhysicsServerExample.cpp | 18 ++- examples/SharedMemory/PosixSharedMemory.cpp | 4 +- examples/SharedMemory/RobotControlExample.cpp | 15 ++- examples/SharedMemory/SharedMemoryCommon.h | 1 + examples/SharedMemory/main.cpp | 54 +++++++-- 14 files changed, 215 insertions(+), 92 deletions(-) diff --git a/examples/ExampleBrowser/main.cpp b/examples/ExampleBrowser/main.cpp index f292bb751..c3442de73 100644 --- a/examples/ExampleBrowser/main.cpp +++ b/examples/ExampleBrowser/main.cpp @@ -13,6 +13,9 @@ #include "ExampleEntries.h" #include "Bullet3Common/b3Logging.h" + + + int main(int argc, char* argv[]) { b3CommandLineArgs args(argc,argv); diff --git a/examples/RollingFrictionDemo/RollingFrictionDemo.cpp b/examples/RollingFrictionDemo/RollingFrictionDemo.cpp index 55f079a78..1d3213124 100644 --- a/examples/RollingFrictionDemo/RollingFrictionDemo.cpp +++ b/examples/RollingFrictionDemo/RollingFrictionDemo.cpp @@ -229,6 +229,9 @@ void RollingFrictionDemo::initPhysics() } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + + if (0) + { btSerializer* s = new btDefaultSerializer; m_dynamicsWorld->serialize(s); b3ResourcePath p; @@ -239,6 +242,7 @@ void RollingFrictionDemo::initPhysics() fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f); fclose(f); } + } } diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index e068b409a..bdfeba9e5 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -77,38 +77,43 @@ PhysicsClientSharedMemory::PhysicsClientSharedMemory() PhysicsClientSharedMemory::~PhysicsClientSharedMemory() { - m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); + if (m_data->m_isConnected) + { + disconnectSharedMemory(); + } delete m_data->m_sharedMemory; delete m_data; } +void PhysicsClientSharedMemory::disconnectSharedMemory () +{ + if (m_data->m_isConnected) + { + m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); + m_data->m_isConnected = false; + } +} + bool PhysicsClientSharedMemory::isConnected() const { return m_data->m_isConnected ; } -bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization) +bool PhysicsClientSharedMemory::connect() { - bool allowCreation = true; + ///server always has to create and initialize shared memory + bool allowCreation = false; m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE, allowCreation); if (m_data->m_testBlock1) { if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) { - if (allowSharedMemoryInitialization) - { - InitSharedMemoryBlock(m_data->m_testBlock1); - b3Printf("Created and initialized shared memory block"); - m_data->m_isConnected = true; - } else - { - b3Error("Error: please start server before client\n"); - m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); - m_data->m_testBlock1 = 0; - return false; - } - } else + b3Error("Error: please start server before client\n"); + m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); + m_data->m_testBlock1 = 0; + return false; + } else { b3Printf("Connected to existing shared memory, status OK.\n"); m_data->m_isConnected = true; diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 29e00f19e..1f46363b6 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -15,7 +15,9 @@ public: virtual ~PhysicsClientSharedMemory(); //return true if connection succesfull, can also check 'isConnected' - virtual bool connect(bool allowSharedMemoryInitialization = true); + virtual bool connect(); + + virtual void disconnectSharedMemory (); virtual bool isConnected() const; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 14f58cd31..8d627322a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -191,10 +191,11 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* c } -b3PhysicsClientHandle b3ConnectSharedMemory( int allowSharedMemoryInitialization) +b3PhysicsClientHandle b3ConnectSharedMemory() { PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory(); - cl->connect(allowSharedMemoryInitialization); + ///client should never create shared memory, only the server does + cl->connect(); return (b3PhysicsClientHandle ) cl; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 35a1a5801..d726d2740 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -12,7 +12,8 @@ B3_DECLARE_HANDLE(b3PhysicsRobotHandle); extern "C" { #endif -b3PhysicsClientHandle b3ConnectSharedMemory(int allowSharedMemoryInitialization); +///make sure to start the server first! +b3PhysicsClientHandle b3ConnectSharedMemory(); void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 3f7e6a833..7eb507c05 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -9,6 +9,15 @@ #include "PhysicsClient.h" #include "SharedMemoryCommands.h" +struct MyMotorInfo2 +{ + btScalar m_velTarget; + btScalar m_maxForce; + int m_uIndex; +}; + +#define MAX_NUM_MOTORS 128 + class PhysicsClientExample : public SharedMemoryCommon { protected: @@ -22,6 +31,11 @@ protected: public: + //@todo, add accessor methods + MyMotorInfo2 m_motorTargetVelocities[MAX_NUM_MOTORS]; + int m_numMotors; + + PhysicsClientExample(GUIHelperInterface* helper); virtual ~PhysicsClientExample(); @@ -42,11 +56,20 @@ public: { return m_wantsTermination; } + + virtual bool isConnected() + { + return m_physicsClient.isConnected(); + } + void enqueueCommand(const SharedMemoryCommand& orgCommand); }; + + + void MyCallback(int buttonId, bool buttonState, void* userPtr) { PhysicsClientExample* cl = (PhysicsClientExample*) userPtr; @@ -58,8 +81,8 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr) case CMD_LOAD_URDF: { command.m_type =CMD_LOAD_URDF; - sprintf(command.m_urdfArguments.m_urdfFileName,"hinge.urdf");//r2d2.urdf"); - command.m_urdfArguments.m_initialPosition[0] = 0.0; + sprintf(command.m_urdfArguments.m_urdfFileName,"kuka_lwr/kuka.urdf"); + command.m_urdfArguments.m_initialPosition[0] = 0.0; command.m_urdfArguments.m_initialPosition[1] = 0.0; command.m_urdfArguments.m_initialPosition[2] = 0.0; command.m_urdfArguments.m_initialOrientation[0] = 0.0; @@ -92,36 +115,29 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr) case CMD_SEND_DESIRED_STATE: { - command.m_type =CMD_SEND_DESIRED_STATE; - int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE; + command.m_type =CMD_SEND_DESIRED_STATE; + int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE; - command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode; - //todo: expose a drop box in the GUI for this - switch (controlMode) - { - case CONTROL_MODE_VELOCITY: - { - for (int i=0;im_numMotors;i++) + { + btScalar targetVel = cl->m_motorTargetVelocities[i].m_velTarget; + + int uIndex = cl->m_motorTargetVelocities[i].m_uIndex; + if (targetVel>1) + { + printf("testme"); + } + command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel; + + } + cl->enqueueCommand(command); break; } @@ -200,7 +216,7 @@ void PhysicsClientExample::initPhysics() if (!m_physicsClient.connect()) { - b3Warning("Cannot eonnect to physics client"); + b3Warning("Cannot connect to physics client"); } } @@ -222,6 +238,36 @@ void PhysicsClientExample::stepSimulation(float deltaTime) b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); } + + if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) + { + for (int i=0;im_velTarget = 0.f; + motorInfo->m_uIndex = info.m_uIndex; + + SliderParams slider(motorName,&motorInfo->m_velTarget); + slider.m_minVal=-4; + slider.m_maxVal=4; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + m_numMotors++; + } + } + + } + + } } if (m_physicsClient.canSubmitCommand()) diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 726d85a30..faadd1f19 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -92,7 +92,7 @@ PhysicsServerSharedMemory::~PhysicsServerSharedMemory() -bool PhysicsServerSharedMemory::connectSharedMemory(bool allowSharedMemoryInitialization, class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper) +bool PhysicsServerSharedMemory::connectSharedMemory( class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper) { m_data->m_dynamicsWorld = dynamicsWorld; m_data->m_guiHelper = guiHelper; @@ -100,34 +100,37 @@ bool PhysicsServerSharedMemory::connectSharedMemory(bool allowSharedMemoryInitia bool allowCreation = true; bool allowConnectToExistingSharedMemory = false; + if (m_data->m_isConnected) + { + b3Warning("connectSharedMemory, while already connected"); + return m_data->m_isConnected; + } + + m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE,allowCreation); if (m_data->m_testBlock1) { - if (!allowConnectToExistingSharedMemory || (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)) + int magicId =m_data->m_testBlock1->m_magicId; + b3Printf("magicId = %d\n", magicId); + + if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) { - if (allowSharedMemoryInitialization) - { - InitSharedMemoryBlock(m_data->m_testBlock1); - b3Printf("Created and initialized shared memory block"); - m_data->m_isConnected = true; - } else - { - b3Error("Error: please start server before client\n"); - m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); - m_data->m_testBlock1 = 0; - return false; - } + InitSharedMemoryBlock(m_data->m_testBlock1); + b3Printf("Created and initialized shared memory block\n"); + m_data->m_isConnected = true; } else { - b3Printf("Connected to existing shared memory, status OK.\n"); - m_data->m_isConnected = true; + b3Error("Server cannot connect to existing shared memory, disconnecting shared memory.\n"); + m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); + m_data->m_testBlock1 = 0; + m_data->m_isConnected = false; } } else { b3Error("Cannot connect to shared memory"); - return false; + m_data->m_isConnected = false; } - return true; + return m_data->m_isConnected; } @@ -693,7 +696,7 @@ void PhysicsServerSharedMemory::processClientCommands() case CMD_CREATE_BOX_COLLISION_SHAPE: { btVector3 halfExtents(1,1,1); - if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_HALF_EXTENTS) + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS) { halfExtents = btVector3( clientCmd.m_createBoxShapeArguments.m_halfExtentsX, @@ -702,7 +705,7 @@ void PhysicsServerSharedMemory::processClientCommands() } btTransform startTrans; startTrans.setIdentity(); - if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_INITIAL_POSITION) + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_POSITION) { startTrans.setOrigin(btVector3( clientCmd.m_createBoxShapeArguments.m_initialPosition[0], @@ -710,8 +713,10 @@ void PhysicsServerSharedMemory::processClientCommands() clientCmd.m_createBoxShapeArguments.m_initialPosition[2])); } - if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_INITIAL_ORIENTATION) + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION) { + + b3Printf("test\n"); startTrans.setRotation(btQuaternion( clientCmd.m_createBoxShapeArguments.m_initialOrientation[0], clientCmd.m_createBoxShapeArguments.m_initialOrientation[1], diff --git a/examples/SharedMemory/PhysicsServer.h b/examples/SharedMemory/PhysicsServer.h index 2b326cbb6..c069bb2fd 100644 --- a/examples/SharedMemory/PhysicsServer.h +++ b/examples/SharedMemory/PhysicsServer.h @@ -20,7 +20,7 @@ public: virtual ~PhysicsServerSharedMemory(); //todo: implement option to allocated shared memory from client - virtual bool connectSharedMemory(bool allowSharedMemoryInitialization, class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper); + virtual bool connectSharedMemory(class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper); virtual void disconnectSharedMemory (bool deInitializeSharedMemory); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index e7f6c6d9c..3d0ad9ae7 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -24,9 +24,9 @@ class PhysicsServerExample : public SharedMemoryCommon { PhysicsServerSharedMemory m_physicsServer; - - bool m_wantsShutdown; + bool m_wantsShutdown; + bool m_isConnected; public: @@ -50,11 +50,14 @@ public: } virtual bool wantsTermination(); + virtual bool isConnected(); + }; PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper) :SharedMemoryCommon(helper), -m_wantsShutdown(false) +m_wantsShutdown(false), +m_isConnected(false) { b3Printf("Started PhysicsServer\n"); } @@ -65,6 +68,12 @@ PhysicsServerExample::~PhysicsServerExample() { bool deInitializeSharedMemory = true; m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory); + m_isConnected = false; +} + +bool PhysicsServerExample::isConnected() +{ + return m_isConnected; } void PhysicsServerExample::initPhysics() @@ -80,8 +89,7 @@ void PhysicsServerExample::initPhysics() grav[upAxis] = 0;//-9.8; this->m_dynamicsWorld->setGravity(grav); - bool allowSharedMemoryInitialization = true; - m_physicsServer.connectSharedMemory(allowSharedMemoryInitialization, m_dynamicsWorld,m_guiHelper); + m_isConnected = m_physicsServer.connectSharedMemory( m_dynamicsWorld,m_guiHelper); } diff --git a/examples/SharedMemory/PosixSharedMemory.cpp b/examples/SharedMemory/PosixSharedMemory.cpp index 3bf0165dc..00b3029b7 100644 --- a/examples/SharedMemory/PosixSharedMemory.cpp +++ b/examples/SharedMemory/PosixSharedMemory.cpp @@ -40,10 +40,10 @@ struct btPointerCaster }; }; -void* PosixSharedMemory::allocateSharedMemory(int key, int size, bool /*allowCreation*/) +void* PosixSharedMemory::allocateSharedMemory(int key, int size, bool allowCreation) { #ifdef TEST_SHARED_MEMORY - int flags = IPC_CREAT | 0666; + int flags = (allowCreation ? IPC_CREAT : 0) | 0666; int id = shmget((key_t) key, (size_t) size,flags); if (id < 0) { diff --git a/examples/SharedMemory/RobotControlExample.cpp b/examples/SharedMemory/RobotControlExample.cpp index d9ec47176..6249fc211 100644 --- a/examples/SharedMemory/RobotControlExample.cpp +++ b/examples/SharedMemory/RobotControlExample.cpp @@ -71,8 +71,13 @@ public: } virtual bool wantsTermination(); + virtual bool isConnected(); }; +bool RobotControlExample::isConnected() +{ + return m_physicsClient.isConnected(); +} void MyCallback2(int buttonId, bool buttonState, void* userPtr) { @@ -131,6 +136,12 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr) case CMD_CREATE_BOX_COLLISION_SHAPE: { command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE; + command.m_updateFlags = BOX_SHAPE_HAS_INITIAL_POSITION; + command.m_createBoxShapeArguments.m_initialPosition[0] = 0; + command.m_createBoxShapeArguments.m_initialPosition[1] = 0; + command.m_createBoxShapeArguments.m_initialPosition[2] = -3; + + cl->enqueueCommand(command); break; } @@ -232,6 +243,7 @@ m_numMotors(0) RobotControlExample::~RobotControlExample() { bool deInitializeSharedMemory = true; + m_physicsClient.disconnectSharedMemory(); m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory); } @@ -248,8 +260,7 @@ void RobotControlExample::initPhysics() grav[upAxis] = 0;//-9.8; this->m_dynamicsWorld->setGravity(grav); - bool allowSharedMemoryInitialization = true; - m_physicsServer.connectSharedMemory(allowSharedMemoryInitialization, m_dynamicsWorld,m_guiHelper); + m_physicsServer.connectSharedMemory( m_dynamicsWorld,m_guiHelper); if (m_guiHelper && m_guiHelper->getParameterInterface()) { diff --git a/examples/SharedMemory/SharedMemoryCommon.h b/examples/SharedMemory/SharedMemoryCommon.h index e479acd33..04192d974 100644 --- a/examples/SharedMemory/SharedMemoryCommon.h +++ b/examples/SharedMemory/SharedMemoryCommon.h @@ -12,6 +12,7 @@ public: } virtual bool wantsTermination()=0; + virtual bool isConnected()=0; }; #endif// diff --git a/examples/SharedMemory/main.cpp b/examples/SharedMemory/main.cpp index c78f4245f..88b6c1ee8 100644 --- a/examples/SharedMemory/main.cpp +++ b/examples/SharedMemory/main.cpp @@ -22,27 +22,63 @@ subject to the following restrictions: #include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "SharedMemoryCommon.h" +#include +#include +#include + + +static SharedMemoryCommon* example = NULL; +static bool interrupted = false; + +#ifndef _WIN32 +#include +static void cleanup(int signo) +{ + if (interrupted) { // this is the second time, we're hanging somewhere + // if (example) { + // example->abort(); + // } + b3Printf("Aborting and deleting SharedMemoryCommon object"); + sleep(1); + delete example; + errx(EXIT_FAILURE, "aborted example on signal %d", signo); + } + interrupted = true; + warnx("caught signal %d", signo); +} +#endif//_WIN32 + int main(int argc, char* argv[]) { - +#ifndef _WIN32 + struct sigaction action; + memset(&action, 0x0, sizeof(action)); + action.sa_handler = cleanup; + static const int signos[] = { SIGHUP, SIGINT, SIGQUIT, SIGABRT, SIGSEGV, SIGPIPE, SIGTERM }; + for (int ii(0); ii < sizeof(signos) / sizeof(*signos); ++ii) { + if (0 != sigaction(signos[ii], &action, NULL)) { + err(EXIT_FAILURE, "signal %d", signos[ii]); + } + } +#endif + b3CommandLineArgs args(argc, argv); DummyGUIHelper noGfx; CommonExampleOptions options(&noGfx); - SharedMemoryCommon* example = 0; if (args.CheckCmdLineFlag("client")) - { - example = (SharedMemoryCommon*)PhysicsClientCreateFunc(options); - }else - { - example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options); - } + { + example = (SharedMemoryCommon*)PhysicsClientCreateFunc(options); + }else + { + example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options); + } example->initPhysics(); - while (!example->wantsTermination()) + while (example->isConnected() && !(example->wantsTermination() || interrupted)) { example->stepSimulation(1.f/60.f); } From f750275cf9f5673339d4ac53174a6d5d8e92156a Mon Sep 17 00:00:00 2001 From: = <=> Date: Thu, 6 Aug 2015 12:07:08 -0700 Subject: [PATCH 3/3] fix _WIN32 build (there is no ) --- examples/SharedMemory/main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/examples/SharedMemory/main.cpp b/examples/SharedMemory/main.cpp index 88b6c1ee8..cef825f09 100644 --- a/examples/SharedMemory/main.cpp +++ b/examples/SharedMemory/main.cpp @@ -22,8 +22,7 @@ subject to the following restrictions: #include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "SharedMemoryCommon.h" -#include -#include + #include @@ -31,6 +30,8 @@ static SharedMemoryCommon* example = NULL; static bool interrupted = false; #ifndef _WIN32 +#include +#include #include static void cleanup(int signo) {