From 57f50f177844e6471837b4afe64710c7ce02b446 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 26 Oct 2015 17:56:21 -0700 Subject: [PATCH] Create a PhysicsLoopBack that includes both PhysicsClient and PhysicsServer over shared memory. Add a test for PhysicsLoopBack. --- examples/SharedMemory/PhysicsClientC_API.cpp | 7 +- examples/SharedMemory/PhysicsClientC_API.h | 2 +- examples/SharedMemory/PhysicsLoopBack.cpp | 115 ++++++++++++++++++ examples/SharedMemory/PhysicsLoopBack.h | 58 +++++++++ examples/SharedMemory/PhysicsLoopBackC_API.h | 19 +++ .../SharedMemory/PhysicsLoopBackC_Api.cpp | 15 +++ test/SharedMemory/premake4.lua | 55 ++++++++- test/SharedMemory/test.c | 69 ++++++----- 8 files changed, 306 insertions(+), 34 deletions(-) create mode 100644 examples/SharedMemory/PhysicsLoopBack.cpp create mode 100644 examples/SharedMemory/PhysicsLoopBack.h create mode 100644 examples/SharedMemory/PhysicsLoopBackC_API.h create mode 100644 examples/SharedMemory/PhysicsLoopBackC_Api.cpp diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 2a81fcdab..06d1f6778 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -398,6 +398,7 @@ b3PhysicsClientHandle b3ConnectSharedMemory(int key) return (b3PhysicsClientHandle ) cl; } + void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -452,7 +453,11 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle) int b3CanSubmitCommand(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; - return (int)cl->canSubmitCommand(); + if (cl) + { + return (int)cl->canSubmitCommand(); + } + return false; } int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 6d37cfdff..23aa5f2d4 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -15,7 +15,7 @@ B3_DECLARE_HANDLE(b3SharedMemoryStatusHandle); extern "C" { #endif -///make sure to start the server first, before connecting client to physics server +///make sure to start the server first, before connecting client to a physics server over shared memory or UDP b3PhysicsClientHandle b3ConnectSharedMemory(int key); void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp new file mode 100644 index 000000000..3b55d1582 --- /dev/null +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -0,0 +1,115 @@ +#include "PhysicsLoopBack.h" +#include "PhysicsServer.h" +#include "PhysicsClientSharedMemory.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" + +struct PhysicsLoopBackInternalData +{ + PhysicsClientSharedMemory* m_physicsClient; + PhysicsServerSharedMemory* m_physicsServer; + DummyGUIHelper m_noGfx; + + + PhysicsLoopBackInternalData() + :m_physicsClient(0), + m_physicsServer(0) + { + } +}; + +PhysicsLoopBack::PhysicsLoopBack() +{ + m_data = new PhysicsLoopBackInternalData; + m_data->m_physicsServer = new PhysicsServerSharedMemory(); + m_data->m_physicsClient = new PhysicsClientSharedMemory(); +} + +PhysicsLoopBack::~PhysicsLoopBack() +{ + delete m_data->m_physicsClient; + delete m_data->m_physicsServer; + delete m_data; +} + + +// return true if connection succesfull, can also check 'isConnected' +bool PhysicsLoopBack::connect() +{ + m_data->m_physicsServer->connectSharedMemory(&m_data->m_noGfx); + m_data->m_physicsClient->connect(); + return m_data->m_physicsClient->isConnected(); +} + +////todo: rename to 'disconnect' +void PhysicsLoopBack::disconnectSharedMemory() +{ + m_data->m_physicsClient->disconnectSharedMemory(); + m_data->m_physicsServer->disconnectSharedMemory(true); +} + +bool PhysicsLoopBack::isConnected() const +{ + return m_data->m_physicsClient->isConnected(); +} + +// return non-null if there is a status, nullptr otherwise +const SharedMemoryStatus* PhysicsLoopBack::processServerStatus() +{ + m_data->m_physicsServer->processClientCommands(); + return m_data->m_physicsClient->processServerStatus(); +} + +SharedMemoryCommand* PhysicsLoopBack::getAvailableSharedMemoryCommand() +{ + return m_data->m_physicsClient->getAvailableSharedMemoryCommand(); +} + +bool PhysicsLoopBack::canSubmitCommand() const +{ + return m_data->m_physicsClient->canSubmitCommand(); +} + +bool PhysicsLoopBack::submitClientCommand(const struct SharedMemoryCommand& command) +{ + return m_data->m_physicsClient->submitClientCommand(command); +} + +int PhysicsLoopBack::getNumJoints(int bodyIndex) const +{ + return m_data->m_physicsClient->getNumJoints(bodyIndex); +} + +void PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const +{ + m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info); +} + +///todo: move this out of the +void PhysicsLoopBack::setSharedMemoryKey(int key) +{ + m_data->m_physicsServer->setSharedMemoryKey(key); + m_data->m_physicsClient->setSharedMemoryKey(key); +} + +void PhysicsLoopBack::uploadBulletFileToSharedMemory(const char* data, int len) +{ + m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len); +} + +int PhysicsLoopBack::getNumDebugLines() const +{ + return m_data->m_physicsClient->getNumDebugLines(); +} + +const float* PhysicsLoopBack::getDebugLinesFrom() const +{ + return m_data->m_physicsClient->getDebugLinesFrom(); +} +const float* PhysicsLoopBack::getDebugLinesTo() const +{ + return m_data->m_physicsClient->getDebugLinesTo(); +} +const float* PhysicsLoopBack::getDebugLinesColor() const +{ + return m_data->m_physicsClient->getDebugLinesColor(); +} diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h new file mode 100644 index 000000000..ea66ad177 --- /dev/null +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -0,0 +1,58 @@ +#ifndef PHYSICS_LOOP_BACK_H +#define PHYSICS_LOOP_BACK_H + +//#include "SharedMemoryCommands.h" + + +#include "PhysicsClient.h" +#include "LinearMath/btVector3.h" + +///todo: the PhysicsClient API was designed with shared memory in mind, +///now it become more general we need to move out the shared memory specifics away +///for example naming [disconnectSharedMemory -> disconnect] [ move setSharedMemoryKey to shared memory specific subclass ] + +class PhysicsLoopBack : public PhysicsClient +{ + struct PhysicsLoopBackInternalData* m_data; + +public: + + PhysicsLoopBack(); + + virtual ~PhysicsLoopBack(); + + // return true if connection succesfull, can also check 'isConnected' + virtual bool connect(); + + ////todo: rename to 'disconnect' + virtual void disconnectSharedMemory(); + + virtual bool isConnected() const; + + // return non-null if there is a status, nullptr otherwise + virtual const SharedMemoryStatus* processServerStatus(); + + virtual SharedMemoryCommand* getAvailableSharedMemoryCommand(); + + virtual bool canSubmitCommand() const; + + virtual bool submitClientCommand(const struct SharedMemoryCommand& command); + + virtual int getNumJoints(int bodyIndex) const; + + virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + + ///todo: move this out of the + virtual void setSharedMemoryKey(int key); + + void uploadBulletFileToSharedMemory(const char* data, int len); + + virtual int getNumDebugLines() const; + + virtual const float* getDebugLinesFrom() const; + virtual const float* getDebugLinesTo() const; + virtual const float* getDebugLinesColor() const; + +}; + +#endif //PHYSICS_LOOP_BACK_H diff --git a/examples/SharedMemory/PhysicsLoopBackC_API.h b/examples/SharedMemory/PhysicsLoopBackC_API.h new file mode 100644 index 000000000..6adb69e9c --- /dev/null +++ b/examples/SharedMemory/PhysicsLoopBackC_API.h @@ -0,0 +1,19 @@ +#ifndef PHYSICS_LOOPBACK_C_API_H +#define PHYSICS_LOOPBACK_C_API_H + +#include "PhysicsClientC_API.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +///think more about naming. The b3ConnectPhysicsLoopback +b3PhysicsClientHandle b3ConnectPhysicsLoopback(int key); + + +#ifdef __cplusplus +} +#endif + +#endif //PHYSICS_LOOPBACK_C_API_H diff --git a/examples/SharedMemory/PhysicsLoopBackC_Api.cpp b/examples/SharedMemory/PhysicsLoopBackC_Api.cpp new file mode 100644 index 000000000..c1b47b960 --- /dev/null +++ b/examples/SharedMemory/PhysicsLoopBackC_Api.cpp @@ -0,0 +1,15 @@ +#include "PhysicsLoopBackC_API.h" + +#include "PhysicsLoopBack.h" + + + +//think more about naming. The b3ConnectPhysicsLoopback +b3PhysicsClientHandle b3ConnectPhysicsLoopback(int key) +{ + PhysicsLoopBack* loopBack = new PhysicsLoopBack(); + loopBack->setSharedMemoryKey(key); + bool connected = loopBack->connect(); + return (b3PhysicsClientHandle )loopBack; +} + diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index aa71e7180..1eeb98178 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -1,4 +1,4 @@ - project ("Test_SharedMemoryPhysicsClient") +project ("Test_SharedMemoryPhysicsClient") language "C++" kind "ConsoleApp" @@ -25,4 +25,57 @@ "../../examples/Utils/b3ResourcePath.cpp", "../../examples/Utils/b3ResourcePath.h", } + +project ("Test_PhysicsLoopBack") + + language "C++" + kind "ConsoleApp" + + includedirs {"../../src", "../../examples/SharedMemory", + "../../examples/ThirdPartyLibs"} + defines {"PHYSICS_LOOP_BACK"} + links { + "BulletFileLoader", + "BulletWorldImporter", + "Bullet3Common", + "BulletDynamics", + "BulletCollision", + "LinearMath" + } + + files { + "test.c", + "../../examples/SharedMemory/PhysicsClient.cpp", + "../../examples/SharedMemory/PhysicsClient.h", + "../../examples/SharedMemory/PhysicsServer.cpp", + "../../examples/SharedMemory/PhysicsServer.h", + "../../examples/SharedMemory/PhysicsLoopback.cpp", + "../../examples/SharedMemory/PhysicsLoopback.h", + "../../examples/SharedMemory/PhysicsLoopbackC_Api.cpp", + "../../examples/SharedMemory/PhysicsLoopbackC_Api.h", + "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp", + "../../examples/SharedMemory/PhysicsClientSharedMemory.h", + "../../examples/SharedMemory/PhysicsClientC_API.cpp", + "../../examples/SharedMemory/PhysicsClientC_API.h", + "../../examples/SharedMemory/Win32SharedMemory.cpp", + "../../examples/SharedMemory/Win32SharedMemory.h", + "../../examples/SharedMemory/PosixSharedMemory.cpp", + "../../examples/SharedMemory/PosixSharedMemory.h", + "../../examples/Utils/b3ResourcePath.cpp", + "../../examples/Utils/b3ResourcePath.h", + "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", + "../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", + "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp", + "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h", + "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", + "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp", + "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp", + "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp", + "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", + "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", + "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", + "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", + } \ No newline at end of file diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index 1c7fd4cfb..ed32209d9 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -1,13 +1,14 @@ //#include "SharedMemoryCommands.h" #include "PhysicsClientC_API.h" + +#ifdef PHYSICS_LOOP_BACK +#include "PhysicsLoopbackC_API.h" +#endif //PHYSICS_LOOP_BACK + #include "SharedMemoryPublic.h" #include "Bullet3Common/b3Logging.h" #include -struct test -{ - int unused; -}; #include @@ -24,12 +25,18 @@ int main(int argc, char* argv[]) int imuLinkIndex = -1; - b3PhysicsClientHandle sm; + b3PhysicsClientHandle sm=0; int bodyIndex = -1; printf("hello world\n"); - +#ifdef PHYSICS_LOOP_BACK + sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY); +#else sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY); +#endif + + + if (b3CanSubmitCommand(sm)) { { @@ -150,35 +157,35 @@ int main(int argc, char* argv[]) } { - b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm)); - } + b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm)); + if (sensorJointIndexLeft>=0) + { + + struct b3JointSensorState sensorState; + b3GetJointState(sm,state,sensorJointIndexLeft,&sensorState); + + b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft, + sensorState.m_jointForceTorque[0], + sensorState.m_jointForceTorque[1], + sensorState.m_jointForceTorque[2]); + + } + if (sensorJointIndexRight>=0) + { + struct b3JointSensorState sensorState; + b3GetJointState(sm,state,sensorJointIndexRight,&sensorState); + + b3GetJointInfo(sm,bodyIndex,sensorJointIndexRight,&sensorState); + b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight, + sensorState.m_jointForceTorque[0], + sensorState.m_jointForceTorque[1], + sensorState.m_jointForceTorque[2]); - if (sensorJointIndexLeft>=0) - { - - struct b3JointSensorState info; - - b3GetJointInfo(sm,bodyIndex,sensorJointIndexLeft,&info); - b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft, - info.m_jointForceTorque[0], - info.m_jointForceTorque[1], - info.m_jointForceTorque[2]); - - } + } + } - if (sensorJointIndexRight>=0) - { - struct b3JointSensorState info; - - b3GetJointInfo(sm,bodyIndex,sensorJointIndexRight,&info); - b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight, - info.m_jointForceTorque[0], - info.m_jointForceTorque[1], - info.m_jointForceTorque[2]); - - } { b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));