diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 3bbf75bdf..d99f7c1b2 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -23,6 +23,17 @@ SET(App_ExampleBrowser_SRCS ../SharedMemory/PhysicsClientExample.cpp ../SharedMemory/PosixSharedMemory.cpp ../SharedMemory/Win32SharedMemory.cpp + ../SharedMemory/PhysicsServerSharedMemory.cpp + ../SharedMemory/PhysicsDirect.cpp + ../SharedMemory/PhysicsDirect.h + ../SharedMemory/PhysicsDirectC_API.cpp + ../SharedMemory/PhysicsDirectC_API.h + ../SharedMemory/PhysicsLoopBack.cpp + ../SharedMemory/PhysicsLoopBack.h + ../SharedMemory/PhysicsLoopBackC_Api.cpp + ../SharedMemory/PhysicsLoopBackC_Api.h + ../SharedMemory/PhysicsServerCommandProcessor.cpp + ../SharedMemory/PhysicsServerCommandProcessor.h ../BasicDemo/BasicExample.cpp ../BasicDemo/BasicExample.h ../ForkLift/ForkLiftDemo.cpp diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index d1503e05f..5554c1a65 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -57,10 +57,21 @@ "../SharedMemory/PhysicsServerExample.cpp", "../SharedMemory/PhysicsClientExample.cpp", "../SharedMemory/PhysicsServer.cpp", + "../SharedMemory/PhysicsServerSharedMemory.cpp", "../SharedMemory/PhysicsClientSharedMemory.cpp", "../SharedMemory/PhysicsClient.cpp", "../SharedMemory/PosixSharedMemory.cpp", "../SharedMemory/Win32SharedMemory.cpp", + "../SharedMemory/PhysicsDirect.cpp", + "../SharedMemory/PhysicsDirect.h", + "../SharedMemory/PhysicsDirectC_API.cpp", + "../SharedMemory/PhysicsDirectC_API.h", + "../SharedMemory/PhysicsLoopBack.cpp", + "../SharedMemory/PhysicsLoopBack.h", + "../SharedMemory/PhysicsLoopBackC_Api.cpp", + "../SharedMemory/PhysicsLoopBackC_Api.h", + "../SharedMemory/PhysicsServerCommandProcessor.cpp", + "../SharedMemory/PhysicsServerCommandProcessor.h", "../MultiThreading/MultiThreadingExample.cpp", "../MultiThreading/b3PosixThreadSupport.cpp", "../MultiThreading/b3Win32ThreadSupport.cpp", diff --git a/examples/SharedMemory/BodyJointInfoUtility.h b/examples/SharedMemory/BodyJointInfoUtility.h new file mode 100644 index 000000000..ca0c1bf49 --- /dev/null +++ b/examples/SharedMemory/BodyJointInfoUtility.h @@ -0,0 +1,75 @@ +#ifndef BODY_JOINT_INFO_UTILITY_H +#define BODY_JOINT_INFO_UTILITY_H + +#include "Bullet3Common/b3Logging.h" + +namespace Bullet +{ + class btMultiBodyDoubleData; + class btMultiBodyFloatData; +}; + +inline char* strDup(const char* const str) +{ +#ifdef _WIN32 + return _strdup(str); +#else + return strdup(str); +#endif +} + +template void addJointInfoFromMultiBodyData(const T* mb, U* bodyJoints, bool verboseOutput) +{ + if (mb->m_baseName) + { + if (verboseOutput) + { + b3Printf("mb->m_baseName = %s\n", mb->m_baseName); + } + } + int qOffset = 7; + int uOffset = 6; + + 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 (verboseOutput) { + b3Printf("mb->m_links[%d].m_linkName = %s\n", link, + mb->m_links[link].m_linkName); + } + info.m_linkName = strDup(mb->m_links[link].m_linkName); + } + if (mb->m_links[link].m_jointName) { + if (verboseOutput) { + b3Printf("mb->m_links[%d].m_jointName = %s\n", link, + mb->m_links[link].m_jointName); + } + info.m_jointName = strDup(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; + } + bodyJoints->m_jointInfo.push_back(info); + } + qOffset += mb->m_links[link].m_posVarCount; + uOffset += mb->m_links[link].m_dofCount; + } + +} + + + +#endif //BODY_JOINT_INFO_UTILITY_H \ No newline at end of file diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index a17c40c0a..d069c40a5 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -8,6 +8,8 @@ #include "PhysicsClientC_API.h" #include "PhysicsClient.h" //#include "SharedMemoryCommands.h" +#include "PhysicsLoopBackC_API.h" +#include "PhysicsDirectC_API.h" #include "PhysicsClientC_API.h" @@ -444,7 +446,10 @@ void PhysicsClientExample::initPhysics() m_selectedBody = -1; m_prevSelectedBody = -1; - m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey); + //m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey); + m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY); + //m_physicsClientHandle = b3ConnectPhysicsDirect(); + if (!b3CanSubmitCommand(m_physicsClientHandle)) { b3Warning("Cannot connect to physics client"); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 7c435da2d..35d7d8227 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -10,26 +10,11 @@ #include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h" #include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h" #include "SharedMemoryBlock.h" +#include "BodyJointInfoUtility.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 BodyJointInfoCache { @@ -72,14 +57,8 @@ struct PhysicsClientSharedMemoryInternalData { bool canSubmitCommand() const; }; -static char* strDup(const char* const str) -{ -#ifdef _WIN32 - return _strdup(str); -#else - return strdup(str); -#endif -} + + int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const { @@ -184,6 +163,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type; // consume the command + switch (serverCmd.m_type) { case CMD_CLIENT_COMMAND_COMPLETED: { if (m_data->m_verboseOutput) { @@ -199,7 +179,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0) { bParse::btBulletFile bf( - this->m_data->m_testBlock1->m_bulletStreamDataServerToClient, + this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor, serverCmd.m_dataStreamArguments.m_streamChunkLength); bf.setFileDNAisMemoryDNA(); bf.parse(false); @@ -209,98 +189,21 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { m_data->m_bodyJointMap.insert(bodyIndex,bodyJoints); for (int i = 0; i < bf.m_multiBodies.size(); i++) { - int flag = bf.getFlags(); - int qOffset = 7; - int uOffset = 6; + + int flag = bf.getFlags(); + 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 = strDup(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 = strDup(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; - } - bodyJoints->m_jointInfo.push_back(info); - } - qOffset += mb->m_links[link].m_posVarCount; - uOffset += mb->m_links[link].m_dofCount; - } - - } else { + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } 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 = strDup(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 = strDup(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; - } - bodyJoints->m_jointInfo.push_back(info); - } - qOffset += mb->m_links[link].m_posVarCount; - uOffset += mb->m_links[link].m_dofCount; - } + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); } } if (bf.ok()) { @@ -406,6 +309,9 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { if (m_data->m_verboseOutput) { b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n"); } + m_data->m_debugLinesFrom.clear(); + m_data->m_debugLinesTo.clear(); + m_data->m_debugLinesColor.clear(); for (int i=0;im_bodyJointMap.size();i++) { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); @@ -437,12 +343,12 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines; float* linesFrom = - (float*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]; + (float*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0]; float* linesTo = - (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0] + + (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0] + numLines * 3 * sizeof(float)); float* linesColor = - (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0] + + (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0] + 2 * numLines * 3 * sizeof(float)); m_data->m_debugLinesFrom.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp new file mode 100644 index 000000000..f6f0adf90 --- /dev/null +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -0,0 +1,330 @@ +#include "PhysicsDirect.h" + +#include "PhysicsClientSharedMemory.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "SharedMemoryCommands.h" +#include "PhysicsServerCommandProcessor.h" +#include "LinearMath/btHashMap.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h" +#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h" +#include "BodyJointInfoUtility.h" + + +struct BodyJointInfoCache2 +{ + btAlignedObjectArray m_jointInfo; +}; + +struct PhysicsDirectInternalData +{ + DummyGUIHelper m_noGfx; + + SharedMemoryCommand m_command; + SharedMemoryStatus m_serverStatus; + bool m_hasStatus; + bool m_verboseOutput; + + btAlignedObjectArray m_debugLinesFrom; + btAlignedObjectArray m_debugLinesTo; + btAlignedObjectArray m_debugLinesColor; + + btHashMap m_bodyJointMap; + + char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; + + PhysicsServerCommandProcessor* m_commandProcessor; + + PhysicsDirectInternalData() + :m_hasStatus(false), + m_verboseOutput(false) + { + } +}; + +PhysicsDirect::PhysicsDirect() +{ + m_data = new PhysicsDirectInternalData; + m_data->m_commandProcessor = new PhysicsServerCommandProcessor; + + +} + +PhysicsDirect::~PhysicsDirect() +{ + delete m_data->m_commandProcessor; + delete m_data; +} + + +// return true if connection succesfull, can also check 'isConnected' +bool PhysicsDirect::connect() +{ + m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx); + return true; +} + +////todo: rename to 'disconnect' +void PhysicsDirect::disconnectSharedMemory() +{ + m_data->m_commandProcessor->setGuiHelper(0); +} + +bool PhysicsDirect::isConnected() const +{ + return true; +} + +// return non-null if there is a status, nullptr otherwise +const SharedMemoryStatus* PhysicsDirect::processServerStatus() +{ + SharedMemoryStatus* stat = 0; + if (m_data->m_hasStatus) + { + stat = &m_data->m_serverStatus; + m_data->m_hasStatus = false; + } + return stat; +} + +SharedMemoryCommand* PhysicsDirect::getAvailableSharedMemoryCommand() +{ + return &m_data->m_command; +} + +bool PhysicsDirect::canSubmitCommand() const +{ + return true; +} + +bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + + bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + btAssert(m_data->m_serverStatus.m_type == 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_bulletStreamDataServerToClient[0]; + float* linesTo = + (float*)(&m_data->m_bulletStreamDataServerToClient[0] + + numLines * 3 * sizeof(float)); + float* linesColor = + (float*)(&m_data->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; + } + + if (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0) + { + command.m_type = CMD_REQUEST_DEBUG_LINES; + command.m_requestDebugLinesArguments.m_startingLineIndex = + serverCmd.m_sendDebugLinesArgs.m_numDebugLines + + serverCmd.m_sendDebugLinesArgs.m_startingLineIndex; + } + } + + } while (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0); + + return m_data->m_hasStatus; +} + +bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command) +{ + if (command.m_type==CMD_REQUEST_DEBUG_LINES) + { + return processDebugLines(command); + } + + bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + switch (m_data->m_serverStatus.m_type) + { + case CMD_RESET_SIMULATION_COMPLETED: + { + m_data->m_debugLinesFrom.clear(); + m_data->m_debugLinesTo.clear(); + m_data->m_debugLinesColor.clear(); + for (int i=0;im_bodyJointMap.size();i++) + { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + for (int j=0;jm_jointInfo.size();j++) { + if (bodyJoints->m_jointInfo[j].m_jointName) + { + free(bodyJoints->m_jointInfo[j].m_jointName); + } + if (bodyJoints->m_jointInfo[j].m_linkName) + { + free(bodyJoints->m_jointInfo[j].m_linkName); + } + } + delete (*bodyJointsPtr); + } + } + m_data->m_bodyJointMap.clear(); + + break; + } + case CMD_URDF_LOADING_COMPLETED: + { + if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0) + { + bParse::btBulletFile bf( + &m_data->m_bulletStreamDataServerToClient[0], + serverCmd.m_dataStreamArguments.m_streamChunkLength); + bf.setFileDNAisMemoryDNA(); + bf.parse(false); + int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId; + + BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; + m_data->m_bodyJointMap.insert(bodyIndex,bodyJoints); + + for (int i = 0; i < bf.m_multiBodies.size(); i++) + { + int flag = bf.getFlags(); + if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) + { + Bullet::btMultiBodyDoubleData* mb = + (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; + + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } else + { + Bullet::btMultiBodyFloatData* mb = + (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; + + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } + } + if (bf.ok()) { + if (m_data->m_verboseOutput) + { + b3Printf("Received robot description ok!\n"); + } + } else + { + b3Warning("Robot description not received"); + } + } + break; + } + + default: + { + // b3Error("Unknown server status type"); + } + }; + + + } + return hasStatus; +} + +int PhysicsDirect::getNumJoints(int bodyIndex) const +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + return bodyJoints->m_jointInfo.size(); + } + btAssert(0); + return 0; +} + +void PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + info = bodyJoints->m_jointInfo[jointIndex]; + } +} + +///todo: move this out of the +void PhysicsDirect::setSharedMemoryKey(int key) +{ + //m_data->m_physicsServer->setSharedMemoryKey(key); + //m_data->m_physicsClient->setSharedMemoryKey(key); +} + +void PhysicsDirect::uploadBulletFileToSharedMemory(const char* data, int len) +{ + //m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len); +} + +int PhysicsDirect::getNumDebugLines() const +{ + return m_data->m_debugLinesFrom.size(); +} + +const float* PhysicsDirect::getDebugLinesFrom() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesFrom[0].m_x; + } + return 0; +} +const float* PhysicsDirect::getDebugLinesTo() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesTo[0].m_x; + } + return 0; +} +const float* PhysicsDirect::getDebugLinesColor() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesColor[0].m_x; + } + return 0; +} diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h new file mode 100644 index 000000000..bb84affdf --- /dev/null +++ b/examples/SharedMemory/PhysicsDirect.h @@ -0,0 +1,62 @@ +#ifndef PHYSICS_DIRECT_H +#define PHYSICS_DIRECT_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 ] +///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands +class PhysicsDirect : public PhysicsClient +{ +protected: + + struct PhysicsDirectInternalData* m_data; + + bool processDebugLines(const struct SharedMemoryCommand& orgCommand); + +public: + + PhysicsDirect(); + + virtual ~PhysicsDirect(); + + // 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_DIRECT_H diff --git a/examples/SharedMemory/PhysicsDirectC_API.cpp b/examples/SharedMemory/PhysicsDirectC_API.cpp new file mode 100644 index 000000000..e5e73b203 --- /dev/null +++ b/examples/SharedMemory/PhysicsDirectC_API.cpp @@ -0,0 +1,14 @@ +#include "PhysicsDirectC_API.h" + +#include "PhysicsDirect.h" + + + +//think more about naming. The b3ConnectPhysicsLoopback +b3PhysicsClientHandle b3ConnectPhysicsDirect() +{ + PhysicsDirect* direct = new PhysicsDirect(); + bool connected = direct->connect(); + return (b3PhysicsClientHandle )direct; +} + diff --git a/examples/SharedMemory/PhysicsDirectC_API.h b/examples/SharedMemory/PhysicsDirectC_API.h new file mode 100644 index 000000000..17681af61 --- /dev/null +++ b/examples/SharedMemory/PhysicsDirectC_API.h @@ -0,0 +1,19 @@ +#ifndef PHYSICS_DIRECT_C_API_H +#define PHYSICS_DIRECT_C_API_H + +#include "PhysicsClientC_API.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +///think more about naming. Directly execute commands without transport (no shared memory, UDP, socket, grpc etc) +b3PhysicsClientHandle b3ConnectPhysicsDirect(); + + +#ifdef __cplusplus +} +#endif + +#endif //PHYSICS_DIRECT_C_API_H diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 3b55d1582..90f7a3418 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -1,5 +1,5 @@ #include "PhysicsLoopBack.h" -#include "PhysicsServer.h" +#include "PhysicsServerSharedMemory.h" #include "PhysicsClientSharedMemory.h" #include "../CommonInterfaces/CommonGUIHelperInterface.h" diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index d4f598834..ba512881c 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -1,1928 +1,5 @@ #include "PhysicsServer.h" -#include "PosixSharedMemory.h" -#include "Win32SharedMemory.h" -#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" -#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" -#include "../Importers/ImportURDFDemo/URDF2Bullet.h" -#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" -#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" -#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" -#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" -#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" -#include "../CommonInterfaces/CommonRenderInterface.h" - -#include "btBulletDynamicsCommon.h" - -#include "LinearMath/btTransform.h" - -#include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h" -#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" -#include "LinearMath/btSerializer.h" -#include "Bullet3Common/b3Logging.h" -#include "../CommonInterfaces/CommonGUIHelperInterface.h" -#include "SharedMemoryBlock.h" - -struct UrdfLinkNameMapUtil +PhysicsServer::~PhysicsServer() { - btMultiBody* m_mb; - btDefaultSerializer* m_memSerializer; - - UrdfLinkNameMapUtil():m_mb(0),m_memSerializer(0) - { - } -}; - - -struct SharedMemoryDebugDrawer : public btIDebugDraw -{ - - int m_debugMode; - btAlignedObjectArray m_lines2; - - SharedMemoryDebugDrawer () - :m_debugMode(0) - { - } - virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color) - { - } - - virtual void reportErrorWarning(const char* warningString) - { - } - - virtual void draw3dText(const btVector3& location,const char* textString) - { - } - - virtual void setDebugMode(int debugMode) - { - m_debugMode = debugMode; - } - - virtual int getDebugMode() const - { - return m_debugMode; - } - virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color) - { - SharedMemLines line; - line.m_from = from; - line.m_to = to; - line.m_color = color; - m_lines2.push_back(line); - } -}; - - -struct InteralBodyData -{ - btMultiBody* m_multiBody; - btRigidBody* m_rigidBody; - int m_testData; - - btTransform m_rootLocalInertialFrame; - - InteralBodyData() - :m_multiBody(0), - m_rigidBody(0), - m_testData(0) - { - m_rootLocalInertialFrame.setIdentity(); - } -}; - -///todo: templatize this -struct InternalBodyHandle : public InteralBodyData -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - - int m_nextFreeHandle; - void SetNextFree(int next) - { - m_nextFreeHandle = next; - } - int GetNextFree() const - { - return m_nextFreeHandle; - } -}; - -class btCommandChunk -{ -public: - int m_chunkCode; - int m_length; - void *m_oldPtr; - int m_dna_nr; - int m_number; -}; - - -class bCommandChunkPtr4 -{ -public: - bCommandChunkPtr4(){} - int code; - int len; - union - { - int m_uniqueInt; - }; - int dna_nr; - int nr; -}; - -// ----------------------------------------------------- // -class bCommandChunkPtr8 -{ -public: - bCommandChunkPtr8(){} - int code, len; - union - { - int m_uniqueInts[2]; - }; - int dna_nr, nr; -}; - - - -struct CommandLogger -{ - FILE* m_file; - - void writeHeader(unsigned char* buffer) const - { - -#ifdef BT_USE_DOUBLE_PRECISION - memcpy(buffer, "BT3CMDd", 7); -#else - memcpy(buffer, "BT3CMDf", 7); -#endif //BT_USE_DOUBLE_PRECISION - - int littleEndian= 1; - littleEndian= ((char*)&littleEndian)[0]; - - if (sizeof(void*)==8) - { - buffer[7] = '-'; - } else - { - buffer[7] = '_'; - } - - if (littleEndian) - { - buffer[8]='v'; - } else - { - buffer[8]='V'; - } - - buffer[9] = 0; - buffer[10] = 0; - buffer[11] = 0; - - int ver = btGetVersion(); - if (ver>=0 && ver<999) - { - sprintf((char*)&buffer[9],"%d",ver); - } - - } - - void logCommand(SharedMemoryBlock* testBlock1) - { - btCommandChunk chunk; - chunk.m_chunkCode = testBlock1->m_clientCommands[0].m_type; - chunk.m_oldPtr = 0; - chunk.m_dna_nr = 0; - chunk.m_length = sizeof(SharedMemoryCommand); - chunk.m_number = 1; - fwrite((const char*)&chunk,sizeof(btCommandChunk), 1,m_file); - fwrite((const char*)&testBlock1->m_clientCommands[0],sizeof(SharedMemoryCommand),1,m_file); - } - - CommandLogger(const char* fileName) - { - m_file = fopen(fileName,"wb"); - unsigned char buf[15]; - buf[12] = 12; - buf[13] = 13; - buf[14] = 14; - writeHeader(buf); - fwrite(buf,12,1,m_file); - } - virtual ~CommandLogger() - { - fclose(m_file); - } -}; - - -struct CommandLogPlayback -{ - unsigned char m_header[12]; - FILE* m_file; - bool m_bitsVary; - bool m_fileIs64bit; - - - CommandLogPlayback(const char* fileName) - { - m_file = fopen(fileName,"rb"); - if (m_file) - { - fread(m_header,12,1,m_file); - } - unsigned char c = m_header[7]; - m_fileIs64bit = (c=='-'); - - const bool VOID_IS_8 = ((sizeof(void*)==8)); - m_bitsVary = (VOID_IS_8 != m_fileIs64bit); - - - - } - virtual ~CommandLogPlayback() - { - if (m_file) - { - fclose(m_file); - m_file=0; - } - } - bool processNextCommand(SharedMemoryCommand* cmd) - { - if (m_file) - { - size_t s = 0; - - - if (m_fileIs64bit) - { - bCommandChunkPtr8 chunk8; - s = fread((void*)&chunk8,sizeof(bCommandChunkPtr8),1,m_file); - } else - { - bCommandChunkPtr4 chunk4; - s = fread((void*)&chunk4,sizeof(bCommandChunkPtr4),1,m_file); - } - - if (s==1) - { - s = fread(cmd,sizeof(SharedMemoryCommand),1,m_file); - return (s==1); - } - } - return false; - - } -}; - -struct PhysicsServerInternalData -{ - ///handle management - btAlignedObjectArray m_bodyHandles; - int m_numUsedHandles; // number of active handles - - int m_firstFreeHandle; // free handles list - InternalBodyHandle* getHandle(int handle) - { - btAssert(handle>=0); - btAssert(handle=m_bodyHandles.size())) - { - return 0; - } - return &m_bodyHandles[handle]; - } - const InternalBodyHandle* getHandle(int handle) const - { - return &m_bodyHandles[handle]; - } - - void increaseHandleCapacity(int extraCapacity) - { - int curCapacity = m_bodyHandles.size(); - btAssert(curCapacity == m_numUsedHandles); - int newCapacity = curCapacity + extraCapacity; - m_bodyHandles.resize(newCapacity); - - { - for (int i = curCapacity; i < newCapacity; i++) - m_bodyHandles[i].SetNextFree(i + 1); - - - m_bodyHandles[newCapacity - 1].SetNextFree(-1); - } - m_firstFreeHandle = curCapacity; - } - void initHandles() - { - m_numUsedHandles = 0; - m_firstFreeHandle = -1; - - increaseHandleCapacity(1); - } - - void exitHandles() - { - m_bodyHandles.resize(0); - m_firstFreeHandle = -1; - m_numUsedHandles = 0; - } - - int allocHandle() - { - btAssert(m_firstFreeHandle>=0); - - int handle = m_firstFreeHandle; - m_firstFreeHandle = getHandle(handle)->GetNextFree(); - m_numUsedHandles++; - - if (m_firstFreeHandle<0) - { - int curCapacity = m_bodyHandles.size(); - int additionalCapacity= m_bodyHandles.size(); - increaseHandleCapacity(additionalCapacity); - - - getHandle(handle)->SetNextFree(m_firstFreeHandle); - } - - - return handle; - } - - - void freeHandle(int handle) - { - btAssert(handle >= 0); - - getHandle(handle)->SetNextFree(m_firstFreeHandle); - m_firstFreeHandle = handle; - - m_numUsedHandles--; - } - - ///end handle management - - - SharedMemoryInterface* m_sharedMemory; - SharedMemoryBlock* m_testBlock1; - CommandLogger* m_commandLogger; - CommandLogPlayback* m_logPlayback; - - bool m_isConnected; - btScalar m_physicsDeltaTime; - btAlignedObjectArray m_multiBodyJointFeedbacks; - - - - btAlignedObjectArray m_worldImporters; - btAlignedObjectArray m_urdfLinkNameMapper; - btHashMap m_multiBodyJointMotorMap; - btAlignedObjectArray m_strings; - - btAlignedObjectArray m_collisionShapes; - btBroadphaseInterface* m_broadphase; - btCollisionDispatcher* m_dispatcher; - btMultiBodyConstraintSolver* m_solver; - btDefaultCollisionConfiguration* m_collisionConfiguration; - btMultiBodyDynamicsWorld* m_dynamicsWorld; - SharedMemoryDebugDrawer* m_remoteDebugDrawer; - - - - - struct GUIHelperInterface* m_guiHelper; - int m_sharedMemoryKey; - - bool m_verboseOutput; - - - //data for picking objects - class btRigidBody* m_pickedBody; - class btTypedConstraint* m_pickedConstraint; - class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point; - btVector3 m_oldPickingPos; - btVector3 m_hitPos; - btScalar m_oldPickingDist; - bool m_prevCanSleep; - - PhysicsServerInternalData() - :m_sharedMemory(0), - m_testBlock1(0), - m_commandLogger(0), - m_logPlayback(0), - m_isConnected(false), - m_physicsDeltaTime(1./240.), - m_dynamicsWorld(0), - m_remoteDebugDrawer(0), - m_guiHelper(0), - m_sharedMemoryKey(SHARED_MEMORY_KEY), - m_verboseOutput(false), - m_pickedBody(0), - m_pickedConstraint(0), - m_pickingMultiBodyPoint2Point(0) - { - - initHandles(); -#if 0 - btAlignedObjectArray bla; - - for (int i=0;i<1024;i++) - { - int handle = allocHandle(); - bla.push_back(handle); - InternalBodyHandle* body = getHandle(handle); - InteralBodyData* body2 = body; - } - for (int i=0;im_serverCommands[0]; - serverCmd .m_type = statusType; - serverCmd.m_sequenceNumber = sequenceNumber; - serverCmd.m_timeStamp = timeStamp; - return serverCmd; - } - void submitServerStatus(SharedMemoryStatus& status) - { - m_testBlock1->m_numServerCommands++; - } - -}; - - -PhysicsServerSharedMemory::PhysicsServerSharedMemory() -{ - m_data = new PhysicsServerInternalData(); - -#ifdef _WIN32 - m_data->m_sharedMemory = new Win32SharedMemoryServer(); -#else - m_data->m_sharedMemory = new PosixSharedMemory(); -#endif - - createEmptyDynamicsWorld(); - - -} - -PhysicsServerSharedMemory::~PhysicsServerSharedMemory() -{ - deleteDynamicsWorld(); - if (m_data->m_commandLogger) - { - delete m_data->m_commandLogger; - m_data->m_commandLogger = 0; - } - - delete m_data; -} - -void PhysicsServerSharedMemory::setSharedMemoryKey(int key) -{ - m_data->m_sharedMemoryKey = key; -} - - -void PhysicsServerSharedMemory::createEmptyDynamicsWorld() -{ - ///collision configuration contains default setup for memory, collision setup - m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration(); - //m_collisionConfiguration->setConvexConvexMultipointIterations(); - - ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) - m_data->m_dispatcher = new btCollisionDispatcher(m_data->m_collisionConfiguration); - - m_data->m_broadphase = new btDbvtBroadphase(); - - m_data->m_solver = new btMultiBodyConstraintSolver; - - m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); - - m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer(); - - - m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); -} - -void PhysicsServerSharedMemory::deleteDynamicsWorld() -{ - - for (int i=0;im_multiBodyJointFeedbacks.size();i++) - { - delete m_data->m_multiBodyJointFeedbacks[i]; - } - m_data->m_multiBodyJointFeedbacks.clear(); - - - for (int i=0;im_worldImporters.size();i++) - { - delete m_data->m_worldImporters[i]; - } - m_data->m_worldImporters.clear(); - - for (int i=0;im_urdfLinkNameMapper.size();i++) - { - delete m_data->m_urdfLinkNameMapper[i]; - } - m_data->m_urdfLinkNameMapper.clear(); - - m_data->m_multiBodyJointMotorMap.clear(); - - for (int i=0;im_strings.size();i++) - { - delete m_data->m_strings[i]; - } - m_data->m_strings.clear(); - - - if (m_data->m_dynamicsWorld) - { - - int i; - for (i = m_data->m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--) - { - m_data->m_dynamicsWorld->removeConstraint(m_data->m_dynamicsWorld->getConstraint(i)); - } - for (i = m_data->m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) - { - btCollisionObject* obj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i]; - btRigidBody* body = btRigidBody::upcast(obj); - if (body && body->getMotionState()) - { - delete body->getMotionState(); - } - m_data->m_dynamicsWorld->removeCollisionObject(obj); - delete obj; - } - } - //delete collision shapes - for (int j = 0; jm_collisionShapes.size(); j++) - { - btCollisionShape* shape = m_data->m_collisionShapes[j]; - delete shape; - } - m_data->m_collisionShapes.clear(); - - delete m_data->m_dynamicsWorld; - m_data->m_dynamicsWorld=0; - - delete m_data->m_remoteDebugDrawer; - m_data->m_remoteDebugDrawer =0; - - delete m_data->m_solver; - m_data->m_solver=0; - - delete m_data->m_broadphase; - m_data->m_broadphase=0; - - delete m_data->m_dispatcher; - m_data->m_dispatcher=0; - - delete m_data->m_collisionConfiguration; - m_data->m_collisionConfiguration=0; - -} - -bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* guiHelper) -{ - m_data->m_guiHelper = guiHelper; - - if (m_data->m_guiHelper) - { - m_data->m_guiHelper->createPhysicsDebugDrawer(m_data->m_dynamicsWorld); - } - - bool allowCreation = true; - - - if (m_data->m_isConnected) - { - b3Warning("connectSharedMemory, while already connected"); - return m_data->m_isConnected; - } - - - int counter = 0; - do - { - - m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE,allowCreation); - if (m_data->m_testBlock1) - { - int magicId =m_data->m_testBlock1->m_magicId; - if (m_data->m_verboseOutput) - { - b3Printf("magicId = %d\n", magicId); - } - - if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) - { - InitSharedMemoryBlock(m_data->m_testBlock1); - if (m_data->m_verboseOutput) - { - b3Printf("Created and initialized shared memory block\n"); - } - m_data->m_isConnected = true; - } else - { - m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); - m_data->m_testBlock1 = 0; - m_data->m_isConnected = false; - } - } else - { - b3Error("Cannot connect to shared memory"); - m_data->m_isConnected = false; - } - } while (counter++ < 10 && !m_data->m_isConnected); - - if (!m_data->m_isConnected) - { - b3Error("Server cannot connect to shared memory.\n"); - } - - return m_data->m_isConnected; -} - - -void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory) -{ - if (m_data->m_guiHelper) - { - if (m_data->m_guiHelper && m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getDebugDrawer()) - { - m_data->m_dynamicsWorld->setDebugDrawer(0); - } - } - if (m_data->m_verboseOutput) - { - b3Printf("releaseSharedMemory1\n"); - } - if (m_data->m_testBlock1) - { - if (m_data->m_verboseOutput) - { - b3Printf("m_testBlock1\n"); - } - if (deInitializeSharedMemory) - { - m_data->m_testBlock1->m_magicId = 0; - if (m_data->m_verboseOutput) - { - b3Printf("De-initialized shared memory, magic id = %d\n",m_data->m_testBlock1->m_magicId); - } - } - btAssert(m_data->m_sharedMemory); - m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); - } - if (m_data->m_sharedMemory) - { - if (m_data->m_verboseOutput) - { - b3Printf("m_sharedMemory\n"); - } - delete m_data->m_sharedMemory; - m_data->m_sharedMemory = 0; - m_data->m_testBlock1 = 0; - } -} - -void PhysicsServerSharedMemory::releaseSharedMemory() -{ - if (m_data->m_verboseOutput) - { - b3Printf("releaseSharedMemory1\n"); - } - if (m_data->m_testBlock1) - { - if (m_data->m_verboseOutput) - { - b3Printf("m_testBlock1\n"); - } - m_data->m_testBlock1->m_magicId = 0; - if (m_data->m_verboseOutput) - { - b3Printf("magic id = %d\n",m_data->m_testBlock1->m_magicId); - } - btAssert(m_data->m_sharedMemory); - m_data->m_sharedMemory->releaseSharedMemory( m_data->m_sharedMemoryKey -, SHARED_MEMORY_SIZE); - } - if (m_data->m_sharedMemory) - { - if (m_data->m_verboseOutput) - { - b3Printf("m_sharedMemory\n"); - } - delete m_data->m_sharedMemory; - m_data->m_sharedMemory = 0; - m_data->m_testBlock1 = 0; - } -} - - -bool PhysicsServerSharedMemory::supportsJointMotor(btMultiBody* mb, int mbLinkIndex) -{ - bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute - ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic); - return canHaveMotor; - -} - -//for testing, create joint motors for revolute and prismatic joints -void PhysicsServerSharedMemory::createJointMotors(btMultiBody* mb) -{ - int numLinks = mb->getNumLinks(); - for (int i=0;isetMaxAppliedImpulse(0); - m_data->m_multiBodyJointMotorMap.insert(mbLinkIndex,motor); - m_data->m_dynamicsWorld->addMultiBodyConstraint(motor); - } - - } -} - - - -bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, - bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr) -{ - btAssert(m_data->m_dynamicsWorld); - if (!m_data->m_dynamicsWorld) - { - b3Error("loadUrdf: No valid m_dynamicsWorld"); - return false; - } - - BulletURDFImporter u2b(m_data->m_guiHelper); - - - bool loadOk = u2b.loadURDF(fileName, useFixedBase); - if (loadOk) - { - //get a body index - int bodyUniqueId = m_data->allocHandle(); - if (bodyUniqueIdPtr) - *bodyUniqueIdPtr= bodyUniqueId; - - InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); - - { - btScalar mass = 0; - bodyHandle->m_rootLocalInertialFrame.setIdentity(); - btVector3 localInertiaDiagonal(0,0,0); - int urdfLinkIndex = u2b.getRootLinkIndex(); - u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame); - } - if (m_data->m_verboseOutput) - { - 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_data->m_guiHelper); - - ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix()); - btMultiBody* mb = creation.getBulletMultiBody(); - if (useMultiBody) - { - if (mb) - { - bodyHandle->m_multiBody = mb; - createJointMotors(mb); - - //serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire - UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil; - m_data->m_urdfLinkNameMapper.push_back(util); - util->m_mb = mb; - util->m_memSerializer = new btDefaultSerializer(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE,(unsigned char*)m_data->m_testBlock1->m_bulletStreamDataServerToClient); - //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); - util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0); - - for (int i=0;igetNumLinks();i++) - { - //disable serialization of the collision objects - util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0); - int urdfLinkIndex = creation.m_mb2urdfLink[i]; - std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str()); - m_data->m_strings.push_back(linkName); - util->m_memSerializer->registerNameForPointer(linkName->c_str(),linkName->c_str()); - mb->getLink(i).m_linkName = linkName->c_str(); - - std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str()); - m_data->m_strings.push_back(jointName); - util->m_memSerializer->registerNameForPointer(jointName->c_str(),jointName->c_str()); - mb->getLink(i).m_jointName = jointName->c_str(); - } - - std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); - m_data->m_strings.push_back(baseName); - - - util->m_memSerializer->registerNameForPointer(baseName->c_str(),baseName->c_str()); - mb->setBaseName(baseName->c_str()); - - - util->m_memSerializer->insertHeader(); - - int len = mb->calculateSerializeBufferSize(); - btChunk* chunk = util->m_memSerializer->allocate(len,1); - const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer); - util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb); - - return true; - } else - { - b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); - return false; - } - } else - { - btAssert(0); - - return true; - } - - } - - return false; -} - - - - - - - -void PhysicsServerSharedMemory::processClientCommands() -{ - if (m_data->m_isConnected && m_data->m_testBlock1) - { - if (m_data->m_logPlayback) - { - if (m_data->m_testBlock1->m_numServerCommands>m_data->m_testBlock1->m_numProcessedServerCommands) - { - m_data->m_testBlock1->m_numProcessedServerCommands++; - } - //push a command from log file - bool hasCommand = m_data->m_logPlayback->processNextCommand(&m_data->m_testBlock1->m_clientCommands[0]); - if (hasCommand) - { - m_data->m_testBlock1->m_numClientCommands++; - } - } - ///we ignore overflow of integer for now - if (m_data->m_testBlock1->m_numClientCommands> m_data->m_testBlock1->m_numProcessedClientCommands) - { - - - //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands - btAssert(m_data->m_testBlock1->m_numClientCommands==m_data->m_testBlock1->m_numProcessedClientCommands+1); - - const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0]; - if (m_data->m_commandLogger) - { - m_data->m_commandLogger->logCommand(m_data->m_testBlock1); - } - - m_data->m_testBlock1->m_numProcessedClientCommands++; - - //no timestamp yet - int timeStamp = 0; - - //consume the command - switch (clientCmd.m_type) - { - case CMD_SEND_BULLET_DATA_STREAM: - { - if (m_data->m_verboseOutput) - { - b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength); - } - - btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); - m_data->m_worldImporters.push_back(worldImporter); - bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength); - - if (completedOk) - { - SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); - m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - m_data->submitServerStatus(status); - } else - { - SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,clientCmd.m_sequenceNumber,timeStamp); - m_data->submitServerStatus(status); - } - - break; - } - case CMD_REQUEST_DEBUG_LINES: - { - int curFlags =m_data->m_remoteDebugDrawer->getDebugMode(); - - int debugMode = clientCmd.m_requestDebugLinesArguments.m_debugMode;//clientCmd.btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb; - int startingLineIndex = clientCmd.m_requestDebugLinesArguments.m_startingLineIndex; - if (startingLineIndex<0) - { - b3Warning("startingLineIndex should be non-negative"); - startingLineIndex = 0; - } - - if (clientCmd.m_requestDebugLinesArguments.m_startingLineIndex==0) - { - m_data->m_remoteDebugDrawer->m_lines2.resize(0); - //|btIDebugDraw::DBG_DrawAabb| - // btIDebugDraw::DBG_DrawConstraints |btIDebugDraw::DBG_DrawConstraintLimits ; - m_data->m_remoteDebugDrawer->setDebugMode(debugMode); - btIDebugDraw* oldDebugDrawer = m_data->m_dynamicsWorld->getDebugDrawer(); - m_data->m_dynamicsWorld->setDebugDrawer(m_data->m_remoteDebugDrawer); - m_data->m_dynamicsWorld->debugDrawWorld(); - m_data->m_dynamicsWorld->setDebugDrawer(oldDebugDrawer); - m_data->m_remoteDebugDrawer->setDebugMode(curFlags); - } - - //9 floats per line: 3 floats for 'from', 3 floats for 'to' and 3 floats for 'color' - int maxNumLines = SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE/(sizeof(float)*9)-1; - if (startingLineIndex >m_data->m_remoteDebugDrawer->m_lines2.size()) - { - b3Warning("m_startingLineIndex exceeds total number of debug lines"); - startingLineIndex =m_data->m_remoteDebugDrawer->m_lines2.size(); - } - - int numLines = btMin(maxNumLines,m_data->m_remoteDebugDrawer->m_lines2.size()-startingLineIndex); - - if (numLines) - { - - 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)); - - for (int i=0;im_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.x(); - linesTo[i*3] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.x(); - linesColor[i*3] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.x(); - - linesFrom[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.y(); - linesTo[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.y(); - linesColor[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.y(); - - linesFrom[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.z(); - linesTo[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.z(); - linesColor[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.z(); - } - } - - SharedMemoryStatus& status = m_data->createServerStatus(CMD_DEBUG_LINES_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); - status.m_sendDebugLinesArgs.m_numDebugLines = numLines; - status.m_sendDebugLinesArgs.m_startingLineIndex = startingLineIndex; - status.m_sendDebugLinesArgs.m_numRemainingDebugLines = m_data->m_remoteDebugDrawer->m_lines2.size()-(startingLineIndex+numLines); - m_data->submitServerStatus(status); - - break; - } - case CMD_LOAD_URDF: - { - - const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments; - if (m_data->m_verboseOutput) - { - b3Printf("Processed CMD_LOAD_URDF:%s", urdfArgs.m_urdfFileName); - } - btAssert((clientCmd.m_updateFlags&URDF_ARGS_FILE_NAME) !=0); - btAssert(urdfArgs.m_urdfFileName); - btVector3 initialPos(0,0,0); - btQuaternion initialOrn(0,0,0,1); - if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_POSITION) - { - initialPos[0] = urdfArgs.m_initialPosition[0]; - initialPos[1] = urdfArgs.m_initialPosition[1]; - initialPos[2] = urdfArgs.m_initialPosition[2]; - } - if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION) - { - initialOrn[0] = urdfArgs.m_initialOrientation[0]; - initialOrn[1] = urdfArgs.m_initialOrientation[1]; - initialOrn[2] = urdfArgs.m_initialOrientation[2]; - initialOrn[3] = urdfArgs.m_initialOrientation[3]; - } - bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? urdfArgs.m_useMultiBody : true; - bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? urdfArgs.m_useFixedBase: false; - int bodyUniqueId; - //load the actual URDF and send a report: completed or failed - bool completedOk = loadUrdf(urdfArgs.m_urdfFileName, - initialPos,initialOrn, - useMultiBody, useFixedBase,&bodyUniqueId); - - - if (completedOk) - { - - m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); - if (m_data->m_urdfLinkNameMapper.size()) - { - status.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); - status.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; - } - m_data->submitServerStatus(status); - - } else - { - SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_FAILED,clientCmd.m_sequenceNumber,timeStamp); - m_data->submitServerStatus(status); - } - - - - - break; - } - case CMD_CREATE_SENSOR: - { - if (m_data->m_verboseOutput) - { - b3Printf("Processed CMD_CREATE_SENSOR"); - } - int bodyUniqueId = clientCmd.m_createSensorArguments.m_bodyUniqueId; - InteralBodyData* body = m_data->getHandle(bodyUniqueId); - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - btAssert(mb); - for (int i=0;igetLink(jointIndex).m_jointFeedback) - { - b3Warning("CMD_CREATE_SENSOR: sensor for joint [%d] already enabled", jointIndex); - } else - { - btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback(); - fb->m_reactionForces.setZero(); - mb->getLink(jointIndex).m_jointFeedback = fb; - m_data->m_multiBodyJointFeedbacks.push_back(fb); - }; - - } else - { - if (mb->getLink(jointIndex).m_jointFeedback) - { - m_data->m_multiBodyJointFeedbacks.remove(mb->getLink(jointIndex).m_jointFeedback); - delete mb->getLink(jointIndex).m_jointFeedback; - mb->getLink(jointIndex).m_jointFeedback=0; - } else - { - b3Warning("CMD_CREATE_SENSOR: cannot perform sensor removal request, no sensor on joint [%d]", jointIndex); - }; - - } - } - - } else - { - b3Warning("No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet"); - } - -#if 0 - //todo(erwincoumans) here is some sample code to hook up a force/torque sensor for btTypedConstraint/btRigidBody - /* - for (int i=0;im_dynamicsWorld->getNumConstraints();i++) - { - btTypedConstraint* c = m_data->m_dynamicsWorld->getConstraint(i); - btJointFeedback* fb = new btJointFeedback(); - m_data->m_jointFeedbacks.push_back(fb); - c->setJointFeedback(fb); - - - } - */ -#endif - - SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); - m_data->submitServerStatus(serverCmd); - break; - } - case CMD_SEND_DESIRED_STATE: - { - if (m_data->m_verboseOutput) - { - b3Printf("Processed CMD_SEND_DESIRED_STATE"); - } - - int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId; - InteralBodyData* body = m_data->getHandle(bodyUniqueId); - - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - btAssert(mb); - - switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) - { - case CONTROL_MODE_TORQUE: - { - if (m_data->m_verboseOutput) - { - b3Printf("Using CONTROL_MODE_TORQUE"); - } - mb->clearForcesAndTorques(); - - int torqueIndex = 0; - btVector3 f(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]); - btVector3 t(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]); - torqueIndex+=6; - mb->addBaseForce(f); - mb->addBaseTorque(t); - for (int link=0;linkgetNumLinks();link++) - { - - for (int dof=0;dofgetLink(link).m_dofCount;dof++) - { - double torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; - mb->addJointTorqueMultiDof(link,dof,torque); - torqueIndex++; - } - } - break; - } - case CONTROL_MODE_VELOCITY: - { - if (m_data->m_verboseOutput) - { - b3Printf("Using CONTROL_MODE_VELOCITY"); - } - - int numMotors = 0; - //find the joint motors and apply the desired velocity and maximum force/torque - { - int dofIndex = 6;//skip the 3 linear + 3 angular degree of freedom entries of the base - for (int link=0;linkgetNumLinks();link++) - { - if (supportsJointMotor(mb,link)) - { - - btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link]; - if (motorPtr) - { - btMultiBodyJointMotor* motor = *motorPtr; - btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; - motor->setVelocityTarget(desiredVelocity); - - btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; - motor->setMaxAppliedImpulse(maxImp); - numMotors++; - - } - } - dofIndex += mb->getLink(link).m_dofCount; - } - } - break; - } - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - if (m_data->m_verboseOutput) - { - b3Printf("Using CONTROL_MODE_POSITION_VELOCITY_PD"); - } - //compute the force base on PD control - mb->clearForcesAndTorques(); - - int numMotors = 0; - //find the joint motors and apply the desired velocity and maximum force/torque - { - int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base - int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base - for (int link=0;linkgetNumLinks();link++) - { - if (supportsJointMotor(mb,link)) - { - - btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link]; - if (motorPtr) - { - btMultiBodyJointMotor* motor = *motorPtr; - - btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; - btScalar desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; - - btScalar kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; - btScalar kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; - - int dof1 = 0; - btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1]; - btScalar currentVelocity = mb->getJointVelMultiDof(link)[dof1]; - btScalar positionStabiliationTerm = (desiredPosition-currentPosition)/m_data->m_physicsDeltaTime; - btScalar velocityError = (desiredVelocity - currentVelocity); - - desiredVelocity = kp * positionStabiliationTerm + - kd * velocityError; - - motor->setVelocityTarget(desiredVelocity); - - btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime; - - motor->setMaxAppliedImpulse(1000);//maxImp); - numMotors++; - } - - } - velIndex += mb->getLink(link).m_dofCount; - posIndex += mb->getLink(link).m_posVarCount; - } - } - - break; - } - default: - { - b3Warning("m_controlMode not implemented yet"); - break; - } - - } - } - - SharedMemoryStatus& status = m_data->createServerStatus(CMD_DESIRED_STATE_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); - m_data->submitServerStatus(status); - break; - } - case CMD_REQUEST_ACTUAL_STATE: - { - if (m_data->m_verboseOutput) - { - b3Printf("Sending the actual state (Q,U)"); - } - int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; - InteralBodyData* body = m_data->getHandle(bodyUniqueId); - - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); - - serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; - int totalDegreeOfFreedomQ = 0; - int totalDegreeOfFreedomU = 0; - - - //always add the base, even for static (non-moving objects) - //so that we can easily move the 'fixed' base when needed - //do we don't use this conditional "if (!mb->hasFixedBase())" - { - btTransform tr; - tr.setOrigin(mb->getBasePos()); - tr.setRotation(mb->getWorldToBaseRot().inverse()); - - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = - body->m_rootLocalInertialFrame.getOrigin()[0]; - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = - body->m_rootLocalInertialFrame.getOrigin()[1]; - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = - body->m_rootLocalInertialFrame.getOrigin()[2]; - - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = - body->m_rootLocalInertialFrame.getRotation()[0]; - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = - body->m_rootLocalInertialFrame.getRotation()[1]; - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] = - body->m_rootLocalInertialFrame.getRotation()[2]; - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = - body->m_rootLocalInertialFrame.getRotation()[3]; - - - - //base position in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; - - //base orientation, quaternion x,y,z,w, in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; - totalDegreeOfFreedomQ +=7;//pos + quaternion - - //base linear velocity (in world space, carthesian) - serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2]; - - //base angular velocity (in world space, carthesian) - serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = mb->getBaseOmega()[2]; - totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF - } - for (int l=0;lgetNumLinks();l++) - { - for (int d=0;dgetLink(l).m_posVarCount;d++) - { - serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d]; - } - for (int d=0;dgetLink(l).m_dofCount;d++) - { - serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; - } - - if (0 == mb->getLink(l).m_jointFeedback) - { - for (int d=0;d<6;d++) - { - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+d]=0; - } - } else - { - btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear(); - btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getAngular(); - - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = sensedForce[0]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = sensedForce[1]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = sensedForce[2]; - - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = sensedTorque[0]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2]; - } - } - - serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; - serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; - - m_data->submitServerStatus(serverCmd); - - } else - { - if (body && body->m_rigidBody) - { - btRigidBody* rb = body->m_rigidBody; - SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); - serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; - int totalDegreeOfFreedomQ = 0; - int totalDegreeOfFreedomU = 0; - - btTransform tr = rb->getWorldTransform(); - //base position in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; - - //base orientation, quaternion x,y,z,w, in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; - totalDegreeOfFreedomQ +=7;//pos + quaternion - - //base linear velocity (in world space, carthesian) - serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = rb->getLinearVelocity()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = rb->getLinearVelocity()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = rb->getLinearVelocity()[2]; - - //base angular velocity (in world space, carthesian) - serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = rb->getAngularVelocity()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = rb->getAngularVelocity()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = rb->getAngularVelocity()[2]; - totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF - - serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; - serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; - - m_data->submitServerStatus(serverCmd); - } else - { - b3Warning("Request state but no multibody or rigid body available"); - SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_FAILED,clientCmd.m_sequenceNumber,timeStamp); - m_data->submitServerStatus(serverCmd); - } - } - - break; - } - case CMD_STEP_FORWARD_SIMULATION: - { - - if (m_data->m_verboseOutput) - { - b3Printf("Step simulation request"); - } - m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0); - - SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_STEP_FORWARD_SIMULATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); - m_data->submitServerStatus(serverCmd); - - break; - } - - case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: - { - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) - { - m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime; - } - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY) - { - btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0], - clientCmd.m_physSimParamArgs.m_gravityAcceleration[1], - clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]); - this->m_data->m_dynamicsWorld->setGravity(grav); - if (m_data->m_verboseOutput) - { - b3Printf("Updated Gravity: %f,%f,%f",grav[0],grav[1],grav[2]); - } - - } - - - SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); - m_data->submitServerStatus(serverCmd); - break; - - }; - case CMD_INIT_POSE: - { - if (m_data->m_verboseOutput) - { - b3Printf("Server Init Pose not implemented yet"); - } - int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId; - InteralBodyData* body = m_data->getHandle(bodyUniqueId); - - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) - { - btVector3 zero(0,0,0); - mb->setBaseVel(zero); - mb->setBasePos(btVector3( - clientCmd.m_initPoseArgs.m_initialStateQ[0], - clientCmd.m_initPoseArgs.m_initialStateQ[1], - clientCmd.m_initPoseArgs.m_initialStateQ[2])); - } - if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) - { - mb->setBaseOmega(btVector3(0,0,0)); - mb->setWorldToBaseRot(btQuaternion( - clientCmd.m_initPoseArgs.m_initialStateQ[3], - clientCmd.m_initPoseArgs.m_initialStateQ[4], - clientCmd.m_initPoseArgs.m_initialStateQ[5], - clientCmd.m_initPoseArgs.m_initialStateQ[6])); - } - if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE) - { - int dofIndex = 7; - for (int i=0;igetNumLinks();i++) - { - if (mb->getLink(i).m_dofCount==1) - { - - mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[dofIndex]); - mb->setJointVel(i,0); - } - dofIndex += mb->getLink(i).m_dofCount; - } - } - } - - SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); - m_data->submitServerStatus(serverCmd); - - break; - } - - - case CMD_RESET_SIMULATION: - { - //clean up all data - if (m_data && m_data->m_guiHelper && m_data->m_guiHelper->getRenderInterface()) - { - m_data->m_guiHelper->getRenderInterface()->removeAllInstances(); - } - deleteDynamicsWorld(); - createEmptyDynamicsWorld(); - m_data->exitHandles(); - m_data->initHandles(); - SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_RESET_SIMULATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); - m_data->submitServerStatus(serverCmd); - - break; - } - case CMD_CREATE_RIGID_BODY: - case CMD_CREATE_BOX_COLLISION_SHAPE: - { - btVector3 halfExtents(1,1,1); - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS) - { - halfExtents = btVector3( - clientCmd.m_createBoxShapeArguments.m_halfExtentsX, - clientCmd.m_createBoxShapeArguments.m_halfExtentsY, - clientCmd.m_createBoxShapeArguments.m_halfExtentsZ); - } - btTransform startTrans; - startTrans.setIdentity(); - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_POSITION) - { - startTrans.setOrigin(btVector3( - clientCmd.m_createBoxShapeArguments.m_initialPosition[0], - clientCmd.m_createBoxShapeArguments.m_initialPosition[1], - clientCmd.m_createBoxShapeArguments.m_initialPosition[2])); - } - - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION) - { - - startTrans.setRotation(btQuaternion( - clientCmd.m_createBoxShapeArguments.m_initialOrientation[0], - clientCmd.m_createBoxShapeArguments.m_initialOrientation[1], - clientCmd.m_createBoxShapeArguments.m_initialOrientation[2], - clientCmd.m_createBoxShapeArguments.m_initialOrientation[3])); - } - - btScalar mass = 0.f; - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_MASS) - { - mass = clientCmd.m_createBoxShapeArguments.m_mass; - } - - int shapeType = COLLISION_SHAPE_TYPE_BOX; - - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE) - { - shapeType = clientCmd.m_createBoxShapeArguments.m_collisionShapeType; - } - - btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); - m_data->m_worldImporters.push_back(worldImporter); - - btCollisionShape* shape = 0; - - switch (shapeType) - { - case COLLISION_SHAPE_TYPE_CYLINDER_X: - { - btScalar radius = halfExtents[1]; - btScalar height = halfExtents[0]; - shape = worldImporter->createCylinderShapeX(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_CYLINDER_Y: - { - btScalar radius = halfExtents[0]; - btScalar height = halfExtents[1]; - shape = worldImporter->createCylinderShapeY(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_CYLINDER_Z: - { - btScalar radius = halfExtents[1]; - btScalar height = halfExtents[2]; - shape = worldImporter->createCylinderShapeZ(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_CAPSULE_X: - { - btScalar radius = halfExtents[1]; - btScalar height = halfExtents[0]; - shape = worldImporter->createCapsuleShapeX(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_CAPSULE_Y: - { - btScalar radius = halfExtents[0]; - btScalar height = halfExtents[1]; - shape = worldImporter->createCapsuleShapeY(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_CAPSULE_Z: - { - btScalar radius = halfExtents[1]; - btScalar height = halfExtents[2]; - shape = worldImporter->createCapsuleShapeZ(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_SPHERE: - { - btScalar radius = halfExtents[0]; - shape = worldImporter->createSphereShape(radius); - break; - } - case COLLISION_SHAPE_TYPE_BOX: - default: - { - shape = worldImporter->createBoxShape(halfExtents); - } - } - - - bool isDynamic = (mass>0); - btRigidBody* rb = worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); - rb->setRollingFriction(0.2); - //m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - btVector4 colorRGBA(1,0,0,1); - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLOR) - { - colorRGBA[0] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[0]; - colorRGBA[1] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[1]; - colorRGBA[2] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[2]; - colorRGBA[3] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[3]; - } - m_data->m_guiHelper->createCollisionShapeGraphicsObject(rb->getCollisionShape()); - m_data->m_guiHelper->createCollisionObjectGraphicsObject(rb,colorRGBA); - - SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_RIGID_BODY_CREATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); - int bodyUniqueId = m_data->allocHandle(); - InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); - serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId; - bodyHandle->m_rootLocalInertialFrame.setIdentity(); - bodyHandle->m_rigidBody = rb; - m_data->submitServerStatus(serverCmd); - - 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"); - - SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_UNKNOWN_COMMAND_FLUSHED,clientCmd.m_sequenceNumber,timeStamp); - m_data->submitServerStatus(serverCmd); - - } - }; - - } - } -} - -void PhysicsServerSharedMemory::renderScene() -{ - if (m_data->m_guiHelper) - { - m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld); - - m_data->m_guiHelper->render(m_data->m_dynamicsWorld); - } - -} - -void PhysicsServerSharedMemory::physicsDebugDraw(int debugDrawFlags) -{ - if (m_data->m_dynamicsWorld) - { - if (m_data->m_dynamicsWorld->getDebugDrawer()) - { - m_data->m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags); - m_data->m_dynamicsWorld->debugDrawWorld(); - } - } -} - - -bool PhysicsServerSharedMemory::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) -{ - if (m_data->m_dynamicsWorld==0) - return false; - - btCollisionWorld::ClosestRayResultCallback rayCallback(rayFromWorld, rayToWorld); - - m_data->m_dynamicsWorld->rayTest(rayFromWorld, rayToWorld, rayCallback); - if (rayCallback.hasHit()) - { - - btVector3 pickPos = rayCallback.m_hitPointWorld; - btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); - if (body) - { - //other exclusions? - if (!(body->isStaticObject() || body->isKinematicObject())) - { - m_data->m_pickedBody = body; - m_data->m_pickedBody->setActivationState(DISABLE_DEACTIVATION); - //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); - btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; - btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); - m_data->m_dynamicsWorld->addConstraint(p2p, true); - m_data->m_pickedConstraint = p2p; - btScalar mousePickClamping = 30.f; - p2p->m_setting.m_impulseClamp = mousePickClamping; - //very weak constraint for picking - p2p->m_setting.m_tau = 0.001f; - } - } else - { - btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject); - if (multiCol && multiCol->m_multiBody) - { - - m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep(); - multiCol->m_multiBody->setCanSleep(false); - - btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos); - - btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos); - //if you add too much energy to the system, causing high angular velocities, simulation 'explodes' - //see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949 - //so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply - //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) - btScalar scaling=1; - p2p->setMaxAppliedImpulse(2*scaling); - - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(p2p); - m_data->m_pickingMultiBodyPoint2Point =p2p; - } - } - - - - // pickObject(pickPos, rayCallback.m_collisionObject); - m_data->m_oldPickingPos = rayToWorld; - m_data->m_hitPos = pickPos; - m_data->m_oldPickingDist = (pickPos - rayFromWorld).length(); - // printf("hit !\n"); - //add p2p - } - return false; -} -bool PhysicsServerSharedMemory::movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) -{ - if (m_data->m_pickedBody && m_data->m_pickedConstraint) - { - btPoint2PointConstraint* pickCon = static_cast(m_data->m_pickedConstraint); - if (pickCon) - { - //keep it at the same picking distance - - btVector3 dir = rayToWorld-rayFromWorld; - dir.normalize(); - dir *= m_data->m_oldPickingDist; - - btVector3 newPivotB = rayFromWorld + dir; - pickCon->setPivotB(newPivotB); - } - } - - if (m_data->m_pickingMultiBodyPoint2Point) - { - //keep it at the same picking distance - - - btVector3 dir = rayToWorld-rayFromWorld; - dir.normalize(); - dir *= m_data->m_oldPickingDist; - - btVector3 newPivotB = rayFromWorld + dir; - - m_data->m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB); - } - - return false; -} -void PhysicsServerSharedMemory::removePickingConstraint() -{ - if (m_data->m_pickedConstraint) - { - m_data->m_dynamicsWorld->removeConstraint(m_data->m_pickedConstraint); - delete m_data->m_pickedConstraint; - m_data->m_pickedConstraint = 0; - m_data->m_pickedBody = 0; - } - if (m_data->m_pickingMultiBodyPoint2Point) - { - m_data->m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(m_data->m_prevCanSleep); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->removeMultiBodyConstraint(m_data->m_pickingMultiBodyPoint2Point); - delete m_data->m_pickingMultiBodyPoint2Point; - m_data->m_pickingMultiBodyPoint2Point = 0; - } -} - -void PhysicsServerSharedMemory::enableCommandLogging(bool enable, const char* fileName) -{ - if (enable) - { - if (0==m_data->m_commandLogger) - { - m_data->m_commandLogger = new CommandLogger(fileName); - } - } else - { - if (0!=m_data->m_commandLogger) - { - delete m_data->m_commandLogger; - m_data->m_commandLogger = 0; - } - } -} - -void PhysicsServerSharedMemory::replayFromLogFile(const char* fileName) -{ - CommandLogPlayback* pb = new CommandLogPlayback(fileName); - m_data->m_logPlayback = pb; -} +} \ No newline at end of file diff --git a/examples/SharedMemory/PhysicsServer.h b/examples/SharedMemory/PhysicsServer.h index d5730aad4..e80af8193 100644 --- a/examples/SharedMemory/PhysicsServer.h +++ b/examples/SharedMemory/PhysicsServer.h @@ -1,66 +1,46 @@ -#ifndef PHYSICS_SERVER_SHARED_MEMORY_H -#define PHYSICS_SERVER_SHARED_MEMORY_H +#ifndef PHYSICS_SERVER_H +#define PHYSICS_SERVER_H #include "LinearMath/btVector3.h" -struct SharedMemLines + + +class PhysicsServer { - btVector3 m_from; - btVector3 m_to; - btVector3 m_color; -}; - - -class PhysicsServerSharedMemory -{ - struct PhysicsServerInternalData* m_data; - -protected: - - void createJointMotors(class btMultiBody* body); - virtual void createEmptyDynamicsWorld(); - virtual void deleteDynamicsWorld(); - - void releaseSharedMemory(); - - bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn, - bool useMultiBody, bool useFixedBase, int* bodyUniqueId); public: - PhysicsServerSharedMemory(); - virtual ~PhysicsServerSharedMemory(); - - virtual void setSharedMemoryKey(int key); - //todo: implement option to allocated shared memory from client - virtual bool connectSharedMemory( struct GUIHelperInterface* guiHelper); + virtual ~PhysicsServer(); + + virtual void setSharedMemoryKey(int key)=0; + + virtual bool connectSharedMemory( struct GUIHelperInterface* guiHelper)=0; - virtual void disconnectSharedMemory (bool deInitializeSharedMemory); + virtual void disconnectSharedMemory (bool deInitializeSharedMemory)=0; - virtual void processClientCommands(); + virtual void processClientCommands()=0; - bool supportsJointMotor(class btMultiBody* body, int linkIndex); +// virtual bool supportsJointMotor(class btMultiBody* body, int linkIndex)=0; //@todo(erwincoumans) Should we have shared memory commands for picking objects? ///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise - virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); - virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); - virtual void removePickingConstraint(); + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0; + virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0; + virtual void removePickingConstraint()=0; //for physicsDebugDraw and renderScene are mainly for debugging purposes //and for physics visualization. The idea is that physicsDebugDraw can also send wireframe //to a physics client, over shared memory - void physicsDebugDraw(int debugDrawFlags); - void renderScene(); + virtual void physicsDebugDraw(int debugDrawFlags)=0; + virtual void renderScene()=0; - void enableCommandLogging(bool enable, const char* fileName); - void replayFromLogFile(const char* fileName); + virtual void enableCommandLogging(bool enable, const char* fileName)=0; + virtual void replayFromLogFile(const char* fileName)=0; - }; -#endif //PHYSICS_SERVER_EXAMPLESHARED_MEMORY_H +#endif //PHYSICS_SERVER_H diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp new file mode 100644 index 000000000..b9df7b9ac --- /dev/null +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -0,0 +1,1812 @@ +#include "PhysicsServerCommandProcessor.h" + + +#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" +#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" +#include "../Importers/ImportURDFDemo/URDF2Bullet.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" +#include "../CommonInterfaces/CommonRenderInterface.h" + +#include "btBulletDynamicsCommon.h" + +#include "LinearMath/btTransform.h" + +#include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" +#include "LinearMath/btSerializer.h" +#include "Bullet3Common/b3Logging.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "SharedMemoryCommands.h" + +struct UrdfLinkNameMapUtil +{ + btMultiBody* m_mb; + btDefaultSerializer* m_memSerializer; + + UrdfLinkNameMapUtil():m_mb(0),m_memSerializer(0) + { + } +}; + + +struct SharedMemoryDebugDrawer : public btIDebugDraw +{ + + int m_debugMode; + btAlignedObjectArray m_lines2; + + SharedMemoryDebugDrawer () + :m_debugMode(0) + { + } + virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color) + { + } + + virtual void reportErrorWarning(const char* warningString) + { + } + + virtual void draw3dText(const btVector3& location,const char* textString) + { + } + + virtual void setDebugMode(int debugMode) + { + m_debugMode = debugMode; + } + + virtual int getDebugMode() const + { + return m_debugMode; + } + virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color) + { + SharedMemLines line; + line.m_from = from; + line.m_to = to; + line.m_color = color; + m_lines2.push_back(line); + } +}; + + +struct InteralBodyData +{ + btMultiBody* m_multiBody; + btRigidBody* m_rigidBody; + int m_testData; + + btTransform m_rootLocalInertialFrame; + + InteralBodyData() + :m_multiBody(0), + m_rigidBody(0), + m_testData(0) + { + m_rootLocalInertialFrame.setIdentity(); + } +}; + +///todo: templatize this +struct InternalBodyHandle : public InteralBodyData +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + + int m_nextFreeHandle; + void SetNextFree(int next) + { + m_nextFreeHandle = next; + } + int GetNextFree() const + { + return m_nextFreeHandle; + } +}; + +class btCommandChunk +{ +public: + int m_chunkCode; + int m_length; + void *m_oldPtr; + int m_dna_nr; + int m_number; +}; + + +class bCommandChunkPtr4 +{ +public: + bCommandChunkPtr4(){} + int code; + int len; + union + { + int m_uniqueInt; + }; + int dna_nr; + int nr; +}; + +// ----------------------------------------------------- // +class bCommandChunkPtr8 +{ +public: + bCommandChunkPtr8(){} + int code, len; + union + { + int m_uniqueInts[2]; + }; + int dna_nr, nr; +}; + + + +struct CommandLogger +{ + FILE* m_file; + + void writeHeader(unsigned char* buffer) const + { + +#ifdef BT_USE_DOUBLE_PRECISION + memcpy(buffer, "BT3CMDd", 7); +#else + memcpy(buffer, "BT3CMDf", 7); +#endif //BT_USE_DOUBLE_PRECISION + + int littleEndian= 1; + littleEndian= ((char*)&littleEndian)[0]; + + if (sizeof(void*)==8) + { + buffer[7] = '-'; + } else + { + buffer[7] = '_'; + } + + if (littleEndian) + { + buffer[8]='v'; + } else + { + buffer[8]='V'; + } + + buffer[9] = 0; + buffer[10] = 0; + buffer[11] = 0; + + int ver = btGetVersion(); + if (ver>=0 && ver<999) + { + sprintf((char*)&buffer[9],"%d",ver); + } + + } + + void logCommand(const SharedMemoryCommand& command) + { + btCommandChunk chunk; + chunk.m_chunkCode = command.m_type; + chunk.m_oldPtr = 0; + chunk.m_dna_nr = 0; + chunk.m_length = sizeof(SharedMemoryCommand); + chunk.m_number = 1; + fwrite((const char*)&chunk,sizeof(btCommandChunk), 1,m_file); + fwrite((const char*)&command,sizeof(SharedMemoryCommand),1,m_file); + } + + CommandLogger(const char* fileName) + { + m_file = fopen(fileName,"wb"); + unsigned char buf[15]; + buf[12] = 12; + buf[13] = 13; + buf[14] = 14; + writeHeader(buf); + fwrite(buf,12,1,m_file); + } + virtual ~CommandLogger() + { + fclose(m_file); + } +}; + + +struct CommandLogPlayback +{ + unsigned char m_header[12]; + FILE* m_file; + bool m_bitsVary; + bool m_fileIs64bit; + + + CommandLogPlayback(const char* fileName) + { + m_file = fopen(fileName,"rb"); + if (m_file) + { + fread(m_header,12,1,m_file); + } + unsigned char c = m_header[7]; + m_fileIs64bit = (c=='-'); + + const bool VOID_IS_8 = ((sizeof(void*)==8)); + m_bitsVary = (VOID_IS_8 != m_fileIs64bit); + + + + } + virtual ~CommandLogPlayback() + { + if (m_file) + { + fclose(m_file); + m_file=0; + } + } + bool processNextCommand(SharedMemoryCommand* cmd) + { + if (m_file) + { + size_t s = 0; + + + if (m_fileIs64bit) + { + bCommandChunkPtr8 chunk8; + s = fread((void*)&chunk8,sizeof(bCommandChunkPtr8),1,m_file); + } else + { + bCommandChunkPtr4 chunk4; + s = fread((void*)&chunk4,sizeof(bCommandChunkPtr4),1,m_file); + } + + if (s==1) + { + s = fread(cmd,sizeof(SharedMemoryCommand),1,m_file); + return (s==1); + } + } + return false; + + } +}; + +struct PhysicsServerCommandProcessorInternalData +{ + ///handle management + btAlignedObjectArray m_bodyHandles; + int m_numUsedHandles; // number of active handles + + int m_firstFreeHandle; // free handles list + InternalBodyHandle* getHandle(int handle) + { + btAssert(handle>=0); + btAssert(handle=m_bodyHandles.size())) + { + return 0; + } + return &m_bodyHandles[handle]; + } + const InternalBodyHandle* getHandle(int handle) const + { + return &m_bodyHandles[handle]; + } + + void increaseHandleCapacity(int extraCapacity) + { + int curCapacity = m_bodyHandles.size(); + btAssert(curCapacity == m_numUsedHandles); + int newCapacity = curCapacity + extraCapacity; + m_bodyHandles.resize(newCapacity); + + { + for (int i = curCapacity; i < newCapacity; i++) + m_bodyHandles[i].SetNextFree(i + 1); + + + m_bodyHandles[newCapacity - 1].SetNextFree(-1); + } + m_firstFreeHandle = curCapacity; + } + void initHandles() + { + m_numUsedHandles = 0; + m_firstFreeHandle = -1; + + increaseHandleCapacity(1); + } + + void exitHandles() + { + m_bodyHandles.resize(0); + m_firstFreeHandle = -1; + m_numUsedHandles = 0; + } + + int allocHandle() + { + btAssert(m_firstFreeHandle>=0); + + int handle = m_firstFreeHandle; + m_firstFreeHandle = getHandle(handle)->GetNextFree(); + m_numUsedHandles++; + + if (m_firstFreeHandle<0) + { + int curCapacity = m_bodyHandles.size(); + int additionalCapacity= m_bodyHandles.size(); + increaseHandleCapacity(additionalCapacity); + + + getHandle(handle)->SetNextFree(m_firstFreeHandle); + } + + + return handle; + } + + + void freeHandle(int handle) + { + btAssert(handle >= 0); + + getHandle(handle)->SetNextFree(m_firstFreeHandle); + m_firstFreeHandle = handle; + + m_numUsedHandles--; + } + + ///end handle management + + + CommandLogger* m_commandLogger; + CommandLogPlayback* m_logPlayback; + + + btScalar m_physicsDeltaTime; + btAlignedObjectArray m_multiBodyJointFeedbacks; + + + + btAlignedObjectArray m_worldImporters; + btAlignedObjectArray m_urdfLinkNameMapper; + btHashMap m_multiBodyJointMotorMap; + btAlignedObjectArray m_strings; + + btAlignedObjectArray m_collisionShapes; + btBroadphaseInterface* m_broadphase; + btCollisionDispatcher* m_dispatcher; + btMultiBodyConstraintSolver* m_solver; + btDefaultCollisionConfiguration* m_collisionConfiguration; + btMultiBodyDynamicsWorld* m_dynamicsWorld; + SharedMemoryDebugDrawer* m_remoteDebugDrawer; + + + + + struct GUIHelperInterface* m_guiHelper; + int m_sharedMemoryKey; + + bool m_verboseOutput; + + + //data for picking objects + class btRigidBody* m_pickedBody; + class btTypedConstraint* m_pickedConstraint; + class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point; + btVector3 m_oldPickingPos; + btVector3 m_hitPos; + btScalar m_oldPickingDist; + bool m_prevCanSleep; + + PhysicsServerCommandProcessorInternalData() + : + m_commandLogger(0), + m_logPlayback(0), + m_physicsDeltaTime(1./240.), + m_dynamicsWorld(0), + m_remoteDebugDrawer(0), + m_guiHelper(0), + m_sharedMemoryKey(SHARED_MEMORY_KEY), + m_verboseOutput(false), + m_pickedBody(0), + m_pickedConstraint(0), + m_pickingMultiBodyPoint2Point(0) + { + + initHandles(); +#if 0 + btAlignedObjectArray bla; + + for (int i=0;i<1024;i++) + { + int handle = allocHandle(); + bla.push_back(handle); + InternalBodyHandle* body = getHandle(handle); + InteralBodyData* body2 = body; + } + for (int i=0;icreatePhysicsDebugDrawer(m_data->m_dynamicsWorld); + } else + { + if (m_data->m_guiHelper && m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getDebugDrawer()) + { + + m_data->m_dynamicsWorld->setDebugDrawer(0); + } + } + m_data->m_guiHelper = guiHelper; +} + + +PhysicsServerCommandProcessor::PhysicsServerCommandProcessor() +{ + m_data = new PhysicsServerCommandProcessorInternalData(); + + createEmptyDynamicsWorld(); +} + +PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor() +{ + deleteDynamicsWorld(); + if (m_data->m_commandLogger) + { + delete m_data->m_commandLogger; + m_data->m_commandLogger = 0; + } + + delete m_data; +} + + + +void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() +{ + ///collision configuration contains default setup for memory, collision setup + m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration(); + //m_collisionConfiguration->setConvexConvexMultipointIterations(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_data->m_dispatcher = new btCollisionDispatcher(m_data->m_collisionConfiguration); + + m_data->m_broadphase = new btDbvtBroadphase(); + + m_data->m_solver = new btMultiBodyConstraintSolver; + + m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); + + m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer(); + + + m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); +} + +void PhysicsServerCommandProcessor::deleteDynamicsWorld() +{ + + for (int i=0;im_multiBodyJointFeedbacks.size();i++) + { + delete m_data->m_multiBodyJointFeedbacks[i]; + } + m_data->m_multiBodyJointFeedbacks.clear(); + + + for (int i=0;im_worldImporters.size();i++) + { + delete m_data->m_worldImporters[i]; + } + m_data->m_worldImporters.clear(); + + for (int i=0;im_urdfLinkNameMapper.size();i++) + { + delete m_data->m_urdfLinkNameMapper[i]; + } + m_data->m_urdfLinkNameMapper.clear(); + + m_data->m_multiBodyJointMotorMap.clear(); + + for (int i=0;im_strings.size();i++) + { + delete m_data->m_strings[i]; + } + m_data->m_strings.clear(); + + + if (m_data->m_dynamicsWorld) + { + + int i; + for (i = m_data->m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--) + { + m_data->m_dynamicsWorld->removeConstraint(m_data->m_dynamicsWorld->getConstraint(i)); + } + for (i = m_data->m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_data->m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + } + //delete collision shapes + for (int j = 0; jm_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_data->m_collisionShapes[j]; + delete shape; + } + m_data->m_collisionShapes.clear(); + + delete m_data->m_dynamicsWorld; + m_data->m_dynamicsWorld=0; + + delete m_data->m_remoteDebugDrawer; + m_data->m_remoteDebugDrawer =0; + + delete m_data->m_solver; + m_data->m_solver=0; + + delete m_data->m_broadphase; + m_data->m_broadphase=0; + + delete m_data->m_dispatcher; + m_data->m_dispatcher=0; + + delete m_data->m_collisionConfiguration; + m_data->m_collisionConfiguration=0; + +} + + + + +bool PhysicsServerCommandProcessor::supportsJointMotor(btMultiBody* mb, int mbLinkIndex) +{ + bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute + ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic); + return canHaveMotor; + +} + +//for testing, create joint motors for revolute and prismatic joints +void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) +{ + int numLinks = mb->getNumLinks(); + for (int i=0;isetMaxAppliedImpulse(0); + m_data->m_multiBodyJointMotorMap.insert(mbLinkIndex,motor); + m_data->m_dynamicsWorld->addMultiBodyConstraint(motor); + } + + } +} + + + +bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, + bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes) +{ + btAssert(m_data->m_dynamicsWorld); + if (!m_data->m_dynamicsWorld) + { + b3Error("loadUrdf: No valid m_dynamicsWorld"); + return false; + } + + BulletURDFImporter u2b(m_data->m_guiHelper); + + + bool loadOk = u2b.loadURDF(fileName, useFixedBase); + if (loadOk) + { + //get a body index + int bodyUniqueId = m_data->allocHandle(); + if (bodyUniqueIdPtr) + *bodyUniqueIdPtr= bodyUniqueId; + + InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); + + { + btScalar mass = 0; + bodyHandle->m_rootLocalInertialFrame.setIdentity(); + btVector3 localInertiaDiagonal(0,0,0); + int urdfLinkIndex = u2b.getRootLinkIndex(); + u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame); + } + if (m_data->m_verboseOutput) + { + 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_data->m_guiHelper); + + ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix()); + btMultiBody* mb = creation.getBulletMultiBody(); + if (useMultiBody) + { + if (mb) + { + bodyHandle->m_multiBody = mb; + createJointMotors(mb); + + + //serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire + UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil; + m_data->m_urdfLinkNameMapper.push_back(util); + util->m_mb = mb; + util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient); + //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); + util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0); + + for (int i=0;igetNumLinks();i++) + { + //disable serialization of the collision objects + util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0); + int urdfLinkIndex = creation.m_mb2urdfLink[i]; + std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str()); + m_data->m_strings.push_back(linkName); + util->m_memSerializer->registerNameForPointer(linkName->c_str(),linkName->c_str()); + mb->getLink(i).m_linkName = linkName->c_str(); + + std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str()); + m_data->m_strings.push_back(jointName); + util->m_memSerializer->registerNameForPointer(jointName->c_str(),jointName->c_str()); + mb->getLink(i).m_jointName = jointName->c_str(); + } + + std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); + m_data->m_strings.push_back(baseName); + + + util->m_memSerializer->registerNameForPointer(baseName->c_str(),baseName->c_str()); + mb->setBaseName(baseName->c_str()); + + + util->m_memSerializer->insertHeader(); + + int len = mb->calculateSerializeBufferSize(); + btChunk* chunk = util->m_memSerializer->allocate(len,1); + const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer); + util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb); + + return true; + } else + { + b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); + return false; + } + } else + { + btAssert(0); + + return true; + } + + } + + return false; +} + + + + + +bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes ) +{ + + bool hasStatus = false; + + { +#if 0 + if (m_data->m_logPlayback) + { + if (m_data->m_testBlock1->m_numServerCommands>m_data->m_testBlock1->m_numProcessedServerCommands) + { + m_data->m_testBlock1->m_numProcessedServerCommands++; + } + //push a command from log file + bool hasCommand = m_data->m_logPlayback->processNextCommand(&m_data->m_testBlock1->m_clientCommands[0]); + if (hasCommand) + { + m_data->m_testBlock1->m_numClientCommands++; + } + } +#endif + + ///we ignore overflow of integer for now + + { + + //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands + + + //const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0]; +#if 0 + if (m_data->m_commandLogger) + { + m_data->m_commandLogger->logCommand(m_data->m_testBlock1); + } +#endif + + //m_data->m_testBlock1->m_numProcessedClientCommands++; + + //no timestamp yet + int timeStamp = 0; + + //consume the command + switch (clientCmd.m_type) + { +#if 0 + case CMD_SEND_BULLET_DATA_STREAM: + { + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength); + } + + btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); + m_data->m_worldImporters.push_back(worldImporter); + bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength); + + if (completedOk) + { + SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + m_data->submitServerStatus(status); + } else + { + SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,clientCmd.m_sequenceNumber,timeStamp); + m_data->submitServerStatus(status); + } + + break; + } +#endif + + case CMD_REQUEST_DEBUG_LINES: + { + int curFlags =m_data->m_remoteDebugDrawer->getDebugMode(); + + int debugMode = clientCmd.m_requestDebugLinesArguments.m_debugMode;//clientCmd.btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb; + int startingLineIndex = clientCmd.m_requestDebugLinesArguments.m_startingLineIndex; + if (startingLineIndex<0) + { + b3Warning("startingLineIndex should be non-negative"); + startingLineIndex = 0; + } + + if (clientCmd.m_requestDebugLinesArguments.m_startingLineIndex==0) + { + m_data->m_remoteDebugDrawer->m_lines2.resize(0); + //|btIDebugDraw::DBG_DrawAabb| + // btIDebugDraw::DBG_DrawConstraints |btIDebugDraw::DBG_DrawConstraintLimits ; + m_data->m_remoteDebugDrawer->setDebugMode(debugMode); + btIDebugDraw* oldDebugDrawer = m_data->m_dynamicsWorld->getDebugDrawer(); + m_data->m_dynamicsWorld->setDebugDrawer(m_data->m_remoteDebugDrawer); + m_data->m_dynamicsWorld->debugDrawWorld(); + m_data->m_dynamicsWorld->setDebugDrawer(oldDebugDrawer); + m_data->m_remoteDebugDrawer->setDebugMode(curFlags); + } + + //9 floats per line: 3 floats for 'from', 3 floats for 'to' and 3 floats for 'color' + int maxNumLines = bufferSizeInBytes/(sizeof(float)*9)-1; + if (startingLineIndex >m_data->m_remoteDebugDrawer->m_lines2.size()) + { + b3Warning("m_startingLineIndex exceeds total number of debug lines"); + startingLineIndex =m_data->m_remoteDebugDrawer->m_lines2.size(); + } + + int numLines = btMin(maxNumLines,m_data->m_remoteDebugDrawer->m_lines2.size()-startingLineIndex); + + if (numLines) + { + + float* linesFrom = (float*)bufferServerToClient; + float* linesTo = (float*)(bufferServerToClient+numLines*3*sizeof(float)); + float* linesColor = (float*)(bufferServerToClient+2*numLines*3*sizeof(float)); + + for (int i=0;im_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.x(); + linesTo[i*3] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.x(); + linesColor[i*3] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.x(); + + linesFrom[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.y(); + linesTo[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.y(); + linesColor[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.y(); + + linesFrom[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.z(); + linesTo[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.z(); + linesColor[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.z(); + } + } + + serverStatusOut.m_type = CMD_DEBUG_LINES_COMPLETED; + + serverStatusOut.m_sendDebugLinesArgs.m_numDebugLines = numLines; + serverStatusOut.m_sendDebugLinesArgs.m_startingLineIndex = startingLineIndex; + serverStatusOut.m_sendDebugLinesArgs.m_numRemainingDebugLines = m_data->m_remoteDebugDrawer->m_lines2.size()-(startingLineIndex+numLines); + hasStatus = true; + + break; + } + + case CMD_LOAD_URDF: + { + + const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments; + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_LOAD_URDF:%s", urdfArgs.m_urdfFileName); + } + btAssert((clientCmd.m_updateFlags&URDF_ARGS_FILE_NAME) !=0); + btAssert(urdfArgs.m_urdfFileName); + btVector3 initialPos(0,0,0); + btQuaternion initialOrn(0,0,0,1); + if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_POSITION) + { + initialPos[0] = urdfArgs.m_initialPosition[0]; + initialPos[1] = urdfArgs.m_initialPosition[1]; + initialPos[2] = urdfArgs.m_initialPosition[2]; + } + if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION) + { + initialOrn[0] = urdfArgs.m_initialOrientation[0]; + initialOrn[1] = urdfArgs.m_initialOrientation[1]; + initialOrn[2] = urdfArgs.m_initialOrientation[2]; + initialOrn[3] = urdfArgs.m_initialOrientation[3]; + } + bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? urdfArgs.m_useMultiBody : true; + bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? urdfArgs.m_useFixedBase: false; + int bodyUniqueId; + //load the actual URDF and send a report: completed or failed + bool completedOk = loadUrdf(urdfArgs.m_urdfFileName, + initialPos,initialOrn, + useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + + + if (completedOk) + { + + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + + serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED; + if (m_data->m_urdfLinkNameMapper.size()) + { + serverStatusOut.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); + } + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; + hasStatus = true; + + } else + { + serverStatusOut.m_type = CMD_URDF_LOADING_FAILED; + hasStatus = true; + } + + + + + break; + } + case CMD_CREATE_SENSOR: + { + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_CREATE_SENSOR"); + } + int bodyUniqueId = clientCmd.m_createSensorArguments.m_bodyUniqueId; + InteralBodyData* body = m_data->getHandle(bodyUniqueId); + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + btAssert(mb); + for (int i=0;igetLink(jointIndex).m_jointFeedback) + { + b3Warning("CMD_CREATE_SENSOR: sensor for joint [%d] already enabled", jointIndex); + } else + { + btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback(); + fb->m_reactionForces.setZero(); + mb->getLink(jointIndex).m_jointFeedback = fb; + m_data->m_multiBodyJointFeedbacks.push_back(fb); + }; + + } else + { + if (mb->getLink(jointIndex).m_jointFeedback) + { + m_data->m_multiBodyJointFeedbacks.remove(mb->getLink(jointIndex).m_jointFeedback); + delete mb->getLink(jointIndex).m_jointFeedback; + mb->getLink(jointIndex).m_jointFeedback=0; + } else + { + b3Warning("CMD_CREATE_SENSOR: cannot perform sensor removal request, no sensor on joint [%d]", jointIndex); + }; + + } + } + + } else + { + b3Warning("No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet"); + } + +#if 0 + //todo(erwincoumans) here is some sample code to hook up a force/torque sensor for btTypedConstraint/btRigidBody + /* + for (int i=0;im_dynamicsWorld->getNumConstraints();i++) + { + btTypedConstraint* c = m_data->m_dynamicsWorld->getConstraint(i); + btJointFeedback* fb = new btJointFeedback(); + m_data->m_jointFeedbacks.push_back(fb); + c->setJointFeedback(fb); + + + } + */ +#endif + + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; + hasStatus = true; + + break; + } + case CMD_SEND_DESIRED_STATE: + { + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_SEND_DESIRED_STATE"); + } + + int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId; + InteralBodyData* body = m_data->getHandle(bodyUniqueId); + + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + btAssert(mb); + + switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) + { + case CONTROL_MODE_TORQUE: + { + if (m_data->m_verboseOutput) + { + b3Printf("Using CONTROL_MODE_TORQUE"); + } + mb->clearForcesAndTorques(); + + int torqueIndex = 0; + btVector3 f(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]); + btVector3 t(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]); + torqueIndex+=6; + mb->addBaseForce(f); + mb->addBaseTorque(t); + for (int link=0;linkgetNumLinks();link++) + { + + for (int dof=0;dofgetLink(link).m_dofCount;dof++) + { + double torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; + mb->addJointTorqueMultiDof(link,dof,torque); + torqueIndex++; + } + } + break; + } + case CONTROL_MODE_VELOCITY: + { + if (m_data->m_verboseOutput) + { + b3Printf("Using CONTROL_MODE_VELOCITY"); + } + + int numMotors = 0; + //find the joint motors and apply the desired velocity and maximum force/torque + { + int dofIndex = 6;//skip the 3 linear + 3 angular degree of freedom entries of the base + for (int link=0;linkgetNumLinks();link++) + { + if (supportsJointMotor(mb,link)) + { + + btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link]; + if (motorPtr) + { + btMultiBodyJointMotor* motor = *motorPtr; + btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; + motor->setVelocityTarget(desiredVelocity); + + btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; + motor->setMaxAppliedImpulse(maxImp); + numMotors++; + + } + } + dofIndex += mb->getLink(link).m_dofCount; + } + } + break; + } + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + if (m_data->m_verboseOutput) + { + b3Printf("Using CONTROL_MODE_POSITION_VELOCITY_PD"); + } + //compute the force base on PD control + mb->clearForcesAndTorques(); + + int numMotors = 0; + //find the joint motors and apply the desired velocity and maximum force/torque + { + int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base + int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base + for (int link=0;linkgetNumLinks();link++) + { + if (supportsJointMotor(mb,link)) + { + + btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link]; + if (motorPtr) + { + btMultiBodyJointMotor* motor = *motorPtr; + + btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; + btScalar desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; + + btScalar kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; + btScalar kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; + + int dof1 = 0; + btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1]; + btScalar currentVelocity = mb->getJointVelMultiDof(link)[dof1]; + btScalar positionStabiliationTerm = (desiredPosition-currentPosition)/m_data->m_physicsDeltaTime; + btScalar velocityError = (desiredVelocity - currentVelocity); + + desiredVelocity = kp * positionStabiliationTerm + + kd * velocityError; + + motor->setVelocityTarget(desiredVelocity); + + btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime; + + motor->setMaxAppliedImpulse(1000);//maxImp); + numMotors++; + } + + } + velIndex += mb->getLink(link).m_dofCount; + posIndex += mb->getLink(link).m_posVarCount; + } + } + + break; + } + default: + { + b3Warning("m_controlMode not implemented yet"); + break; + } + + } + } + + serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; + hasStatus = true; + break; + } + case CMD_REQUEST_ACTUAL_STATE: + { + if (m_data->m_verboseOutput) + { + b3Printf("Sending the actual state (Q,U)"); + } + int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; + InteralBodyData* body = m_data->getHandle(bodyUniqueId); + + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; + + serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; + int totalDegreeOfFreedomQ = 0; + int totalDegreeOfFreedomU = 0; + + + //always add the base, even for static (non-moving objects) + //so that we can easily move the 'fixed' base when needed + //do we don't use this conditional "if (!mb->hasFixedBase())" + { + btTransform tr; + tr.setOrigin(mb->getBasePos()); + tr.setRotation(mb->getWorldToBaseRot().inverse()); + + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = + body->m_rootLocalInertialFrame.getOrigin()[0]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = + body->m_rootLocalInertialFrame.getOrigin()[1]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = + body->m_rootLocalInertialFrame.getOrigin()[2]; + + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = + body->m_rootLocalInertialFrame.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = + body->m_rootLocalInertialFrame.getRotation()[1]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] = + body->m_rootLocalInertialFrame.getRotation()[2]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = + body->m_rootLocalInertialFrame.getRotation()[3]; + + + + //base position in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; + + //base orientation, quaternion x,y,z,w, in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; + totalDegreeOfFreedomQ +=7;//pos + quaternion + + //base linear velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2]; + + //base angular velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = mb->getBaseOmega()[2]; + totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF + } + for (int l=0;lgetNumLinks();l++) + { + for (int d=0;dgetLink(l).m_posVarCount;d++) + { + serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d]; + } + for (int d=0;dgetLink(l).m_dofCount;d++) + { + serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; + } + + if (0 == mb->getLink(l).m_jointFeedback) + { + for (int d=0;d<6;d++) + { + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+d]=0; + } + } else + { + btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear(); + btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getAngular(); + + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = sensedForce[0]; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = sensedForce[1]; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = sensedForce[2]; + + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = sensedTorque[0]; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1]; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2]; + } + } + + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; + + hasStatus = true; + + } else + { + if (body && body->m_rigidBody) + { + btRigidBody* rb = body->m_rigidBody; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; + + serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; + int totalDegreeOfFreedomQ = 0; + int totalDegreeOfFreedomU = 0; + + btTransform tr = rb->getWorldTransform(); + //base position in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; + + //base orientation, quaternion x,y,z,w, in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; + totalDegreeOfFreedomQ +=7;//pos + quaternion + + //base linear velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = rb->getLinearVelocity()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = rb->getLinearVelocity()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = rb->getLinearVelocity()[2]; + + //base angular velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = rb->getAngularVelocity()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = rb->getAngularVelocity()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = rb->getAngularVelocity()[2]; + totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF + + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; + + hasStatus = true; + } else + { + b3Warning("Request state but no multibody or rigid body available"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; + hasStatus = true; + } + } + + break; + } + case CMD_STEP_FORWARD_SIMULATION: + { + + if (m_data->m_verboseOutput) + { + b3Printf("Step simulation request"); + } + m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0); + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; + hasStatus = true; + break; + } + + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: + { + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) + { + m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime; + } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY) + { + btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0], + clientCmd.m_physSimParamArgs.m_gravityAcceleration[1], + clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]); + this->m_data->m_dynamicsWorld->setGravity(grav); + if (m_data->m_verboseOutput) + { + b3Printf("Updated Gravity: %f,%f,%f",grav[0],grav[1],grav[2]); + } + + } + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + hasStatus = true; + break; + + }; + case CMD_INIT_POSE: + { + if (m_data->m_verboseOutput) + { + b3Printf("Server Init Pose not implemented yet"); + } + int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId; + InteralBodyData* body = m_data->getHandle(bodyUniqueId); + + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) + { + btVector3 zero(0,0,0); + mb->setBaseVel(zero); + mb->setBasePos(btVector3( + clientCmd.m_initPoseArgs.m_initialStateQ[0], + clientCmd.m_initPoseArgs.m_initialStateQ[1], + clientCmd.m_initPoseArgs.m_initialStateQ[2])); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) + { + mb->setBaseOmega(btVector3(0,0,0)); + mb->setWorldToBaseRot(btQuaternion( + clientCmd.m_initPoseArgs.m_initialStateQ[3], + clientCmd.m_initPoseArgs.m_initialStateQ[4], + clientCmd.m_initPoseArgs.m_initialStateQ[5], + clientCmd.m_initPoseArgs.m_initialStateQ[6])); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE) + { + int dofIndex = 7; + for (int i=0;igetNumLinks();i++) + { + if (mb->getLink(i).m_dofCount==1) + { + + mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[dofIndex]); + mb->setJointVel(i,0); + } + dofIndex += mb->getLink(i).m_dofCount; + } + } + } + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + hasStatus = true; + + break; + } + + + case CMD_RESET_SIMULATION: + { + //clean up all data + if (m_data && m_data->m_guiHelper && m_data->m_guiHelper->getRenderInterface()) + { + m_data->m_guiHelper->getRenderInterface()->removeAllInstances(); + } + deleteDynamicsWorld(); + createEmptyDynamicsWorld(); + m_data->exitHandles(); + m_data->initHandles(); + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED; + hasStatus = true; + + break; + } + case CMD_CREATE_RIGID_BODY: + case CMD_CREATE_BOX_COLLISION_SHAPE: + { + btVector3 halfExtents(1,1,1); + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS) + { + halfExtents = btVector3( + clientCmd.m_createBoxShapeArguments.m_halfExtentsX, + clientCmd.m_createBoxShapeArguments.m_halfExtentsY, + clientCmd.m_createBoxShapeArguments.m_halfExtentsZ); + } + btTransform startTrans; + startTrans.setIdentity(); + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_POSITION) + { + startTrans.setOrigin(btVector3( + clientCmd.m_createBoxShapeArguments.m_initialPosition[0], + clientCmd.m_createBoxShapeArguments.m_initialPosition[1], + clientCmd.m_createBoxShapeArguments.m_initialPosition[2])); + } + + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION) + { + + startTrans.setRotation(btQuaternion( + clientCmd.m_createBoxShapeArguments.m_initialOrientation[0], + clientCmd.m_createBoxShapeArguments.m_initialOrientation[1], + clientCmd.m_createBoxShapeArguments.m_initialOrientation[2], + clientCmd.m_createBoxShapeArguments.m_initialOrientation[3])); + } + + btScalar mass = 0.f; + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_MASS) + { + mass = clientCmd.m_createBoxShapeArguments.m_mass; + } + + int shapeType = COLLISION_SHAPE_TYPE_BOX; + + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE) + { + shapeType = clientCmd.m_createBoxShapeArguments.m_collisionShapeType; + } + + btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); + m_data->m_worldImporters.push_back(worldImporter); + + btCollisionShape* shape = 0; + + switch (shapeType) + { + case COLLISION_SHAPE_TYPE_CYLINDER_X: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[0]; + shape = worldImporter->createCylinderShapeX(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CYLINDER_Y: + { + btScalar radius = halfExtents[0]; + btScalar height = halfExtents[1]; + shape = worldImporter->createCylinderShapeY(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CYLINDER_Z: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[2]; + shape = worldImporter->createCylinderShapeZ(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CAPSULE_X: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[0]; + shape = worldImporter->createCapsuleShapeX(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CAPSULE_Y: + { + btScalar radius = halfExtents[0]; + btScalar height = halfExtents[1]; + shape = worldImporter->createCapsuleShapeY(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CAPSULE_Z: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[2]; + shape = worldImporter->createCapsuleShapeZ(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_SPHERE: + { + btScalar radius = halfExtents[0]; + shape = worldImporter->createSphereShape(radius); + break; + } + case COLLISION_SHAPE_TYPE_BOX: + default: + { + shape = worldImporter->createBoxShape(halfExtents); + } + } + + + bool isDynamic = (mass>0); + btRigidBody* rb = worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); + rb->setRollingFriction(0.2); + //m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + btVector4 colorRGBA(1,0,0,1); + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLOR) + { + colorRGBA[0] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[0]; + colorRGBA[1] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[1]; + colorRGBA[2] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[2]; + colorRGBA[3] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[3]; + } + m_data->m_guiHelper->createCollisionShapeGraphicsObject(rb->getCollisionShape()); + m_data->m_guiHelper->createCollisionObjectGraphicsObject(rb,colorRGBA); + + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_RIGID_BODY_CREATION_COMPLETED; + + + int bodyUniqueId = m_data->allocHandle(); + InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); + serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId; + bodyHandle->m_rootLocalInertialFrame.setIdentity(); + bodyHandle->m_rigidBody = rb; + hasStatus = true; + 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 =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + hasStatus = true; + 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 =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + hasStatus = true; + break; + } + case CMD_REMOVE_PICKING_CONSTRAINT_BODY: + { + removePickingConstraint(); + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + hasStatus = true; + break; + } + default: + { + b3Error("Unknown command encountered"); + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; + hasStatus = true; + + + } + }; + + } + } + return hasStatus; +} + + +void PhysicsServerCommandProcessor::renderScene() +{ + if (m_data->m_guiHelper) + { + m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld); + + m_data->m_guiHelper->render(m_data->m_dynamicsWorld); + } + +} + +void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags) +{ + if (m_data->m_dynamicsWorld) + { + if (m_data->m_dynamicsWorld->getDebugDrawer()) + { + m_data->m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags); + m_data->m_dynamicsWorld->debugDrawWorld(); + } + } +} + + +bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) +{ + if (m_data->m_dynamicsWorld==0) + return false; + + btCollisionWorld::ClosestRayResultCallback rayCallback(rayFromWorld, rayToWorld); + + m_data->m_dynamicsWorld->rayTest(rayFromWorld, rayToWorld, rayCallback); + if (rayCallback.hasHit()) + { + + btVector3 pickPos = rayCallback.m_hitPointWorld; + btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); + if (body) + { + //other exclusions? + if (!(body->isStaticObject() || body->isKinematicObject())) + { + m_data->m_pickedBody = body; + m_data->m_pickedBody->setActivationState(DISABLE_DEACTIVATION); + //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); + btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; + btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); + m_data->m_dynamicsWorld->addConstraint(p2p, true); + m_data->m_pickedConstraint = p2p; + btScalar mousePickClamping = 30.f; + p2p->m_setting.m_impulseClamp = mousePickClamping; + //very weak constraint for picking + p2p->m_setting.m_tau = 0.001f; + } + } else + { + btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject); + if (multiCol && multiCol->m_multiBody) + { + + m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep(); + multiCol->m_multiBody->setCanSleep(false); + + btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos); + + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos); + //if you add too much energy to the system, causing high angular velocities, simulation 'explodes' + //see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949 + //so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply + //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) + btScalar scaling=1; + p2p->setMaxAppliedImpulse(2*scaling); + + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(p2p); + m_data->m_pickingMultiBodyPoint2Point =p2p; + } + } + + + + // pickObject(pickPos, rayCallback.m_collisionObject); + m_data->m_oldPickingPos = rayToWorld; + m_data->m_hitPos = pickPos; + m_data->m_oldPickingDist = (pickPos - rayFromWorld).length(); + // printf("hit !\n"); + //add p2p + } + return false; +} + + +bool PhysicsServerCommandProcessor::movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) +{ + if (m_data->m_pickedBody && m_data->m_pickedConstraint) + { + btPoint2PointConstraint* pickCon = static_cast(m_data->m_pickedConstraint); + if (pickCon) + { + //keep it at the same picking distance + + btVector3 dir = rayToWorld-rayFromWorld; + dir.normalize(); + dir *= m_data->m_oldPickingDist; + + btVector3 newPivotB = rayFromWorld + dir; + pickCon->setPivotB(newPivotB); + } + } + + if (m_data->m_pickingMultiBodyPoint2Point) + { + //keep it at the same picking distance + + + btVector3 dir = rayToWorld-rayFromWorld; + dir.normalize(); + dir *= m_data->m_oldPickingDist; + + btVector3 newPivotB = rayFromWorld + dir; + + m_data->m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB); + } + + return false; +} + +void PhysicsServerCommandProcessor::removePickingConstraint() +{ + if (m_data->m_pickedConstraint) + { + m_data->m_dynamicsWorld->removeConstraint(m_data->m_pickedConstraint); + delete m_data->m_pickedConstraint; + m_data->m_pickedConstraint = 0; + m_data->m_pickedBody = 0; + } + if (m_data->m_pickingMultiBodyPoint2Point) + { + m_data->m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(m_data->m_prevCanSleep); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->removeMultiBodyConstraint(m_data->m_pickingMultiBodyPoint2Point); + delete m_data->m_pickingMultiBodyPoint2Point; + m_data->m_pickingMultiBodyPoint2Point = 0; + } +} + + +void PhysicsServerCommandProcessor::enableCommandLogging(bool enable, const char* fileName) +{ + if (enable) + { + if (0==m_data->m_commandLogger) + { + m_data->m_commandLogger = new CommandLogger(fileName); + } + } else + { + if (0!=m_data->m_commandLogger) + { + delete m_data->m_commandLogger; + m_data->m_commandLogger = 0; + } + } +} + + +void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName) +{ + CommandLogPlayback* pb = new CommandLogPlayback(fileName); + m_data->m_logPlayback = pb; +} + diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h new file mode 100644 index 000000000..31448f292 --- /dev/null +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -0,0 +1,53 @@ +#ifndef PHYSICS_SERVER_COMMAND_PROCESSOR_H +#define PHYSICS_SERVER_COMMAND_PROCESSOR_H + +#include "LinearMath/btVector3.h" + +struct SharedMemLines +{ + btVector3 m_from; + btVector3 m_to; + btVector3 m_color; +}; + +///todo: naming. Perhaps PhysicsSdkCommandprocessor? +class PhysicsServerCommandProcessor +{ + + struct PhysicsServerCommandProcessorInternalData* m_data; + +protected: + + + bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn, + bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes); + + bool supportsJointMotor(class btMultiBody* body, int linkIndex); + +public: + PhysicsServerCommandProcessor(); + virtual ~PhysicsServerCommandProcessor(); + + void createJointMotors(class btMultiBody* body); + + virtual void createEmptyDynamicsWorld(); + virtual void deleteDynamicsWorld(); + + virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes ); + + virtual void renderScene(); + virtual void physicsDebugDraw(int debugDrawFlags); + virtual void setGuiHelper(struct GUIHelperInterface* guiHelper); + + //@todo(erwincoumans) Should we have shared memory commands for picking objects? + ///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); + virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); + virtual void removePickingConstraint(); + + void enableCommandLogging(bool enable, const char* fileName); + void replayFromLogFile(const char* fileName); + +}; + +#endif //PHYSICS_SERVER_COMMAND_PROCESSOR_H diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 7e21df1d9..8f7df0c5a 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -5,7 +5,7 @@ -#include "PhysicsServer.h" +#include "PhysicsServerSharedMemory.h" #include "SharedMemoryCommon.h" diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp new file mode 100644 index 000000000..dc6846fa6 --- /dev/null +++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp @@ -0,0 +1,304 @@ +#include "PhysicsServerSharedMemory.h" +#include "PosixSharedMemory.h" +#include "Win32SharedMemory.h" + +#include "../CommonInterfaces/CommonRenderInterface.h" + +#include "btBulletDynamicsCommon.h" + +#include "LinearMath/btTransform.h" + +#include "Bullet3Common/b3Logging.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "SharedMemoryBlock.h" + +#include "PhysicsServerCommandProcessor.h" + + + +struct PhysicsServerSharedMemoryInternalData +{ + + ///end handle management + + + SharedMemoryInterface* m_sharedMemory; + SharedMemoryBlock* m_testBlock1; + int m_sharedMemoryKey; + bool m_isConnected; + bool m_verboseOutput; + PhysicsServerCommandProcessor* m_commandProcessor; + + PhysicsServerSharedMemoryInternalData() + :m_sharedMemory(0), + m_testBlock1(0), + m_sharedMemoryKey(SHARED_MEMORY_KEY), + m_isConnected(false), + m_verboseOutput(false), + m_commandProcessor(0) + + { + + } + + SharedMemoryStatus& createServerStatus(int statusType, int sequenceNumber, int timeStamp) + { + SharedMemoryStatus& serverCmd =m_testBlock1->m_serverCommands[0]; + serverCmd .m_type = statusType; + serverCmd.m_sequenceNumber = sequenceNumber; + serverCmd.m_timeStamp = timeStamp; + return serverCmd; + } + void submitServerStatus(SharedMemoryStatus& status) + { + m_testBlock1->m_numServerCommands++; + } + +}; + + +PhysicsServerSharedMemory::PhysicsServerSharedMemory() +{ + m_data = new PhysicsServerSharedMemoryInternalData(); + +#ifdef _WIN32 + m_data->m_sharedMemory = new Win32SharedMemoryServer(); +#else + m_data->m_sharedMemory = new PosixSharedMemory(); +#endif + + m_data->m_commandProcessor = new PhysicsServerCommandProcessor; + m_data->m_commandProcessor ->createEmptyDynamicsWorld(); + + +} + +PhysicsServerSharedMemory::~PhysicsServerSharedMemory() +{ + m_data->m_commandProcessor->deleteDynamicsWorld(); + delete m_data->m_commandProcessor; + delete m_data; +} + +void PhysicsServerSharedMemory::setSharedMemoryKey(int key) +{ + m_data->m_sharedMemoryKey = key; +} + + +bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* guiHelper) +{ + + m_data->m_commandProcessor->setGuiHelper(guiHelper); + + + bool allowCreation = true; + + + if (m_data->m_isConnected) + { + b3Warning("connectSharedMemory, while already connected"); + return m_data->m_isConnected; + } + + + int counter = 0; + do + { + + m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE,allowCreation); + if (m_data->m_testBlock1) + { + int magicId =m_data->m_testBlock1->m_magicId; + if (m_data->m_verboseOutput) + { + b3Printf("magicId = %d\n", magicId); + } + + if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) + { + InitSharedMemoryBlock(m_data->m_testBlock1); + if (m_data->m_verboseOutput) + { + b3Printf("Created and initialized shared memory block\n"); + } + m_data->m_isConnected = true; + } else + { + m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); + m_data->m_testBlock1 = 0; + m_data->m_isConnected = false; + } + } else + { + b3Error("Cannot connect to shared memory"); + m_data->m_isConnected = false; + } + } while (counter++ < 10 && !m_data->m_isConnected); + + if (!m_data->m_isConnected) + { + b3Error("Server cannot connect to shared memory.\n"); + } + + return m_data->m_isConnected; +} + + +void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory) +{ + m_data->m_commandProcessor->setGuiHelper(0); + + if (m_data->m_verboseOutput) + { + b3Printf("releaseSharedMemory1\n"); + } + if (m_data->m_testBlock1) + { + if (m_data->m_verboseOutput) + { + b3Printf("m_testBlock1\n"); + } + if (deInitializeSharedMemory) + { + m_data->m_testBlock1->m_magicId = 0; + if (m_data->m_verboseOutput) + { + b3Printf("De-initialized shared memory, magic id = %d\n",m_data->m_testBlock1->m_magicId); + } + } + btAssert(m_data->m_sharedMemory); + m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); + } + if (m_data->m_sharedMemory) + { + if (m_data->m_verboseOutput) + { + b3Printf("m_sharedMemory\n"); + } + delete m_data->m_sharedMemory; + m_data->m_sharedMemory = 0; + m_data->m_testBlock1 = 0; + } +} + +void PhysicsServerSharedMemory::releaseSharedMemory() +{ + if (m_data->m_verboseOutput) + { + b3Printf("releaseSharedMemory1\n"); + } + if (m_data->m_testBlock1) + { + if (m_data->m_verboseOutput) + { + b3Printf("m_testBlock1\n"); + } + m_data->m_testBlock1->m_magicId = 0; + if (m_data->m_verboseOutput) + { + b3Printf("magic id = %d\n",m_data->m_testBlock1->m_magicId); + } + btAssert(m_data->m_sharedMemory); + m_data->m_sharedMemory->releaseSharedMemory( m_data->m_sharedMemoryKey +, SHARED_MEMORY_SIZE); + } + if (m_data->m_sharedMemory) + { + if (m_data->m_verboseOutput) + { + b3Printf("m_sharedMemory\n"); + } + delete m_data->m_sharedMemory; + m_data->m_sharedMemory = 0; + m_data->m_testBlock1 = 0; + } +} + + + + + + +void PhysicsServerSharedMemory::processClientCommands() +{ + if (m_data->m_isConnected && m_data->m_testBlock1) + { +#if 0 + m_data->m_commandProcessor->processLogCommand(); + + if (m_data->m_logPlayback) + { + if (m_data->m_testBlock1->m_numServerCommands>m_data->m_testBlock1->m_numProcessedServerCommands) + { + m_data->m_testBlock1->m_numProcessedServerCommands++; + } + //push a command from log file + bool hasCommand = m_data->m_logPlayback->processNextCommand(&m_data->m_testBlock1->m_clientCommands[0]); + if (hasCommand) + { + m_data->m_testBlock1->m_numClientCommands++; + } + } +#endif 0 + ///we ignore overflow of integer for now + if (m_data->m_testBlock1->m_numClientCommands> m_data->m_testBlock1->m_numProcessedClientCommands) + { + + + //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands + btAssert(m_data->m_testBlock1->m_numClientCommands==m_data->m_testBlock1->m_numProcessedClientCommands+1); + + const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0]; + + m_data->m_testBlock1->m_numProcessedClientCommands++; + //todo, timeStamp + int timeStamp = 0; + SharedMemoryStatus& serverStatusOut = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); + bool hasStatus = m_data->m_commandProcessor->processCommand(clientCmd, serverStatusOut,&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + if (hasStatus) + { + m_data->submitServerStatus(serverStatusOut); + } + + } + } +} + +void PhysicsServerSharedMemory::renderScene() +{ + m_data->m_commandProcessor->renderScene(); + + + +} + +void PhysicsServerSharedMemory::physicsDebugDraw(int debugDrawFlags) +{ + m_data->m_commandProcessor->physicsDebugDraw(debugDrawFlags); +} + + +bool PhysicsServerSharedMemory::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) +{ + return m_data->m_commandProcessor->pickBody(rayFromWorld,rayToWorld); +} + +bool PhysicsServerSharedMemory::movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) +{ + return m_data->m_commandProcessor->movePickedBody(rayFromWorld,rayToWorld); +} +void PhysicsServerSharedMemory::removePickingConstraint() +{ + m_data->m_commandProcessor->removePickingConstraint(); +} + +void PhysicsServerSharedMemory::enableCommandLogging(bool enable, const char* fileName) +{ + m_data->m_commandProcessor->enableCommandLogging(enable,fileName); +} + +void PhysicsServerSharedMemory::replayFromLogFile(const char* fileName) +{ + m_data->m_commandProcessor->replayFromLogFile(fileName); +} diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.h b/examples/SharedMemory/PhysicsServerSharedMemory.h new file mode 100644 index 000000000..7547bd991 --- /dev/null +++ b/examples/SharedMemory/PhysicsServerSharedMemory.h @@ -0,0 +1,52 @@ +#ifndef PHYSICS_SERVER_SHARED_MEMORY_H +#define PHYSICS_SERVER_SHARED_MEMORY_H + +#include "PhysicsServer.h" + +class PhysicsServerSharedMemory : public PhysicsServer +{ + struct PhysicsServerSharedMemoryInternalData* m_data; + +protected: + + + void releaseSharedMemory(); + + +public: + PhysicsServerSharedMemory(); + virtual ~PhysicsServerSharedMemory(); + + virtual void setSharedMemoryKey(int key); + + //todo: implement option to allocated shared memory from client + virtual bool connectSharedMemory( struct GUIHelperInterface* guiHelper); + + virtual void disconnectSharedMemory (bool deInitializeSharedMemory); + + virtual void processClientCommands(); + + //bool supportsJointMotor(class btMultiBody* body, int linkIndex); + + //@todo(erwincoumans) Should we have shared memory commands for picking objects? + ///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); + virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); + virtual void removePickingConstraint(); + + //for physicsDebugDraw and renderScene are mainly for debugging purposes + //and for physics visualization. The idea is that physicsDebugDraw can also send wireframe + //to a physics client, over shared memory + void physicsDebugDraw(int debugDrawFlags); + void renderScene(); + + void enableCommandLogging(bool enable, const char* fileName); + void replayFromLogFile(const char* fileName); + + +}; + + +#endif //PHYSICS_SERVER_EXAMPLESHARED_MEMORY_H + + diff --git a/examples/SharedMemory/SharedMemoryBlock.h b/examples/SharedMemory/SharedMemoryBlock.h index fa80c36c9..b737765ea 100644 --- a/examples/SharedMemory/SharedMemoryBlock.h +++ b/examples/SharedMemory/SharedMemoryBlock.h @@ -25,10 +25,12 @@ struct SharedMemoryBlock //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]; + char m_bulletStreamDataServerToClientRefactor[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; }; + + //http://stackoverflow.com/questions/24736304/unable-to-use-inline-in-declaration-get-error-c2054 #ifdef _WIN32 __inline diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 1e54b011b..e0b2bf9d4 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -24,6 +24,7 @@ typedef unsigned long long int smUint64_t; #endif +#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024) #define SHARED_MEMORY_SERVER_TEST_C #define MAX_DEGREE_OF_FREEDOM 64 @@ -31,6 +32,27 @@ #define MAX_URDF_FILENAME_LENGTH 1024 #define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH +struct TmpFloat3 +{ + float m_x; + float m_y; + float m_z; +}; + +#ifdef _WIN32 +__inline +#else +inline +#endif +TmpFloat3 CreateTmpFloat3(float x, float y, float z) +{ + TmpFloat3 tmp; + tmp.m_x = x; + tmp.m_y = y; + tmp.m_z = z; + return tmp; +} + enum EnumUrdfArgsUpdateFlags { URDF_ARGS_FILE_NAME=1, diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index b9cec004e..789bfbc76 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -70,6 +70,12 @@ enum COLLISION_SHAPE_TYPE_SPHERE }; +// copied from btMultiBodyLink.h +enum JointType { + eRevoluteType = 0, + ePrismaticType = 1, +}; + struct b3JointInfo { char* m_linkName; diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index c93a4250a..e2930b2bf 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -20,11 +20,25 @@ files { "PhysicsClientSharedMemory.cpp", "PhysicsClientExample.cpp", "PhysicsServerExample.cpp", + "PhysicsServerSharedMemory.cpp", + "PhysicsServerSharedMemory.h", + "PhysicsServer.cpp", + "PhysicsServer.h", "main.cpp", "PhysicsClientC_API.cpp", "PhysicsServer.cpp", "PosixSharedMemory.cpp", "Win32SharedMemory.cpp", + "PhysicsDirect.cpp", + "PhysicsDirect.h", + "PhysicsDirectC_API.cpp", + "PhysicsDirectC_API.h", + "PhysicsLoopBack.cpp", + "PhysicsLoopBack.h", + "PhysicsLoopBackC_Api.cpp", + "PhysicsLoopBackC_Api.h", + "PhysicsServerCommandProcessor.cpp", + "PhysicsServerCommandProcessor.h", "../Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h", "../Importers/ImportURDFDemo/MultiBodyCreationInterface.h", "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index 9ef8f6c93..7ecb46301 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -26,7 +26,7 @@ project ("Test_SharedMemoryPhysicsClient") "../../examples/Utils/b3ResourcePath.h", } -project ("Test_PhysicsLoopBack") +project ("Test_PhysicsServerLoopBack") language "C++" kind "ConsoleApp" @@ -49,6 +49,10 @@ project ("Test_PhysicsLoopBack") "../../examples/SharedMemory/PhysicsClient.h", "../../examples/SharedMemory/PhysicsServer.cpp", "../../examples/SharedMemory/PhysicsServer.h", + "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp", + "../../examples/SharedMemory/PhysicsServerSharedMemory.h", + "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp", + "../../examples/SharedMemory/PhysicsServerCommandProcessor.h", "../../examples/SharedMemory/PhysicsLoopBack.cpp", "../../examples/SharedMemory/PhysicsLoopBack.h", "../../examples/SharedMemory/PhysicsLoopBackC_Api.cpp", @@ -79,3 +83,62 @@ project ("Test_PhysicsLoopBack") "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", } + project ("Test_PhysicsServerDirect") + + language "C++" + kind "ConsoleApp" + + includedirs {"../../src", "../../examples/SharedMemory", + "../../examples/ThirdPartyLibs"} + defines {"PHYSICS_SERVER_DIRECT"} + 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/PhysicsServerSharedMemory.cpp", + "../../examples/SharedMemory/PhysicsServerSharedMemory.h", + "../../examples/SharedMemory/PhysicsDirect.cpp", + "../../examples/SharedMemory/PhysicsDirect.h", + "../../examples/SharedMemory/PhysicsDirectC_API.cpp", + "../../examples/SharedMemory/PhysicsDirectC_API.h", + "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp", + "../../examples/SharedMemory/PhysicsServerCommandProcessor.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 3ae34e0a6..abd716178 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -5,6 +5,11 @@ #include "PhysicsLoopBackC_API.h" #endif //PHYSICS_LOOP_BACK +#ifdef PHYSICS_SERVER_DIRECT +#include "PhysicsDirectC_API.h" +#endif //PHYSICS_SERVER_DIRECT + + #include "SharedMemoryPublic.h" #include "Bullet3Common/b3Logging.h" #include @@ -28,12 +33,17 @@ int main(int argc, char* argv[]) 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 + +#ifdef PHYSICS_SERVER_DIRECT + sm = b3ConnectPhysicsDirect(); +#else//PHYSICS_SERVER_DIRECT + sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY); +#endif //PHYSICS_SERVER_DIRECT