From 82576d0bee003b385bb2a92f2bc2c815cc27c00c Mon Sep 17 00:00:00 2001 From: = <=> Date: Thu, 28 May 2015 16:05:24 -0700 Subject: [PATCH] added preliminary shared memory physics client/server fix a bug in CommonMultiBodyBase: don't use data if it hasn't been allocated --- build3/premake4.lua | 2 +- .../CommonInterfaces/CommonMultiBodyBase.h | 3 + examples/ExampleBrowser/ExampleEntries.cpp | 6 + examples/ExampleBrowser/premake4.lua | 3 + examples/SharedMemory/PhysicsClient.cpp | 80 +++++++++++ examples/SharedMemory/PhysicsClient.h | 6 + examples/SharedMemory/PhysicsServer.cpp | 127 ++++++++++++++++++ examples/SharedMemory/PhysicsServer.h | 9 ++ examples/SharedMemory/PosixSharedMemory.cpp | 84 ++++++++++++ examples/SharedMemory/PosixSharedMemory.h | 21 +++ examples/SharedMemory/SharedMemoryInterface.h | 73 ++++++++++ examples/SharedMemory/main.cpp | 54 ++++++++ examples/SharedMemory/premake4.lua | 22 +++ 13 files changed, 489 insertions(+), 1 deletion(-) create mode 100644 examples/SharedMemory/PhysicsClient.cpp create mode 100644 examples/SharedMemory/PhysicsClient.h create mode 100644 examples/SharedMemory/PhysicsServer.cpp create mode 100644 examples/SharedMemory/PhysicsServer.h create mode 100644 examples/SharedMemory/PosixSharedMemory.cpp create mode 100644 examples/SharedMemory/PosixSharedMemory.h create mode 100644 examples/SharedMemory/SharedMemoryInterface.h create mode 100644 examples/SharedMemory/main.cpp create mode 100644 examples/SharedMemory/premake4.lua diff --git a/build3/premake4.lua b/build3/premake4.lua index 6665f25be..df5f414bf 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -150,7 +150,7 @@ if not _OPTIONS["ios"] then include "../examples/ExampleBrowser" include "../examples/OpenGLWindow" - + include "../examples/SharedMemory" include "../examples/ThirdPartyLibs/Gwen" include "../examples/HelloWorld" diff --git a/examples/CommonInterfaces/CommonMultiBodyBase.h b/examples/CommonInterfaces/CommonMultiBodyBase.h index 46270f430..a2ed9d562 100644 --- a/examples/CommonInterfaces/CommonMultiBodyBase.h +++ b/examples/CommonInterfaces/CommonMultiBodyBase.h @@ -138,9 +138,12 @@ struct CommonMultiBodyBase : public CommonExampleInterface virtual void renderScene() { + if (m_dynamicsWorld) + { m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld); m_guiHelper->render(m_dynamicsWorld); + } } diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 037d245d7..8c6c51dac 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -30,6 +30,9 @@ #include "../FractureDemo/FractureDemo.h" #include "../DynamicControlDemo/MotorDemo.h" #include "../RollingFrictionDemo/RollingFrictionDemo.h" +#include "../SharedMemory/PhysicsServer.h" +#include "../SharedMemory/PhysicsClient.h" + #ifdef ENABLE_LUA #include "../LuaDemo/LuaPhysicsSetup.h" #endif @@ -177,6 +180,9 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(0,"Experiments"), + ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory", + PhysicsServerCreateFunc), + ExampleEntry(1, "Physics Client", "Create a physics client that can communicate with a physics server over shared memory", PhysicsClientCreateFunc), #ifdef ENABLE_LUA ExampleEntry(1,"Lua Script", "Create the dynamics world, collision shapes and rigid bodies using Lua scripting", LuaDemoCreateFunc), diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 3b21f37de..0896fc727 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -50,6 +50,9 @@ files { "**.cpp", "**.h", + "../SharedMemory/PhysicsServer.cpp", + "../SharedMemory/PhysicsClient.cpp", + "../SharedMemory/PosixSharedMemory.cpp", "../BasicDemo/BasicExample.*", "../Benchmarks/*", "../CommonInterfaces/*", diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp new file mode 100644 index 000000000..05f91df1b --- /dev/null +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -0,0 +1,80 @@ + +#include "PhysicsClient.h" + +#include "../CommonInterfaces/CommonMultiBodyBase.h" +#include "PosixSharedMemory.h" + +class PhysicsClient : public CommonMultiBodyBase +{ + SharedMemoryInterface* m_sharedMemory; + SharedMemoryExampleData* m_testBlock1; + +public: + + PhysicsClient(GUIHelperInterface* helper); + virtual ~PhysicsClient(); + + virtual void initPhysics(); + + virtual void stepSimulation(float deltaTime); + + virtual void resetCamera() + { + float dist = 1; + float pitch = 50; + float yaw = 35; + float targetPos[3]={-3,2.8,-2.5}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } + +}; + +PhysicsClient::PhysicsClient(GUIHelperInterface* helper) +:CommonMultiBodyBase(helper), +m_testBlock1(0) +{ + b3Printf("Started PhysicsClient"); + m_sharedMemory = new PosixSharedMemory(); +} + +PhysicsClient::~PhysicsClient() +{ + b3Printf("~PhysicsClient"); + m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); + delete m_sharedMemory; +} + +void PhysicsClient::initPhysics() +{ + m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); + if (m_testBlock1) + { + btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER); + } + +} + +void PhysicsClient::stepSimulation(float deltaTime) +{ + + static int once = true; + + if (m_testBlock1) + { + if (once) + { + once=false; + + b3Printf("Client created CMD_LOAD_URDF"); + m_testBlock1->m_clientCommands[0] =CMD_LOAD_URDF; + m_testBlock1->m_numClientCommands++; + } + } + +} + + +class CommonExampleInterface* PhysicsClientCreateFunc(struct CommonExampleOptions& options) +{ + return new PhysicsClient(options.m_guiHelper); +} diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h new file mode 100644 index 000000000..b38a10b5a --- /dev/null +++ b/examples/SharedMemory/PhysicsClient.h @@ -0,0 +1,6 @@ +#ifndef PHYSICS_CLIENT_H +#define PHYSICS_CLIENT_H + +class CommonExampleInterface* PhysicsClientCreateFunc(struct CommonExampleOptions& options); + +#endif diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp new file mode 100644 index 000000000..06a6224f9 --- /dev/null +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -0,0 +1,127 @@ + +#include "PhysicsServer.h" + +#include "../CommonInterfaces/CommonMultiBodyBase.h" +#include "PosixSharedMemory.h" + + + +class PhysicsServer : public CommonMultiBodyBase +{ + SharedMemoryInterface* m_sharedMemory; + SharedMemoryExampleData* m_testBlock1; + +public: + + PhysicsServer(GUIHelperInterface* helper); + + virtual ~PhysicsServer(); + + virtual void initPhysics(); + + virtual void stepSimulation(float deltaTime); + + virtual void resetCamera() + { + float dist = 1; + float pitch = 50; + float yaw = 35; + float targetPos[3]={-3,2.8,-2.5}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } + +}; + +PhysicsServer::PhysicsServer(GUIHelperInterface* helper) +:CommonMultiBodyBase(helper), +m_testBlock1(0) +{ + b3Printf("Started PhysicsServer\n"); + m_sharedMemory = new PosixSharedMemory(); +} + +PhysicsServer::~PhysicsServer() +{ + if (m_testBlock1) + { + m_testBlock1->m_magicId = 0; + b3Printf("magic id = %d\n",m_testBlock1->m_magicId); + } + m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); + delete m_sharedMemory; +} + +void PhysicsServer::initPhysics() +{ + m_testBlock1 = (SharedMemoryExampleData*) m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); + +// btAssert(m_testBlock1); + if (m_testBlock1) + { + // btAssert(m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER); + if (m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER) + { + b3Printf("Warning: shared memory is already initialized, did you already spawn a server?\n"); + } + + m_testBlock1->m_numClientCommands = 0; + m_testBlock1->m_numServerCommands = 0; + m_testBlock1->m_numProcessedClientCommands=0; + m_testBlock1->m_numProcessedServerCommands=0; + + m_testBlock1->m_magicId = SHARED_MEMORY_MAGIC_NUMBER; + b3Printf("Shared memory succesfully allocated\n"); + } else + { + b3Error("Couldn't allocated shared memory, is it implemented on your operating system?\n"); + } +} + +void PhysicsServer::stepSimulation(float deltaTime) +{ + + if (m_testBlock1) + { + ///we ignore overflow of integer for now + if (m_testBlock1->m_numClientCommands> m_testBlock1->m_numProcessedClientCommands) + { + + //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands + btAssert(m_testBlock1->m_numClientCommands==m_testBlock1->m_numProcessedClientCommands+1); + + //consume the command + switch (m_testBlock1->m_clientCommands[0]) + { + case CMD_LOAD_URDF: + { + b3Printf("Processed CMD_LOAD_URDF"); + + //load the actual URDF and send a report: completed or failed + + m_testBlock1->m_serverCommands[0] =CMD_URDF_LOADING_COMPLETED; + m_testBlock1->m_numServerCommands++; + + //CMD_URDF_LOADING_COMPLETED, + //CMD_URDF_LOADING_FAILED, + } + default: + { + + } + }; + + m_testBlock1->m_numProcessedClientCommands++; + + //process the command right now + + } + } +} + + +class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options) +{ + return new PhysicsServer(options.m_guiHelper); +} + + diff --git a/examples/SharedMemory/PhysicsServer.h b/examples/SharedMemory/PhysicsServer.h new file mode 100644 index 000000000..43ab883d7 --- /dev/null +++ b/examples/SharedMemory/PhysicsServer.h @@ -0,0 +1,9 @@ +#ifndef PHYSICS_SERVER_H +#define PHYSICS_SERVER_H + + +class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options); + +#endif //PHYSICS_SERVER_H + + diff --git a/examples/SharedMemory/PosixSharedMemory.cpp b/examples/SharedMemory/PosixSharedMemory.cpp new file mode 100644 index 000000000..8e48212e4 --- /dev/null +++ b/examples/SharedMemory/PosixSharedMemory.cpp @@ -0,0 +1,84 @@ +#include "PosixSharedMemory.h" +#include "Bullet3Common/b3Logging.h" +#include "LinearMath/btScalar.h" //for btAssert +#ifdef __APPLE__ +//#define TEST_SHARED_MEMORY +#endif + +#include + +#ifdef TEST_SHARED_MEMORY + +#include +#include + +#endif + +struct PosixSharedMemoryInteralData +{ + +}; +PosixSharedMemory::PosixSharedMemory() +{ + + m_internalData = new PosixSharedMemoryInteralData; +} + +PosixSharedMemory::~PosixSharedMemory() +{ + delete m_internalData; +} + +struct btPointerCaster +{ + union + { + void* ptr; + ptrdiff_t integer; + }; +}; + +void* PosixSharedMemory::allocateSharedMemory(int key, int size) +{ +#ifdef TEST_SHARED_MEMORY + int flags = IPC_CREAT | 0666; + int id = shmget((key_t) key, (size_t) size,flags); + if (id < 0) + { + b3Error("shmget error"); + } else + { + btPointerCaster result; + result.ptr = shmat(id,0,0); + if (result.integer == -1) + { + b3Error("shmat returned -1"); + } else + { + return result.ptr; + } + } +#else + //not implemented yet + btAssert(0); +#endif + return 0; +} +void PosixSharedMemory::releaseSharedMemory(int key, int size) +{ +#ifdef TEST_SHARED_MEMORY + int flags = 0666; + int id = shmget((key_t) key, (size_t) size,flags); + if (id < 0) + { + b3Error("shmget error"); + } else + { + int result = shmctl(id,IPC_RMID,0); + if (result == -1) + { + b3Error("shmat returned -1"); + } + } +#endif +} \ No newline at end of file diff --git a/examples/SharedMemory/PosixSharedMemory.h b/examples/SharedMemory/PosixSharedMemory.h new file mode 100644 index 000000000..479e4e504 --- /dev/null +++ b/examples/SharedMemory/PosixSharedMemory.h @@ -0,0 +1,21 @@ +#ifndef POSIX_SHARED_MEMORY_H +#define POSIX_SHARED_MEMORY_H + +#include "SharedMemoryInterface.h" + + + +class PosixSharedMemory : public SharedMemoryInterface +{ + + struct PosixSharedMemoryInteralData* m_internalData; + +public: + PosixSharedMemory(); + virtual ~PosixSharedMemory(); + + virtual void* allocateSharedMemory(int key, int size); + virtual void releaseSharedMemory(int key, int size); +}; + +#endif // diff --git a/examples/SharedMemory/SharedMemoryInterface.h b/examples/SharedMemory/SharedMemoryInterface.h new file mode 100644 index 000000000..3922c6212 --- /dev/null +++ b/examples/SharedMemory/SharedMemoryInterface.h @@ -0,0 +1,73 @@ +#ifndef SHARED_MEMORY_INTERFACE_H +#define SHARED_MEMORY_INTERFACE_H + +#define SHARED_MEMORY_KEY 12345 +#define SHARED_MEMORY_MAGIC_NUMBER 64738 +#define SHARED_MEMORY_MAX_COMMANDS 64 + +enum SharedMemoryServerCommand{ + CMD_URDF_LOADING_COMPLETED, + CMD_URDF_LOADING_FAILED, + + CMD_SERVER_STATE_UPDATE_COMPLETED, + CMD_STEP_FORWARD_SIMULATION_COMPLETED, + CMD_MAX_SERVER_COMMANDS +}; + +enum SharedMemoryClientCommand{ + CMD_LOAD_URDF, + CMD_STATE_UPDATED, + CMD_STEP_FORWARD_SIMULATION, //includes CMD_REQUEST_STATE + CMD_MAX_CLIENT_COMMANDS +}; + +#define SHARED_MEMORY_SERVER_TEST_C +#define MAX_DEGREE_OF_FREEDOM 1024 +#define MAX_NUM_SENSORS 1024 +#define MAX_URDF_FILENAME_LENGTH 1024 + +struct CommandArguments +{ + double m_deltaTimeInSeconds; + char m_urdfFileName[MAX_URDF_FILENAME_LENGTH]; +}; + +struct SharedMemoryExampleData +{ + int m_magicId; + int m_clientCommands[SHARED_MEMORY_MAX_COMMANDS]; + int m_serverCommands[SHARED_MEMORY_MAX_COMMANDS]; + + int m_numClientCommands; + int m_numProcessedClientCommands; + + int m_numServerCommands; + int m_numProcessedServerCommands; + + + double m_stateQ[MAX_DEGREE_OF_FREEDOM]; + double m_stateQdot[MAX_DEGREE_OF_FREEDOM]; + double m_stateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information + + CommandArguments m_clientCommandArguments[SHARED_MEMORY_MAX_COMMANDS]; + CommandArguments m_serverCommandArguments[SHARED_MEMORY_MAX_COMMANDS]; + +}; + +#define SHARED_MEMORY_SIZE sizeof(SharedMemoryExampleData) + + + + +class SharedMemoryInterface +{ + public: + virtual ~SharedMemoryInterface() + { + } + + virtual void* allocateSharedMemory(int key, int size) =0; + virtual void releaseSharedMemory(int key, int size) =0; +}; +#endif + diff --git a/examples/SharedMemory/main.cpp b/examples/SharedMemory/main.cpp new file mode 100644 index 000000000..7990b1b14 --- /dev/null +++ b/examples/SharedMemory/main.cpp @@ -0,0 +1,54 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "PhysicsServer.h" +#include "PhysicsClient.h" +#include "Bullet3Common/b3CommandLineArgs.h" + +#include "../CommonInterfaces/CommonExampleInterface.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" + + + + +int main(int argc, char* argv[]) +{ + + b3CommandLineArgs args(argc, argv); + struct PhysicsInterface* pint = 0; + + DummyGUIHelper noGfx; + + CommonExampleOptions options(&noGfx); + CommonExampleInterface* example = 0; + + if (args.CheckCmdLineFlag("client")) + { + example = PhysicsClientCreateFunc(options); + }else + { + example = PhysicsServerCreateFunc(options); + } + + example->initPhysics(); + example->stepSimulation(1.f/60.f); + example->exitPhysics(); + + delete example; + + return 0; +} + diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua new file mode 100644 index 000000000..8cf05a80a --- /dev/null +++ b/examples/SharedMemory/premake4.lua @@ -0,0 +1,22 @@ + +project "App_SharedMemoryPhysics" + +if _OPTIONS["ios"] then + kind "WindowedApp" +else + kind "ConsoleApp" +end + +includedirs {"../../src"} + +links { + "Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath" +} + +language "C++" + +files { + "**.cpp", + "**.h", +} +