diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index 05f91df1b..26cd59733 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -49,7 +49,13 @@ 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); + // btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER); + if (m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) + { + b3Error("Error: please start server before client"); + m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); + m_testBlock1 = 0; + } } } @@ -66,7 +72,8 @@ void PhysicsClient::stepSimulation(float deltaTime) once=false; b3Printf("Client created CMD_LOAD_URDF"); - m_testBlock1->m_clientCommands[0] =CMD_LOAD_URDF; + m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF; + sprintf(m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf"); m_testBlock1->m_numClientCommands++; } } diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 06a6224f9..f19fdf991 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -3,6 +3,10 @@ #include "../CommonInterfaces/CommonMultiBodyBase.h" #include "PosixSharedMemory.h" +#include "../Importers/ImportURDFDemo/MyURDFImporter.h" +#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" +#include "../Importers/ImportURDFDemo/URDF2Bullet.h" + @@ -21,6 +25,9 @@ public: virtual void stepSimulation(float deltaTime); + bool loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, + bool useMultiBody, bool useFixedBase); + virtual void resetCamera() { float dist = 1; @@ -53,6 +60,8 @@ PhysicsServer::~PhysicsServer() void PhysicsServer::initPhysics() { + createEmptyDynamicsWorld(); + m_testBlock1 = (SharedMemoryExampleData*) m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); // btAssert(m_testBlock1); @@ -77,9 +86,34 @@ void PhysicsServer::initPhysics() } } +bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, + bool useMultiBody, bool useFixedBase) +{ + + MyURDFImporter u2b(m_guiHelper); + bool loadOk = u2b.loadURDF(fileName); + if (loadOk) + { + b3Printf("loaded %s OK!", fileName); + + btTransform tr; + tr.setIdentity(); + tr.setOrigin(pos); + tr.setRotation(orn); + int rootLinkIndex = u2b.getRootLinkIndex(); + // printf("urdf root link index = %d\n",rootLinkIndex); + MyMultiBodyCreator creation(m_guiHelper); + bool m_useMultiBody = true; + ConvertURDF2Bullet(u2b,creation, tr,m_dynamicsWorld,useMultiBody,u2b.getPathPrefix()); + btMultiBody* mb = creation.getBulletMultiBody(); + + return false; + } +} + void PhysicsServer::stepSimulation(float deltaTime) { - + if (m_testBlock1) { ///we ignore overflow of integer for now @@ -89,20 +123,31 @@ void PhysicsServer::stepSimulation(float deltaTime) //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); + const SharedMemoryCommand& clientCmd =m_testBlock1->m_clientCommands[0]; + //consume the command - switch (m_testBlock1->m_clientCommands[0]) + switch (clientCmd.m_type) { case CMD_LOAD_URDF: { - b3Printf("Processed CMD_LOAD_URDF"); + b3Printf("Processed CMD_LOAD_URDF:%s",clientCmd.m_urdfArguments.m_urdfFileName); //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, + bool completedOk = loadUrdf(clientCmd.m_urdfArguments.m_urdfFileName, + btVector3(0,0,0), btQuaternion(0,0,0,1),true,true ); + SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0]; + + if (completedOk) + { + serverCmd.m_type =CMD_URDF_LOADING_COMPLETED; + } else + { + serverCmd.m_type =CMD_URDF_LOADING_FAILED; + + } + m_testBlock1->m_numServerCommands++; } default: { diff --git a/examples/SharedMemory/PosixSharedMemory.cpp b/examples/SharedMemory/PosixSharedMemory.cpp index 8e48212e4..69a54eace 100644 --- a/examples/SharedMemory/PosixSharedMemory.cpp +++ b/examples/SharedMemory/PosixSharedMemory.cpp @@ -1,8 +1,10 @@ #include "PosixSharedMemory.h" #include "Bullet3Common/b3Logging.h" #include "LinearMath/btScalar.h" //for btAssert + #ifdef __APPLE__ -//#define TEST_SHARED_MEMORY +#define TEST_SHARED_MEMORY + #endif #include @@ -71,13 +73,13 @@ void PosixSharedMemory::releaseSharedMemory(int key, int size) int id = shmget((key_t) key, (size_t) size,flags); if (id < 0) { - b3Error("shmget error"); + b3Error("PosixSharedMemory::releaseSharedMemory: shmget error"); } else { int result = shmctl(id,IPC_RMID,0); if (result == -1) { - b3Error("shmat returned -1"); + b3Error("PosixSharedMemory::releaseSharedMemory: shmat returned -1"); } } #endif diff --git a/examples/SharedMemory/SharedMemoryInterface.h b/examples/SharedMemory/SharedMemoryInterface.h index 3922c6212..42ee4d516 100644 --- a/examples/SharedMemory/SharedMemoryInterface.h +++ b/examples/SharedMemory/SharedMemoryInterface.h @@ -1,7 +1,7 @@ #ifndef SHARED_MEMORY_INTERFACE_H #define SHARED_MEMORY_INTERFACE_H -#define SHARED_MEMORY_KEY 12345 +#define SHARED_MEMORY_KEY 12347 #define SHARED_MEMORY_MAGIC_NUMBER 64738 #define SHARED_MEMORY_MAX_COMMANDS 64 @@ -26,17 +26,33 @@ enum SharedMemoryClientCommand{ #define MAX_NUM_SENSORS 1024 #define MAX_URDF_FILENAME_LENGTH 1024 -struct CommandArguments + +struct UrdfCommandArgument +{ + char m_urdfFileName[MAX_URDF_FILENAME_LENGTH]; +}; + +struct StepSimulationCommandArgument { double m_deltaTimeInSeconds; - char m_urdfFileName[MAX_URDF_FILENAME_LENGTH]; +}; + +struct SharedMemoryCommand +{ + int m_type; + + union + { + UrdfCommandArgument m_urdfArguments; + StepSimulationCommandArgument m_stepSimulationArguments; + }; }; struct SharedMemoryExampleData { int m_magicId; - int m_clientCommands[SHARED_MEMORY_MAX_COMMANDS]; - int m_serverCommands[SHARED_MEMORY_MAX_COMMANDS]; + SharedMemoryCommand m_clientCommands[SHARED_MEMORY_MAX_COMMANDS]; + SharedMemoryCommand m_serverCommands[SHARED_MEMORY_MAX_COMMANDS]; int m_numClientCommands; int m_numProcessedClientCommands; @@ -44,13 +60,17 @@ struct SharedMemoryExampleData int m_numServerCommands; int m_numProcessedServerCommands; + + //desired state is only written by the client, read-only access by server is expected + double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM]; + double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM]; + double m_desiredStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information - 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 + //actual state is only written by the server, read-only access by client is expected + double m_actualStateQ[MAX_DEGREE_OF_FREEDOM]; + double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM]; + double m_actualStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information - CommandArguments m_clientCommandArguments[SHARED_MEMORY_MAX_COMMANDS]; - CommandArguments m_serverCommandArguments[SHARED_MEMORY_MAX_COMMANDS]; };