From 3fe9138e8ce95d8ac741bf598597ce0f715fa709 Mon Sep 17 00:00:00 2001 From: = <=> Date: Thu, 23 Jul 2015 11:51:25 -0700 Subject: [PATCH] minor refactoring --- examples/ExampleBrowser/CMakeLists.txt | 1 + examples/ExampleBrowser/premake4.lua | 1 + examples/SharedMemory/PhysicsClient.cpp | 9 ++-- examples/SharedMemory/PhysicsClient.h | 13 +---- .../SharedMemory/PhysicsClientExample.cpp | 4 +- examples/SharedMemory/PhysicsServer.cpp | 9 ++-- .../SharedMemory/PhysicsServerExample.cpp | 1 - examples/SharedMemory/RobotControlExample.cpp | 26 +++++++--- examples/SharedMemory/SharedMemoryCommands.h | 51 +++++++++++-------- examples/SharedMemory/SharedMemoryInterface.h | 44 +--------------- examples/SharedMemory/main.cpp | 1 - 11 files changed, 64 insertions(+), 96 deletions(-) diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 153789cf7..05db45faa 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -15,6 +15,7 @@ SET(App_ExampleBrowser_SRCS main.cpp ExampleEntries.cpp ExampleEntries.h + ../SharedMemory/PhysicsClientC_API.cpp ../SharedMemory/PhysicsServer.cpp ../SharedMemory/PhysicsClient.cpp ../SharedMemory/PhysicsServerExample.cpp diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 34c5fcc01..d5798f23d 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -50,6 +50,7 @@ files { "**.cpp", "**.h", + "../SharedMemory/PhysicsClientC_API.cpp", "../SharedMemory/PhysicsServerExample.cpp", "../SharedMemory/PhysicsClientExample.cpp", "../SharedMemory/RobotControlExample.cpp", diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index 644581976..bd86dce6c 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -6,6 +6,7 @@ #include "../Utils/b3ResourcePath.h" #include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h" #include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h" +#include "SharedMemoryBlock.h" //copied from btMultiBodyLink.h @@ -18,7 +19,7 @@ enum JointType struct PhysicsClientSharedMemoryInternalData { SharedMemoryInterface* m_sharedMemory; - SharedMemoryExampleData* m_testBlock1; + SharedMemoryBlock* m_testBlock1; btAlignedObjectArray m_robotMultiBodyData; btAlignedObjectArray m_poweredJointInfo; @@ -89,7 +90,7 @@ bool PhysicsClientSharedMemory::isConnected() const bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization) { bool allowCreation = true; - m_data->m_testBlock1 = (SharedMemoryExampleData*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE, allowCreation); + m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE, allowCreation); if (m_data->m_testBlock1) { @@ -97,7 +98,7 @@ bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization) { if (allowSharedMemoryInitialization) { - InitSharedMemoryExampleData(m_data->m_testBlock1); + InitSharedMemoryBlock(m_data->m_testBlock1); b3Printf("Created and initialized shared memory block"); m_data->m_isConnected = true; } else @@ -123,7 +124,7 @@ bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization) -bool PhysicsClientSharedMemory::processServerStatus(ServerStatus& serverStatus) +bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverStatus) { btAssert(m_data->m_testBlock1); bool hasStatus = false; diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 83e022364..4d11f91ac 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -3,17 +3,6 @@ #include "SharedMemoryCommands.h" -#include - -struct PoweredJointInfo -{ - std::string m_linkName; - std::string m_jointName; - int m_jointType; - int m_qIndex; - int m_uIndex; -}; - class PhysicsClientSharedMemory //: public CommonPhysicsClientInterface { @@ -31,7 +20,7 @@ public: virtual bool isConnected() const; // return true if there is a status, and fill in 'serverStatus' - virtual bool processServerStatus(ServerStatus& serverStatus); + virtual bool processServerStatus(SharedMemoryStatus& serverStatus); virtual bool canSubmitCommand() const; diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index da9327c83..929727963 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -211,7 +211,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime) if (m_physicsClient.isConnected()) { - ServerStatus status; + SharedMemoryStatus status; bool hasStatus = m_physicsClient.processServerStatus(status); if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) { @@ -219,7 +219,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime) { PoweredJointInfo info; m_physicsClient.getPoweredJointInfo(i,info); - b3Printf("1-DOF PoweredJoint %s at q-index %d and u-index %d\n",info.m_jointName.c_str(),info.m_qIndex,info.m_uIndex); + b3Printf("1-DOF PoweredJoint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); } } diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 7614a7632..06b68f834 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -17,6 +17,7 @@ #include "LinearMath/btSerializer.h" #include "Bullet3Common/b3Logging.h" #include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "SharedMemoryBlock.h" struct UrdfLinkNameMapUtil { @@ -31,7 +32,7 @@ struct UrdfLinkNameMapUtil struct PhysicsServerInternalData { SharedMemoryInterface* m_sharedMemory; - SharedMemoryExampleData* m_testBlock1; + SharedMemoryBlock* m_testBlock1; bool m_isConnected; btScalar m_physicsDeltaTime; //btAlignedObjectArray m_jointFeedbacks; @@ -84,14 +85,14 @@ bool PhysicsServerSharedMemory::connectSharedMemory(bool allowSharedMemoryInitia bool allowCreation = true; - m_data->m_testBlock1 = (SharedMemoryExampleData*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE,allowCreation); + 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) { - InitSharedMemoryExampleData(m_data->m_testBlock1); + InitSharedMemoryBlock(m_data->m_testBlock1); b3Printf("Created and initialized shared memory block"); m_data->m_isConnected = true; } else @@ -212,7 +213,7 @@ bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3& tr.setIdentity(); tr.setOrigin(pos); tr.setRotation(orn); - int rootLinkIndex = u2b.getRootLinkIndex(); + //int rootLinkIndex = u2b.getRootLinkIndex(); // printf("urdf root link index = %d\n",rootLinkIndex); MyMultiBodyCreator creation(m_data->m_guiHelper); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index d1dc52dc1..e7f6c6d9c 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -57,7 +57,6 @@ PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper) m_wantsShutdown(false) { b3Printf("Started PhysicsServer\n"); - } diff --git a/examples/SharedMemory/RobotControlExample.cpp b/examples/SharedMemory/RobotControlExample.cpp index 418972fba..1ccb38e25 100644 --- a/examples/SharedMemory/RobotControlExample.cpp +++ b/examples/SharedMemory/RobotControlExample.cpp @@ -10,6 +10,7 @@ #include "PhysicsClient.h" #include "SharedMemoryCommon.h" #include "../Utils/b3Clock.h" +#include "PhysicsClientC_API.h" //const char* blaatnaam = "basename"; @@ -100,11 +101,20 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr) case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { - command.m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS; - command.m_physSimParamArgs.m_gravityAcceleration[0] = 0; - command.m_physSimParamArgs.m_gravityAcceleration[1] = 0; - command.m_physSimParamArgs.m_gravityAcceleration[2] = -10; - command.m_updateFlags = SIM_PARAM_UPDATE_GRAVITY; + //#ifdef USE_C_API + b3InitPhysicsParamCommand(&command); + b3PhysicsParamSetGravity(&command, 0,0,-10); + + +// #else +// +// command.m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS; +// command.m_physSimParamArgs.m_gravityAcceleration[0] = 0; +// command.m_physSimParamArgs.m_gravityAcceleration[1] = 0; +// command.m_physSimParamArgs.m_gravityAcceleration[2] = -10; +// command.m_physSimParamArgs.m_updateFlags = SIM_PARAM_UPDATE_GRAVITY; +// #endif // USE_C_API + cl->enqueueCommand(command); break; @@ -286,7 +296,7 @@ void RobotControlExample::stepSimulation(float deltaTime) if (m_physicsClient.isConnected()) { - ServerStatus status; + SharedMemoryStatus status; bool hasStatus = m_physicsClient.processServerStatus(status); if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) { @@ -294,12 +304,12 @@ void RobotControlExample::stepSimulation(float deltaTime) { PoweredJointInfo info; m_physicsClient.getPoweredJointInfo(i,info); - b3Printf("1-DOF PoweredJoint %s at q-index %d and u-index %d\n",info.m_jointName.c_str(),info.m_qIndex,info.m_uIndex); + b3Printf("1-DOF PoweredJoint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); if (m_numMotorsm_velTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 2c7dd04e4..5a58b119d 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -63,11 +63,11 @@ enum EnumSharedMemoryServerStatus struct UrdfArgs { - char m_urdfFileName[MAX_URDF_FILENAME_LENGTH]; - double m_initialPosition[3]; - double m_initialOrientation[4]; - bool m_useMultiBody; - bool m_useFixedBase; + char m_urdfFileName[MAX_URDF_FILENAME_LENGTH]; + double m_initialPosition[3]; + double m_initialOrientation[4]; + int m_useMultiBody; + int m_useFixedBase; }; @@ -81,7 +81,7 @@ struct SetJointFeedbackArgs { int m_bodyUniqueId; int m_linkId; - bool m_isEnabled; + int m_isEnabled; }; //todo: discuss and decide about control mode and combinations @@ -132,7 +132,6 @@ enum EnumUpdateFlags ///The control mode determines the state variables used for motor control. struct SendPhysicsSimulationParameters { - double m_deltaTime; double m_gravityAcceleration[3]; int m_numSimulationSubSteps; @@ -161,25 +160,26 @@ struct SendActualStateArgs }; +typedef struct SharedMemoryCommand SharedMemoryCommand_t; struct SharedMemoryCommand { - int m_type; + int m_type; + smUint64_t m_timeStamp; int m_sequenceNumber; - smUint64_t m_timeStamp; - //a bit fields to tell which parameters need updating - //for example m_updateFlags = SIM_PARAM_UPDATE_DELTA_TIME | SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS; - smUint64_t m_updateFlags; + //a bit fields to tell which parameters need updating + //for example m_updateFlags = SIM_PARAM_UPDATE_DELTA_TIME | SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS; + int m_updateFlags; union { - UrdfArgs m_urdfArguments; - InitPoseArgs m_initPoseArgs; - SendPhysicsSimulationParameters m_physSimParamArgs; - BulletDataStreamArgs m_dataStreamArguments; - SendDesiredStateArgs m_sendDesiredStateCommandArgument; - RequestActualStateArgs m_requestActualStateInformationCommandArgument; + struct UrdfArgs m_urdfArguments; + struct InitPoseArgs m_initPoseArgs; + struct SendPhysicsSimulationParameters m_physSimParamArgs; + struct BulletDataStreamArgs m_dataStreamArguments; + struct SendDesiredStateArgs m_sendDesiredStateCommandArgument; + struct RequestActualStateArgs m_requestActualStateInformationCommandArgument; }; }; @@ -193,11 +193,20 @@ struct SharedMemoryStatus union { - BulletDataStreamArgs m_dataStreamArguments; - SendActualStateArgs m_sendActualStateArgs; + struct BulletDataStreamArgs m_dataStreamArguments; + struct SendActualStateArgs m_sendActualStateArgs; }; }; -typedef SharedMemoryStatus ServerStatus; +struct PoweredJointInfo +{ + char* m_linkName; + char* m_jointName; + int m_jointType; + int m_qIndex; + int m_uIndex; +}; + + #endif //SHARED_MEMORY_COMMANDS_H diff --git a/examples/SharedMemory/SharedMemoryInterface.h b/examples/SharedMemory/SharedMemoryInterface.h index 084559451..2beae04d0 100644 --- a/examples/SharedMemory/SharedMemoryInterface.h +++ b/examples/SharedMemory/SharedMemoryInterface.h @@ -1,49 +1,6 @@ #ifndef SHARED_MEMORY_INTERFACE_H #define SHARED_MEMORY_INTERFACE_H -#define SHARED_MEMORY_KEY 12347 -#define SHARED_MEMORY_MAGIC_NUMBER 64738 -#define SHARED_MEMORY_MAX_COMMANDS 32 -#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024) - -#include "SharedMemoryCommands.h" - -struct SharedMemoryExampleData -{ - int m_magicId; - SharedMemoryCommand m_clientCommands[SHARED_MEMORY_MAX_COMMANDS]; - SharedMemoryStatus m_serverCommands[SHARED_MEMORY_MAX_COMMANDS]; - - int m_numClientCommands; - int m_numProcessedClientCommands; - - int m_numServerCommands; - int m_numProcessedServerCommands; - - //m_bulletStreamDataClientToServer is a way for the client to create collision shapes, rigid bodies and constraints - //the Bullet data structures are more general purpose than the capabilities of a URDF file. - char m_bulletStreamDataClientToServer[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; - - //m_bulletStreamDataServerToClient is used to send (debug) data from server to client, for - //example to provide all details of a multibody including joint/link names, after loading a URDF file. - char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; -}; - - -inline void InitSharedMemoryExampleData(SharedMemoryExampleData* sharedMemoryBlock) -{ - sharedMemoryBlock->m_numClientCommands = 0; - sharedMemoryBlock->m_numServerCommands = 0; - sharedMemoryBlock->m_numProcessedClientCommands=0; - sharedMemoryBlock->m_numProcessedServerCommands=0; - sharedMemoryBlock->m_magicId = SHARED_MEMORY_MAGIC_NUMBER; -} - -#define SHARED_MEMORY_SIZE sizeof(SharedMemoryExampleData) - - - - class SharedMemoryInterface { public: @@ -54,5 +11,6 @@ class SharedMemoryInterface virtual void* allocateSharedMemory(int key, int size, bool allowCreation) =0; virtual void releaseSharedMemory(int key, int size) =0; }; + #endif diff --git a/examples/SharedMemory/main.cpp b/examples/SharedMemory/main.cpp index cdf144af7..c78f4245f 100644 --- a/examples/SharedMemory/main.cpp +++ b/examples/SharedMemory/main.cpp @@ -27,7 +27,6 @@ int main(int argc, char* argv[]) { b3CommandLineArgs args(argc, argv); - struct PhysicsInterface* pint = 0; DummyGUIHelper noGfx;