diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 317b89dbc..3d40b8c81 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -16,6 +16,7 @@ SET(App_ExampleBrowser_SRCS ExampleEntries.cpp ExampleEntries.h ../SharedMemory/PhysicsServer.cpp + ../SharedMemory/PhysicsClientSharedMemory.cpp ../SharedMemory/PhysicsClient.cpp ../SharedMemory/PhysicsClientC_API.cpp ../SharedMemory/PhysicsServerExample.cpp diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index fcc3330ab..439b64670 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -57,6 +57,7 @@ "../SharedMemory/PhysicsServerExample.cpp", "../SharedMemory/PhysicsClientExample.cpp", "../SharedMemory/PhysicsServer.cpp", + "../SharedMemory/PhysicsClientSharedMemory.cpp", "../SharedMemory/PhysicsClient.cpp", "../SharedMemory/PosixSharedMemory.cpp", "../SharedMemory/Win32SharedMemory.cpp", diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index 4fe6067d7..4eada50a8 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -1,610 +1,3 @@ #include "PhysicsClient.h" -#include "PosixSharedMemory.h" -#include "Win32SharedMemory.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btVector3.h" -#include "Bullet3Common/b3Logging.h" -#include "../Utils/b3ResourcePath.h" -#include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h" -#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h" -#include "SharedMemoryBlock.h" - - -//copied from btMultiBodyLink.h -enum JointType -{ - eRevoluteType = 0, - ePrismaticType = 1, -}; - -struct TmpFloat3 -{ - float m_x; - float m_y; - float m_z; - -}; - -TmpFloat3 CreateTmpFloat3(float x,float y, float z) -{ - TmpFloat3 tmp; tmp.m_x = x;tmp.m_y = y;tmp.m_z = z; return tmp; -} - -struct PhysicsClientSharedMemoryInternalData -{ - SharedMemoryInterface* m_sharedMemory; - SharedMemoryBlock* m_testBlock1; - - btAlignedObjectArray m_robotMultiBodyData; - btAlignedObjectArray m_jointInfo; - btAlignedObjectArray m_debugLinesFrom; - btAlignedObjectArray m_debugLinesTo; - btAlignedObjectArray m_debugLinesColor; - - SharedMemoryStatus m_lastServerStatus; - - int m_counter; - bool m_serverLoadUrdfOK; - bool m_isConnected; - bool m_waitingForServer; - bool m_hasLastServerStatus; - int m_sharedMemoryKey; - bool m_verboseOutput; - - PhysicsClientSharedMemoryInternalData() - :m_sharedMemory(0), - m_testBlock1(0), - m_counter(0), - m_serverLoadUrdfOK(false), - m_isConnected(false), - m_waitingForServer(false), - m_hasLastServerStatus(false), - m_sharedMemoryKey(SHARED_MEMORY_KEY), - m_verboseOutput(false) - { - } - - - - void processServerStatus(); - - bool canSubmitCommand() const; - - -}; - - -int PhysicsClientSharedMemory::getNumJoints() const -{ - return m_data->m_jointInfo.size(); -} - -void PhysicsClientSharedMemory::getJointInfo(int index, b3JointInfo& info) const -{ - info = m_data->m_jointInfo[index]; -} - - -PhysicsClientSharedMemory::PhysicsClientSharedMemory() - -{ - m_data = new PhysicsClientSharedMemoryInternalData; - -#ifdef _WIN32 - m_data->m_sharedMemory = new Win32SharedMemoryClient(); -#else - m_data->m_sharedMemory = new PosixSharedMemory(); -#endif - -} - -PhysicsClientSharedMemory::~PhysicsClientSharedMemory() -{ - if (m_data->m_isConnected) - { - disconnectSharedMemory(); - } - delete m_data->m_sharedMemory; - delete m_data; -} - -void PhysicsClientSharedMemory::setSharedMemoryKey(int key) -{ - m_data->m_sharedMemoryKey = key; -} - -void PhysicsClientSharedMemory::disconnectSharedMemory () -{ - if (m_data->m_isConnected) - { - m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); - m_data->m_isConnected = false; - } -} - -bool PhysicsClientSharedMemory::isConnected() const -{ - return m_data->m_isConnected ; -} - -bool PhysicsClientSharedMemory::connect() -{ - ///server always has to create and initialize shared memory - bool allowCreation = false; - m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE, allowCreation); - - if (m_data->m_testBlock1) - { - if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) - { - b3Error("Error: please start server before client\n"); - m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); - m_data->m_testBlock1 = 0; - return false; - } else - { - if (m_data->m_verboseOutput) - { - b3Printf("Connected to existing shared memory, status OK.\n"); - } - m_data->m_isConnected = true; - } - } else - { - b3Error("Cannot connect to shared memory"); - return false; - } - return true; -} - - - - -const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() -{ - SharedMemoryStatus* stat = 0; - - if (!m_data->m_testBlock1) - { - return 0; - } - - if (!m_data->m_waitingForServer) - { - return 0; - } - - if (m_data->m_testBlock1->m_numServerCommands> m_data->m_testBlock1->m_numProcessedServerCommands) - { - btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1); - - const SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; - m_data->m_lastServerStatus = serverCmd; - - EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type; - //consume the command - switch (serverCmd.m_type) - { - case CMD_CLIENT_COMMAND_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Server completed command"); - } - break; - } - case CMD_URDF_LOADING_COMPLETED: - { - m_data->m_serverLoadUrdfOK = true; - if (m_data->m_verboseOutput) - { - b3Printf("Server loading the URDF OK\n"); - } - - if (serverCmd.m_dataStreamArguments.m_streamChunkLength>0) - { - bParse::btBulletFile* bf = new bParse::btBulletFile(this->m_data->m_testBlock1->m_bulletStreamDataServerToClient,serverCmd.m_dataStreamArguments.m_streamChunkLength); - bf->setFileDNAisMemoryDNA(); - bf->parse(false); - m_data->m_robotMultiBodyData.push_back(bf); - - for (int i=0;im_multiBodies.size();i++) - { - int flag = bf->getFlags(); - int qOffset = 7; - int uOffset=6; - - if ((flag&bParse::FD_DOUBLE_PRECISION)!=0) - { - Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf->m_multiBodies[i]; - if (mb->m_baseName) - { - if (m_data->m_verboseOutput) - { - b3Printf("mb->m_baseName = %s\n",mb->m_baseName); - } - } - - for (int link=0;linkm_numLinks;link++) - { - { - b3JointInfo info; - info.m_flags = 0; - info.m_qIndex = (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1; - info.m_uIndex = (0 < mb->m_links[link].m_dofCount) ? uOffset : -1; - - if (mb->m_links[link].m_linkName) - { - if (m_data->m_verboseOutput) - { - b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName); - } - info.m_linkName = mb->m_links[link].m_linkName; - } - if (mb->m_links[link].m_jointName) - { - if (m_data->m_verboseOutput) - { - b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName); - } - info.m_jointName = mb->m_links[link].m_jointName; - info.m_jointType = mb->m_links[link].m_jointType; - } - if ((mb->m_links[link].m_jointType == eRevoluteType)|| - (mb->m_links[link].m_jointType == ePrismaticType)) - { - info.m_flags |= JOINT_HAS_MOTORIZED_POWER; - } - m_data->m_jointInfo.push_back(info); - } - qOffset+= mb->m_links[link].m_posVarCount; - uOffset+= mb->m_links[link].m_dofCount; - - } - - } else - { - Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*) bf->m_multiBodies[i]; - if (mb->m_baseName) - { - if (m_data->m_verboseOutput) - { - b3Printf("mb->m_baseName = %s\n",mb->m_baseName); - } - } - for (int link=0;linkm_numLinks;link++) - { - { - b3JointInfo info; - info.m_flags = 0; - info.m_qIndex = (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1; - info.m_uIndex = (0 < mb->m_links[link].m_dofCount) ? uOffset : -1; - - if (mb->m_links[link].m_linkName) - { - if (m_data->m_verboseOutput) - { - b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName); - } - info.m_linkName = mb->m_links[link].m_linkName; - } - if (mb->m_links[link].m_jointName) - { - if (m_data->m_verboseOutput) - { - b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName); - } - info.m_jointName = mb->m_links[link].m_jointName; - info.m_jointType = mb->m_links[link].m_jointType; - } - if ((mb->m_links[link].m_jointType == eRevoluteType)|| - (mb->m_links[link].m_jointType == ePrismaticType)) - { - info.m_flags |= JOINT_HAS_MOTORIZED_POWER; - } - m_data->m_jointInfo.push_back(info); - } - qOffset+= mb->m_links[link].m_posVarCount; - uOffset+= mb->m_links[link].m_dofCount; - } - - } - } - if (bf->ok()) - { - if (m_data->m_verboseOutput) - { - b3Printf("Received robot description ok!\n"); - } - } else - { - b3Warning("Robot description not received"); - } - } - break; - } - case CMD_DESIRED_STATE_RECEIVED_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Server received desired state"); - } - break; - } - case CMD_STEP_FORWARD_SIMULATION_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Server completed step simulation"); - } - break; - } - case CMD_URDF_LOADING_FAILED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Server failed loading the URDF...\n"); - } - m_data->m_serverLoadUrdfOK = false; - break; - } - - case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Server received bullet data stream OK\n"); - } - - - - - break; - } - case CMD_BULLET_DATA_STREAM_RECEIVED_FAILED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Server failed receiving bullet data stream\n"); - } - - break; - } - - - case CMD_ACTUAL_STATE_UPDATE_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Received actual state\n"); - } - SharedMemoryStatus& command = m_data->m_testBlock1->m_serverCommands[0]; - - int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ; - int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU; - if (m_data->m_verboseOutput) - { - b3Printf("size Q = %d, size U = %d\n", numQ,numU); - } - char msg[1024]; - - { - sprintf(msg,"Q=["); - - for (int i=0;im_verboseOutput) - { - b3Printf(msg); - } - - { - sprintf(msg,"U=["); - - for (int i=0;im_verboseOutput) - { - b3Printf(msg); - } - - - if (m_data->m_verboseOutput) - { - b3Printf("\n"); - } - break; - } - case CMD_RESET_SIMULATION_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n"); - } - for (int i=0;im_robotMultiBodyData.size();i++) - { - delete m_data->m_robotMultiBodyData[i]; - } - m_data->m_robotMultiBodyData.clear(); - - m_data->m_jointInfo.clear(); - break; - } - case CMD_DEBUG_LINES_COMPLETED: - { - if (m_data->m_verboseOutput) - { - b3Printf("Success receiving %d debug lines",serverCmd.m_sendDebugLinesArgs.m_numDebugLines); - } - - int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines; - float* linesFrom = (float*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]; - float* linesTo = (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*3*sizeof(float)); - float* linesColor = (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*3*sizeof(float)); - - m_data->m_debugLinesFrom.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+numLines); - m_data->m_debugLinesTo.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+numLines); - m_data->m_debugLinesColor.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+numLines); - - for (int i=0;im_debugLinesFrom[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = from; - m_data->m_debugLinesTo[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = to; - m_data->m_debugLinesColor[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = color; - } - - break; - } - case CMD_DEBUG_LINES_OVERFLOW_FAILED: - { - b3Warning("Error receiving debug lines"); - m_data->m_debugLinesFrom.resize(0); - m_data->m_debugLinesTo.resize(0); - m_data->m_debugLinesColor.resize(0); - - break; - } - - default: - { - b3Error("Unknown server status\n"); - btAssert(0); - } - }; - - - m_data->m_testBlock1->m_numProcessedServerCommands++; - //we don't have more than 1 command outstanding (in total, either server or client) - btAssert(m_data->m_testBlock1->m_numProcessedServerCommands == m_data->m_testBlock1->m_numServerCommands); - - if (m_data->m_testBlock1->m_numServerCommands == m_data->m_testBlock1->m_numProcessedServerCommands) - { - m_data->m_waitingForServer = false; - } else - { - m_data->m_waitingForServer = true; - } - - if ((serverCmd.m_type == CMD_DEBUG_LINES_COMPLETED) && (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines>0)) - { - SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; - - //continue requesting debug lines for drawing - command.m_type =CMD_REQUEST_DEBUG_LINES; - command.m_requestDebugLinesArguments.m_startingLineIndex = serverCmd.m_sendDebugLinesArgs.m_numDebugLines+serverCmd.m_sendDebugLinesArgs.m_startingLineIndex; - submitClientCommand(command); - return 0; - } - - return &m_data->m_lastServerStatus; - - } else - { - if (m_data->m_verboseOutput) - { - b3Printf("m_numServerStatus = %d, processed = %d\n", m_data->m_testBlock1->m_numServerCommands, - m_data->m_testBlock1->m_numProcessedServerCommands); - } - } - return 0; -} - -bool PhysicsClientSharedMemory::canSubmitCommand() const -{ - return (m_data->m_isConnected && !m_data->m_waitingForServer); -} - -struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() -{ - return &m_data->m_testBlock1->m_clientCommands[0]; -} - -bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& command) -{ - ///at the moment we allow a maximum of 1 outstanding command, so we check for this - //once the server processed the command and returns a status, we clear the flag "m_data->m_waitingForServer" and allow submitting the next command - btAssert(!m_data->m_waitingForServer); - - if (!m_data->m_waitingForServer) - { - if (&m_data->m_testBlock1->m_clientCommands[0] != &command) - { - m_data->m_testBlock1->m_clientCommands[0] = command; - } - m_data->m_testBlock1->m_numClientCommands++; - m_data->m_waitingForServer = true; - return true; - } - return false; -} - -void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, int len) -{ - btAssert(len=SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) - { - b3Warning("uploadBulletFileToSharedMemory %d exceeds max size %d\n",len,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - } else - { - for (int i=0;im_testBlock1->m_bulletStreamDataClientToServer[i] = data[i]; - } - } -} - - -const float* PhysicsClientSharedMemory::getDebugLinesFrom() const -{ - if (m_data->m_debugLinesFrom.size()) - { - return &m_data->m_debugLinesFrom[0].m_x; - } - return 0; -} -const float* PhysicsClientSharedMemory::getDebugLinesTo() const -{ - if (m_data->m_debugLinesTo.size()) - { - return &m_data->m_debugLinesTo[0].m_x; - } - return 0; -} -const float* PhysicsClientSharedMemory::getDebugLinesColor() const -{ - if (m_data->m_debugLinesColor.size()) - { - return &m_data->m_debugLinesColor[0].m_x; - } - return 0; -} -int PhysicsClientSharedMemory::getNumDebugLines() const -{ - return m_data->m_debugLinesFrom.size(); -} +PhysicsClient::~PhysicsClient() {} diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 1b9f8dd52..9090596b8 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -4,46 +4,39 @@ //#include "SharedMemoryCommands.h" #include "LinearMath/btVector3.h" -class PhysicsClientSharedMemory -{ - struct PhysicsClientSharedMemoryInternalData* m_data; -protected: - +class PhysicsClient { public: + virtual ~PhysicsClient(); - PhysicsClientSharedMemory(); - virtual ~PhysicsClientSharedMemory(); + // return true if connection succesfull, can also check 'isConnected' + virtual bool connect() = 0; - //return true if connection succesfull, can also check 'isConnected' - virtual bool connect(); - - virtual void disconnectSharedMemory (); + virtual void disconnectSharedMemory() = 0; - virtual bool isConnected() const; + virtual bool isConnected() const = 0; - // return true if there is a status, and fill in 'serverStatus' - virtual const struct SharedMemoryStatus* processServerStatus(); - - virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand(); - - virtual bool canSubmitCommand() const; - - virtual bool submitClientCommand(const struct SharedMemoryCommand& command); + // return non-null if there is a status, nullptr otherwise + virtual const struct SharedMemoryStatus* processServerStatus() = 0; - virtual int getNumJoints() const; - - virtual void getJointInfo(int index, struct b3JointInfo& info) const; - - virtual void setSharedMemoryKey(int key); + virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand() = 0; - virtual void uploadBulletFileToSharedMemory(const char* data, int len); + virtual bool canSubmitCommand() const = 0; - virtual int getNumDebugLines() const; + virtual bool submitClientCommand(const struct SharedMemoryCommand& command) = 0; - virtual const float* getDebugLinesFrom() const; - virtual const float* getDebugLinesTo() const; - virtual const float* getDebugLinesColor() const; + virtual int getNumJoints() const = 0; + virtual void getJointInfo(int index, struct b3JointInfo& info) const = 0; + + virtual void setSharedMemoryKey(int key) = 0; + + virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 0; + + virtual int getNumDebugLines() const = 0; + + virtual const float* getDebugLinesFrom() const = 0; + virtual const float* getDebugLinesTo() const = 0; + virtual const float* getDebugLinesColor() const = 0; }; -#endif //BT_PHYSICS_CLIENT_API_H +#endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index ad8cb6aea..62e75e042 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1,5 +1,5 @@ #include "PhysicsClientC_API.h" -#include "PhysicsClient.h" +#include "PhysicsClientSharedMemory.h" #include "Bullet3Common/b3Scalar.h" #include #include "SharedMemoryCommands.h" @@ -8,7 +8,7 @@ b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); @@ -70,7 +70,7 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -101,7 +101,7 @@ int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -113,7 +113,7 @@ b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle ph b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -128,7 +128,7 @@ b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHand b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int controlMode) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -139,11 +139,11 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle phy return (b3SharedMemoryCommandHandle) command; } -int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) +int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); - command->m_sendDesiredStateCommandArgument.m_desiredStateQ[dofIndex] = value; + command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value; return 0; } @@ -191,7 +191,7 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -200,9 +200,22 @@ b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandl return (b3SharedMemoryCommandHandle) command; } +void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + b3Assert(status); + b3JointInfo info; + b3GetJointInfo(physClient, jointIndex, &info); + state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex]; + state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex]; + for (int ii(0); ii < 6; ++ii) { + state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii]; + } +} + b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -257,7 +270,7 @@ int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -310,13 +323,13 @@ b3PhysicsClientHandle b3ConnectSharedMemory(int key) void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; delete cl; } b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; const SharedMemoryStatus* stat = cl->processServerStatus(); return (b3SharedMemoryStatusHandle) stat; @@ -339,14 +352,14 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle) int b3CanSubmitCommand(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; return (int)cl->canSubmitCommand(); } int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; return (int)cl->submitClientCommand(*command); } @@ -368,22 +381,58 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan int b3GetNumJoints(b3PhysicsClientHandle physClient) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; return cl->getNumJoints(); } void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3JointInfo* info) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; cl->getJointInfo(linkIndex,*info); } +int b3PickBody(struct SharedMemoryCommand *command, + double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, + double rayToWorldX, double rayToWorldY, double rayToWorldZ) +{ + b3Assert(command); + b3Assert(command->m_type == CMD_PICK_BODY); + command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX; + command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY; + command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ; + command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX; + command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY; + command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ; + return 0; +} +int b3MovePickedBody(struct SharedMemoryCommand *command, + double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, + double rayToWorldX, double rayToWorldY, double rayToWorldZ) +{ + b3Assert(command); + b3Assert(command->m_type == CMD_MOVE_PICKED_BODY); + command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX; + command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY; + command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ; + command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX; + command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY; + command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ; + return 0; +} + +int b3RemovePickingConstraint(b3SharedMemoryCommandHandle commandHandle) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REMOVE_PICKING_CONSTRAINT_BODY); + return 0; +} b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -396,7 +445,7 @@ b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle } void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(lines); if (lines) @@ -409,4 +458,3 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l } } - diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 201c92f1d..7e6c3c771 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -63,11 +63,11 @@ int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, ///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE) b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode); ///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD -int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); +int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value); int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); //Only use when controlMode is CONTROL_MODE_VELOCITY -int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); +int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); ///Only use if when controlMode is CONTROL_MODE_TORQUE, int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); @@ -85,11 +85,19 @@ int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient); -int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int dofIndex, int enable); +int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable); int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable); b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient); +void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state); +int b3PickBody(struct SharedMemoryCommand *command, + double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, + double rayToWorldX, double rayToWorldY, double rayToWorldZ); +int b3MovePickedBody(struct SharedMemoryCommand *command, + double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, + double rayToWorldX, double rayToWorldY, double rayToWorldZ); +int b3RemovePickingConstraint(struct SharedMemoryCommand *command); #ifdef __cplusplus diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 9e2cfb7a5..adb8a6acb 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -5,7 +5,7 @@ #include "SharedMemoryCommon.h" #include "../CommonInterfaces/CommonParameterInterface.h" - +#include "PhysicsClientC_API.h" #include "PhysicsClient.h" //#include "SharedMemoryCommands.h" @@ -154,8 +154,6 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr) { cl->enqueueCommand(buttonId); } - - } void PhysicsClientExample::enqueueCommand(int commandId) @@ -177,9 +175,9 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) double startPosX = 0; double startPosY = 0; double startPosZ = 0; - int ret = b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ); + b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ); // ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1); - ret = b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp new file mode 100644 index 000000000..71bf35d4a --- /dev/null +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -0,0 +1,527 @@ +#include "PhysicsClientSharedMemory.h" +#include "PosixSharedMemory.h" +#include "Win32SharedMemory.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btVector3.h" + +#include "Bullet3Common/b3Logging.h" +#include "../Utils/b3ResourcePath.h" +#include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h" +#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h" +#include "SharedMemoryBlock.h" + +// copied from btMultiBodyLink.h +enum JointType { + eRevoluteType = 0, + ePrismaticType = 1, +}; + +struct TmpFloat3 { + float m_x; + float m_y; + float m_z; +}; + +TmpFloat3 CreateTmpFloat3(float x, float y, float z) { + TmpFloat3 tmp; + tmp.m_x = x; + tmp.m_y = y; + tmp.m_z = z; + return tmp; +} + +struct PhysicsClientSharedMemoryInternalData { + SharedMemoryInterface* m_sharedMemory; + SharedMemoryBlock* m_testBlock1; + + btAlignedObjectArray m_robotMultiBodyData; + btAlignedObjectArray m_jointInfo; + btAlignedObjectArray m_debugLinesFrom; + btAlignedObjectArray m_debugLinesTo; + btAlignedObjectArray m_debugLinesColor; + + SharedMemoryStatus m_lastServerStatus; + + int m_counter; + bool m_serverLoadUrdfOK; + bool m_isConnected; + bool m_waitingForServer; + bool m_hasLastServerStatus; + int m_sharedMemoryKey; + bool m_verboseOutput; + + PhysicsClientSharedMemoryInternalData() + : m_sharedMemory(0), + m_testBlock1(0), + m_counter(0), + m_serverLoadUrdfOK(false), + m_isConnected(false), + m_waitingForServer(false), + m_hasLastServerStatus(false), + m_sharedMemoryKey(SHARED_MEMORY_KEY), + m_verboseOutput(false) {} + + void processServerStatus(); + + bool canSubmitCommand() const; +}; + +int PhysicsClientSharedMemory::getNumJoints() const { return m_data->m_jointInfo.size(); } + +void PhysicsClientSharedMemory::getJointInfo(int index, b3JointInfo& info) const { + info = m_data->m_jointInfo[index]; +} + +PhysicsClientSharedMemory::PhysicsClientSharedMemory() + +{ + m_data = new PhysicsClientSharedMemoryInternalData; + +#ifdef _WIN32 + m_data->m_sharedMemory = new Win32SharedMemoryClient(); +#else + m_data->m_sharedMemory = new PosixSharedMemory(); +#endif +} + +PhysicsClientSharedMemory::~PhysicsClientSharedMemory() { + if (m_data->m_isConnected) { + disconnectSharedMemory(); + } + delete m_data->m_sharedMemory; + delete m_data; +} + +void PhysicsClientSharedMemory::setSharedMemoryKey(int key) { m_data->m_sharedMemoryKey = key; } + +void PhysicsClientSharedMemory::disconnectSharedMemory() { + if (m_data->m_isConnected) { + m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); + m_data->m_isConnected = false; + } +} + +bool PhysicsClientSharedMemory::isConnected() const { return m_data->m_isConnected; } + +bool PhysicsClientSharedMemory::connect() { + /// server always has to create and initialize shared memory + bool allowCreation = false; + m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory( + m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE, allowCreation); + + if (m_data->m_testBlock1) { + if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) { + b3Error("Error: please start server before client\n"); + m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, + SHARED_MEMORY_SIZE); + m_data->m_testBlock1 = 0; + return false; + } else { + if (m_data->m_verboseOutput) { + b3Printf("Connected to existing shared memory, status OK.\n"); + } + m_data->m_isConnected = true; + } + } else { + b3Error("Cannot connect to shared memory"); + return false; + } + return true; +} + +const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { + SharedMemoryStatus* stat = 0; + + if (!m_data->m_testBlock1) { + return 0; + } + + if (!m_data->m_waitingForServer) { + return 0; + } + + if (m_data->m_testBlock1->m_numServerCommands > + m_data->m_testBlock1->m_numProcessedServerCommands) { + btAssert(m_data->m_testBlock1->m_numServerCommands == + m_data->m_testBlock1->m_numProcessedServerCommands + 1); + + const SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0]; + m_data->m_lastServerStatus = serverCmd; + + EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type; + // consume the command + switch (serverCmd.m_type) { + case CMD_CLIENT_COMMAND_COMPLETED: { + if (m_data->m_verboseOutput) { + b3Printf("Server completed command"); + } + break; + } + case CMD_URDF_LOADING_COMPLETED: { + m_data->m_serverLoadUrdfOK = true; + if (m_data->m_verboseOutput) { + b3Printf("Server loading the URDF OK\n"); + } + + if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0) { + bParse::btBulletFile* bf = new bParse::btBulletFile( + this->m_data->m_testBlock1->m_bulletStreamDataServerToClient, + serverCmd.m_dataStreamArguments.m_streamChunkLength); + bf->setFileDNAisMemoryDNA(); + bf->parse(false); + m_data->m_robotMultiBodyData.push_back(bf); + + for (int i = 0; i < bf->m_multiBodies.size(); i++) { + int flag = bf->getFlags(); + int qOffset = 7; + int uOffset = 6; + + if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) { + Bullet::btMultiBodyDoubleData* mb = + (Bullet::btMultiBodyDoubleData*)bf->m_multiBodies[i]; + if (mb->m_baseName) { + if (m_data->m_verboseOutput) { + b3Printf("mb->m_baseName = %s\n", mb->m_baseName); + } + } + + for (int link = 0; link < mb->m_numLinks; link++) { + { + b3JointInfo info; + info.m_flags = 0; + info.m_jointIndex = link; + info.m_qIndex = + (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1; + info.m_uIndex = + (0 < mb->m_links[link].m_dofCount) ? uOffset : -1; + + if (mb->m_links[link].m_linkName) { + if (m_data->m_verboseOutput) { + b3Printf("mb->m_links[%d].m_linkName = %s\n", link, + mb->m_links[link].m_linkName); + } + info.m_linkName = mb->m_links[link].m_linkName; + } + if (mb->m_links[link].m_jointName) { + if (m_data->m_verboseOutput) { + b3Printf("mb->m_links[%d].m_jointName = %s\n", link, + mb->m_links[link].m_jointName); + } + info.m_jointName = mb->m_links[link].m_jointName; + info.m_jointType = mb->m_links[link].m_jointType; + } + if ((mb->m_links[link].m_jointType == eRevoluteType) || + (mb->m_links[link].m_jointType == ePrismaticType)) { + info.m_flags |= JOINT_HAS_MOTORIZED_POWER; + } + m_data->m_jointInfo.push_back(info); + } + qOffset += mb->m_links[link].m_posVarCount; + uOffset += mb->m_links[link].m_dofCount; + } + + } else { + Bullet::btMultiBodyFloatData* mb = + (Bullet::btMultiBodyFloatData*)bf->m_multiBodies[i]; + if (mb->m_baseName) { + if (m_data->m_verboseOutput) { + b3Printf("mb->m_baseName = %s\n", mb->m_baseName); + } + } + for (int link = 0; link < mb->m_numLinks; link++) { + { + b3JointInfo info; + info.m_flags = 0; + info.m_jointIndex = link; + info.m_qIndex = + (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1; + info.m_uIndex = + (0 < mb->m_links[link].m_dofCount) ? uOffset : -1; + + if (mb->m_links[link].m_linkName) { + if (m_data->m_verboseOutput) { + b3Printf("mb->m_links[%d].m_linkName = %s\n", link, + mb->m_links[link].m_linkName); + } + info.m_linkName = mb->m_links[link].m_linkName; + } + if (mb->m_links[link].m_jointName) { + if (m_data->m_verboseOutput) { + b3Printf("mb->m_links[%d].m_jointName = %s\n", link, + mb->m_links[link].m_jointName); + } + info.m_jointName = mb->m_links[link].m_jointName; + info.m_jointType = mb->m_links[link].m_jointType; + } + if ((mb->m_links[link].m_jointType == eRevoluteType) || + (mb->m_links[link].m_jointType == ePrismaticType)) { + info.m_flags |= JOINT_HAS_MOTORIZED_POWER; + } + m_data->m_jointInfo.push_back(info); + } + qOffset += mb->m_links[link].m_posVarCount; + uOffset += mb->m_links[link].m_dofCount; + } + } + } + if (bf->ok()) { + if (m_data->m_verboseOutput) { + b3Printf("Received robot description ok!\n"); + } + } else { + b3Warning("Robot description not received"); + } + } + break; + } + case CMD_DESIRED_STATE_RECEIVED_COMPLETED: { + if (m_data->m_verboseOutput) { + b3Printf("Server received desired state"); + } + break; + } + case CMD_STEP_FORWARD_SIMULATION_COMPLETED: { + if (m_data->m_verboseOutput) { + b3Printf("Server completed step simulation"); + } + break; + } + case CMD_URDF_LOADING_FAILED: { + if (m_data->m_verboseOutput) { + b3Printf("Server failed loading the URDF...\n"); + } + m_data->m_serverLoadUrdfOK = false; + break; + } + + case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED: { + if (m_data->m_verboseOutput) { + b3Printf("Server received bullet data stream OK\n"); + } + + break; + } + case CMD_BULLET_DATA_STREAM_RECEIVED_FAILED: { + if (m_data->m_verboseOutput) { + b3Printf("Server failed receiving bullet data stream\n"); + } + + break; + } + + case CMD_ACTUAL_STATE_UPDATE_COMPLETED: { + if (m_data->m_verboseOutput) { + b3Printf("Received actual state\n"); + } + SharedMemoryStatus& command = m_data->m_testBlock1->m_serverCommands[0]; + + int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ; + int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU; + if (m_data->m_verboseOutput) { + b3Printf("size Q = %d, size U = %d\n", numQ, numU); + } + char msg[1024]; + + { + sprintf(msg, "Q=["); + + for (int i = 0; i < numQ; i++) { + if (i < numQ - 1) { + sprintf(msg, "%s%f,", msg, + command.m_sendActualStateArgs.m_actualStateQ[i]); + } else { + sprintf(msg, "%s%f", msg, + command.m_sendActualStateArgs.m_actualStateQ[i]); + } + } + sprintf(msg, "%s]", msg); + } + if (m_data->m_verboseOutput) { + b3Printf(msg); + } + + { + sprintf(msg, "U=["); + + for (int i = 0; i < numU; i++) { + if (i < numU - 1) { + sprintf(msg, "%s%f,", msg, + command.m_sendActualStateArgs.m_actualStateQdot[i]); + } else { + sprintf(msg, "%s%f", msg, + command.m_sendActualStateArgs.m_actualStateQdot[i]); + } + } + sprintf(msg, "%s]", msg); + } + if (m_data->m_verboseOutput) { + b3Printf(msg); + } + + if (m_data->m_verboseOutput) { + b3Printf("\n"); + } + break; + } + case CMD_RESET_SIMULATION_COMPLETED: { + if (m_data->m_verboseOutput) { + b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n"); + } + for (int i = 0; i < m_data->m_robotMultiBodyData.size(); i++) { + delete m_data->m_robotMultiBodyData[i]; + } + m_data->m_robotMultiBodyData.clear(); + + m_data->m_jointInfo.clear(); + break; + } + case CMD_DEBUG_LINES_COMPLETED: { + if (m_data->m_verboseOutput) { + b3Printf("Success receiving %d debug lines", + serverCmd.m_sendDebugLinesArgs.m_numDebugLines); + } + + int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines; + float* linesFrom = + (float*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]; + float* linesTo = + (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0] + + numLines * 3 * sizeof(float)); + float* linesColor = + (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0] + + 2 * numLines * 3 * sizeof(float)); + + m_data->m_debugLinesFrom.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + + numLines); + m_data->m_debugLinesTo.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + + numLines); + m_data->m_debugLinesColor.resize( + serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + numLines); + + for (int i = 0; i < numLines; i++) { + TmpFloat3 from = CreateTmpFloat3(linesFrom[i * 3], linesFrom[i * 3 + 1], + linesFrom[i * 3 + 2]); + TmpFloat3 to = + CreateTmpFloat3(linesTo[i * 3], linesTo[i * 3 + 1], linesTo[i * 3 + 2]); + TmpFloat3 color = CreateTmpFloat3(linesColor[i * 3], linesColor[i * 3 + 1], + linesColor[i * 3 + 2]); + + m_data + ->m_debugLinesFrom[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] = + from; + m_data->m_debugLinesTo[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] = + to; + m_data->m_debugLinesColor[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + + i] = color; + } + + break; + } + case CMD_DEBUG_LINES_OVERFLOW_FAILED: { + b3Warning("Error receiving debug lines"); + m_data->m_debugLinesFrom.resize(0); + m_data->m_debugLinesTo.resize(0); + m_data->m_debugLinesColor.resize(0); + + break; + } + + default: { + b3Error("Unknown server status\n"); + btAssert(0); + } + }; + + m_data->m_testBlock1->m_numProcessedServerCommands++; + // we don't have more than 1 command outstanding (in total, either server or client) + btAssert(m_data->m_testBlock1->m_numProcessedServerCommands == + m_data->m_testBlock1->m_numServerCommands); + + if (m_data->m_testBlock1->m_numServerCommands == + m_data->m_testBlock1->m_numProcessedServerCommands) { + m_data->m_waitingForServer = false; + } else { + m_data->m_waitingForServer = true; + } + + if ((serverCmd.m_type == CMD_DEBUG_LINES_COMPLETED) && + (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0)) { + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + + // continue requesting debug lines for drawing + command.m_type = CMD_REQUEST_DEBUG_LINES; + command.m_requestDebugLinesArguments.m_startingLineIndex = + serverCmd.m_sendDebugLinesArgs.m_numDebugLines + + serverCmd.m_sendDebugLinesArgs.m_startingLineIndex; + submitClientCommand(command); + return 0; + } + + return &m_data->m_lastServerStatus; + + } else { + if (m_data->m_verboseOutput) { + b3Printf("m_numServerStatus = %d, processed = %d\n", + m_data->m_testBlock1->m_numServerCommands, + m_data->m_testBlock1->m_numProcessedServerCommands); + } + } + return 0; +} + +bool PhysicsClientSharedMemory::canSubmitCommand() const { + return (m_data->m_isConnected && !m_data->m_waitingForServer); +} + +struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() { + return &m_data->m_testBlock1->m_clientCommands[0]; +} + +bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& command) { + /// at the moment we allow a maximum of 1 outstanding command, so we check for this + // once the server processed the command and returns a status, we clear the flag + // "m_data->m_waitingForServer" and allow submitting the next command + btAssert(!m_data->m_waitingForServer); + + if (!m_data->m_waitingForServer) { + if (&m_data->m_testBlock1->m_clientCommands[0] != &command) { + m_data->m_testBlock1->m_clientCommands[0] = command; + } + m_data->m_testBlock1->m_numClientCommands++; + m_data->m_waitingForServer = true; + return true; + } + return false; +} + +void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, int len) { + btAssert(len < SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + if (len >= SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) { + b3Warning("uploadBulletFileToSharedMemory %d exceeds max size %d\n", len, + SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } else { + for (int i = 0; i < len; i++) { + m_data->m_testBlock1->m_bulletStreamDataClientToServer[i] = data[i]; + } + } +} + +const float* PhysicsClientSharedMemory::getDebugLinesFrom() const { + if (m_data->m_debugLinesFrom.size()) { + return &m_data->m_debugLinesFrom[0].m_x; + } + return 0; +} +const float* PhysicsClientSharedMemory::getDebugLinesTo() const { + if (m_data->m_debugLinesTo.size()) { + return &m_data->m_debugLinesTo[0].m_x; + } + return 0; +} +const float* PhysicsClientSharedMemory::getDebugLinesColor() const { + if (m_data->m_debugLinesColor.size()) { + return &m_data->m_debugLinesColor[0].m_x; + } + return 0; +} +int PhysicsClientSharedMemory::getNumDebugLines() const { return m_data->m_debugLinesFrom.size(); } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h new file mode 100644 index 000000000..fb5e10181 --- /dev/null +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -0,0 +1,48 @@ +#ifndef BT_PHYSICS_CLIENT_SHARED_MEMORY_API_H +#define BT_PHYSICS_CLIENT_SHARED_MEMORY_API_H + +#include "PhysicsClient.h" + +//#include "SharedMemoryCommands.h" +#include "LinearMath/btVector3.h" + +class PhysicsClientSharedMemory : public PhysicsClient { + struct PhysicsClientSharedMemoryInternalData* m_data; + +protected: +public: + PhysicsClientSharedMemory(); + virtual ~PhysicsClientSharedMemory(); + + // return true if connection succesfull, can also check 'isConnected' + virtual bool connect(); + + virtual void disconnectSharedMemory(); + + virtual bool isConnected() const; + + // return non-null if there is a status, nullptr otherwise + virtual const struct SharedMemoryStatus* processServerStatus(); + + virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand(); + + virtual bool canSubmitCommand() const; + + virtual bool submitClientCommand(const struct SharedMemoryCommand& command); + + virtual int getNumJoints() const; + + virtual void getJointInfo(int index, struct b3JointInfo& info) const; + + virtual void setSharedMemoryKey(int key); + + virtual 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 // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 7d64dbfc9..1aabe081d 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -1067,8 +1067,20 @@ void PhysicsServerSharedMemory::processClientCommands() { b3Printf("Server Init Pose not implemented yet"); } - ///@todo: implement this - m_data->m_dynamicsWorld->setGravity(btVector3(0,0,0)); + int body_unique_id = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId; + if (m_data->m_dynamicsWorld->getNumMultibodies()>body_unique_id) + { + btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(body_unique_id); + mb->setBasePos(btVector3( + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[0], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[1], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[2])); + mb->setWorldToBaseRot(btQuaternion( + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[3], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[4], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[5], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[6])); + } SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(serverCmd); @@ -1136,6 +1148,46 @@ void PhysicsServerSharedMemory::processClientCommands() break; } + case CMD_PICK_BODY: + { + pickBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0], + clientCmd.m_pickBodyArguments.m_rayFromWorld[1], + clientCmd.m_pickBodyArguments.m_rayFromWorld[2]), + btVector3(clientCmd.m_pickBodyArguments.m_rayToWorld[0], + clientCmd.m_pickBodyArguments.m_rayToWorld[1], + clientCmd.m_pickBodyArguments.m_rayToWorld[2])); + + SharedMemoryStatus &serverCmd = + m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED, + clientCmd.m_sequenceNumber, timeStamp); + m_data->submitServerStatus(serverCmd); + break; + } + case CMD_MOVE_PICKED_BODY: + { + movePickedBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0], + clientCmd.m_pickBodyArguments.m_rayFromWorld[1], + clientCmd.m_pickBodyArguments.m_rayFromWorld[2]), + btVector3(clientCmd.m_pickBodyArguments.m_rayToWorld[0], + clientCmd.m_pickBodyArguments.m_rayToWorld[1], + clientCmd.m_pickBodyArguments.m_rayToWorld[2])); + + SharedMemoryStatus &serverCmd = + m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED, + clientCmd.m_sequenceNumber, timeStamp); + m_data->submitServerStatus(serverCmd); + break; + } + case CMD_REMOVE_PICKING_CONSTRAINT_BODY: + { + removePickingConstraint(); + + SharedMemoryStatus &serverCmd = + m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED, + clientCmd.m_sequenceNumber, timeStamp); + m_data->submitServerStatus(serverCmd); + break; + } default: { b3Error("Unknown command encountered"); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 9c8d2cb1d..40343d08c 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -25,7 +25,6 @@ #endif - #define SHARED_MEMORY_SERVER_TEST_C #define MAX_DEGREE_OF_FREEDOM 256 #define MAX_NUM_SENSORS 256 @@ -92,6 +91,12 @@ struct SendDebugLinesArgs int m_numRemainingDebugLines; }; +struct PickBodyArgs +{ + double m_rayFromWorld[3]; + double m_rayToWorld[3]; +}; + ///Controlling a robot involves sending the desired state to its joint motor controllers. ///The control mode determines the state variables used for motor control. @@ -225,6 +230,7 @@ struct SharedMemoryCommand struct CreateSensorArgs m_createSensorArguments; struct CreateBoxShapeArgs m_createBoxShapeArguments; struct RequestDebugLinesArgs m_requestDebugLinesArguments; + struct PickBodyArgs m_pickBodyArguments; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index c73670b70..fcdd9ff73 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -20,6 +20,9 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_DEBUG_LINES, CMD_STEP_FORWARD_SIMULATION, CMD_RESET_SIMULATION, + CMD_PICK_BODY, + CMD_MOVE_PICKED_BODY, + CMD_REMOVE_PICKING_CONSTRAINT_BODY, CMD_MAX_CLIENT_COMMANDS }; @@ -61,10 +64,17 @@ struct b3JointInfo int m_jointType; int m_qIndex; int m_uIndex; - /// + int m_jointIndex; int m_flags; }; +struct b3JointSensorState +{ + double m_jointPosition; + double m_jointVelocity; + double m_jointForceTorque[6]; /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */ +}; + struct b3DebugLines { int m_numDebugLines; diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index 63a5d0e90..c93a4250a 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -17,6 +17,7 @@ language "C++" files { "PhysicsClient.cpp", + "PhysicsClientSharedMemory.cpp", "PhysicsClientExample.cpp", "PhysicsServerExample.cpp", "main.cpp", diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index de781f51e..aa71e7180 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -14,6 +14,8 @@ "test.c", "../../examples/SharedMemory/PhysicsClient.cpp", "../../examples/SharedMemory/PhysicsClient.h", + "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp", + "../../examples/SharedMemory/PhysicsClientSharedMemory.h", "../../examples/SharedMemory/PhysicsClientC_API.cpp", "../../examples/SharedMemory/PhysicsClientC_API.h", "../../examples/SharedMemory/Win32SharedMemory.cpp",