From bfc85ff1fd1181dc875d27f5de9cd2a7d5b5d062 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 27 Jun 2018 23:43:42 -0700 Subject: [PATCH 1/9] PyBullet: TCP connection, optimized getVREvents to allow faster Windows -> Linux Vive tracking state communication. --- examples/SharedMemory/PhysicsClientTCP.cpp | 13 ++++- examples/SharedMemory/tcp/main.cpp | 63 ++++++++++++++++------ 2 files changed, 57 insertions(+), 19 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientTCP.cpp b/examples/SharedMemory/PhysicsClientTCP.cpp index f959325c0..8ac5b74f8 100644 --- a/examples/SharedMemory/PhysicsClientTCP.cpp +++ b/examples/SharedMemory/PhysicsClientTCP.cpp @@ -181,8 +181,17 @@ bool TcpNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryComma } else { - sz = sizeof(SharedMemoryCommand); - data = (unsigned char*)&clientCmd; + + if (clientCmd.m_type == CMD_REQUEST_VR_EVENTS_DATA) + { + sz = 3 * sizeof(int) + sizeof(smUint64_t) + 16; + data = (unsigned char*)&clientCmd; + } + else + { + sz = sizeof(SharedMemoryCommand); + data = (unsigned char*)&clientCmd; + } } m_data->m_tcpSocket.Send((const uint8 *)data,sz); diff --git a/examples/SharedMemory/tcp/main.cpp b/examples/SharedMemory/tcp/main.cpp index 6c2df155d..234b2312e 100644 --- a/examples/SharedMemory/tcp/main.cpp +++ b/examples/SharedMemory/tcp/main.cpp @@ -184,11 +184,16 @@ int main(int argc, char *argv[]) cmdPtr = &cmd; cmd.m_type = *(int*)&bytesReceived[0]; } - + + if (numBytesRec == sizeof(SharedMemoryCommand)) { cmdPtr = (SharedMemoryCommand*)&bytesReceived[0]; } + else + { + cmdPtr = (SharedMemoryCommand*)&bytesReceived[0]; + } if (cmdPtr) { SharedMemoryStatus serverStatus; @@ -207,7 +212,7 @@ int main(int argc, char *argv[]) } if (gVerboseNetworkMessagesServer) { - printf("buffer.size = %d\n", buffer.size()); + //printf("buffer.size = %d\n", buffer.size()); printf("serverStatus.m_numDataStreamBytes = %d\n", serverStatus.m_numDataStreamBytes); } if (hasStatus) @@ -234,25 +239,49 @@ int main(int argc, char *argv[]) } else { - //create packetData with [int packetSizeInBytes, status, streamBytes) - packetData.resize(4 + sizeof(SharedMemoryStatus) + serverStatus.m_numDataStreamBytes); - int sz = packetData.size(); - int curPos = 0; - - MySerializeInt(sz, &packetData[curPos]); - curPos += 4; - for (int i = 0; i < sizeof(SharedMemoryStatus); i++) + if (cmdPtr->m_type == CMD_REQUEST_VR_EVENTS_DATA) { - packetData[i + curPos] = statBytes[i]; + int headerSize = 16+5 * sizeof(int) + sizeof(smUint64_t) + sizeof(char*) + sizeof(b3VRControllerEvent)*serverStatus.m_sendVREvents.m_numVRControllerEvents; + packetData.resize(4 + headerSize); + int sz = packetData.size(); + int curPos = 0; + MySerializeInt(sz, &packetData[curPos]); + curPos += 4; + for (int i = 0; i < headerSize; i++) + { + packetData[i + curPos] = statBytes[i]; + } + curPos += headerSize; + pClient->Send(&packetData[0], packetData.size()); } - curPos += sizeof(SharedMemoryStatus); - - for (int i = 0; i < serverStatus.m_numDataStreamBytes; i++) + else { - packetData[i + curPos] = buffer[i]; + //create packetData with [int packetSizeInBytes, status, streamBytes) + packetData.resize(4 + sizeof(SharedMemoryStatus) + serverStatus.m_numDataStreamBytes); + int sz = packetData.size(); + int curPos = 0; + + if (gVerboseNetworkMessagesServer) + { + //printf("buffer.size = %d\n", buffer.size()); + printf("serverStatus packed size = %d\n", sz); + } + + MySerializeInt(sz, &packetData[curPos]); + curPos += 4; + for (int i = 0; i < sizeof(SharedMemoryStatus); i++) + { + packetData[i + curPos] = statBytes[i]; + } + curPos += sizeof(SharedMemoryStatus); + + for (int i = 0; i < serverStatus.m_numDataStreamBytes; i++) + { + packetData[i + curPos] = buffer[i]; + } + + pClient->Send(&packetData[0], packetData.size()); } - - pClient->Send( &packetData[0], packetData.size() ); } } From 0d8564db22edbe10820df70133daddc1b1330fc6 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sun, 1 Jul 2018 14:42:32 -0700 Subject: [PATCH 2/9] PyBullet: added preliminary DART and MuJoCo backend files, MuJoCo can loadMJCF, stepSimulation and getBasePositionAndOrientation, DART is empty. PyBullet: add 2d biped example., --- data/mjcf/capsule.xml | 2 +- examples/SharedMemory/SharedMemoryPublic.h | 2 + .../SharedMemory/dart/DARTPhysicsC_API.cpp | 16 + examples/SharedMemory/dart/DARTPhysicsC_API.h | 20 + .../SharedMemory/dart/DARTPhysicsClient.cpp | 1596 ++++++++++++++++ .../SharedMemory/dart/DARTPhysicsClient.h | 124 ++ .../DARTPhysicsServerCommandProcessor.cpp | 39 + .../dart/DARTPhysicsServerCommandProcessor.h | 31 + .../mujoco/MuJoCoPhysicsC_API.cpp | 15 + .../SharedMemory/mujoco/MuJoCoPhysicsC_API.h | 20 + .../mujoco/MuJoCoPhysicsClient.cpp | 1598 +++++++++++++++++ .../SharedMemory/mujoco/MuJoCoPhysicsClient.h | 123 ++ .../MuJoCoPhysicsServerCommandProcessor.cpp | 1032 +++++++++++ .../MuJoCoPhysicsServerCommandProcessor.h | 45 + .../pybullet/examples/biped2d_pybullet.py | 40 + .../pybullet_data/biped/biped2d_pybullet.urdf | 273 +++ examples/pybullet/pybullet.c | 32 + 17 files changed, 5007 insertions(+), 1 deletion(-) create mode 100644 examples/SharedMemory/dart/DARTPhysicsC_API.cpp create mode 100644 examples/SharedMemory/dart/DARTPhysicsC_API.h create mode 100644 examples/SharedMemory/dart/DARTPhysicsClient.cpp create mode 100644 examples/SharedMemory/dart/DARTPhysicsClient.h create mode 100644 examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.cpp create mode 100644 examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.h create mode 100644 examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.cpp create mode 100644 examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.h create mode 100644 examples/SharedMemory/mujoco/MuJoCoPhysicsClient.cpp create mode 100644 examples/SharedMemory/mujoco/MuJoCoPhysicsClient.h create mode 100644 examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.cpp create mode 100644 examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.h create mode 100644 examples/pybullet/examples/biped2d_pybullet.py create mode 100644 examples/pybullet/gym/pybullet_data/biped/biped2d_pybullet.urdf diff --git a/data/mjcf/capsule.xml b/data/mjcf/capsule.xml index 7e58ce12c..14fc484d5 100644 --- a/data/mjcf/capsule.xml +++ b/data/mjcf/capsule.xml @@ -5,7 +5,7 @@ - + diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 7e7338e36..dd4df163c 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -743,6 +743,8 @@ enum eCONNECT_METHOD { eCONNECT_GUI_SERVER=7, eCONNECT_GUI_MAIN_THREAD=8, eCONNECT_SHARED_MEMORY_SERVER=9, + eCONNECT_DART=10, + eCONNECT_MUJOCO=11, }; enum eURDF_Flags diff --git a/examples/SharedMemory/dart/DARTPhysicsC_API.cpp b/examples/SharedMemory/dart/DARTPhysicsC_API.cpp new file mode 100644 index 000000000..785f17f61 --- /dev/null +++ b/examples/SharedMemory/dart/DARTPhysicsC_API.cpp @@ -0,0 +1,16 @@ +#ifdef BT_ENABLE_DART +#include "DARTPhysicsC_API.h" +#include "DARTPhysicsServerCommandProcessor.h" +#include "DARTPhysicsClient.h" + +//think more about naming. The b3ConnectPhysicsLoopback +B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDART() +{ + DARTPhysicsServerCommandProcessor* sdk = new DARTPhysicsServerCommandProcessor; + + DARTPhysicsClient* direct = new DARTPhysicsClient(sdk,true); + bool connected; + connected = direct->connect(); + return (b3PhysicsClientHandle )direct; +} +#endif//BT_ENABLE_DART \ No newline at end of file diff --git a/examples/SharedMemory/dart/DARTPhysicsC_API.h b/examples/SharedMemory/dart/DARTPhysicsC_API.h new file mode 100644 index 000000000..e268c8dcf --- /dev/null +++ b/examples/SharedMemory/dart/DARTPhysicsC_API.h @@ -0,0 +1,20 @@ +#ifndef DART_PHYSICS_C_API_H +#define DART_PHYSICS_C_API_H + +#ifdef BT_ENABLE_DART + +#include "../PhysicsClientC_API.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//think more about naming. The b3ConnectPhysicsLoopback +B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDART(); + +#ifdef __cplusplus +} +#endif + +#endif //BT_ENABLE_DART +#endif //DART_PHYSICS_C_API_H diff --git a/examples/SharedMemory/dart/DARTPhysicsClient.cpp b/examples/SharedMemory/dart/DARTPhysicsClient.cpp new file mode 100644 index 000000000..19e99202d --- /dev/null +++ b/examples/SharedMemory/dart/DARTPhysicsClient.cpp @@ -0,0 +1,1596 @@ +#include "DARTPhysicsClient.h" + +#include "../PhysicsClientSharedMemory.h" +#include "../../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../SharedMemoryCommands.h" +#include "../PhysicsCommandProcessorInterface.h" +#include "../../Utils/b3Clock.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" +#include + +#include "../SharedMemoryUserData.h" +#include "LinearMath/btQuickprof.h" + +struct DARTUserDataCache { + btHashMap m_userDataMap; + btHashMap m_keyToUserDataIdMap; + + ~DARTUserDataCache() + { + + } +}; + +struct BodyJointInfoCache2 +{ + std::string m_baseName; + btAlignedObjectArray m_jointInfo; + std::string m_bodyName; + + // Joint index -> user data. + btHashMap m_jointToUserDataMap; + + ~BodyJointInfoCache2() { + } +}; + + + +struct DARTPhysicsDirectInternalData +{ + DummyGUIHelper m_noGfx; + + btAlignedObjectArray m_serverDNA; + SharedMemoryCommand m_command; + SharedMemoryStatus m_serverStatus; + + SharedMemoryCommand m_tmpInfoRequestCommand; + SharedMemoryStatus m_tmpInfoStatus; + bool m_hasStatus; + bool m_verboseOutput; + + btAlignedObjectArray m_debugLinesFrom; + btAlignedObjectArray m_debugLinesTo; + btAlignedObjectArray m_debugLinesColor; + + btHashMap m_bodyJointMap; + btHashMap m_userConstraintInfoMap; + + btAlignedObjectArray m_profileTimings; + btHashMap m_profileTimingStringArray; + + char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; + btAlignedObjectArray m_cachedMassMatrix; + int m_cachedCameraPixelsWidth; + int m_cachedCameraPixelsHeight; + btAlignedObjectArray m_cachedCameraPixelsRGBA; + btAlignedObjectArray m_cachedCameraDepthBuffer; + btAlignedObjectArray m_cachedSegmentationMask; + + btAlignedObjectArray m_cachedContactPoints; + btAlignedObjectArray m_cachedOverlappingObjects; + + btAlignedObjectArray m_cachedVisualShapes; + btAlignedObjectArray m_cachedCollisionShapes; + + btAlignedObjectArray m_cachedVREvents; + + btAlignedObjectArray m_cachedKeyboardEvents; + btAlignedObjectArray m_cachedMouseEvents; + + btAlignedObjectArray m_raycastHits; + + PhysicsCommandProcessorInterface* m_commandProcessor; + bool m_ownsCommandProcessor; + double m_timeOutInSeconds; + + DARTPhysicsDirectInternalData() + :m_hasStatus(false), + m_verboseOutput(false), + m_cachedCameraPixelsWidth(0), + m_cachedCameraPixelsHeight(0), + m_commandProcessor(NULL), + m_ownsCommandProcessor(false), + m_timeOutInSeconds(1e30) + { + memset(&m_command, 0, sizeof(m_command)); + memset(&m_serverStatus, 0, sizeof(m_serverStatus)); + memset(m_bulletStreamDataServerToClient, 0, sizeof(m_bulletStreamDataServerToClient)); + } +}; + +DARTPhysicsClient::DARTPhysicsClient(PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership) +{ + int sz = sizeof(SharedMemoryCommand); + int sz2 = sizeof(SharedMemoryStatus); + + m_data = new DARTPhysicsDirectInternalData; + m_data->m_commandProcessor = physSdk; + m_data->m_ownsCommandProcessor = passSdkOwnership; + +} + +DARTPhysicsClient::~DARTPhysicsClient() +{ + for (int i=0;im_profileTimingStringArray.size();i++) + { + std::string** str = m_data->m_profileTimingStringArray.getAtIndex(i); + if (str) + { + delete *str; + } + } + m_data->m_profileTimingStringArray.clear(); + + if (m_data->m_commandProcessor->isConnected()) + { + m_data->m_commandProcessor->disconnect(); + } + if (m_data->m_ownsCommandProcessor) + { + delete m_data->m_commandProcessor; + } + + resetData(); + + + + delete m_data; +} + +void DARTPhysicsClient::resetData() +{ + 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) + { + delete (*bodyJointsPtr); + } + } + m_data->m_bodyJointMap.clear(); + m_data->m_userConstraintInfoMap.clear(); +} + +// return true if connection succesfull, can also check 'isConnected' +bool DARTPhysicsClient::connect() +{ + bool connected = m_data->m_commandProcessor->connect(); + m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx); + + + if (connected) + //also request serialization data + { + SharedMemoryCommand command; + command.m_type = CMD_REQUEST_INTERNAL_DATA; + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + if (hasStatus) + { + postProcessStatus(m_data->m_serverStatus); + } + else + { + b3Clock clock; + double timeSec = clock.getTimeInSeconds(); + + while ((!hasStatus) && (clock.getTimeInSeconds()-timeSec <10 )) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + } + } + + return connected; +} + +// return true if connection succesfull, can also check 'isConnected' +bool DARTPhysicsClient::connect(struct GUIHelperInterface* guiHelper) +{ + bool connected = m_data->m_commandProcessor->connect(); + + m_data->m_commandProcessor->setGuiHelper(guiHelper); + + return connected; +} + +void DARTPhysicsClient::renderScene() +{ + int renderFlags = 0; + m_data->m_commandProcessor->renderScene(renderFlags); +} + +void DARTPhysicsClient::debugDraw(int debugDrawMode) +{ + m_data->m_commandProcessor->physicsDebugDraw(debugDrawMode); +} + +////todo: rename to 'disconnect' +void DARTPhysicsClient::disconnectSharedMemory() +{ + m_data->m_commandProcessor->disconnect(); + m_data->m_commandProcessor->setGuiHelper(0); +} + +bool DARTPhysicsClient::isConnected() const +{ + return m_data->m_commandProcessor->isConnected(); +} + +// return non-null if there is a status, nullptr otherwise +const SharedMemoryStatus* DARTPhysicsClient::processServerStatus() +{ + + if (!m_data->m_hasStatus) + { + m_data->m_hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + SharedMemoryStatus* stat = 0; + if (m_data->m_hasStatus) + { + stat = &m_data->m_serverStatus; + + postProcessStatus(m_data->m_serverStatus); + + m_data->m_hasStatus = false; + } + return stat; +} + +SharedMemoryCommand* DARTPhysicsClient::getAvailableSharedMemoryCommand() +{ + return &m_data->m_command; +} + +bool DARTPhysicsClient::canSubmitCommand() const +{ + return m_data->m_commandProcessor->isConnected(); +} + +bool DARTPhysicsClient::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); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + 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) + { + m_data->m_hasStatus = false; + + 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 DARTPhysicsClient::processVisualShapeData(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); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + + if (m_data->m_verboseOutput) + { + b3Printf("Visual Shape Information Request OK\n"); + } + int startVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex; + int numVisualShapesCopied = serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + m_data->m_cachedVisualShapes.resize(startVisualShapeIndex + numVisualShapesCopied); + b3VisualShapeData* shapeData = (b3VisualShapeData*)&m_data->m_bulletStreamDataServerToClient[0]; + for (int i = 0; i < numVisualShapesCopied; i++) + { + m_data->m_cachedVisualShapes[startVisualShapeIndex + i] = shapeData[i]; + } + + if (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes >0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied) + { + m_data->m_hasStatus = false; + + command.m_type = CMD_REQUEST_VISUAL_SHAPE_INFO; + command.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + command.m_requestVisualShapeDataArguments.m_bodyUniqueId = serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId; + } + } + } while (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes > 0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied); + + return m_data->m_hasStatus; +} + +bool DARTPhysicsClient::processOverlappingObjects(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); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + if (m_data->m_verboseOutput) + { + b3Printf("Overlapping Objects Request OK\n"); + } + + int startOverlapIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex; + int numOverlapCopied = serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied; + m_data->m_cachedOverlappingObjects.resize(startOverlapIndex + numOverlapCopied); + b3OverlappingObject* objects = (b3OverlappingObject*)&m_data->m_bulletStreamDataServerToClient[0]; + + for (int i = 0; i < numOverlapCopied; i++) + { + m_data->m_cachedOverlappingObjects[startOverlapIndex + i] = objects[i]; + } + + if (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied) + { + m_data->m_hasStatus = false; + command.m_type = CMD_REQUEST_AABB_OVERLAP; + command.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied; + } + + } + } while (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied); + + return m_data->m_hasStatus; + +} + + + +bool DARTPhysicsClient::processContactPointData(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); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + if (m_data->m_verboseOutput) + { + b3Printf("Contact Point Information Request OK\n"); + } + int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex; + int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + + m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied); + + b3ContactPointData* contactData = (b3ContactPointData*)&m_data->m_bulletStreamDataServerToClient[0]; + + for (int i=0;im_cachedContactPoints[startContactIndex+i] = contactData[i]; + } + + if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied) + { + + m_data->m_hasStatus = false; + + command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION; + command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex+serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + command.m_requestContactPointArguments.m_objectAIndexFilter = -1; + command.m_requestContactPointArguments.m_objectBIndexFilter = -1; + + } + + } + } while (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied); + + return m_data->m_hasStatus; + +} + + +bool DARTPhysicsClient::processCamera(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); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + btAssert(m_data->m_serverStatus.m_type == CMD_CAMERA_IMAGE_COMPLETED); + + if (m_data->m_verboseOutput) + { + b3Printf("Camera image OK\n"); + } + + int numBytesPerPixel = 4;//RGBA + int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex+ + serverCmd.m_sendPixelDataArguments.m_numPixelsCopied+ + serverCmd.m_sendPixelDataArguments.m_numRemainingPixels; + + m_data->m_cachedCameraPixelsWidth = 0; + m_data->m_cachedCameraPixelsHeight = 0; + + int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth*serverCmd.m_sendPixelDataArguments.m_imageHeight; + + m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel); + m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels); + m_data->m_cachedSegmentationMask.resize(numTotalPixels); + m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel); + + + unsigned char* rgbaPixelsReceived = + (unsigned char*)&m_data->m_bulletStreamDataServerToClient[0]; + + float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]); + int* segmentationMaskBuffer = (int*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*8]); + + // printf("pixel = %d\n", rgbaPixelsReceived[0]); + + for (int i=0;im_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i]; + } + for (int i=0;im_cachedSegmentationMask[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i]; + } + for (int i=0;im_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] + = rgbaPixelsReceived[i]; + } + + if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied) + { + + m_data->m_hasStatus = false; + + // continue requesting remaining pixels + command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA; + command.m_requestPixelDataArguments.m_startPixelIndex = + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex + + serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; + + } else + { + m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth; + m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight; + } + } + } while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied); + + return m_data->m_hasStatus; + + +} + + +void DARTPhysicsClient::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd) +{ + + BodyJointInfoCache2** cachePtr = m_data->m_bodyJointMap[bodyUniqueId]; + //don't process same bodyUniqueId multiple times + if (cachePtr) + { + return; + } + + bParse::btBulletFile bf( + &m_data->m_bulletStreamDataServerToClient[0], + serverCmd.m_numDataStreamBytes); + if (m_data->m_serverDNA.size()) + { + bf.setFileDNA(false, &m_data->m_serverDNA[0], m_data->m_serverDNA.size()); + } + else + { + bf.setFileDNAisMemoryDNA(); + } + bf.parse(false); + + BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; + m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints); + bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; + + 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]; + + if (mb->m_baseName) + { + bodyJoints->m_baseName = mb->m_baseName; + } + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } else + { + Bullet::btMultiBodyFloatData* mb = + (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; + + + if (mb->m_baseName) + { + bodyJoints->m_baseName = mb->m_baseName; + } + 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"); + } +} + +void DARTPhysicsClient::processAddUserData(const struct SharedMemoryStatus& serverCmd) { + const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_userDataResponseArgs.m_userDataGlobalId; + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + DARTUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; + if (!userDataCachePtr) + { + DARTUserDataCache cache; + (*bodyJointsPtr)->m_jointToUserDataMap.insert(userDataGlobalId.m_linkIndex, cache); + } + userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; + + const char *dataStream = m_data->m_bulletStreamDataServerToClient; + + b3UserDataValue userDataValue; + userDataValue.m_type = serverCmd.m_userDataResponseArgs.m_valueType; + userDataValue.m_length = serverCmd.m_userDataResponseArgs.m_valueLength; + SharedMemoryUserData *userDataPtr = userDataCachePtr->m_userDataMap[userDataGlobalId.m_userDataId]; + if (userDataPtr) { + // Only replace the value. + (userDataPtr)->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type); + } + else { + // Add a new user data entry. + (userDataCachePtr)->m_userDataMap.insert(userDataGlobalId.m_userDataId, SharedMemoryUserData(serverCmd.m_userDataResponseArgs.m_key)); + userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; + userDataPtr->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type); + (userDataCachePtr)->m_keyToUserDataIdMap.insert(serverCmd.m_userDataResponseArgs.m_key, userDataGlobalId.m_userDataId); + } + } +} + +void DARTPhysicsClient::postProcessStatus(const struct SharedMemoryStatus& serverCmd) +{ + switch (serverCmd.m_type) + { + + case CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Raycast completed"); + } + m_data->m_raycastHits.clear(); + b3RayHitInfo *rayHits = (b3RayHitInfo *)m_data->m_bulletStreamDataServerToClient; + for (int i=0;im_raycastHits.push_back(rayHits[i]); + } + break; + } + case CMD_REQUEST_VR_EVENTS_DATA_COMPLETED: + { + + if (m_data->m_verboseOutput) + { + b3Printf("Request VR Events completed"); + } + m_data->m_cachedVREvents.resize(serverCmd.m_sendVREvents.m_numVRControllerEvents); + for (int i=0;i< serverCmd.m_sendVREvents.m_numVRControllerEvents;i++) + { + m_data->m_cachedVREvents[i] = serverCmd.m_sendVREvents.m_controllerEvents[i]; + } + break; + } + case CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Request keyboard events completed"); + } + m_data->m_cachedKeyboardEvents.resize(serverCmd.m_sendKeyboardEvents.m_numKeyboardEvents); + for (int i=0;im_cachedKeyboardEvents[i] = serverCmd.m_sendKeyboardEvents.m_keyboardEvents[i]; + } + break; + } + + case CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED: + { + B3_PROFILE("CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED"); + if (m_data->m_verboseOutput) + { + b3Printf("Request mouse events completed"); + } + m_data->m_cachedMouseEvents.resize(serverCmd.m_sendMouseEvents.m_numMouseEvents); + for (int i=0;im_cachedMouseEvents[i] = serverCmd.m_sendMouseEvents.m_mouseEvents[i]; + } + break; + } + + case CMD_REQUEST_INTERNAL_DATA_COMPLETED: + { + if (serverCmd.m_numDataStreamBytes) + { + int numStreamBytes = serverCmd.m_numDataStreamBytes; + m_data->m_serverDNA.resize(numStreamBytes); + for (int i = 0; i < numStreamBytes; i++) + { + m_data->m_serverDNA[i] = m_data->m_bulletStreamDataServerToClient[i]; + } + } + break; + } + case CMD_RESET_SIMULATION_COMPLETED: + { + resetData(); + break; + } + + case CMD_USER_CONSTRAINT_INFO_COMPLETED: + case CMD_USER_CONSTRAINT_COMPLETED: + { + int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; + m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs); + break; + } + case CMD_REMOVE_USER_CONSTRAINT_COMPLETED: + { + int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; + m_data->m_userConstraintInfoMap.remove(cid); + break; + } + case CMD_REMOVE_BODY_FAILED: + { + b3Warning("Remove body failed\n"); + break; + } + case CMD_REMOVE_BODY_COMPLETED: + { + for (int i=0;im_userConstraintInfoMap.remove(key); + } + + break; + } + case CMD_CHANGE_USER_CONSTRAINT_COMPLETED: + { + int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; + b3UserConstraint* userConstraintPtr = m_data->m_userConstraintInfoMap[cid]; + if (userConstraintPtr) + { + const b3UserConstraint* serverConstraint = &serverCmd.m_userConstraintResultArgs; + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B) + { + userConstraintPtr->m_childFrame[0] = serverConstraint->m_childFrame[0]; + userConstraintPtr->m_childFrame[1] = serverConstraint->m_childFrame[1]; + userConstraintPtr->m_childFrame[2] = serverConstraint->m_childFrame[2]; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) + { + userConstraintPtr->m_childFrame[3] = serverConstraint->m_childFrame[3]; + userConstraintPtr->m_childFrame[4] = serverConstraint->m_childFrame[4]; + userConstraintPtr->m_childFrame[5] = serverConstraint->m_childFrame[5]; + userConstraintPtr->m_childFrame[6] = serverConstraint->m_childFrame[6]; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) + { + userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) + { + userConstraintPtr->m_gearRatio = serverConstraint->m_gearRatio; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET) + { + userConstraintPtr->m_relativePositionTarget = serverConstraint->m_relativePositionTarget; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP) + { + userConstraintPtr->m_erp = serverConstraint->m_erp; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) + { + userConstraintPtr->m_gearAuxLink = serverConstraint->m_gearAuxLink; + } + } + break; + } + case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED: + { + break; + } + case CMD_SYNC_BODY_INFO_COMPLETED: + case CMD_MJCF_LOADING_COMPLETED: + case CMD_SDF_LOADING_COMPLETED: + { + //we'll stream further info from the physics server + //so serverCmd will be invalid, make a copy + + int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints; + for (int i=0;im_tmpInfoRequestCommand.m_type = CMD_USER_CONSTRAINT; + m_data->m_tmpInfoRequestCommand.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO; + m_data->m_tmpInfoRequestCommand.m_userConstraintArguments.m_userConstraintUniqueId = constraintUid; + + bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + if (hasStatus) + { + int cid = m_data->m_tmpInfoStatus.m_userConstraintResultArgs.m_userConstraintUniqueId; + m_data->m_userConstraintInfoMap.insert(cid,m_data->m_tmpInfoStatus.m_userConstraintResultArgs); + } + } + + int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; + for (int i = 0; im_tmpInfoRequestCommand.m_type = CMD_REQUEST_BODY_INFO; + m_data->m_tmpInfoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId; + + bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + if (hasStatus) + { + processBodyJointInfo(bodyUniqueId, m_data->m_tmpInfoStatus); + } + } + break; + } + case CMD_CREATE_MULTI_BODY_COMPLETED: + case CMD_URDF_LOADING_COMPLETED: + { + + if (serverCmd.m_numDataStreamBytes > 0) + { + int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId; + processBodyJointInfo(bodyIndex, serverCmd); + } + break; + } + case CMD_BULLET_LOADING_FAILED: + { + b3Warning("Couldn't load .bullet file"); + break; + } + case CMD_BULLET_LOADING_COMPLETED: + { + break; + } + + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED: + { + break; + } + + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED: + { + b3Warning("requestOpenGLVisualizeCamera failed"); + break; + } + case CMD_REMOVE_USER_CONSTRAINT_FAILED: + { + b3Warning("removeConstraint failed"); + break; + } + case CMD_CHANGE_USER_CONSTRAINT_FAILED: + { + //b3Warning("changeConstraint failed"); + break; + } + + case CMD_USER_CONSTRAINT_FAILED: + { + b3Warning("createConstraint failed"); + break; + } + + case CMD_CREATE_COLLISION_SHAPE_FAILED: + { + b3Warning("createCollisionShape failed"); + break; + } + case CMD_CREATE_COLLISION_SHAPE_COMPLETED: + { + break; + } + + case CMD_CREATE_VISUAL_SHAPE_FAILED: + { + b3Warning("createVisualShape failed"); + break; + } + case CMD_CREATE_VISUAL_SHAPE_COMPLETED: + { + break; + } + + case CMD_CREATE_MULTI_BODY_FAILED: + { + b3Warning("createMultiBody failed"); + break; + } + case CMD_REQUEST_COLLISION_INFO_COMPLETED: + { + break; + } + case CMD_REQUEST_COLLISION_INFO_FAILED: + { + b3Warning("Request getCollisionInfo failed"); + break; + } + + case CMD_CUSTOM_COMMAND_COMPLETED: + { + break; + } + case CMD_CUSTOM_COMMAND_FAILED: + { + b3Warning("custom plugin command failed"); + break; + } + case CMD_CLIENT_COMMAND_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_FAILED: + { + b3Warning("jacobian calculation failed"); + break; + } + case CMD_CALCULATED_MASS_MATRIX_FAILED: + { + b3Warning("calculate mass matrix failed"); + break; + } + case CMD_CALCULATED_MASS_MATRIX_COMPLETED: + { + double* matrixData = (double*)&m_data->m_bulletStreamDataServerToClient[0]; + m_data->m_cachedMassMatrix.resize(serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount); + for (int i=0;im_cachedMassMatrix[i] = matrixData[i]; + } + break; + } + case CMD_ACTUAL_STATE_UPDATE_COMPLETED: + { + break; + } + case CMD_DESIRED_STATE_RECEIVED_COMPLETED: + { + break; + } + case CMD_STEP_FORWARD_SIMULATION_COMPLETED: + { + break; + } + case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED: + { + break; + } + case CMD_SAVE_STATE_COMPLETED: + { + break; + } + case CMD_COLLISION_SHAPE_INFO_FAILED: + { + b3Warning("getCollisionShapeData failed"); + break; + } + case CMD_COLLISION_SHAPE_INFO_COMPLETED: + { + B3_PROFILE("CMD_COLLISION_SHAPE_INFO_COMPLETED"); + if (m_data->m_verboseOutput) + { + b3Printf("Collision Shape Information Request OK\n"); + } + int numCollisionShapesCopied = serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes; + m_data->m_cachedCollisionShapes.resize(numCollisionShapesCopied); + b3CollisionShapeData* shapeData = (b3CollisionShapeData*)&m_data->m_bulletStreamDataServerToClient[0]; + for (int i = 0; i < numCollisionShapesCopied; i++) + { + m_data->m_cachedCollisionShapes[i] = shapeData[i]; + } + break; + } + case CMD_RESTORE_STATE_FAILED: + { + b3Warning("restoreState failed"); + break; + } + case CMD_RESTORE_STATE_COMPLETED: + { + break; + } + case CMD_BULLET_SAVING_COMPLETED: + { + break; + } + case CMD_LOAD_SOFT_BODY_FAILED: + { + b3Warning("loadSoftBody failed"); + break; + } + case CMD_LOAD_SOFT_BODY_COMPLETED: + { + break; + } + case CMD_SYNC_USER_DATA_FAILED: + { + b3Warning("Synchronizing user data failed."); + break; + } + case CMD_ADD_USER_DATA_FAILED: + { + b3Warning("Adding user data failed (do the specified body and link exist?)"); + break; + } + case CMD_REMOVE_USER_DATA_FAILED: + { + b3Warning("Removing user data failed"); + break; + } + case CMD_ADD_USER_DATA_COMPLETED: + { + processAddUserData(serverCmd); + break; + } + case CMD_SYNC_USER_DATA_COMPLETED: + { + B3_PROFILE("CMD_SYNC_USER_DATA_COMPLETED"); + // Remove all cached user data entries. + for(int i=0; im_bodyJointMap.size(); i++) + { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); + if (bodyJointsPtr && *bodyJointsPtr) + { + (*bodyJointsPtr)->m_jointToUserDataMap.clear(); + } + } + const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers; + b3UserDataGlobalIdentifier *identifiers = new b3UserDataGlobalIdentifier[numIdentifiers]; + memcpy(identifiers, &m_data->m_bulletStreamDataServerToClient[0], numIdentifiers * sizeof(b3UserDataGlobalIdentifier)); + + for (int i=0; im_tmpInfoRequestCommand.m_type = CMD_REQUEST_USER_DATA; + m_data->m_tmpInfoRequestCommand.m_userDataRequestArgs = identifiers[i]; + + bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + if (hasStatus) + { + processAddUserData(m_data->m_tmpInfoStatus); + } + } + delete[] identifiers; + break; + } + case CMD_REMOVE_USER_DATA_COMPLETED: + { + const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_removeUserDataResponseArgs; + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) { + DARTUserDataCache *userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; + if (userDataCachePtr) + { + SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; + if (userDataPtr) { + (userDataCachePtr)->m_keyToUserDataIdMap.remove((userDataPtr)->m_key.c_str()); + (userDataCachePtr)->m_userDataMap.remove(userDataGlobalId.m_userDataId); + } + } + } + break; + } + default: + { + //b3Warning("Unknown server status type"); + } + }; + + +} +bool DARTPhysicsClient::submitClientCommand(const struct SharedMemoryCommand& command) +{ + if (command.m_type==CMD_REQUEST_DEBUG_LINES) + { + return processDebugLines(command); + } + + if (command.m_type==CMD_REQUEST_CAMERA_IMAGE_DATA) + { + return processCamera(command); + } + if (command.m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION) + { + return processContactPointData(command); + } + + if (command.m_type == CMD_REQUEST_VISUAL_SHAPE_INFO) + { + return processVisualShapeData(command); + } + if (command.m_type == CMD_REQUEST_AABB_OVERLAP) + { + return processOverlappingObjects(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) + { + postProcessStatus(m_data->m_serverStatus); + m_data->m_hasStatus = false; + } + */ + return hasStatus; +} + +int DARTPhysicsClient::getNumBodies() const +{ + return m_data->m_bodyJointMap.size(); +} + +void DARTPhysicsClient::removeCachedBody(int bodyUniqueId) +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + delete (*bodyJointsPtr); + m_data->m_bodyJointMap.remove(bodyUniqueId); + } +} + + +int DARTPhysicsClient::getNumUserConstraints() const +{ + return m_data->m_userConstraintInfoMap.size(); +} + +int DARTPhysicsClient::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const +{ + b3UserConstraint* constraintPtr =m_data->m_userConstraintInfoMap[constraintUniqueId]; + if (constraintPtr) + { + info = *constraintPtr; + return 1; + } + return 0; +} + +int DARTPhysicsClient::getUserConstraintId(int serialIndex) const +{ + if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints())) + { + return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1(); + } + return -1; +} + +int DARTPhysicsClient::getBodyUniqueId(int serialIndex) const +{ + if ((serialIndex >= 0) && (serialIndex < getNumBodies())) + { + return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1(); + } + return -1; +} + +bool DARTPhysicsClient::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + strcpy(info.m_baseName,bodyJoints->m_baseName.c_str()); + strcpy(info.m_bodyName ,bodyJoints->m_bodyName .c_str()); + return true; + } + + return false; +} + +int DARTPhysicsClient::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; +} + +bool DARTPhysicsClient::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + if ((jointIndex >=0) && (jointIndex < bodyJoints->m_jointInfo.size())) + { + info = bodyJoints->m_jointInfo[jointIndex]; + return true; + } + } + return false; +} + + +void DARTPhysicsClient::setSharedMemoryKey(int key) +{ +} + + +void DARTPhysicsClient::uploadBulletFileToSharedMemory(const char* data, int len) +{ + if (len>SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) + { + len = SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE; + } + for (int i=0;im_bulletStreamDataServerToClient[i] = data[i]; + } + //m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len); +} + +void DARTPhysicsClient::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) +{ + int curNumStreamingRays = command.m_requestRaycastIntersections.m_numStreamingRays; + int newNumRays = curNumStreamingRays + numRays; + btAssert(newNumRaysm_bulletStreamDataServerToClient; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0]; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1]; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2]; + command.m_requestRaycastIntersections.m_numStreamingRays++; + } + + } + +} + + +int DARTPhysicsClient::getNumDebugLines() const +{ + return m_data->m_debugLinesFrom.size(); +} + +const float* DARTPhysicsClient::getDebugLinesFrom() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesFrom[0].m_x; + } + return 0; +} +const float* DARTPhysicsClient::getDebugLinesTo() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesTo[0].m_x; + } + return 0; +} +const float* DARTPhysicsClient::getDebugLinesColor() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesColor[0].m_x; + } + return 0; +} + +void DARTPhysicsClient::getCachedCameraImage(b3CameraImageData* cameraData) +{ + if (cameraData) + { + cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth; + cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight; + cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0; + cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0; + cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMask.size()? &m_data->m_cachedSegmentationMask[0] : 0; + } +} + +void DARTPhysicsClient::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) +{ + contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size(); + contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0; + +} + +void DARTPhysicsClient::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects) +{ + overlappingObjects->m_numOverlappingObjects = m_data->m_cachedOverlappingObjects.size(); + overlappingObjects->m_overlappingObjects = m_data->m_cachedOverlappingObjects.size() ? + &m_data->m_cachedOverlappingObjects[0] : 0; +} + + +void DARTPhysicsClient::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) +{ + visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size(); + visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0; +} + +void DARTPhysicsClient::getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo) +{ + collisionShapesInfo->m_numCollisionShapes = m_data->m_cachedCollisionShapes.size(); + collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes ? &m_data->m_cachedCollisionShapes[0] : 0; +} + + + +void DARTPhysicsClient::getCachedVREvents(struct b3VREventsData* vrEventsData) +{ + vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size(); + vrEventsData->m_controllerEvents = vrEventsData->m_numControllerEvents? + &m_data->m_cachedVREvents[0] : 0; +} + +void DARTPhysicsClient::getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData) +{ + keyboardEventsData->m_numKeyboardEvents = m_data->m_cachedKeyboardEvents.size(); + keyboardEventsData->m_keyboardEvents = keyboardEventsData->m_numKeyboardEvents? + &m_data->m_cachedKeyboardEvents[0] : 0; +} + +void DARTPhysicsClient::getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData) +{ + mouseEventsData->m_numMouseEvents = m_data->m_cachedMouseEvents.size(); + mouseEventsData->m_mouseEvents = mouseEventsData->m_numMouseEvents? + &m_data->m_cachedMouseEvents[0] : 0; +} + + +void DARTPhysicsClient::getCachedRaycastHits(struct b3RaycastInformation* raycastHits) +{ + raycastHits->m_numRayHits = m_data->m_raycastHits.size(); + raycastHits->m_rayHits = raycastHits->m_numRayHits? &m_data->m_raycastHits[0] : 0; +} + +void DARTPhysicsClient::getCachedMassMatrix(int dofCountCheck, double* massMatrix) +{ + int sz = dofCountCheck*dofCountCheck; + if (sz == m_data->m_cachedMassMatrix.size()) + { + for (int i=0;im_cachedMassMatrix[i]; + } + } +} + +void DARTPhysicsClient::setTimeOut(double timeOutInSeconds) +{ + m_data->m_timeOutInSeconds = timeOutInSeconds; +} + +double DARTPhysicsClient::getTimeOut() const +{ + return m_data->m_timeOutInSeconds; +} + +bool DARTPhysicsClient::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (!bodyJointsPtr || !(*bodyJointsPtr)) { + return false; + } + DARTUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; + if (!userDataCachePtr) + { + return false; + } + SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataId]; + if (!userDataPtr) + { + return false; + } + valueOut.m_type = userDataPtr->m_type; + valueOut.m_length = userDataPtr->m_bytes.size(); + valueOut.m_data1 = userDataPtr->m_bytes.size()? &userDataPtr->m_bytes[0] : 0; + return true; +} + +int DARTPhysicsClient::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (!bodyJointsPtr || !(*bodyJointsPtr)) { + return -1; + } + DARTUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; + if (!userDataCachePtr) { + return -1; + } + int *userDataId = (userDataCachePtr)->m_keyToUserDataIdMap[key]; + if (!userDataId) { + return -1; + } + return *userDataId; +} + +int DARTPhysicsClient::getNumUserData(int bodyUniqueId, int linkIndex) const { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (!bodyJointsPtr || !(*bodyJointsPtr)) { + return 0; + } + DARTUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; + if (!userDataCachePtr) { + return 0; + } + return (userDataCachePtr)->m_userDataMap.size(); +} + +void DARTPhysicsClient::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (!bodyJointsPtr || !(*bodyJointsPtr)) { + *keyOut = 0; + *userDataIdOut = -1; + return; + } + DARTUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; + if (!userDataCachePtr || userDataIndex >= (userDataCachePtr)->m_userDataMap.size()) + { + *keyOut = 0; + *userDataIdOut = -1; + return; + } + *userDataIdOut = (userDataCachePtr)->m_userDataMap.getKeyAtIndex(userDataIndex).getUid1(); + SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex); + *keyOut = (userDataPtr)->m_key.c_str(); +} + + + +void DARTPhysicsClient::pushProfileTiming(const char* timingName) +{ + std::string** strPtr = m_data->m_profileTimingStringArray[timingName]; + std::string* str = 0; + if (strPtr) + { + str = *strPtr; + } else + { + str = new std::string(timingName); + m_data->m_profileTimingStringArray.insert(timingName,str); + } + m_data->m_profileTimings.push_back(new CProfileSample(str->c_str())); +} + + +void DARTPhysicsClient::popProfileTiming() +{ + if (m_data->m_profileTimings.size()) + { + CProfileSample* sample = m_data->m_profileTimings[m_data->m_profileTimings.size()-1]; + m_data->m_profileTimings.pop_back(); + delete sample; + } +} diff --git a/examples/SharedMemory/dart/DARTPhysicsClient.h b/examples/SharedMemory/dart/DARTPhysicsClient.h new file mode 100644 index 000000000..466aaeb50 --- /dev/null +++ b/examples/SharedMemory/dart/DARTPhysicsClient.h @@ -0,0 +1,124 @@ +#ifndef DART_PHYSICS_CLIENT_H +#define DART_PHYSICS_CLIENT_H + + +#include "../PhysicsClient.h" + +///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands +class DARTPhysicsClient : public PhysicsClient +{ +protected: + + struct DARTPhysicsDirectInternalData* m_data; + + bool processDebugLines(const struct SharedMemoryCommand& orgCommand); + + bool processCamera(const struct SharedMemoryCommand& orgCommand); + + bool processContactPointData(const struct SharedMemoryCommand& orgCommand); + + bool processOverlappingObjects(const struct SharedMemoryCommand& orgCommand); + + bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand); + + void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); + + void processAddUserData(const struct SharedMemoryStatus& serverCmd); + + void postProcessStatus(const struct SharedMemoryStatus& serverCmd); + + void resetData(); + + void removeCachedBody(int bodyUniqueId); + +public: + + DARTPhysicsClient(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership); + + virtual ~DARTPhysicsClient(); + + // return true if connection succesfull, can also check 'isConnected' + //it is OK to pass a null pointer for the gui helper + 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 getNumBodies() const; + + virtual int getBodyUniqueId(int serialIndex) const; + + virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const; + + virtual int getNumJoints(int bodyIndex) const; + + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + + virtual int getNumUserConstraints() const; + + virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const; + + virtual int getUserConstraintId(int serialIndex) const; + + ///todo: move this out of the + virtual void setSharedMemoryKey(int key); + + void uploadBulletFileToSharedMemory(const char* data, int len); + + virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays); + + virtual int getNumDebugLines() const; + + virtual const float* getDebugLinesFrom() const; + virtual const float* getDebugLinesTo() const; + virtual const float* getDebugLinesColor() const; + + virtual void getCachedCameraImage(b3CameraImageData* cameraData); + + virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); + + virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects); + + virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo); + + virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo); + + virtual void getCachedVREvents(struct b3VREventsData* vrEventsData); + + virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData); + + virtual void getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData); + + virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); + + virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix); + + //the following APIs are for internal use for visualization: + virtual bool connect(struct GUIHelperInterface* guiHelper); + virtual void renderScene(); + virtual void debugDraw(int debugDrawMode); + + virtual void setTimeOut(double timeOutInSeconds); + virtual double getTimeOut() const; + + virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const; + virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const; + virtual int getNumUserData(int bodyUniqueId, int linkIndex) const; + virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const; + + virtual void pushProfileTiming(const char* timingName); + virtual void popProfileTiming(); +}; + +#endif //DART_PHYSICS__H diff --git a/examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.cpp b/examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.cpp new file mode 100644 index 000000000..713f7d654 --- /dev/null +++ b/examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.cpp @@ -0,0 +1,39 @@ +#include "DARTPhysicsServerCommandProcessor.h" + + +DARTPhysicsServerCommandProcessor::DARTPhysicsServerCommandProcessor() +{ +} + +DARTPhysicsServerCommandProcessor::~DARTPhysicsServerCommandProcessor() +{ + +} + +bool DARTPhysicsServerCommandProcessor::connect() +{ + return false; +} + +void DARTPhysicsServerCommandProcessor::disconnect() +{ + +} + +bool DARTPhysicsServerCommandProcessor::isConnected() const +{ + return false; +} + +bool DARTPhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + return false; +} + +bool DARTPhysicsServerCommandProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + return false; +} + + + diff --git a/examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.h b/examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.h new file mode 100644 index 000000000..bbbfbfa19 --- /dev/null +++ b/examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.h @@ -0,0 +1,31 @@ +#ifndef DART_PHYSICS_SERVER_COMMAND_PROCESSOR_H +#define DART_PHYSICS_SERVER_COMMAND_PROCESSOR_H + +#include "../PhysicsCommandProcessorInterface.h" + +class DARTPhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface +{ + +public: + DARTPhysicsServerCommandProcessor(); + + virtual ~DARTPhysicsServerCommandProcessor(); + + virtual bool connect(); + + virtual void disconnect(); + + virtual bool isConnected() const; + + virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + + virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + + virtual void renderScene(int renderFlags){} + virtual void physicsDebugDraw(int debugDrawFlags){} + virtual void setGuiHelper(struct GUIHelperInterface* guiHelper){} + virtual void setTimeOut(double timeOutInSeconds){} + +}; + +#endif //DART_PHYSICS_COMMAND_PROCESSOR_H diff --git a/examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.cpp b/examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.cpp new file mode 100644 index 000000000..2372a879f --- /dev/null +++ b/examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.cpp @@ -0,0 +1,15 @@ +#ifdef BT_ENABLE_MUJOCO +#include "MuJoCoPhysicsC_API.h" +#include "MuJoCoPhysicsServerCommandProcessor.h" +#include "MuJoCoPhysicsClient.h" + +B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsMuJoCo() +{ + MuJoCoPhysicsServerCommandProcessor* sdk = new MuJoCoPhysicsServerCommandProcessor; + + MuJoCoPhysicsClient* direct = new MuJoCoPhysicsClient(sdk,true); + bool connected; + connected = direct->connect(); + return (b3PhysicsClientHandle )direct; +} +#endif//BT_ENABLE_MUJOCO \ No newline at end of file diff --git a/examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.h b/examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.h new file mode 100644 index 000000000..b9ff9814d --- /dev/null +++ b/examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.h @@ -0,0 +1,20 @@ +#ifndef MUJOCO_PHYSICS_C_API_H +#define MUJOCO_PHYSICS_C_API_H + +#ifdef BT_ENABLE_MUJOCO + +#include "../PhysicsClientC_API.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//think more about naming. The b3ConnectPhysicsLoopback +B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsMuJoCo(); + +#ifdef __cplusplus +} +#endif + +#endif //BT_ENABLE_MUJOCO +#endif //MUJOCO_PHYSICS_C_API_H diff --git a/examples/SharedMemory/mujoco/MuJoCoPhysicsClient.cpp b/examples/SharedMemory/mujoco/MuJoCoPhysicsClient.cpp new file mode 100644 index 000000000..c78b381a0 --- /dev/null +++ b/examples/SharedMemory/mujoco/MuJoCoPhysicsClient.cpp @@ -0,0 +1,1598 @@ +#ifdef BT_ENABLE_MUJOCO +#include "MuJoCoPhysicsClient.h" + +#include "../PhysicsClientSharedMemory.h" +#include "../../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../SharedMemoryCommands.h" +#include "../PhysicsCommandProcessorInterface.h" +#include "../../Utils/b3Clock.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" +#include + +#include "../SharedMemoryUserData.h" +#include "LinearMath/btQuickprof.h" + +struct MuJoCoUserDataCache { + btHashMap m_userDataMap; + btHashMap m_keyToUserDataIdMap; + + ~MuJoCoUserDataCache() + { + + } +}; + +struct BodyJointInfoCache2 +{ + std::string m_baseName; + btAlignedObjectArray m_jointInfo; + std::string m_bodyName; + + // Joint index -> user data. + btHashMap m_jointToUserDataMap; + + ~BodyJointInfoCache2() { + } +}; + + + +struct MuJoCoPhysicsDirectInternalData +{ + DummyGUIHelper m_noGfx; + + btAlignedObjectArray m_serverDNA; + SharedMemoryCommand m_command; + SharedMemoryStatus m_serverStatus; + + SharedMemoryCommand m_tmpInfoRequestCommand; + SharedMemoryStatus m_tmpInfoStatus; + bool m_hasStatus; + bool m_verboseOutput; + + btAlignedObjectArray m_debugLinesFrom; + btAlignedObjectArray m_debugLinesTo; + btAlignedObjectArray m_debugLinesColor; + + btHashMap m_bodyJointMap; + btHashMap m_userConstraintInfoMap; + + btAlignedObjectArray m_profileTimings; + btHashMap m_profileTimingStringArray; + + char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; + btAlignedObjectArray m_cachedMassMatrix; + int m_cachedCameraPixelsWidth; + int m_cachedCameraPixelsHeight; + btAlignedObjectArray m_cachedCameraPixelsRGBA; + btAlignedObjectArray m_cachedCameraDepthBuffer; + btAlignedObjectArray m_cachedSegmentationMask; + + btAlignedObjectArray m_cachedContactPoints; + btAlignedObjectArray m_cachedOverlappingObjects; + + btAlignedObjectArray m_cachedVisualShapes; + btAlignedObjectArray m_cachedCollisionShapes; + + btAlignedObjectArray m_cachedVREvents; + + btAlignedObjectArray m_cachedKeyboardEvents; + btAlignedObjectArray m_cachedMouseEvents; + + btAlignedObjectArray m_raycastHits; + + PhysicsCommandProcessorInterface* m_commandProcessor; + bool m_ownsCommandProcessor; + double m_timeOutInSeconds; + + MuJoCoPhysicsDirectInternalData() + :m_hasStatus(false), + m_verboseOutput(false), + m_cachedCameraPixelsWidth(0), + m_cachedCameraPixelsHeight(0), + m_commandProcessor(NULL), + m_ownsCommandProcessor(false), + m_timeOutInSeconds(1e30) + { + memset(&m_command, 0, sizeof(m_command)); + memset(&m_serverStatus, 0, sizeof(m_serverStatus)); + memset(m_bulletStreamDataServerToClient, 0, sizeof(m_bulletStreamDataServerToClient)); + } +}; + +MuJoCoPhysicsClient::MuJoCoPhysicsClient(PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership) +{ + int sz = sizeof(SharedMemoryCommand); + int sz2 = sizeof(SharedMemoryStatus); + + m_data = new MuJoCoPhysicsDirectInternalData; + m_data->m_commandProcessor = physSdk; + m_data->m_ownsCommandProcessor = passSdkOwnership; +} + +MuJoCoPhysicsClient::~MuJoCoPhysicsClient() +{ + for (int i=0;im_profileTimingStringArray.size();i++) + { + std::string** str = m_data->m_profileTimingStringArray.getAtIndex(i); + if (str) + { + delete *str; + } + } + m_data->m_profileTimingStringArray.clear(); + + if (m_data->m_commandProcessor->isConnected()) + { + m_data->m_commandProcessor->disconnect(); + } + if (m_data->m_ownsCommandProcessor) + { + delete m_data->m_commandProcessor; + } + + resetData(); + + + + delete m_data; +} + +void MuJoCoPhysicsClient::resetData() +{ + 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) + { + delete (*bodyJointsPtr); + } + } + m_data->m_bodyJointMap.clear(); + m_data->m_userConstraintInfoMap.clear(); +} + +// return true if connection succesfull, can also check 'isConnected' +bool MuJoCoPhysicsClient::connect() +{ + bool connected = m_data->m_commandProcessor->connect(); + m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx); + + + if (connected) + //also request serialization data + { + SharedMemoryCommand command; + command.m_type = CMD_REQUEST_INTERNAL_DATA; + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + if (hasStatus) + { + postProcessStatus(m_data->m_serverStatus); + } + else + { + b3Clock clock; + double timeSec = clock.getTimeInSeconds(); + + while ((!hasStatus) && (clock.getTimeInSeconds()-timeSec <10 )) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + } + } + + return connected; +} + +// return true if connection succesfull, can also check 'isConnected' +bool MuJoCoPhysicsClient::connect(struct GUIHelperInterface* guiHelper) +{ + bool connected = m_data->m_commandProcessor->connect(); + + m_data->m_commandProcessor->setGuiHelper(guiHelper); + + return connected; +} + +void MuJoCoPhysicsClient::renderScene() +{ + int renderFlags = 0; + m_data->m_commandProcessor->renderScene(renderFlags); +} + +void MuJoCoPhysicsClient::debugDraw(int debugDrawMode) +{ + m_data->m_commandProcessor->physicsDebugDraw(debugDrawMode); +} + +////todo: rename to 'disconnect' +void MuJoCoPhysicsClient::disconnectSharedMemory() +{ + m_data->m_commandProcessor->disconnect(); + m_data->m_commandProcessor->setGuiHelper(0); +} + +bool MuJoCoPhysicsClient::isConnected() const +{ + return m_data->m_commandProcessor->isConnected(); +} + +// return non-null if there is a status, nullptr otherwise +const SharedMemoryStatus* MuJoCoPhysicsClient::processServerStatus() +{ + + if (!m_data->m_hasStatus) + { + m_data->m_hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + SharedMemoryStatus* stat = 0; + if (m_data->m_hasStatus) + { + stat = &m_data->m_serverStatus; + + postProcessStatus(m_data->m_serverStatus); + + m_data->m_hasStatus = false; + } + return stat; +} + +SharedMemoryCommand* MuJoCoPhysicsClient::getAvailableSharedMemoryCommand() +{ + return &m_data->m_command; +} + +bool MuJoCoPhysicsClient::canSubmitCommand() const +{ + return m_data->m_commandProcessor->isConnected(); +} + +bool MuJoCoPhysicsClient::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); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + 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) + { + m_data->m_hasStatus = false; + + 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 MuJoCoPhysicsClient::processVisualShapeData(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); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + + if (m_data->m_verboseOutput) + { + b3Printf("Visual Shape Information Request OK\n"); + } + int startVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex; + int numVisualShapesCopied = serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + m_data->m_cachedVisualShapes.resize(startVisualShapeIndex + numVisualShapesCopied); + b3VisualShapeData* shapeData = (b3VisualShapeData*)&m_data->m_bulletStreamDataServerToClient[0]; + for (int i = 0; i < numVisualShapesCopied; i++) + { + m_data->m_cachedVisualShapes[startVisualShapeIndex + i] = shapeData[i]; + } + + if (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes >0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied) + { + m_data->m_hasStatus = false; + + command.m_type = CMD_REQUEST_VISUAL_SHAPE_INFO; + command.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + command.m_requestVisualShapeDataArguments.m_bodyUniqueId = serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId; + } + } + } while (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes > 0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied); + + return m_data->m_hasStatus; +} + +bool MuJoCoPhysicsClient::processOverlappingObjects(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); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + if (m_data->m_verboseOutput) + { + b3Printf("Overlapping Objects Request OK\n"); + } + + int startOverlapIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex; + int numOverlapCopied = serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied; + m_data->m_cachedOverlappingObjects.resize(startOverlapIndex + numOverlapCopied); + b3OverlappingObject* objects = (b3OverlappingObject*)&m_data->m_bulletStreamDataServerToClient[0]; + + for (int i = 0; i < numOverlapCopied; i++) + { + m_data->m_cachedOverlappingObjects[startOverlapIndex + i] = objects[i]; + } + + if (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied) + { + m_data->m_hasStatus = false; + command.m_type = CMD_REQUEST_AABB_OVERLAP; + command.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied; + } + + } + } while (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied); + + return m_data->m_hasStatus; + +} + + + +bool MuJoCoPhysicsClient::processContactPointData(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); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + if (m_data->m_verboseOutput) + { + b3Printf("Contact Point Information Request OK\n"); + } + int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex; + int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + + m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied); + + b3ContactPointData* contactData = (b3ContactPointData*)&m_data->m_bulletStreamDataServerToClient[0]; + + for (int i=0;im_cachedContactPoints[startContactIndex+i] = contactData[i]; + } + + if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied) + { + + m_data->m_hasStatus = false; + + command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION; + command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex+serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + command.m_requestContactPointArguments.m_objectAIndexFilter = -1; + command.m_requestContactPointArguments.m_objectBIndexFilter = -1; + + } + + } + } while (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied); + + return m_data->m_hasStatus; + +} + + +bool MuJoCoPhysicsClient::processCamera(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); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + btAssert(m_data->m_serverStatus.m_type == CMD_CAMERA_IMAGE_COMPLETED); + + if (m_data->m_verboseOutput) + { + b3Printf("Camera image OK\n"); + } + + int numBytesPerPixel = 4;//RGBA + int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex+ + serverCmd.m_sendPixelDataArguments.m_numPixelsCopied+ + serverCmd.m_sendPixelDataArguments.m_numRemainingPixels; + + m_data->m_cachedCameraPixelsWidth = 0; + m_data->m_cachedCameraPixelsHeight = 0; + + int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth*serverCmd.m_sendPixelDataArguments.m_imageHeight; + + m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel); + m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels); + m_data->m_cachedSegmentationMask.resize(numTotalPixels); + m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel); + + + unsigned char* rgbaPixelsReceived = + (unsigned char*)&m_data->m_bulletStreamDataServerToClient[0]; + + float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]); + int* segmentationMaskBuffer = (int*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*8]); + + // printf("pixel = %d\n", rgbaPixelsReceived[0]); + + for (int i=0;im_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i]; + } + for (int i=0;im_cachedSegmentationMask[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i]; + } + for (int i=0;im_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] + = rgbaPixelsReceived[i]; + } + + if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied) + { + + m_data->m_hasStatus = false; + + // continue requesting remaining pixels + command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA; + command.m_requestPixelDataArguments.m_startPixelIndex = + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex + + serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; + + } else + { + m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth; + m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight; + } + } + } while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied); + + return m_data->m_hasStatus; + + +} + + +void MuJoCoPhysicsClient::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd) +{ + + BodyJointInfoCache2** cachePtr = m_data->m_bodyJointMap[bodyUniqueId]; + //don't process same bodyUniqueId multiple times + if (cachePtr) + { + return; + } + + bParse::btBulletFile bf( + &m_data->m_bulletStreamDataServerToClient[0], + serverCmd.m_numDataStreamBytes); + if (m_data->m_serverDNA.size()) + { + bf.setFileDNA(false, &m_data->m_serverDNA[0], m_data->m_serverDNA.size()); + } + else + { + bf.setFileDNAisMemoryDNA(); + } + bf.parse(false); + + BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; + m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints); + bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; + + 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]; + + if (mb->m_baseName) + { + bodyJoints->m_baseName = mb->m_baseName; + } + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } else + { + Bullet::btMultiBodyFloatData* mb = + (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; + + + if (mb->m_baseName) + { + bodyJoints->m_baseName = mb->m_baseName; + } + 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"); + } +} + +void MuJoCoPhysicsClient::processAddUserData(const struct SharedMemoryStatus& serverCmd) { + const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_userDataResponseArgs.m_userDataGlobalId; + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + MuJoCoUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; + if (!userDataCachePtr) + { + MuJoCoUserDataCache cache; + (*bodyJointsPtr)->m_jointToUserDataMap.insert(userDataGlobalId.m_linkIndex, cache); + } + userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; + + const char *dataStream = m_data->m_bulletStreamDataServerToClient; + + b3UserDataValue userDataValue; + userDataValue.m_type = serverCmd.m_userDataResponseArgs.m_valueType; + userDataValue.m_length = serverCmd.m_userDataResponseArgs.m_valueLength; + SharedMemoryUserData *userDataPtr = userDataCachePtr->m_userDataMap[userDataGlobalId.m_userDataId]; + if (userDataPtr) { + // Only replace the value. + (userDataPtr)->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type); + } + else { + // Add a new user data entry. + (userDataCachePtr)->m_userDataMap.insert(userDataGlobalId.m_userDataId, SharedMemoryUserData(serverCmd.m_userDataResponseArgs.m_key)); + userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; + userDataPtr->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type); + (userDataCachePtr)->m_keyToUserDataIdMap.insert(serverCmd.m_userDataResponseArgs.m_key, userDataGlobalId.m_userDataId); + } + } +} + +void MuJoCoPhysicsClient::postProcessStatus(const struct SharedMemoryStatus& serverCmd) +{ + switch (serverCmd.m_type) + { + + case CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Raycast completed"); + } + m_data->m_raycastHits.clear(); + b3RayHitInfo *rayHits = (b3RayHitInfo *)m_data->m_bulletStreamDataServerToClient; + for (int i=0;im_raycastHits.push_back(rayHits[i]); + } + break; + } + case CMD_REQUEST_VR_EVENTS_DATA_COMPLETED: + { + + if (m_data->m_verboseOutput) + { + b3Printf("Request VR Events completed"); + } + m_data->m_cachedVREvents.resize(serverCmd.m_sendVREvents.m_numVRControllerEvents); + for (int i=0;i< serverCmd.m_sendVREvents.m_numVRControllerEvents;i++) + { + m_data->m_cachedVREvents[i] = serverCmd.m_sendVREvents.m_controllerEvents[i]; + } + break; + } + case CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Request keyboard events completed"); + } + m_data->m_cachedKeyboardEvents.resize(serverCmd.m_sendKeyboardEvents.m_numKeyboardEvents); + for (int i=0;im_cachedKeyboardEvents[i] = serverCmd.m_sendKeyboardEvents.m_keyboardEvents[i]; + } + break; + } + + case CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED: + { + B3_PROFILE("CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED"); + if (m_data->m_verboseOutput) + { + b3Printf("Request mouse events completed"); + } + m_data->m_cachedMouseEvents.resize(serverCmd.m_sendMouseEvents.m_numMouseEvents); + for (int i=0;im_cachedMouseEvents[i] = serverCmd.m_sendMouseEvents.m_mouseEvents[i]; + } + break; + } + + case CMD_REQUEST_INTERNAL_DATA_COMPLETED: + { + if (serverCmd.m_numDataStreamBytes) + { + int numStreamBytes = serverCmd.m_numDataStreamBytes; + m_data->m_serverDNA.resize(numStreamBytes); + for (int i = 0; i < numStreamBytes; i++) + { + m_data->m_serverDNA[i] = m_data->m_bulletStreamDataServerToClient[i]; + } + } + break; + } + case CMD_RESET_SIMULATION_COMPLETED: + { + resetData(); + break; + } + + case CMD_USER_CONSTRAINT_INFO_COMPLETED: + case CMD_USER_CONSTRAINT_COMPLETED: + { + int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; + m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs); + break; + } + case CMD_REMOVE_USER_CONSTRAINT_COMPLETED: + { + int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; + m_data->m_userConstraintInfoMap.remove(cid); + break; + } + case CMD_REMOVE_BODY_FAILED: + { + b3Warning("Remove body failed\n"); + break; + } + case CMD_REMOVE_BODY_COMPLETED: + { + for (int i=0;im_userConstraintInfoMap.remove(key); + } + + break; + } + case CMD_CHANGE_USER_CONSTRAINT_COMPLETED: + { + int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; + b3UserConstraint* userConstraintPtr = m_data->m_userConstraintInfoMap[cid]; + if (userConstraintPtr) + { + const b3UserConstraint* serverConstraint = &serverCmd.m_userConstraintResultArgs; + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B) + { + userConstraintPtr->m_childFrame[0] = serverConstraint->m_childFrame[0]; + userConstraintPtr->m_childFrame[1] = serverConstraint->m_childFrame[1]; + userConstraintPtr->m_childFrame[2] = serverConstraint->m_childFrame[2]; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) + { + userConstraintPtr->m_childFrame[3] = serverConstraint->m_childFrame[3]; + userConstraintPtr->m_childFrame[4] = serverConstraint->m_childFrame[4]; + userConstraintPtr->m_childFrame[5] = serverConstraint->m_childFrame[5]; + userConstraintPtr->m_childFrame[6] = serverConstraint->m_childFrame[6]; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) + { + userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) + { + userConstraintPtr->m_gearRatio = serverConstraint->m_gearRatio; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET) + { + userConstraintPtr->m_relativePositionTarget = serverConstraint->m_relativePositionTarget; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP) + { + userConstraintPtr->m_erp = serverConstraint->m_erp; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) + { + userConstraintPtr->m_gearAuxLink = serverConstraint->m_gearAuxLink; + } + } + break; + } + case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED: + { + break; + } + case CMD_SYNC_BODY_INFO_COMPLETED: + case CMD_MJCF_LOADING_COMPLETED: + case CMD_SDF_LOADING_COMPLETED: + { + //we'll stream further info from the physics server + //so serverCmd will be invalid, make a copy + + int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints; + for (int i=0;im_tmpInfoRequestCommand.m_type = CMD_USER_CONSTRAINT; + m_data->m_tmpInfoRequestCommand.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO; + m_data->m_tmpInfoRequestCommand.m_userConstraintArguments.m_userConstraintUniqueId = constraintUid; + + bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + if (hasStatus) + { + int cid = m_data->m_tmpInfoStatus.m_userConstraintResultArgs.m_userConstraintUniqueId; + m_data->m_userConstraintInfoMap.insert(cid,m_data->m_tmpInfoStatus.m_userConstraintResultArgs); + } + } + + int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; + for (int i = 0; im_tmpInfoRequestCommand.m_type = CMD_REQUEST_BODY_INFO; + m_data->m_tmpInfoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId; + + bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + if (hasStatus) + { + processBodyJointInfo(bodyUniqueId, m_data->m_tmpInfoStatus); + } + } + break; + } + case CMD_CREATE_MULTI_BODY_COMPLETED: + case CMD_URDF_LOADING_COMPLETED: + { + + if (serverCmd.m_numDataStreamBytes > 0) + { + int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId; + processBodyJointInfo(bodyIndex, serverCmd); + } + break; + } + case CMD_BULLET_LOADING_FAILED: + { + b3Warning("Couldn't load .bullet file"); + break; + } + case CMD_BULLET_LOADING_COMPLETED: + { + break; + } + + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED: + { + break; + } + + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED: + { + b3Warning("requestOpenGLVisualizeCamera failed"); + break; + } + case CMD_REMOVE_USER_CONSTRAINT_FAILED: + { + b3Warning("removeConstraint failed"); + break; + } + case CMD_CHANGE_USER_CONSTRAINT_FAILED: + { + //b3Warning("changeConstraint failed"); + break; + } + + case CMD_USER_CONSTRAINT_FAILED: + { + b3Warning("createConstraint failed"); + break; + } + + case CMD_CREATE_COLLISION_SHAPE_FAILED: + { + b3Warning("createCollisionShape failed"); + break; + } + case CMD_CREATE_COLLISION_SHAPE_COMPLETED: + { + break; + } + + case CMD_CREATE_VISUAL_SHAPE_FAILED: + { + b3Warning("createVisualShape failed"); + break; + } + case CMD_CREATE_VISUAL_SHAPE_COMPLETED: + { + break; + } + + case CMD_CREATE_MULTI_BODY_FAILED: + { + b3Warning("createMultiBody failed"); + break; + } + case CMD_REQUEST_COLLISION_INFO_COMPLETED: + { + break; + } + case CMD_REQUEST_COLLISION_INFO_FAILED: + { + b3Warning("Request getCollisionInfo failed"); + break; + } + + case CMD_CUSTOM_COMMAND_COMPLETED: + { + break; + } + case CMD_CUSTOM_COMMAND_FAILED: + { + b3Warning("custom plugin command failed"); + break; + } + case CMD_CLIENT_COMMAND_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_FAILED: + { + b3Warning("jacobian calculation failed"); + break; + } + case CMD_CALCULATED_MASS_MATRIX_FAILED: + { + b3Warning("calculate mass matrix failed"); + break; + } + case CMD_CALCULATED_MASS_MATRIX_COMPLETED: + { + double* matrixData = (double*)&m_data->m_bulletStreamDataServerToClient[0]; + m_data->m_cachedMassMatrix.resize(serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount); + for (int i=0;im_cachedMassMatrix[i] = matrixData[i]; + } + break; + } + case CMD_ACTUAL_STATE_UPDATE_COMPLETED: + { + break; + } + case CMD_DESIRED_STATE_RECEIVED_COMPLETED: + { + break; + } + case CMD_STEP_FORWARD_SIMULATION_COMPLETED: + { + break; + } + case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED: + { + break; + } + case CMD_SAVE_STATE_COMPLETED: + { + break; + } + case CMD_COLLISION_SHAPE_INFO_FAILED: + { + b3Warning("getCollisionShapeData failed"); + break; + } + case CMD_COLLISION_SHAPE_INFO_COMPLETED: + { + B3_PROFILE("CMD_COLLISION_SHAPE_INFO_COMPLETED"); + if (m_data->m_verboseOutput) + { + b3Printf("Collision Shape Information Request OK\n"); + } + int numCollisionShapesCopied = serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes; + m_data->m_cachedCollisionShapes.resize(numCollisionShapesCopied); + b3CollisionShapeData* shapeData = (b3CollisionShapeData*)&m_data->m_bulletStreamDataServerToClient[0]; + for (int i = 0; i < numCollisionShapesCopied; i++) + { + m_data->m_cachedCollisionShapes[i] = shapeData[i]; + } + break; + } + case CMD_RESTORE_STATE_FAILED: + { + b3Warning("restoreState failed"); + break; + } + case CMD_RESTORE_STATE_COMPLETED: + { + break; + } + case CMD_BULLET_SAVING_COMPLETED: + { + break; + } + case CMD_LOAD_SOFT_BODY_FAILED: + { + b3Warning("loadSoftBody failed"); + break; + } + case CMD_LOAD_SOFT_BODY_COMPLETED: + { + break; + } + case CMD_SYNC_USER_DATA_FAILED: + { + b3Warning("Synchronizing user data failed."); + break; + } + case CMD_ADD_USER_DATA_FAILED: + { + b3Warning("Adding user data failed (do the specified body and link exist?)"); + break; + } + case CMD_REMOVE_USER_DATA_FAILED: + { + b3Warning("Removing user data failed"); + break; + } + case CMD_ADD_USER_DATA_COMPLETED: + { + processAddUserData(serverCmd); + break; + } + case CMD_SYNC_USER_DATA_COMPLETED: + { + B3_PROFILE("CMD_SYNC_USER_DATA_COMPLETED"); + // Remove all cached user data entries. + for(int i=0; im_bodyJointMap.size(); i++) + { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); + if (bodyJointsPtr && *bodyJointsPtr) + { + (*bodyJointsPtr)->m_jointToUserDataMap.clear(); + } + } + const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers; + b3UserDataGlobalIdentifier *identifiers = new b3UserDataGlobalIdentifier[numIdentifiers]; + memcpy(identifiers, &m_data->m_bulletStreamDataServerToClient[0], numIdentifiers * sizeof(b3UserDataGlobalIdentifier)); + + for (int i=0; im_tmpInfoRequestCommand.m_type = CMD_REQUEST_USER_DATA; + m_data->m_tmpInfoRequestCommand.m_userDataRequestArgs = identifiers[i]; + + bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + if (hasStatus) + { + processAddUserData(m_data->m_tmpInfoStatus); + } + } + delete[] identifiers; + break; + } + case CMD_REMOVE_USER_DATA_COMPLETED: + { + const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_removeUserDataResponseArgs; + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) { + MuJoCoUserDataCache *userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; + if (userDataCachePtr) + { + SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; + if (userDataPtr) { + (userDataCachePtr)->m_keyToUserDataIdMap.remove((userDataPtr)->m_key.c_str()); + (userDataCachePtr)->m_userDataMap.remove(userDataGlobalId.m_userDataId); + } + } + } + break; + } + default: + { + //b3Warning("Unknown server status type"); + } + }; + + +} +bool MuJoCoPhysicsClient::submitClientCommand(const struct SharedMemoryCommand& command) +{ + if (command.m_type==CMD_REQUEST_DEBUG_LINES) + { + return processDebugLines(command); + } + + if (command.m_type==CMD_REQUEST_CAMERA_IMAGE_DATA) + { + return processCamera(command); + } + if (command.m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION) + { + return processContactPointData(command); + } + + if (command.m_type == CMD_REQUEST_VISUAL_SHAPE_INFO) + { + return processVisualShapeData(command); + } + if (command.m_type == CMD_REQUEST_AABB_OVERLAP) + { + return processOverlappingObjects(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) + { + postProcessStatus(m_data->m_serverStatus); + m_data->m_hasStatus = false; + } + */ + return hasStatus; +} + +int MuJoCoPhysicsClient::getNumBodies() const +{ + return m_data->m_bodyJointMap.size(); +} + +void MuJoCoPhysicsClient::removeCachedBody(int bodyUniqueId) +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + delete (*bodyJointsPtr); + m_data->m_bodyJointMap.remove(bodyUniqueId); + } +} + + +int MuJoCoPhysicsClient::getNumUserConstraints() const +{ + return m_data->m_userConstraintInfoMap.size(); +} + +int MuJoCoPhysicsClient::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const +{ + b3UserConstraint* constraintPtr =m_data->m_userConstraintInfoMap[constraintUniqueId]; + if (constraintPtr) + { + info = *constraintPtr; + return 1; + } + return 0; +} + +int MuJoCoPhysicsClient::getUserConstraintId(int serialIndex) const +{ + if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints())) + { + return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1(); + } + return -1; +} + +int MuJoCoPhysicsClient::getBodyUniqueId(int serialIndex) const +{ + if ((serialIndex >= 0) && (serialIndex < getNumBodies())) + { + return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1(); + } + return -1; +} + +bool MuJoCoPhysicsClient::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + strcpy(info.m_baseName,bodyJoints->m_baseName.c_str()); + strcpy(info.m_bodyName ,bodyJoints->m_bodyName .c_str()); + return true; + } + + return false; +} + +int MuJoCoPhysicsClient::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; +} + +bool MuJoCoPhysicsClient::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + if ((jointIndex >=0) && (jointIndex < bodyJoints->m_jointInfo.size())) + { + info = bodyJoints->m_jointInfo[jointIndex]; + return true; + } + } + return false; +} + + +void MuJoCoPhysicsClient::setSharedMemoryKey(int key) +{ +} + + +void MuJoCoPhysicsClient::uploadBulletFileToSharedMemory(const char* data, int len) +{ + if (len>SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) + { + len = SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE; + } + for (int i=0;im_bulletStreamDataServerToClient[i] = data[i]; + } + //m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len); +} + +void MuJoCoPhysicsClient::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) +{ + int curNumStreamingRays = command.m_requestRaycastIntersections.m_numStreamingRays; + int newNumRays = curNumStreamingRays + numRays; + btAssert(newNumRaysm_bulletStreamDataServerToClient; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0]; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1]; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2]; + command.m_requestRaycastIntersections.m_numStreamingRays++; + } + + } + +} + + +int MuJoCoPhysicsClient::getNumDebugLines() const +{ + return m_data->m_debugLinesFrom.size(); +} + +const float* MuJoCoPhysicsClient::getDebugLinesFrom() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesFrom[0].m_x; + } + return 0; +} +const float* MuJoCoPhysicsClient::getDebugLinesTo() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesTo[0].m_x; + } + return 0; +} +const float* MuJoCoPhysicsClient::getDebugLinesColor() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesColor[0].m_x; + } + return 0; +} + +void MuJoCoPhysicsClient::getCachedCameraImage(b3CameraImageData* cameraData) +{ + if (cameraData) + { + cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth; + cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight; + cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0; + cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0; + cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMask.size()? &m_data->m_cachedSegmentationMask[0] : 0; + } +} + +void MuJoCoPhysicsClient::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) +{ + contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size(); + contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0; + +} + +void MuJoCoPhysicsClient::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects) +{ + overlappingObjects->m_numOverlappingObjects = m_data->m_cachedOverlappingObjects.size(); + overlappingObjects->m_overlappingObjects = m_data->m_cachedOverlappingObjects.size() ? + &m_data->m_cachedOverlappingObjects[0] : 0; +} + + +void MuJoCoPhysicsClient::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) +{ + visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size(); + visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0; +} + +void MuJoCoPhysicsClient::getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo) +{ + collisionShapesInfo->m_numCollisionShapes = m_data->m_cachedCollisionShapes.size(); + collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes ? &m_data->m_cachedCollisionShapes[0] : 0; +} + + + +void MuJoCoPhysicsClient::getCachedVREvents(struct b3VREventsData* vrEventsData) +{ + vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size(); + vrEventsData->m_controllerEvents = vrEventsData->m_numControllerEvents? + &m_data->m_cachedVREvents[0] : 0; +} + +void MuJoCoPhysicsClient::getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData) +{ + keyboardEventsData->m_numKeyboardEvents = m_data->m_cachedKeyboardEvents.size(); + keyboardEventsData->m_keyboardEvents = keyboardEventsData->m_numKeyboardEvents? + &m_data->m_cachedKeyboardEvents[0] : 0; +} + +void MuJoCoPhysicsClient::getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData) +{ + mouseEventsData->m_numMouseEvents = m_data->m_cachedMouseEvents.size(); + mouseEventsData->m_mouseEvents = mouseEventsData->m_numMouseEvents? + &m_data->m_cachedMouseEvents[0] : 0; +} + + +void MuJoCoPhysicsClient::getCachedRaycastHits(struct b3RaycastInformation* raycastHits) +{ + raycastHits->m_numRayHits = m_data->m_raycastHits.size(); + raycastHits->m_rayHits = raycastHits->m_numRayHits? &m_data->m_raycastHits[0] : 0; +} + +void MuJoCoPhysicsClient::getCachedMassMatrix(int dofCountCheck, double* massMatrix) +{ + int sz = dofCountCheck*dofCountCheck; + if (sz == m_data->m_cachedMassMatrix.size()) + { + for (int i=0;im_cachedMassMatrix[i]; + } + } +} + +void MuJoCoPhysicsClient::setTimeOut(double timeOutInSeconds) +{ + m_data->m_timeOutInSeconds = timeOutInSeconds; +} + +double MuJoCoPhysicsClient::getTimeOut() const +{ + return m_data->m_timeOutInSeconds; +} + +bool MuJoCoPhysicsClient::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (!bodyJointsPtr || !(*bodyJointsPtr)) { + return false; + } + MuJoCoUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; + if (!userDataCachePtr) + { + return false; + } + SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataId]; + if (!userDataPtr) + { + return false; + } + valueOut.m_type = userDataPtr->m_type; + valueOut.m_length = userDataPtr->m_bytes.size(); + valueOut.m_data1 = userDataPtr->m_bytes.size()? &userDataPtr->m_bytes[0] : 0; + return true; +} + +int MuJoCoPhysicsClient::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (!bodyJointsPtr || !(*bodyJointsPtr)) { + return -1; + } + MuJoCoUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; + if (!userDataCachePtr) { + return -1; + } + int *userDataId = (userDataCachePtr)->m_keyToUserDataIdMap[key]; + if (!userDataId) { + return -1; + } + return *userDataId; +} + +int MuJoCoPhysicsClient::getNumUserData(int bodyUniqueId, int linkIndex) const { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (!bodyJointsPtr || !(*bodyJointsPtr)) { + return 0; + } + MuJoCoUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; + if (!userDataCachePtr) { + return 0; + } + return (userDataCachePtr)->m_userDataMap.size(); +} + +void MuJoCoPhysicsClient::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (!bodyJointsPtr || !(*bodyJointsPtr)) { + *keyOut = 0; + *userDataIdOut = -1; + return; + } + MuJoCoUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; + if (!userDataCachePtr || userDataIndex >= (userDataCachePtr)->m_userDataMap.size()) + { + *keyOut = 0; + *userDataIdOut = -1; + return; + } + *userDataIdOut = (userDataCachePtr)->m_userDataMap.getKeyAtIndex(userDataIndex).getUid1(); + SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex); + *keyOut = (userDataPtr)->m_key.c_str(); +} + + + +void MuJoCoPhysicsClient::pushProfileTiming(const char* timingName) +{ + std::string** strPtr = m_data->m_profileTimingStringArray[timingName]; + std::string* str = 0; + if (strPtr) + { + str = *strPtr; + } else + { + str = new std::string(timingName); + m_data->m_profileTimingStringArray.insert(timingName,str); + } + m_data->m_profileTimings.push_back(new CProfileSample(str->c_str())); +} + + +void MuJoCoPhysicsClient::popProfileTiming() +{ + if (m_data->m_profileTimings.size()) + { + CProfileSample* sample = m_data->m_profileTimings[m_data->m_profileTimings.size()-1]; + m_data->m_profileTimings.pop_back(); + delete sample; + } +} + +#endif //BT_ENABLE_MUJOCO diff --git a/examples/SharedMemory/mujoco/MuJoCoPhysicsClient.h b/examples/SharedMemory/mujoco/MuJoCoPhysicsClient.h new file mode 100644 index 000000000..2c91d8426 --- /dev/null +++ b/examples/SharedMemory/mujoco/MuJoCoPhysicsClient.h @@ -0,0 +1,123 @@ +#ifndef MUJOCO_PHYSICS_CLIENT_H +#define MUJOCO_PHYSICS_CLIENT_H + +#include "../PhysicsClient.h" + +///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands +class MuJoCoPhysicsClient : public PhysicsClient +{ +protected: + + struct MuJoCoPhysicsDirectInternalData* m_data; + + bool processDebugLines(const struct SharedMemoryCommand& orgCommand); + + bool processCamera(const struct SharedMemoryCommand& orgCommand); + + bool processContactPointData(const struct SharedMemoryCommand& orgCommand); + + bool processOverlappingObjects(const struct SharedMemoryCommand& orgCommand); + + bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand); + + void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); + + void processAddUserData(const struct SharedMemoryStatus& serverCmd); + + void postProcessStatus(const struct SharedMemoryStatus& serverCmd); + + void resetData(); + + void removeCachedBody(int bodyUniqueId); + +public: + + MuJoCoPhysicsClient(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership); + + virtual ~MuJoCoPhysicsClient(); + + // return true if connection succesfull, can also check 'isConnected' + //it is OK to pass a null pointer for the gui helper + 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 getNumBodies() const; + + virtual int getBodyUniqueId(int serialIndex) const; + + virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const; + + virtual int getNumJoints(int bodyIndex) const; + + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + + virtual int getNumUserConstraints() const; + + virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const; + + virtual int getUserConstraintId(int serialIndex) const; + + ///todo: move this out of the + virtual void setSharedMemoryKey(int key); + + void uploadBulletFileToSharedMemory(const char* data, int len); + + virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays); + + virtual int getNumDebugLines() const; + + virtual const float* getDebugLinesFrom() const; + virtual const float* getDebugLinesTo() const; + virtual const float* getDebugLinesColor() const; + + virtual void getCachedCameraImage(b3CameraImageData* cameraData); + + virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); + + virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects); + + virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo); + + virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo); + + virtual void getCachedVREvents(struct b3VREventsData* vrEventsData); + + virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData); + + virtual void getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData); + + virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); + + virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix); + + //the following APIs are for internal use for visualization: + virtual bool connect(struct GUIHelperInterface* guiHelper); + virtual void renderScene(); + virtual void debugDraw(int debugDrawMode); + + virtual void setTimeOut(double timeOutInSeconds); + virtual double getTimeOut() const; + + virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const; + virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const; + virtual int getNumUserData(int bodyUniqueId, int linkIndex) const; + virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const; + + virtual void pushProfileTiming(const char* timingName); + virtual void popProfileTiming(); +}; + +#endif //MUJOCO_PHYSICS_CLIENT_H diff --git a/examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.cpp b/examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.cpp new file mode 100644 index 000000000..701742fa7 --- /dev/null +++ b/examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.cpp @@ -0,0 +1,1032 @@ +#ifdef BT_ENABLE_MUJOCO +#include "MuJoCoPhysicsServerCommandProcessor.h" +#include "mujoco.h" +#include +#include "../SharedMemoryCommands.h" +#include "LinearMath/btQuickprof.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "LinearMath/btMinMax.h" +#include "Bullet3Common/b3FileUtils.h" +#include "../../Utils/b3ResourcePath.h" + + +struct MuJoCoPhysicsServerCommandProcessorInternalData +{ + bool m_isConnected; + bool m_verboseOutput; + double m_physicsDeltaTime; + int m_numSimulationSubSteps; + + mjModel* m_mujocoModel; + mjData* m_mujocoData; + + b3AlignedObjectArray m_mjcfRecentLoadedBodies; + MuJoCoPhysicsServerCommandProcessorInternalData() + :m_isConnected(false), + m_verboseOutput(false), + m_mujocoModel(0), + m_mujocoData(0), + m_physicsDeltaTime(1./240.), + m_numSimulationSubSteps(0) + { + } +}; + +MuJoCoPhysicsServerCommandProcessor::MuJoCoPhysicsServerCommandProcessor() +{ + m_data = new MuJoCoPhysicsServerCommandProcessorInternalData; +} + +MuJoCoPhysicsServerCommandProcessor::~MuJoCoPhysicsServerCommandProcessor() +{ + delete m_data; + +} + +bool MuJoCoPhysicsServerCommandProcessor::connect() +{ + if (m_data->m_isConnected) + { + printf("already connected\n"); + return true; + } + + printf("MuJoCo Pro library version %.2lf\n", 0.01*mj_version()); + if( mjVERSION_HEADER!=mj_version() ) + mju_error("Headers and library have different versions"); + + // activate MuJoCo license + int result = mj_activate("mjkey.txt"); + if (result==1) + { + m_data->m_isConnected = true; + return true; + } + + return false; +} + +void MuJoCoPhysicsServerCommandProcessor::resetSimulation() +{ + if (m_data->m_mujocoModel) + { + mj_deleteModel(m_data->m_mujocoModel); + m_data->m_mujocoModel=0; + } + if (m_data->m_mujocoData) + { + mj_deleteData(m_data->m_mujocoData); + m_data->m_mujocoData = 0; + } +} + +void MuJoCoPhysicsServerCommandProcessor::disconnect() +{ + resetSimulation(); + + m_data->m_isConnected = false; +} + +bool MuJoCoPhysicsServerCommandProcessor::isConnected() const +{ + return m_data->m_isConnected; +} + +bool MuJoCoPhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + // BT_PROFILE("processCommand"); + + int sz = sizeof(SharedMemoryStatus); + int sz2 = sizeof(SharedMemoryCommand); + + bool hasStatus = false; + + + serverStatusOut.m_type = CMD_INVALID_STATUS; + serverStatusOut.m_numDataStreamBytes = 0; + serverStatusOut.m_dataStream = 0; + + //consume the command + switch (clientCmd.m_type) + { + case CMD_REQUEST_INTERNAL_DATA: + { + hasStatus = processRequestInternalDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + + case CMD_SYNC_BODY_INFO: + { + hasStatus = processSyncBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SYNC_USER_DATA: + { + hasStatus = processSyncUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_MJCF: + { + hasStatus = processLoadMJCFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_BODY_INFO: + { + hasStatus = processRequestBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_STEP_FORWARD_SIMULATION: + { + hasStatus = processForwardDynamicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: + { + hasStatus = processSendPhysicsParametersCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + + }; + + case CMD_REQUEST_ACTUAL_STATE: + { + hasStatus = processRequestActualStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_RESET_SIMULATION: + { + hasStatus = processResetSimulationCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + default: + { + BT_PROFILE("CMD_UNKNOWN"); + printf("Unknown command encountered: %d",clientCmd.m_type); + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; + hasStatus = true; + } + +#if 0 + case CMD_STATE_LOGGING: + { + hasStatus = processStateLoggingCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SET_VR_CAMERA_STATE: + { + hasStatus = processSetVRCameraStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_VR_EVENTS_DATA: + { + hasStatus = processRequestVREventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_REQUEST_MOUSE_EVENTS_DATA: + { + hasStatus = processRequestMouseEventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_REQUEST_KEYBOARD_EVENTS_DATA: + { + hasStatus = processRequestKeyboardEventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + + case CMD_REQUEST_RAY_CAST_INTERSECTIONS: + { + + hasStatus = processRequestRaycastIntersectionsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_REQUEST_DEBUG_LINES: + { + hasStatus = processRequestDebugLinesCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_CAMERA_IMAGE_DATA: + { + hasStatus = processRequestCameraImageCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_BODY_INFO: + { + hasStatus = processRequestBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SAVE_WORLD: + { + hasStatus = processSaveWorldCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_SDF: + { + hasStatus = processLoadSDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_COLLISION_SHAPE: + { + hasStatus = processCreateCollisionShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_VISUAL_SHAPE: + { + hasStatus = processCreateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_MULTI_BODY: + { + hasStatus = processCreateMultiBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SET_ADDITIONAL_SEARCH_PATH: + { + hasStatus = processSetAdditionalSearchPathCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_URDF: + { + hasStatus = processLoadURDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_SOFT_BODY: + { + hasStatus = processLoadSoftBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_SENSOR: + { + hasStatus = processCreateSensorCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_PROFILE_TIMING: + { + hasStatus = processProfileTimingCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_SEND_DESIRED_STATE: + { + hasStatus = processSendDesiredStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_COLLISION_INFO: + { + hasStatus = processRequestCollisionInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + + + case CMD_CHANGE_DYNAMICS_INFO: + { + hasStatus = processChangeDynamicsInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_GET_DYNAMICS_INFO: + { + hasStatus = processGetDynamicsInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS: + { + hasStatus = processRequestPhysicsSimulationParametersCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + + case CMD_INIT_POSE: + { + hasStatus = processInitPoseCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_CREATE_RIGID_BODY: + { + hasStatus = processCreateRigidBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_BOX_COLLISION_SHAPE: + { + //for backward compatibility, CMD_CREATE_BOX_COLLISION_SHAPE is the same as CMD_CREATE_RIGID_BODY + hasStatus = processCreateRigidBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_PICK_BODY: + { + hasStatus = processPickBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_MOVE_PICKED_BODY: + { + hasStatus = processMovePickedBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REMOVE_PICKING_CONSTRAINT_BODY: + { + hasStatus = processRemovePickingConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_AABB_OVERLAP: + { + hasStatus = processRequestAabbOverlapCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA: + { + hasStatus = processRequestOpenGLVisualizeCameraCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CONFIGURE_OPENGL_VISUALIZER: + { + hasStatus = processConfigureOpenGLVisualizerCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_CONTACT_POINT_INFORMATION: + { + hasStatus = processRequestContactpointInformationCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_INVERSE_DYNAMICS: + { + hasStatus = processInverseDynamicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_JACOBIAN: + { + hasStatus = processCalculateJacobianCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_MASS_MATRIX: + { + hasStatus = processCalculateMassMatrixCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_APPLY_EXTERNAL_FORCE: + { + hasStatus = processApplyExternalForceCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REMOVE_BODY: + { + hasStatus = processRemoveBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_USER_CONSTRAINT: + { + hasStatus = processCreateUserConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_INVERSE_KINEMATICS: + { + hasStatus = processCalculateInverseKinematicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_VISUAL_SHAPE_INFO: + { + hasStatus = processRequestVisualShapeInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_COLLISION_SHAPE_INFO: + { + hasStatus = processRequestCollisionShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_UPDATE_VISUAL_SHAPE: + { + hasStatus = processUpdateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CHANGE_TEXTURE: + { + hasStatus = processChangeTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_TEXTURE: + { + hasStatus = processLoadTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_RESTORE_STATE: + { + hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_SAVE_STATE: + { + hasStatus = processSaveStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_LOAD_BULLET: + { + hasStatus = processLoadBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SAVE_BULLET: + { + hasStatus = processSaveBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_MJCF: + { + hasStatus = processLoadMJCFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_USER_DEBUG_DRAW: + { + hasStatus = processUserDebugDrawCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CUSTOM_COMMAND: + { + hasStatus = processCustomCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_USER_DATA: + { + hasStatus = processRequestUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_ADD_USER_DATA: + { + hasStatus = processAddUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REMOVE_USER_DATA: + { + hasStatus = processRemoveUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } +#endif + + }; + + return hasStatus; +} + +bool MuJoCoPhysicsServerCommandProcessor::processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_INTERNAL_DATA"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_COMPLETED; + serverCmd.m_numDataStreamBytes = 0; + return hasStatus; +} + + + +bool MuJoCoPhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_SYNC_BODY_INFO"); + int actualNumBodies = 0; + serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0; + serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; + serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED; + return hasStatus; +} + +bool MuJoCoPhysicsServerCommandProcessor::processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_SYNC_USER_DATA"); + int numIdentifiers = 0; + serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = numIdentifiers; + serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED; + return hasStatus; +} + +bool MuJoCoPhysicsServerCommandProcessor::processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_LOAD_MJCF"); + serverStatusOut.m_type = CMD_MJCF_LOADING_FAILED; + const MjcfArgs& mjcfArgs = clientCmd.m_mjcfArguments; + if (m_data->m_verboseOutput) + { + printf("Processed CMD_LOAD_MJCF:%s", mjcfArgs.m_mjcfFileName); + } + bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (mjcfArgs.m_useMultiBody!=0) : true; + int flags = 0; + if (clientCmd.m_updateFlags&URDF_ARGS_HAS_CUSTOM_URDF_FLAGS) + { + flags |= clientCmd.m_mjcfArguments.m_flags; + } + + const char* fileName = mjcfArgs.m_mjcfFileName; + + if (strlen(fileName)>0) + { + char relativeFileName[1024]; + b3FileUtils fu; + //bool fileFound = fu.findFile(fileName, relativeFileName, 1024); + bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0); + if (!fileFound){ + printf("MJCF file not found: %s\n", fileName); + } else + { + int maxPathLen = 1024; + char pathPrefix[1024]; + fu.extractPath(relativeFileName,pathPrefix,maxPathLen); + + { + char error[1000] = "could not load binary model"; + mjModel* mnew = 0; + if( strlen(relativeFileName)>4 && !strcmp(relativeFileName+strlen(relativeFileName)-4, ".mjb") ) + { + mnew = mj_loadModel(relativeFileName, 0); + } + else + { + mnew = mj_loadXML(relativeFileName, 0, error, 1000); + if (mnew) + { + //replace old one for now + if (m_data->m_mujocoModel) + { + mj_deleteModel(m_data->m_mujocoModel); + } + if (m_data->m_mujocoData) + { + mj_deleteData(m_data->m_mujocoData); + } + m_data->m_mujocoModel = mnew; + m_data->m_mujocoData = mj_makeData(m_data->m_mujocoModel); + mj_forward(m_data->m_mujocoModel, m_data->m_mujocoData); + } + } + if( !mnew ) + { + printf("%s\n", error); + } else + { + int maxBodies = btMin(MAX_SDF_BODIES, mnew->nbody); + + serverStatusOut.m_sdfLoadedArgs.m_numBodies = maxBodies; + for (int i=0;im_mujocoModel && sdfInfoArgs.m_bodyUniqueId>=0 && sdfInfoArgs.m_bodyUniqueIdm_mujocoModel->nbody) + { + const char* name = m_data->m_mujocoModel->names+m_data->m_mujocoModel->name_bodyadr[sdfInfoArgs.m_bodyUniqueId]; + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName,name); + } + + serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; + + return hasStatus; + +} + + +bool MuJoCoPhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_STEP_FORWARD_SIMULATION"); + + if (m_data->m_mujocoModel) + { + if (m_data->m_verboseOutput) + { + b3Printf("Step simulation request"); + b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber); + } + + btScalar deltaTimeScaled = m_data->m_physicsDeltaTime; + + if (m_data->m_numSimulationSubSteps > 0) + { + for (int i=0;im_numSimulationSubSteps;i++) + { + m_data->m_mujocoModel->opt.timestep = m_data->m_physicsDeltaTime/m_data->m_numSimulationSubSteps; + mj_step(m_data->m_mujocoModel,m_data->m_mujocoData); + mj_forward(m_data->m_mujocoModel,m_data->m_mujocoData); + } + } + else + { + m_data->m_mujocoModel->opt.timestep = m_data->m_physicsDeltaTime; + mj_step(m_data->m_mujocoModel,m_data->m_mujocoData); + mj_forward(m_data->m_mujocoModel,m_data->m_mujocoData); + } + } + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; + return hasStatus; + +} + + +bool MuJoCoPhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS"); + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) + { + m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime; + } + +#if 0 + if (clientCmd.m_updateFlags & SIM_PARAM_ENABLE_CONE_FRICTION) + { + if (clientCmd.m_physSimParamArgs.m_enableConeFriction) + { + m_data->m_dynamicsWorld->getSolverInfo().m_solverMode &=~SOLVER_DISABLE_IMPLICIT_CONE_FRICTION; + } else + { + m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |=SOLVER_DISABLE_IMPLICIT_CONE_FRICTION; + } + } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS) + { + m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs = (clientCmd.m_physSimParamArgs.m_deterministicOverlappingPairs!=0); + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION) + { + m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration = clientCmd.m_physSimParamArgs.m_allowedCcdPenetration; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE) + { + gJointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode&JOINT_FEEDBACK_IN_WORLD_SPACE)!=0; + gJointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode&JOINT_FEEDBACK_IN_JOINT_FRAME)!=0; + } + + + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION) + { + m_data->m_useRealTimeSimulation = (clientCmd.m_physSimParamArgs.m_useRealTimeSimulation!=0); + } + + //see + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS) + { + //these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk + gInternalSimFlags = clientCmd.m_physSimParamArgs.m_internalSimFlags; + } + + 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); +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + m_data->m_dynamicsWorld->getWorldInfo().m_gravity=grav; + +#endif + if (m_data->m_verboseOutput) + { + b3Printf("Updated Gravity: %f,%f,%f",grav[0],grav[1],grav[2]); + } + + } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS) + { + m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD) + { + m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = clientCmd.m_physSimParamArgs.m_solverResidualThreshold; + } + + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD) + { + gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_SLOP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = clientCmd.m_physSimParamArgs.m_contactSlop; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_SAT) + { + m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex = clientCmd.m_physSimParamArgs.m_enableSAT!=0; + } + + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE) + { + m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode; + } + + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE) + { + m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse = clientCmd.m_physSimParamArgs.m_useSplitImpulse; + } + if (clientCmd.m_updateFlags &SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD) + { + m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = clientCmd.m_physSimParamArgs.m_splitImpulsePenetrationThreshold; + } + + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS) + { + m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_erp = clientCmd.m_physSimParamArgs.m_defaultNonContactERP; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionERP; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM) + { + m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm = clientCmd.m_physSimParamArgs.m_defaultGlobalCFM; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM) + { + m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM = clientCmd.m_physSimParamArgs.m_frictionCFM; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD) + { + m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold; + } + + + + if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_FILE_CACHING) + { + b3EnableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching); + } + +#endif + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + +bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; + + if (m_data->m_mujocoModel) + { + BT_PROFILE("CMD_REQUEST_ACTUAL_STATE"); + if (m_data->m_verboseOutput) + { + b3Printf("Sending the actual state (Q,U)"); + } + int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; + + if (bodyUniqueId>=0 && bodyUniqueIdm_mujocoModel->nbody) + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; + + serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; + serverCmd.m_sendActualStateArgs.m_numLinks = 0;//todo body->m_multiBody->getNumLinks(); + + int totalDegreeOfFreedomQ = 0; + int totalDegreeOfFreedomU = 0; + + if (serverCmd.m_sendActualStateArgs.m_numLinks>= MAX_DEGREE_OF_FREEDOM) + { + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; + hasStatus = true; + return hasStatus; + } + + bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS)!=0); + bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY)!=0); + + if (computeForwardKinematics || computeLinkVelocities) + { + //todo:check this + mj_forward(m_data->m_mujocoModel, m_data->m_mujocoData); + } + + //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())" + { + int rootLink = 0;//todo check + int type=(m_data->m_mujocoModel->jnt_type+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[rootLink]; + //assume mjJNT_FREE? + int qposAdr = (m_data->m_mujocoModel->jnt_qposadr+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[rootLink]; + mjtNum* pos = m_data->m_mujocoData->xipos+bodyUniqueId*3; + + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = 0; + + mjtNum* orn= m_data->m_mujocoData->xquat+bodyUniqueId*4; + mjtNum* cvel=m_data->m_mujocoData->cvel+bodyUniqueId*6; + + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = 1; + + //base position in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = pos[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = pos[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = pos[2]; + + //base orientation, quaternion x,y,z,w, in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = orn[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = orn[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = orn[2]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = orn[3]; + totalDegreeOfFreedomQ +=7;//pos + quaternion + + //base linear velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = cvel[3];//mb->getBaseVel()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = cvel[4];//mb->getBaseVel()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = cvel[5];//mb->getBaseVel()[2]; + + //base angular velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = cvel[0];//mb->getBaseOmega()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = cvel[1];//mb->getBaseOmega()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = cvel[2];//mb->getBaseOmega()[2]; + totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF + } + + //btAlignedObjectArray omega; + //btAlignedObjectArray linVel; + + + int numLinks = m_data->m_mujocoModel->body_jntnum[bodyUniqueId]; + for (int l=0;lm_mujocoModel->jnt_type+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[l]; + //int type=(m_data->m_mujocoModel->jnt_type+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[l]; + + #if 0 + mjtNum* xpos = + for (int d=0;dgetLink(l).m_posVarCount;d++) + { + serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = 0; + } + for (int d=0;dgetLink(l).m_dofCount;d++) + { + serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = 0; + } + + if (0 == mb->getLink(l).m_jointFeedback) + { + for (int d=0;d<6;d++) + { + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+d]=0; + } + } else + { + + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = 0; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = 0; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = 0; + + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = 0; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = 0; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = 0; + } + + serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0; + #if 0 + if (supportsJointMotor(mb,l)) + { + if (motor && m_data->m_physicsDeltaTime>btScalar(0)) + { + serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0; + } + } + #endif + //btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin(); + //btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation(); + + //btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin(); + //btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation(); + + serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = 0;//linkCOMOrigin.getX(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = 0;//linkCOMOrigin.getY(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = 0;//linkCOMOrigin.getZ(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = 0;//linkCOMRotation.x(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = 0;//linkCOMRotation.y(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = 0;//linkCOMRotation.z(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = 1;//linkCOMRotation.w(); + + + #if 0 + btVector3 worldLinVel(0,0,0); + btVector3 worldAngVel(0,0,0); + + if (computeLinkVelocities) + { + const btMatrix3x3& linkRotMat = mb->getLink(l).m_cachedWorldTransform.getBasis(); + worldLinVel = linkRotMat * linVel[l+1]; + worldAngVel = linkRotMat * omega[l+1]; + } + #endif + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+0] = 0;//worldLinVel[0]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+1] = 0;//worldLinVel[1]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+2] = 0;//worldLinVel[2]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+3] = 0;//worldAngVel[0]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+4] = 0;//worldAngVel[1]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+5] = 0;//worldAngVel[2]; + + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = 0;//linkLocalInertialOrigin.getX(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = 0;//linkLocalInertialOrigin.getY(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+2] = 0;//linkLocalInertialOrigin.getZ(); + + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = 0;//linkLocalInertialRotation.x(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = 0;//linkLocalInertialRotation.y(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = 0;//linkLocalInertialRotation.z(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = 1;//linkLocalInertialRotation.w(); + #endif + } + + + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; + + hasStatus = true; + + } + } + return hasStatus; +} + + +bool MuJoCoPhysicsServerCommandProcessor::processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_RESET_SIMULATION"); + + resetSimulation(); + + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED; + return hasStatus; +} + + + +bool MuJoCoPhysicsServerCommandProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + return false; +} + +#endif //BT_ENABLE_MUJOCO \ No newline at end of file diff --git a/examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.h b/examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.h new file mode 100644 index 000000000..f4a24d0d1 --- /dev/null +++ b/examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.h @@ -0,0 +1,45 @@ +#ifndef MUJOCO_PHYSICS_SERVER_COMMAND_PROCESSOR_H +#define MUJOCO_PHYSICS_SERVER_COMMAND_PROCESSOR_H + +#include "../PhysicsCommandProcessorInterface.h" + +class MuJoCoPhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface +{ + + struct MuJoCoPhysicsServerCommandProcessorInternalData* m_data; + + bool processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + + void resetSimulation(); + +public: + MuJoCoPhysicsServerCommandProcessor(); + + virtual ~MuJoCoPhysicsServerCommandProcessor(); + + virtual bool connect(); + + virtual void disconnect(); + + virtual bool isConnected() const; + + virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + + virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + + virtual void renderScene(int renderFlags){} + virtual void physicsDebugDraw(int debugDrawFlags){} + virtual void setGuiHelper(struct GUIHelperInterface* guiHelper){} + virtual void setTimeOut(double timeOutInSeconds){} + +}; + +#endif //MUJOCO_PHYSICS_COMMAND_PROCESSOR_H diff --git a/examples/pybullet/examples/biped2d_pybullet.py b/examples/pybullet/examples/biped2d_pybullet.py new file mode 100644 index 000000000..01d3aaec9 --- /dev/null +++ b/examples/pybullet/examples/biped2d_pybullet.py @@ -0,0 +1,40 @@ +import pybullet as p +import pybullet_data +import os +import time +GRAVITY = -9.8 +dt = 1e-3 +iters=2000 + +physicsClient = p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) +p.resetSimulation() +#p.setRealTimeSimulation(True) +p.setGravity(0,0,GRAVITY) +p.setTimeStep(dt) +planeId = p.loadURDF("plane.urdf") +cubeStartPos = [0,0,1.13] +cubeStartOrientation = p.getQuaternionFromEuler([0.,0,0]) +botId = p.loadURDF("biped/biped2d_pybullet.urdf", + cubeStartPos, + cubeStartOrientation) + +#disable the default velocity motors +#and set some position control with small force to emulate joint friction/return to a rest pose +jointFrictionForce=1 +for joint in range (p.getNumJoints(botId)): + p.setJointMotorControl2(botId,joint,p.POSITION_CONTROL,force=jointFrictionForce) + +#for i in range(10000): +# p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0) +# p.stepSimulation() +#import ipdb +#ipdb.set_trace() +import time +p.setRealTimeSimulation(1) +while (1): + #p.stepSimulation() + #p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0) + p.setGravity(0,0,GRAVITY) + time.sleep(1/240.) +time.sleep(1000) diff --git a/examples/pybullet/gym/pybullet_data/biped/biped2d_pybullet.urdf b/examples/pybullet/gym/pybullet_data/biped/biped2d_pybullet.urdf new file mode 100644 index 000000000..e440a65cd --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/biped/biped2d_pybullet.urdf @@ -0,0 +1,273 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b7c704305..2bceef77b 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -5,6 +5,14 @@ #include "../SharedMemory/PhysicsClientUDP_C_API.h" #endif //BT_ENABLE_ENET +#ifdef BT_ENABLE_DART +#include "../SharedMemory/dart/DARTPhysicsC_API.h" +#endif + +#ifdef BT_ENABLE_MUJOCO +#include "../SharedMemory/mujoco/MuJoCoPhysicsC_API.h" +#endif + #ifdef BT_ENABLE_CLSOCKET #include "../SharedMemory/PhysicsClientTCP_C_API.h" #endif //BT_ENABLE_CLSOCKET @@ -407,6 +415,22 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P sm = b3ConnectPhysicsDirect(); break; } +#ifdef BT_ENABLE_DART + case eCONNECT_DART: + { + sm = b3ConnectPhysicsDART(); + break; + } +#endif + +#ifdef BT_ENABLE_MUJOCO + case eCONNECT_MUJOCO: + { + sm = b3ConnectPhysicsMuJoCo(); + break; + } +#endif + case eCONNECT_SHARED_MEMORY: { sm = b3ConnectSharedMemory(key); @@ -9549,6 +9573,14 @@ initpybullet(void) PyModule_AddIntConstant(m, "GUI_SERVER", eCONNECT_GUI_SERVER); // user read PyModule_AddIntConstant(m, "GUI_MAIN_THREAD", eCONNECT_GUI_MAIN_THREAD); // user read PyModule_AddIntConstant(m, "SHARED_MEMORY_SERVER", eCONNECT_SHARED_MEMORY_SERVER); // user read +#ifdef BT_ENABLE_DART + PyModule_AddIntConstant(m, "DART", eCONNECT_DART); // user read +#endif + +#ifdef BT_ENABLE_MUJOCO + PyModule_AddIntConstant(m, "MuJoCo", eCONNECT_MUJOCO); // user read +#endif + PyModule_AddIntConstant(m, "SHARED_MEMORY_KEY", SHARED_MEMORY_KEY); PyModule_AddIntConstant(m, "SHARED_MEMORY_KEY2", SHARED_MEMORY_KEY+1); From 23f235e44929ccdfa8284c0f63cdfa10e20eac2b Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sun, 1 Jul 2018 14:49:34 -0700 Subject: [PATCH 3/9] PyBullet: add example to enable another physics engine backend (DART, MuJoCo) --- .../pybullet/examples/otherPhysicsEngine.py | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 examples/pybullet/examples/otherPhysicsEngine.py diff --git a/examples/pybullet/examples/otherPhysicsEngine.py b/examples/pybullet/examples/otherPhysicsEngine.py new file mode 100644 index 000000000..2786b1c66 --- /dev/null +++ b/examples/pybullet/examples/otherPhysicsEngine.py @@ -0,0 +1,32 @@ +import pybullet as p +import time + +#p.connect(p.DIRECT) +#p.connect(p.DART) +p.connect(p.MuJoCo) + +#p.connect(p.GUI) +bodies = p.loadMJCF("mjcf/capsule.xml") +print("bodies=",bodies) + +numBodies = p.getNumBodies() +print("numBodies=",numBodies) +for i in range (numBodies): + print("bodyInfo[",i,"]=",p.getBodyInfo(i)) + +p.setGravity(0,0,-10) +timeStep = 1./240. +p.setPhysicsEngineParameter(fixedTimeStep=timeStep) + +#while (p.isConnected()): +for i in range (1000): + p.stepSimulation() + + for b in bodies: + pos,orn=p.getBasePositionAndOrientation(b) + print("pos[",b,"]=",pos) + print("orn[",b,"]=",orn) + linvel,angvel=p.getBaseVelocity(b) + print("linvel[",b,"]=",linvel) + print("angvel[",b,"]=",angvel) + time.sleep(timeStep) From 9c7aa3a863447c3dd323515266576ccb05ae963f Mon Sep 17 00:00:00 2001 From: Tigran Gasparian Date: Tue, 3 Jul 2018 17:45:19 +0200 Subject: [PATCH 4/9] Changes UserData to use global identifiers and makes linkIndex optional. This removes the need to specify the body id/link index when retrieving a user data entry. Additionally, user data can now optionally be set to visual shapes as well. The following public pybullet APIs have changed (backwards incompatible) addUserData and getUserDataId Makes linkIndex parameter optional (default value is -1) Adds optional visualShapeIndex parameter (default value is -1) getUserData and removeUserData Removes required parameters bodyUniqueId and linkIndex getNumUserData Removes required bodyUniqueId parameter getUserDataInfo Removes required linkIndex parameter Changes returned tuple from (userDataId, key) to (userDataId, key, bodyUniqueId, linkIndex, visualShapeIndex) --- examples/SharedMemory/PhysicsClient.h | 8 +- examples/SharedMemory/PhysicsClientC_API.cpp | 25 +- examples/SharedMemory/PhysicsClientC_API.h | 12 +- .../PhysicsClientSharedMemory.cpp | 155 +++++------- .../SharedMemory/PhysicsClientSharedMemory.h | 8 +- examples/SharedMemory/PhysicsDirect.cpp | 141 ++++------- examples/SharedMemory/PhysicsDirect.h | 8 +- examples/SharedMemory/PhysicsLoopBack.cpp | 18 +- examples/SharedMemory/PhysicsLoopBack.h | 8 +- .../PhysicsServerCommandProcessor.cpp | 197 +++++---------- examples/SharedMemory/SharedMemoryCommands.h | 18 +- examples/SharedMemory/SharedMemoryPublic.h | 8 +- examples/SharedMemory/SharedMemoryUserData.h | 53 +++- examples/pybullet/examples/userData.py | 66 ++--- examples/pybullet/pybullet.c | 73 +++--- examples/pybullet/unittests/userDataTest.py | 235 ++++++++++-------- 16 files changed, 478 insertions(+), 555 deletions(-) diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 6dd537fd6..2b9357155 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -75,10 +75,10 @@ public: virtual void setTimeOut(double timeOutInSeconds) = 0; virtual double getTimeOut() const = 0; - virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const = 0; - virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const = 0; - virtual int getNumUserData(int bodyUniqueId, int linkIndex) const = 0; - virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const = 0; + virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const = 0; + virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const = 0; + virtual int getNumUserData(int bodyUniqueId) const = 0; + virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const = 0; virtual void pushProfileTiming(const char* timingName)=0; virtual void popProfileTiming()=0; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index e5abaa26a..d88e15d42 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2848,7 +2848,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsCl return (b3SharedMemoryCommandHandle) command; } -B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char* key, UserDataValueType valueType, int valueLength, const void *valueData) { +B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, UserDataValueType valueType, int valueLength, const void *valueData) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(strlen(key) < MAX_USER_DATA_KEY_LENGTH); b3Assert(cl); @@ -2860,6 +2860,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsCli command->m_type = CMD_ADD_USER_DATA; command->m_addUserDataRequestArgs.m_bodyUniqueId = bodyUniqueId; command->m_addUserDataRequestArgs.m_linkIndex = linkIndex; + command->m_addUserDataRequestArgs.m_visualShapeIndex = visualShapeIndex; command->m_addUserDataRequestArgs.m_valueType = valueType; command->m_addUserDataRequestArgs.m_valueLength = valueLength; strcpy(command->m_addUserDataRequestArgs.m_key, key); @@ -2868,7 +2869,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsCli return (b3SharedMemoryCommandHandle) command; } -B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId) { +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int userDataId) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); @@ -2876,29 +2877,27 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3Physics b3Assert(command); command->m_type = CMD_REMOVE_USER_DATA; - command->m_removeUserDataRequestArgs.m_bodyUniqueId = bodyUniqueId; - command->m_removeUserDataRequestArgs.m_linkIndex = linkIndex; command->m_removeUserDataRequestArgs.m_userDataId = userDataId; return (b3SharedMemoryCommandHandle) command; } -B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue *valueOut) +B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int userDataId, struct b3UserDataValue *valueOut) { PhysicsClient* cl = (PhysicsClient*)physClient; if (cl) { - return cl->getCachedUserData(bodyUniqueId, linkIndex, userDataId, *valueOut); + return cl->getCachedUserData(userDataId, *valueOut); } return false; } -B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char *key) +B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) { PhysicsClient* cl = (PhysicsClient*)physClient; if (cl) { - return cl->getCachedUserDataId(bodyUniqueId, linkIndex, key); + return cl->getCachedUserDataId(bodyUniqueId, linkIndex, visualShapeIndex, key); } return -1; } @@ -2909,27 +2908,27 @@ B3_SHARED_API int b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHan if (status) { btAssert(status->m_type == CMD_ADD_USER_DATA_COMPLETED); - return status->m_userDataResponseArgs.m_userDataGlobalId.m_userDataId; + return status->m_userDataResponseArgs.m_userDataId; } return -1; } -B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex) +B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId) { PhysicsClient* cl = (PhysicsClient*)physClient; if (cl) { - return cl->getNumUserData(bodyUniqueId, linkIndex); + return cl->getNumUserData(bodyUniqueId); } return 0; } -B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) +B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) { PhysicsClient* cl = (PhysicsClient*)physClient; if (cl) { - cl->getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, keyOut, userDataIdOut); + cl->getUserDataInfo(bodyUniqueId, userDataIndex, keyOut, userDataIdOut, linkIndexOut, visualShapeIndexOut); } } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index e57aaa510..2870f7f1e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -115,14 +115,14 @@ B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqu ///user data handling B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsClientHandle physClient); -B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char* key, enum UserDataValueType valueType, int valueLength, const void *valueData); -B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, enum UserDataValueType valueType, int valueLength, const void *valueData); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int userDataId); -B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue *valueOut); -B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char *key); +B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int userDataId, struct b3UserDataValue *valueOut); +B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key); B3_SHARED_API int b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHandle); -B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex); -B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut); +B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId); +B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut); B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex); ///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 1f3d24c35..f787fec05 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -15,26 +15,12 @@ #include "LinearMath/btQuickprof.h" -struct UserDataCache -{ - btHashMap m_userDataMap; - btHashMap m_keyToUserDataIdMap; - - UserDataCache() - { - } - ~UserDataCache() - { - } -}; - struct BodyJointInfoCache { std::string m_baseName; b3AlignedObjectArray m_jointInfo; std::string m_bodyName; - // Joint index -> user data. - btHashMap m_jointToUserDataMap; + btAlignedObjectArray m_userDataIds; ~BodyJointInfoCache() { @@ -78,7 +64,10 @@ struct PhysicsClientSharedMemoryInternalData { btAlignedObjectArray m_bodyIdsRequestInfo; btAlignedObjectArray m_constraintIdsRequestInfo; - btAlignedObjectArray m_userDataIdsRequestInfo; + btAlignedObjectArray m_userDataIdsRequestInfo; + btHashMap m_userDataMap; + btHashMap m_userDataHandleLookup; + SharedMemoryStatus m_tempBackupServerStatus; @@ -243,6 +232,12 @@ void PhysicsClientSharedMemory::removeCachedBody(int bodyUniqueId) BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; if (bodyJointsPtr && *bodyJointsPtr) { + for(int i=0; i<(*bodyJointsPtr)->m_userDataIds.size(); i++) { + const int userDataId = (*bodyJointsPtr)->m_userDataIds[i]; + SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId]; + m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData)); + m_data->m_userDataMap.remove(userDataId); + } delete (*bodyJointsPtr); m_data->m_bodyJointMap.remove(bodyUniqueId); } @@ -264,6 +259,8 @@ void PhysicsClientSharedMemory::resetData() } m_data->m_bodyJointMap.clear(); m_data->m_userConstraintInfoMap.clear(); + m_data->m_userDataHandleLookup.clear(); + m_data->m_userDataMap.clear(); } void PhysicsClientSharedMemory::setSharedMemoryKey(int key) @@ -1409,25 +1406,26 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); if (bodyJointsPtr && *bodyJointsPtr) { - (*bodyJointsPtr)->m_jointToUserDataMap.clear(); + (*bodyJointsPtr)->m_userDataIds.clear(); } + m_data->m_userDataMap.clear(); + m_data->m_userDataHandleLookup.clear(); } const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers; if (numIdentifiers > 0) { m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus; - const b3UserDataGlobalIdentifier *identifiers = (b3UserDataGlobalIdentifier *)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; - for (int i=0; im_testBlock1->m_bulletStreamDataServerToClientRefactor; + m_data->m_userDataIdsRequestInfo.reserve(numIdentifiers - 1); + // Store the identifiers that still need to be requested. + for (int i=0; im_userDataIdsRequestInfo.push_back(identifiers[i]); } - // Request individual user data entries. - const b3UserDataGlobalIdentifier userDataGlobalId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size()-1]; - m_data->m_userDataIdsRequestInfo.pop_back(); - + // Request individual user data entries, start with last identifier. SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; command.m_type = CMD_REQUEST_USER_DATA; - command.m_userDataRequestArgs = userDataGlobalId; + command.m_userDataRequestArgs.m_userDataId = identifiers[numIdentifiers - 1]; submitClientCommand(command); return 0; } @@ -1435,31 +1433,23 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { if (serverCmd.m_type == CMD_ADD_USER_DATA_COMPLETED || serverCmd.m_type == CMD_REQUEST_USER_DATA_COMPLETED) { B3_PROFILE("CMD_ADD_USER_DATA_COMPLETED"); - const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_userDataResponseArgs.m_userDataGlobalId; - BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; + const UserDataResponseArgs response = serverCmd.m_userDataResponseArgs; + BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[response.m_bodyUniqueId]; if (bodyJointsPtr && *bodyJointsPtr) { - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; - if (!userDataCachePtr) - { - UserDataCache cache; - (*bodyJointsPtr)->m_jointToUserDataMap.insert(userDataGlobalId.m_linkIndex, cache); - } - userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; - const char *dataStream = m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; - - SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; - if (userDataPtr) { + SharedMemoryUserData* userData = m_data->m_userDataMap[response.m_userDataId]; + if (userData) { // Only replace the value. - userDataPtr->replaceValue(dataStream, serverCmd.m_userDataResponseArgs.m_valueLength, serverCmd.m_userDataResponseArgs.m_valueType); + userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType); } else { // Add a new user data entry. - const char *key = serverCmd.m_userDataResponseArgs.m_key; - (userDataCachePtr)->m_userDataMap.insert(userDataGlobalId.m_userDataId, SharedMemoryUserData(key)); - (userDataCachePtr)->m_keyToUserDataIdMap.insert(key, userDataGlobalId.m_userDataId); - userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; - userDataPtr->replaceValue(dataStream, serverCmd.m_userDataResponseArgs.m_valueLength, serverCmd.m_userDataResponseArgs.m_valueType); + const char *key = response.m_key; + m_data->m_userDataMap.insert(response.m_userDataId, SharedMemoryUserData(key, response.m_bodyUniqueId, response.m_linkIndex, response.m_visualShapeIndex)); + userData = m_data->m_userDataMap[response.m_userDataId]; + userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType); + m_data->m_userDataHandleLookup.insert(SharedMemoryUserDataHashKey(userData), response.m_userDataId); + (*bodyJointsPtr)->m_userDataIds.push_back(response.m_userDataId); } } } @@ -1468,12 +1458,12 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { if (m_data->m_userDataIdsRequestInfo.size() > 0) { // Request individual user data entries. - const b3UserDataGlobalIdentifier userDataGlobalId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size()-1]; + const int userDataId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size()-1]; m_data->m_userDataIdsRequestInfo.pop_back(); SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; command.m_type = CMD_REQUEST_USER_DATA; - command.m_userDataRequestArgs = userDataGlobalId; + command.m_userDataRequestArgs.m_userDataId = userDataId; submitClientCommand(command); return 0; } @@ -1482,19 +1472,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { if (serverCmd.m_type == CMD_REMOVE_USER_DATA_COMPLETED) { B3_PROFILE("CMD_REMOVE_USER_DATA_COMPLETED"); - const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_removeUserDataResponseArgs; - BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; - if (bodyJointsPtr && *bodyJointsPtr) { - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; - if (userDataCachePtr) - { - SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; - if (userDataPtr) - { - (userDataCachePtr)->m_keyToUserDataIdMap.remove((userDataPtr)->m_key.c_str()); - (userDataCachePtr)->m_userDataMap.remove(userDataGlobalId.m_userDataId); - } + const int userDataId = serverCmd.m_removeUserDataResponseArgs.m_userDataId; + SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId]; + if (userData) { + BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userData->m_bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) { + (*bodyJointsPtr)->m_userDataIds.remove(userDataId); } + m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData)); + m_data->m_userDataMap.remove(userDataId); } } @@ -1852,16 +1838,8 @@ double PhysicsClientSharedMemory::getTimeOut() const return m_data->m_timeOutInSeconds; } -bool PhysicsClientSharedMemory::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const { - BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; - if (!bodyJointsPtr || !(*bodyJointsPtr)) { - return false; - } - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; - if (!userDataCachePtr) { - return false; - } - SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap[userDataId]; +bool PhysicsClientSharedMemory::getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const { + SharedMemoryUserData *userDataPtr = m_data->m_userDataMap[userDataId]; if (!userDataPtr) { return false; @@ -1872,54 +1850,37 @@ bool PhysicsClientSharedMemory::getCachedUserData(int bodyUniqueId, int linkInde return true; } -int PhysicsClientSharedMemory::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const { - BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; - if (!bodyJointsPtr || !(*bodyJointsPtr)) { - return -1; - } - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; - if (!userDataCachePtr) - { - return -1; - } - int *userDataId = (userDataCachePtr)->m_keyToUserDataIdMap[key]; +int PhysicsClientSharedMemory::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const { + int* userDataId = m_data->m_userDataHandleLookup.find(SharedMemoryUserDataHashKey(key, bodyUniqueId, linkIndex, visualShapeIndex)); if (!userDataId) { return -1; } return *userDataId; } -int PhysicsClientSharedMemory::getNumUserData(int bodyUniqueId, int linkIndex) const { +int PhysicsClientSharedMemory::getNumUserData(int bodyUniqueId) const { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; if (!bodyJointsPtr || !(*bodyJointsPtr)) { return 0; } - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; - if (!userDataCachePtr) - { - return 0; - } - return (userDataCachePtr)->m_userDataMap.size(); + return (*bodyJointsPtr)->m_userDataIds.size(); } -void PhysicsClientSharedMemory::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const { +void PhysicsClientSharedMemory::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; - if (!bodyJointsPtr || !(*bodyJointsPtr)) + if (!bodyJointsPtr || !(*bodyJointsPtr) || userDataIndex < 0 || userDataIndex > (*bodyJointsPtr)->m_userDataIds.size()) { *keyOut = 0; *userDataIdOut = -1; return; } - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; - if (!userDataCachePtr || userDataIndex >= (userDataCachePtr)->m_userDataMap.size()) - { - *keyOut = 0; - *userDataIdOut = -1; - return; - } - *userDataIdOut = (userDataCachePtr)->m_userDataMap.getKeyAtIndex(userDataIndex).getUid1(); - SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex); - *keyOut = (userDataPtr)->m_key.c_str(); + int userDataId = (*bodyJointsPtr)->m_userDataIds[userDataIndex]; + SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId]; + + *userDataIdOut = userDataId; + *keyOut = userData->m_key.c_str(); + *linkIndexOut = userData->m_linkIndex; + *visualShapeIndexOut = userData->m_visualShapeIndex; } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index 10c1c63fb..37d30a55e 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -85,10 +85,10 @@ public: virtual void setTimeOut(double timeOutInSeconds); virtual double getTimeOut() const; - virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const; - virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const; - virtual int getNumUserData(int bodyUniqueId, int linkIndex) const; - virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const; + virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const; + virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const; + virtual int getNumUserData(int bodyUniqueId) const; + virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const; virtual void pushProfileTiming(const char* timingName); virtual void popProfileTiming(); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 45f85d75c..f1fb24548 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -16,24 +16,13 @@ #include "SharedMemoryUserData.h" #include "LinearMath/btQuickprof.h" -struct UserDataCache { - btHashMap m_userDataMap; - btHashMap m_keyToUserDataIdMap; - - ~UserDataCache() - { - - } -}; struct BodyJointInfoCache2 { std::string m_baseName; btAlignedObjectArray m_jointInfo; std::string m_bodyName; - - // Joint index -> user data. - btHashMap m_jointToUserDataMap; + btAlignedObjectArray m_userDataIds; ~BodyJointInfoCache2() { } @@ -85,6 +74,9 @@ struct PhysicsDirectInternalData btAlignedObjectArray m_raycastHits; + btHashMap m_userDataMap; + btHashMap m_userDataHandleLookup; + PhysicsCommandProcessorInterface* m_commandProcessor; bool m_ownsCommandProcessor; double m_timeOutInSeconds; @@ -685,34 +677,23 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta } void PhysicsDirect::processAddUserData(const struct SharedMemoryStatus& serverCmd) { - const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_userDataResponseArgs.m_userDataGlobalId; - BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; - if (bodyJointsPtr && *bodyJointsPtr) - { - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; - if (!userDataCachePtr) - { - UserDataCache cache; - (*bodyJointsPtr)->m_jointToUserDataMap.insert(userDataGlobalId.m_linkIndex, cache); - } - userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; - + const UserDataResponseArgs response = serverCmd.m_userDataResponseArgs; + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[response.m_bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) { const char *dataStream = m_data->m_bulletStreamDataServerToClient; - - b3UserDataValue userDataValue; - userDataValue.m_type = serverCmd.m_userDataResponseArgs.m_valueType; - userDataValue.m_length = serverCmd.m_userDataResponseArgs.m_valueLength; - SharedMemoryUserData *userDataPtr = userDataCachePtr->m_userDataMap[userDataGlobalId.m_userDataId]; - if (userDataPtr) { + SharedMemoryUserData* userData = m_data->m_userDataMap[response.m_userDataId]; + if (userData) { // Only replace the value. - (userDataPtr)->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type); + userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType); } else { // Add a new user data entry. - (userDataCachePtr)->m_userDataMap.insert(userDataGlobalId.m_userDataId, SharedMemoryUserData(serverCmd.m_userDataResponseArgs.m_key)); - userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; - userDataPtr->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type); - (userDataCachePtr)->m_keyToUserDataIdMap.insert(serverCmd.m_userDataResponseArgs.m_key, userDataGlobalId.m_userDataId); + const char *key = response.m_key; + m_data->m_userDataMap.insert(response.m_userDataId, SharedMemoryUserData(key, response.m_bodyUniqueId, response.m_linkIndex, response.m_visualShapeIndex)); + userData = m_data->m_userDataMap[response.m_userDataId]; + userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType); + m_data->m_userDataHandleLookup.insert(SharedMemoryUserDataHashKey(userData), response.m_userDataId); + (*bodyJointsPtr)->m_userDataIds.push_back(response.m_userDataId); } } } @@ -1152,16 +1133,18 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); if (bodyJointsPtr && *bodyJointsPtr) { - (*bodyJointsPtr)->m_jointToUserDataMap.clear(); + (*bodyJointsPtr)->m_userDataIds.clear(); } + m_data->m_userDataMap.clear(); + m_data->m_userDataHandleLookup.clear(); } const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers; - b3UserDataGlobalIdentifier *identifiers = new b3UserDataGlobalIdentifier[numIdentifiers]; - memcpy(identifiers, &m_data->m_bulletStreamDataServerToClient[0], numIdentifiers * sizeof(b3UserDataGlobalIdentifier)); + int *identifiers = new int[numIdentifiers]; + memcpy(identifiers, &m_data->m_bulletStreamDataServerToClient[0], numIdentifiers * sizeof(int)); for (int i=0; im_tmpInfoRequestCommand.m_type = CMD_REQUEST_USER_DATA; - m_data->m_tmpInfoRequestCommand.m_userDataRequestArgs = identifiers[i]; + m_data->m_tmpInfoRequestCommand.m_userDataRequestArgs.m_userDataId = identifiers[i]; bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); @@ -1184,18 +1167,15 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd } case CMD_REMOVE_USER_DATA_COMPLETED: { - const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_removeUserDataResponseArgs; - BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; - if (bodyJointsPtr && *bodyJointsPtr) { - UserDataCache *userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; - if (userDataCachePtr) - { - SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; - if (userDataPtr) { - (userDataCachePtr)->m_keyToUserDataIdMap.remove((userDataPtr)->m_key.c_str()); - (userDataCachePtr)->m_userDataMap.remove(userDataGlobalId.m_userDataId); - } + const int userDataId = serverCmd.m_removeUserDataResponseArgs.m_userDataId; + SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId]; + if (userData) { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userData->m_bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) { + (*bodyJointsPtr)->m_userDataIds.remove(userDataId); } + m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData)); + m_data->m_userDataMap.remove(userDataId); } break; } @@ -1253,6 +1233,12 @@ void PhysicsDirect::removeCachedBody(int bodyUniqueId) BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; if (bodyJointsPtr && *bodyJointsPtr) { + for(int i=0; i<(*bodyJointsPtr)->m_userDataIds.size(); i++) { + const int userDataId = (*bodyJointsPtr)->m_userDataIds[i]; + SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId]; + m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData)); + m_data->m_userDataMap.remove(userDataId); + } delete (*bodyJointsPtr); m_data->m_bodyJointMap.remove(bodyUniqueId); } @@ -1499,72 +1485,49 @@ double PhysicsDirect::getTimeOut() const return m_data->m_timeOutInSeconds; } -bool PhysicsDirect::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const { - BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; - if (!bodyJointsPtr || !(*bodyJointsPtr)) { - return false; - } - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; - if (!userDataCachePtr) - { - return false; - } - SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataId]; +bool PhysicsDirect::getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const { + SharedMemoryUserData *userDataPtr = m_data->m_userDataMap[userDataId]; if (!userDataPtr) { return false; } - valueOut.m_type = userDataPtr->m_type; + valueOut.m_type = (userDataPtr)->m_type; valueOut.m_length = userDataPtr->m_bytes.size(); valueOut.m_data1 = userDataPtr->m_bytes.size()? &userDataPtr->m_bytes[0] : 0; return true; } -int PhysicsDirect::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const { - BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; - if (!bodyJointsPtr || !(*bodyJointsPtr)) { - return -1; - } - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; - if (!userDataCachePtr) { - return -1; - } - int *userDataId = (userDataCachePtr)->m_keyToUserDataIdMap[key]; +int PhysicsDirect::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const { + int* userDataId = m_data->m_userDataHandleLookup.find(SharedMemoryUserDataHashKey(key, bodyUniqueId, linkIndex, visualShapeIndex)); if (!userDataId) { return -1; } return *userDataId; } -int PhysicsDirect::getNumUserData(int bodyUniqueId, int linkIndex) const { +int PhysicsDirect::getNumUserData(int bodyUniqueId) const { BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; if (!bodyJointsPtr || !(*bodyJointsPtr)) { return 0; } - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; - if (!userDataCachePtr) { - return 0; - } - return (userDataCachePtr)->m_userDataMap.size(); + return (*bodyJointsPtr)->m_userDataIds.size(); } -void PhysicsDirect::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const { +void PhysicsDirect::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const { BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; - if (!bodyJointsPtr || !(*bodyJointsPtr)) { - *keyOut = 0; - *userDataIdOut = -1; - return; - } - UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; - if (!userDataCachePtr || userDataIndex >= (userDataCachePtr)->m_userDataMap.size()) + if (!bodyJointsPtr || !(*bodyJointsPtr) || userDataIndex <= 0 || userDataIndex > (*bodyJointsPtr)->m_userDataIds.size()) { *keyOut = 0; *userDataIdOut = -1; return; } - *userDataIdOut = (userDataCachePtr)->m_userDataMap.getKeyAtIndex(userDataIndex).getUid1(); - SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex); - *keyOut = (userDataPtr)->m_key.c_str(); + int userDataId = (*bodyJointsPtr)->m_userDataIds[userDataIndex]; + SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId]; + + *userDataIdOut = userDataId; + *keyOut = userData->m_key.c_str(); + *linkIndexOut = userData->m_linkIndex; + *visualShapeIndexOut = userData->m_visualShapeIndex; } diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 2b4a9d634..f0c75e750 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -115,10 +115,10 @@ public: virtual void setTimeOut(double timeOutInSeconds); virtual double getTimeOut() const; - virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const; - virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const; - virtual int getNumUserData(int bodyUniqueId, int linkIndex) const; - virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const; + virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const; + virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const; + virtual int getNumUserData(int bodyUniqueId) const; + virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const; virtual void pushProfileTiming(const char* timingName); virtual void popProfileTiming(); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 4a4b00d70..f615f0903 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -231,20 +231,20 @@ double PhysicsLoopBack::getTimeOut() const return m_data->m_physicsClient->getTimeOut(); } -bool PhysicsLoopBack::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const { - return m_data->m_physicsClient->getCachedUserData(bodyUniqueId, linkIndex, userDataId, valueOut); +bool PhysicsLoopBack::getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const { + return m_data->m_physicsClient->getCachedUserData(userDataId, valueOut); } -int PhysicsLoopBack::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const { - return m_data->m_physicsClient->getCachedUserDataId(bodyUniqueId, linkIndex, key); +int PhysicsLoopBack::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const { + return m_data->m_physicsClient->getCachedUserDataId(bodyUniqueId, linkIndex, visualShapeIndex, key); } -int PhysicsLoopBack::getNumUserData(int bodyUniqueId, int linkIndex) const { - return m_data->m_physicsClient->getNumUserData(bodyUniqueId, linkIndex); +int PhysicsLoopBack::getNumUserData(int bodyUniqueId) const { + return m_data->m_physicsClient->getNumUserData(bodyUniqueId); } -void PhysicsLoopBack::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const { - m_data->m_physicsClient->getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, keyOut, userDataIdOut); +void PhysicsLoopBack::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const { + m_data->m_physicsClient->getUserDataInfo(bodyUniqueId, userDataIndex, keyOut, userDataIdOut, linkIndexOut, visualShapeIndexOut); } void PhysicsLoopBack::pushProfileTiming(const char* timingName) @@ -254,4 +254,4 @@ void PhysicsLoopBack::pushProfileTiming(const char* timingName) void PhysicsLoopBack::popProfileTiming() { m_data->m_physicsClient->popProfileTiming(); -} \ No newline at end of file +} diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index e18fa22f8..d5449452e 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -89,10 +89,10 @@ public: virtual void setTimeOut(double timeOutInSeconds); virtual double getTimeOut() const; - virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const; - virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const; - virtual int getNumUserData(int bodyUniqueId, int linkIndex) const; - virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const; + virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const; + virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const; + virtual int getNumUserData(int bodyUniqueId) const; + virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const; virtual void pushProfileTiming(const char* timingName); virtual void popProfileTiming(); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d3c515834..502d757f5 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -222,72 +222,6 @@ struct InternalCollisionShapeData #include "SharedMemoryUserData.h" -/** - * Holds all custom user data entries for a link. - */ -struct InternalLinkUserData { - // Used to look up user data entry handles for string keys. - btHashMap m_keyToHandleMap; - b3ResizablePool > m_userData; - - // Adds or replaces a user data entry. - // Returns the user data handle. - const int add(const char* key, const char* bytes, int len, int type) - { - // If an entry with the key already exists, just update the value. - int userDataId = getUserDataId(key); - if (userDataId != -1) { - SharedMemoryUserData* userData = m_userData.getHandle(userDataId); - b3Assert(userData); - userData->replaceValue(bytes, len, type); - return userDataId; - } - - userDataId = m_userData.allocHandle(); - SharedMemoryUserData* userData = m_userData.getHandle(userDataId); - userData->m_key = key; - userData->replaceValue(bytes, len, type); - m_keyToHandleMap.insert(userData->m_key.c_str(), userDataId); - return userDataId; - } - - // Returns the user data handle for a specified key or -1 if not found. - const int getUserDataId(const char* key) const - { - const int* userDataIdPtr = m_keyToHandleMap.find(key); - if (userDataIdPtr) - { - return *userDataIdPtr; - } - return -1; - } - - // Removes a user data entry given the handle. - // Returns true when the entry was removed, false otherwise. - const bool remove(int userDataId) - { - const SharedMemoryUserData* userData = m_userData.getHandle(userDataId); - if (!userData) - { - return false; - } - m_keyToHandleMap.remove(userData->m_key.c_str()); - m_userData.freeHandle(userDataId); - return true; - } - - // Returns the user data given the user data id. null otherwise. - const SharedMemoryUserData* getUserData(const int userDataId) const - { - return m_userData.getHandle(userDataId); - } - - void getUserDataIds(b3AlignedObjectArray &userDataIds) const - { - m_userData.getUsedHandles(userDataIds); - } -}; - struct InternalBodyData { btMultiBody* m_multiBody; @@ -303,7 +237,7 @@ struct InternalBodyData btAlignedObjectArray m_rigidBodyJoints; btAlignedObjectArray m_rigidBodyJointNames; btAlignedObjectArray m_rigidBodyLinkNames; - btHashMap m_linkUserDataMap; + btAlignedObjectArray m_userDataHandles; #ifdef B3_ENABLE_TINY_AUDIO b3HashMap m_audioSources; @@ -328,10 +262,7 @@ struct InternalBodyData m_rigidBodyJoints.clear(); m_rigidBodyJointNames.clear(); m_rigidBodyLinkNames.clear(); - for(int i=0; i m_bodyHandles; b3ResizablePool m_userCollisionShapeHandles; b3ResizablePool m_userVisualShapeHandles; - - + b3ResizablePool> m_userDataHandles; + btHashMap m_userDataHandleLookup; b3PluginManager m_pluginManager; @@ -5042,34 +4973,11 @@ bool PhysicsServerCommandProcessor::processSyncUserDataCommand(const struct Shar bool hasStatus = true; BT_PROFILE("CMD_SYNC_USER_DATA"); - b3UserDataGlobalIdentifier *userDataIdentifiers = (b3UserDataGlobalIdentifier *)bufferServerToClient; - int numIdentifiers = 0; - b3AlignedObjectArray bodyHandles; - m_data->m_bodyHandles.getUsedHandles(bodyHandles); - for (int i=0; im_bodyHandles.getHandle(bodyHandle); - if (!body) { - continue; - } - for (int j=0; jm_linkUserDataMap.size(); j++) { - const int linkIndex = body->m_linkUserDataMap.getKeyAtIndex(j).getUid1(); - InternalLinkUserData **userDataPtr = body->m_linkUserDataMap.getAtIndex(j); - if (!userDataPtr) { - continue; - } - b3AlignedObjectArray userDataIds; - (*userDataPtr)->getUserDataIds(userDataIds); - for (int k=0; k userDataHandles; + m_data->m_userDataHandles.getUsedHandles(userDataHandles); + memcpy(bufferServerToClient, &userDataHandles[0], sizeof(int) * userDataHandles.size()); - serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = numIdentifiers; + serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = userDataHandles.size(); serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED; return hasStatus; } @@ -5080,21 +4988,16 @@ bool PhysicsServerCommandProcessor::processRequestUserDataCommand(const struct S BT_PROFILE("CMD_REQUEST_USER_DATA"); serverStatusOut.m_type = CMD_REQUEST_USER_DATA_FAILED; - InternalBodyData *body = m_data->m_bodyHandles.getHandle(clientCmd.m_userDataRequestArgs.m_bodyUniqueId); - if (!body) { - return hasStatus; - } - const int linkIndex = clientCmd.m_userDataRequestArgs.m_linkIndex; - InternalLinkUserData **userDataPtr = body->m_linkUserDataMap[linkIndex]; - if (!userDataPtr) { - return hasStatus; - } - const SharedMemoryUserData *userData = (*userDataPtr)->getUserData(clientCmd.m_userDataRequestArgs.m_userDataId); + SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(clientCmd.m_userDataRequestArgs.m_userDataId); if (!userData) { return hasStatus; } - btAssert(bufferSizeInBytes >= userData->m_bytes.size()); - serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId = clientCmd.m_userDataRequestArgs; + + btAssert(bufferSizeInBytes >= userData->m_bytes.size()); + serverStatusOut.m_userDataResponseArgs.m_userDataId = clientCmd.m_userDataRequestArgs.m_userDataId; + serverStatusOut.m_userDataResponseArgs.m_bodyUniqueId = userData->m_bodyUniqueId; + serverStatusOut.m_userDataResponseArgs.m_linkIndex = userData->m_linkIndex; + serverStatusOut.m_userDataResponseArgs.m_visualShapeIndex = userData->m_visualShapeIndex; serverStatusOut.m_userDataResponseArgs.m_valueType = userData->m_type; serverStatusOut.m_userDataResponseArgs.m_valueLength = userData->m_bytes.size(); serverStatusOut.m_type = CMD_REQUEST_USER_DATA_COMPLETED; @@ -5117,27 +5020,43 @@ bool PhysicsServerCommandProcessor::processAddUserDataCommand(const struct Share { return hasStatus; } - + InternalBodyData *body = m_data->m_bodyHandles.getHandle(clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId); if (!body) { return hasStatus; } - const int linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex; - InternalLinkUserData **userDataPtr = body->m_linkUserDataMap[linkIndex]; - if (!userDataPtr) { - InternalLinkUserData *userData = new InternalLinkUserData; - userDataPtr = &userData; - body->m_linkUserDataMap.insert(linkIndex, userData); + + SharedMemoryUserDataHashKey userDataIdentifier( + clientCmd.m_addUserDataRequestArgs.m_key, + clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId, + clientCmd.m_addUserDataRequestArgs.m_linkIndex, + clientCmd.m_addUserDataRequestArgs.m_visualShapeIndex); + + int* userDataHandlePtr = m_data->m_userDataHandleLookup.find(userDataIdentifier); + int userDataHandle = userDataHandlePtr ? *userDataHandlePtr : m_data->m_userDataHandles.allocHandle(); + + SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(userDataHandle); + if (!userData) { + return hasStatus; } - const int userDataId = (*userDataPtr)->add(clientCmd.m_addUserDataRequestArgs.m_key, - bufferServerToClient,clientCmd.m_addUserDataRequestArgs.m_valueLength, + if (!userDataHandlePtr) { + userData->m_key = clientCmd.m_addUserDataRequestArgs.m_key; + userData->m_bodyUniqueId = clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId; + userData->m_linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex; + userData->m_visualShapeIndex = clientCmd.m_addUserDataRequestArgs.m_visualShapeIndex; + m_data->m_userDataHandleLookup.insert(userDataIdentifier, userDataHandle); + body->m_userDataHandles.push_back(userDataHandle); + } + userData->replaceValue(bufferServerToClient, + clientCmd.m_addUserDataRequestArgs.m_valueLength, clientCmd.m_addUserDataRequestArgs.m_valueType); serverStatusOut.m_type = CMD_ADD_USER_DATA_COMPLETED; - serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_userDataId = userDataId; - serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_bodyUniqueId = clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId; - serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex; + serverStatusOut.m_userDataResponseArgs.m_userDataId = userDataHandle; + serverStatusOut.m_userDataResponseArgs.m_bodyUniqueId = clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId; + serverStatusOut.m_userDataResponseArgs.m_linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex; + serverStatusOut.m_userDataResponseArgs.m_visualShapeIndex = clientCmd.m_addUserDataRequestArgs.m_visualShapeIndex; serverStatusOut.m_userDataResponseArgs.m_valueLength = clientCmd.m_addUserDataRequestArgs.m_valueLength; serverStatusOut.m_userDataResponseArgs.m_valueType = clientCmd.m_addUserDataRequestArgs.m_valueType; strcpy(serverStatusOut.m_userDataResponseArgs.m_key, clientCmd.m_addUserDataRequestArgs.m_key); @@ -5152,19 +5071,20 @@ bool PhysicsServerCommandProcessor::processRemoveUserDataCommand(const struct Sh BT_PROFILE("CMD_REMOVE_USER_DATA"); serverStatusOut.m_type = CMD_REMOVE_USER_DATA_FAILED; - InternalBodyData *body = m_data->m_bodyHandles.getHandle(clientCmd.m_removeUserDataRequestArgs.m_bodyUniqueId); + SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(clientCmd.m_removeUserDataRequestArgs.m_userDataId); + if (!userData) { + return hasStatus; + } + + InternalBodyData *body = m_data->m_bodyHandles.getHandle(userData->m_bodyUniqueId); if (!body) { return hasStatus; } - const int linkIndex = clientCmd.m_removeUserDataRequestArgs.m_linkIndex; - InternalLinkUserData **userDataPtr = body->m_linkUserDataMap[linkIndex]; - if (!userDataPtr) { - return hasStatus; - } - const bool removed = (*userDataPtr)->remove(clientCmd.m_removeUserDataRequestArgs.m_userDataId); - if (!removed) { - return hasStatus; - } + body->m_userDataHandles.remove(clientCmd.m_removeUserDataRequestArgs.m_userDataId); + + m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData)); + m_data->m_userDataHandles.freeHandle(clientCmd.m_removeUserDataRequestArgs.m_userDataId); + serverStatusOut.m_removeUserDataResponseArgs = clientCmd.m_removeUserDataRequestArgs; serverStatusOut.m_type = CMD_REMOVE_USER_DATA_COMPLETED; return hasStatus; @@ -8278,6 +8198,12 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared bodyHandle->m_rigidBody=0; serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED; } + for (int i=0; i < bodyHandle->m_userDataHandles.size(); i++) { + int userDataHandle = bodyHandle->m_userDataHandles[i]; + SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(userDataHandle); + m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData)); + m_data->m_userDataHandles.freeHandle(userDataHandle); + } m_data->m_bodyHandles.freeHandle(bodyUniqueId); } @@ -10495,6 +10421,9 @@ void PhysicsServerCommandProcessor::resetSimulation() m_data->m_userCollisionShapeHandles.exitHandles(); m_data->m_userCollisionShapeHandles.initHandles(); + m_data->m_userDataHandles.exitHandles(); + m_data->m_userDataHandles.initHandles(); + m_data->m_userDataHandleLookup.clear(); } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index b94ed8666..e529f66b1 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -993,13 +993,20 @@ struct b3StateSerializationArguments struct SyncUserDataArgs { // User data identifiers stored in m_bulletStreamDataServerToClientRefactor - // as as array of b3UserDataGlobalIdentifier objects + // as as array of integers. int m_numUserDataIdentifiers; }; +struct UserDataRequestArgs { + int m_userDataId; +}; + struct UserDataResponseArgs { - b3UserDataGlobalIdentifier m_userDataGlobalId; + int m_userDataId; + int m_bodyUniqueId; + int m_linkIndex; + int m_visualShapeIndex; int m_valueType; int m_valueLength; char m_key[MAX_USER_DATA_KEY_LENGTH]; @@ -1010,6 +1017,7 @@ struct AddUserDataRequestArgs { int m_bodyUniqueId; int m_linkIndex; + int m_visualShapeIndex; int m_valueType; int m_valueLength; char m_key[MAX_USER_DATA_KEY_LENGTH]; @@ -1073,9 +1081,9 @@ struct SharedMemoryCommand struct b3CustomCommand m_customCommandArgs; struct b3StateSerializationArguments m_loadStateArguments; struct RequestCollisionShapeDataArgs m_requestCollisionShapeDataArguments; - struct b3UserDataGlobalIdentifier m_userDataRequestArgs; + struct UserDataRequestArgs m_userDataRequestArgs; struct AddUserDataRequestArgs m_addUserDataRequestArgs; - struct b3UserDataGlobalIdentifier m_removeUserDataRequestArgs; + struct UserDataRequestArgs m_removeUserDataRequestArgs; }; }; @@ -1150,7 +1158,7 @@ struct SharedMemoryStatus struct SendCollisionShapeDataArgs m_sendCollisionShapeArgs; struct SyncUserDataArgs m_syncUserDataArgs; struct UserDataResponseArgs m_userDataResponseArgs; - struct b3UserDataGlobalIdentifier m_removeUserDataResponseArgs; + struct UserDataRequestArgs m_removeUserDataResponseArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 7e7338e36..86d55db3c 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -274,6 +274,7 @@ struct b3JointInfo enum UserDataValueType { + USER_DATA_VALUE_TYPE_NOT_USET = -1, // Data represents generic byte array. USER_DATA_VALUE_TYPE_BYTES = 0, // Data represents C-string @@ -287,13 +288,6 @@ struct b3UserDataValue char* m_data1; }; -struct b3UserDataGlobalIdentifier -{ - int m_bodyUniqueId; - int m_linkIndex; - int m_userDataId; -}; - struct b3UserConstraint { int m_parentBodyIndex; diff --git a/examples/SharedMemory/SharedMemoryUserData.h b/examples/SharedMemory/SharedMemoryUserData.h index a29f59eb0..991714557 100644 --- a/examples/SharedMemory/SharedMemoryUserData.h +++ b/examples/SharedMemory/SharedMemoryUserData.h @@ -3,6 +3,7 @@ #include #include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btHashMap.h" #include "SharedMemoryPublic.h" struct SharedMemoryUserData @@ -10,20 +11,22 @@ struct SharedMemoryUserData std::string m_key; int m_type; + int m_bodyUniqueId; + int m_linkIndex; + int m_visualShapeIndex; + btAlignedObjectArray m_bytes; SharedMemoryUserData() - :m_type(-1) + :m_type(-1), m_bodyUniqueId(-1), m_linkIndex(-1), m_visualShapeIndex(-1) { } - // Takes ownership of the passed key and value arguments. - SharedMemoryUserData(const char* key) - :m_key(key) + SharedMemoryUserData(const char* key, int bodyUniqueId, int linkIndex, int visualShapeIndex) + :m_key(key), m_type(-1), m_bodyUniqueId(bodyUniqueId), m_linkIndex(linkIndex), m_visualShapeIndex(visualShapeIndex) { } - // Takes ownership of the data pointed to by newValue. void replaceValue(const char* bytes, int len, int type) { m_type = type; @@ -45,4 +48,44 @@ struct SharedMemoryUserData } }; +class SharedMemoryUserDataHashKey { + unsigned int m_hash = 0; + + btHashString m_key; + btHashInt m_bodyUniqueId; + btHashInt m_linkIndex; + btHashInt m_visualShapeIndex; + +public: + SIMD_FORCE_INLINE unsigned int getHash()const { + return m_hash; + } + + SharedMemoryUserDataHashKey() : m_hash(0) {} + + SharedMemoryUserDataHashKey(const struct SharedMemoryUserData *userData) + : m_key(userData->m_key.c_str()), + m_bodyUniqueId(userData->m_bodyUniqueId), + m_linkIndex(userData->m_linkIndex), + m_visualShapeIndex(userData->m_visualShapeIndex) { + calculateHash(); + } + + SharedMemoryUserDataHashKey(const char *key, int bodyUniqueId, int linkIndex, int visualShapeIndex) + : m_key(key), m_bodyUniqueId(bodyUniqueId), m_linkIndex(linkIndex), m_visualShapeIndex(visualShapeIndex) { + calculateHash(); + } + + void calculateHash() { + m_hash = m_key.getHash() ^ m_bodyUniqueId.getHash() ^ m_linkIndex.getHash() ^ m_visualShapeIndex.getHash(); + } + + bool equals(const SharedMemoryUserDataHashKey& other) const { + return m_bodyUniqueId.equals(other.m_bodyUniqueId) && + m_linkIndex.equals(other.m_linkIndex) && + m_visualShapeIndex.equals(other.m_visualShapeIndex) && + m_key.equals(other.m_key); + } +}; + #endif //SHARED_MEMORY_USER_DATA_H diff --git a/examples/pybullet/examples/userData.py b/examples/pybullet/examples/userData.py index 17080a4e5..164c470b5 100644 --- a/examples/pybullet/examples/userData.py +++ b/examples/pybullet/examples/userData.py @@ -21,16 +21,16 @@ plane_id = client.loadURDF(PLANE_PATH) print ("Plane ID: %s" % plane_id) print ("Adding user data to plane") -MyKey1 = client.addUserData(plane_id, 0, "MyKey1", "MyValue1") -MyKey2 = client.addUserData(plane_id, 0, "MyKey2", "MyValue2") -MyKey3 = client.addUserData(plane_id, 0, "MyKey3", "MyValue3") -MyKey4 = client.addUserData(plane_id, 0, "MyKey4", "MyValue4") +MyKey1 = client.addUserData(plane_id, "MyKey1", "MyValue1") +MyKey2 = client.addUserData(plane_id, "MyKey2", "MyValue2") +MyKey3 = client.addUserData(plane_id, "MyKey3", "MyValue3") +MyKey4 = client.addUserData(plane_id, "MyKey4", "MyValue4") print ("Retrieving cached user data") -print (client.getUserData(plane_id, 0, MyKey1)) -print (client.getUserData(plane_id, 0, MyKey2)) -print (client.getUserData(plane_id, 0, MyKey3)) -print (client.getUserData(plane_id, 0, MyKey4)) +print (client.getUserData(MyKey1)) +print (client.getUserData(MyKey2)) +print (client.getUserData(MyKey3)) +print (client.getUserData(MyKey4)) print ("Disconnecting") del client @@ -39,18 +39,18 @@ print ("Reconnecting") client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD) print ("Retrieving synced user data") -print (client.getUserData(plane_id, 0, MyKey1)) -print (client.getUserData(plane_id, 0, MyKey2)) -print (client.getUserData(plane_id, 0, MyKey3)) -print (client.getUserData(plane_id, 0, MyKey4)) +print (client.getUserData(MyKey1)) +print (client.getUserData(MyKey2)) +print (client.getUserData(MyKey3)) +print (client.getUserData(MyKey4)) -print ("Number of user data entries: %s" % client.getNumUserData(plane_id, 0)) +print ("Number of user data entries: %s" % client.getNumUserData(plane_id)) print ("Overriding user data") -client.addUserData(plane_id, 0, "MyKey1", "MyNewValue") +client.addUserData(plane_id, "MyKey1", "MyNewValue") print ("Cached overridden data") -print (client.getUserData(plane_id, 0, MyKey1)) +print (client.getUserData(MyKey1)) print ("Disconnecting") @@ -61,50 +61,50 @@ client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD) print ("Synced overridden data") -print (client.getUserData(plane_id, 0, MyKey1)) +print (client.getUserData(MyKey1)) print ("Getting user data ID") -print ("Retrieved ID: %s, ID retrieved from addUserData: %s" % (client.getUserDataId(plane_id, 0, "MyKey2"), MyKey2)) +print ("Retrieved ID: %s, ID retrieved from addUserData: %s" % (client.getUserDataId(plane_id, "MyKey2"), MyKey2)) print ("Removing user data") -client.removeUserData(plane_id, 0, MyKey2) +client.removeUserData(MyKey2) print ("Retrieving cached removed data") -print (client.getUserData(plane_id, 0, MyKey2)) +print (client.getUserData(MyKey2)) print ("Syncing") client.syncUserData() print ("Retrieving removed removed data") -print (client.getUserData(plane_id, 0, MyKey2)) +print (client.getUserData(MyKey2)) print ("Iterating over all user data entries and printing results") -for i in range(client.getNumUserData(plane_id, 0)): - userDataId, key = client.getUserDataInfo(plane_id, 0, i) - print ("Info: (%s, %s)" % (userDataId, key)) - print ("Value: %s" % client.getUserData(plane_id, 0, userDataId)) +for i in range(client.getNumUserData(plane_id)): + userDataId, key, bodyId, linkIndex, visualShapeIndex = client.getUserDataInfo(plane_id, i) + print ("Info: (%s, %s, %s, %s, %s)" % (userDataId, key, bodyId, linkIndex, visualShapeIndex)) + print ("Value: %s" % client.getUserData(userDataId)) print ("Removing body") client.removeBody(plane_id) print ("Retrieving user data") -print (client.getUserData(plane_id, 0, MyKey1)) -print (client.getUserData(plane_id, 0, MyKey3)) -print (client.getUserData(plane_id, 0, MyKey4)) +print (client.getUserData(MyKey1)) +print (client.getUserData(MyKey3)) +print (client.getUserData(MyKey4)) print ("Syncing") client.syncUserData() print ("Retrieving user data") -print (client.getUserData(plane_id, 0, MyKey1)) -print (client.getUserData(plane_id, 0, MyKey3)) -print (client.getUserData(plane_id, 0, MyKey4)) +print (client.getUserData(MyKey1)) +print (client.getUserData(MyKey3)) +print (client.getUserData(MyKey4)) plane_id2 = client.loadURDF(PLANE_PATH) print ("Plane1: %s, plane2: %s" % (plane_id, plane_id2)) print ("Retrieving user data") -print (client.getUserData(plane_id, 0, MyKey1)) -print (client.getUserData(plane_id, 0, MyKey3)) -print (client.getUserData(plane_id, 0, MyKey4)) +print (client.getUserData(MyKey1)) +print (client.getUserData(MyKey3)) +print (client.getUserData(MyKey4)) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b7c704305..a319ab937 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -689,18 +689,19 @@ static PyObject* pybullet_addUserData(PyObject* self, PyObject* args, PyObject* b3PhysicsClientHandle sm = 0; int physicsClientId = 0; int bodyUniqueId = -1; - int linkIndex = -2; + int linkIndex = -1; + int visualShapeIndex = -1; const char* key = ""; const char* value = ""; // TODO: Change this to a PyObject and detect the type dynamically. - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "key", "value", "physicsClientId", NULL}; + static char* kwlist[] = {"bodyUniqueId", "key", "value", "linkIndex", "visualShapeIndex", "physicsClientId", NULL}; b3SharedMemoryCommandHandle command; b3SharedMemoryStatusHandle statusHandle; int statusType; int userDataId; int valueLen=-1; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiss|i", kwlist, &bodyUniqueId, &linkIndex, &key, &value, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iss|iii", kwlist, &bodyUniqueId, &key, &value, &linkIndex, &visualShapeIndex, &physicsClientId)) { return NULL; } @@ -712,7 +713,7 @@ static PyObject* pybullet_addUserData(PyObject* self, PyObject* args, PyObject* } valueLen = strlen(value)+1; - command = b3InitAddUserDataCommand(sm, bodyUniqueId, linkIndex, key, USER_DATA_VALUE_TYPE_STRING, valueLen, value); + command = b3InitAddUserDataCommand(sm, bodyUniqueId, linkIndex, visualShapeIndex, key, USER_DATA_VALUE_TYPE_STRING, valueLen, value); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); @@ -730,16 +731,14 @@ static PyObject* pybullet_removeUserData(PyObject* self, PyObject* args, PyObjec { b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - int bodyUniqueId = -1; - int linkIndex = -1; int userDataId = -1; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataId", "physicsClientId", NULL}; + static char* kwlist[] = {"userDataId", "physicsClientId", NULL}; b3SharedMemoryCommandHandle command; b3SharedMemoryStatusHandle statusHandle; int statusType; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataId, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &userDataId, &physicsClientId)) { return NULL; } @@ -750,7 +749,7 @@ static PyObject* pybullet_removeUserData(PyObject* self, PyObject* args, PyObjec return NULL; } - command = b3InitRemoveUserDataCommand(sm, bodyUniqueId, linkIndex, userDataId); + command = b3InitRemoveUserDataCommand(sm, userDataId); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); @@ -770,15 +769,16 @@ static PyObject* pybullet_getUserDataId(PyObject* self, PyObject* args, PyObject int physicsClientId = 0; int bodyUniqueId = -1; int linkIndex = -1; + int visualShapeIndex = -1; const char* key = ""; int userDataId; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "key", "physicsClientId", NULL}; + static char* kwlist[] = {"bodyUniqueId", "key", "linkIndex", "visualShapeIndex", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iis|i", kwlist, &bodyUniqueId, &linkIndex, &key, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|iii", kwlist, &bodyUniqueId, &key, &linkIndex, &visualShapeIndex, &physicsClientId)) { return NULL; } @@ -789,7 +789,7 @@ static PyObject* pybullet_getUserDataId(PyObject* self, PyObject* args, PyObject return NULL; } - userDataId = b3GetUserDataId(sm, bodyUniqueId, linkIndex, key); + userDataId = b3GetUserDataId(sm, bodyUniqueId, linkIndex, visualShapeIndex, key); return PyInt_FromLong(userDataId); } @@ -797,16 +797,14 @@ static PyObject* pybullet_getUserData(PyObject* self, PyObject* args, PyObject* { b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - int bodyUniqueId = -1; - int linkIndex = -1; int userDataId = -1; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataId", "physicsClientId", NULL}; + static char* kwlist[] = {"userDataId", "physicsClientId", NULL}; struct b3UserDataValue value; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataId, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &userDataId, &physicsClientId)) { return NULL; } @@ -818,7 +816,7 @@ static PyObject* pybullet_getUserData(PyObject* self, PyObject* args, PyObject* } - if (!b3GetUserData(sm, bodyUniqueId, linkIndex, userDataId, &value)) { + if (!b3GetUserData(sm, userDataId, &value)) { Py_INCREF(Py_None); return Py_None; @@ -837,15 +835,14 @@ static PyObject* pybullet_getNumUserData(PyObject* self, PyObject* args, PyObjec b3PhysicsClientHandle sm = 0; int physicsClientId = 0; int bodyUniqueId = -1; - int linkIndex = -1; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL}; + static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL}; int numUserData; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId)) { return NULL; } @@ -856,7 +853,7 @@ static PyObject* pybullet_getNumUserData(PyObject* self, PyObject* args, PyObjec return NULL; } - numUserData = b3GetNumUserData(sm, bodyUniqueId, linkIndex); + numUserData = b3GetNumUserData(sm, bodyUniqueId); return PyInt_FromLong(numUserData); } @@ -865,16 +862,17 @@ static PyObject* pybullet_getUserDataInfo(PyObject* self, PyObject* args, PyObje b3PhysicsClientHandle sm = 0; int physicsClientId = 0; int bodyUniqueId = -1; - int linkIndex = -1; int userDataIndex = -1; + int linkIndex = -1; + int visualShapeIndex = -1; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataIndex", "physicsClientId", NULL}; + static char* kwlist[] = {"bodyUniqueId", "userDataIndex", "physicsClientId", NULL}; const char* key = 0; int userDataId = -1; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataIndex, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &userDataIndex, &physicsClientId)) { return NULL; } @@ -885,16 +883,19 @@ static PyObject* pybullet_getUserDataInfo(PyObject* self, PyObject* args, PyObje return NULL; } - b3GetUserDataInfo(sm, bodyUniqueId, linkIndex, userDataIndex, &key, &userDataId); + b3GetUserDataInfo(sm, bodyUniqueId, userDataIndex, &key, &userDataId, &linkIndex, &visualShapeIndex); if (key == 0 || userDataId == -1) { PyErr_SetString(SpamError, "Could not get user data info."); return NULL; } { - PyObject *userDataInfoTuple = PyTuple_New(2); + PyObject *userDataInfoTuple = PyTuple_New(5); PyTuple_SetItem(userDataInfoTuple, 0, PyInt_FromLong(userDataId)); PyTuple_SetItem(userDataInfoTuple, 1, PyString_FromString(key)); + PyTuple_SetItem(userDataInfoTuple, 2, PyInt_FromLong(bodyUniqueId)); + PyTuple_SetItem(userDataInfoTuple, 3, PyInt_FromLong(linkIndex)); + PyTuple_SetItem(userDataInfoTuple, 4, PyInt_FromLong(visualShapeIndex)); return userDataInfoTuple; } } @@ -9131,28 +9132,28 @@ static PyMethodDef SpamMethods[] = { "Update user data, in case other clients made changes."}, {"addUserData", (PyCFunction)pybullet_addUserData, METH_VARARGS | METH_KEYWORDS, - "addUserData(bodyUniqueId, linkIndex, key, value, physicsClientId=0)\n" - "Adds or updates a user data entry to a link. Returns user data identifier."}, + "addUserData(bodyUniqueId, key, value, linkIndex=-1, visualShapeIndex=-1, physicsClientId=0)\n" + "Adds or updates a user data entry. Returns user data identifier."}, {"getUserData", (PyCFunction)pybullet_getUserData, METH_VARARGS | METH_KEYWORDS, - "getUserData(bodyUniqueId, linkIndex, userDataId, physicsClientId=0)\n" + "getUserData(userDataId, physicsClientId=0)\n" "Returns the user data value."}, {"removeUserData", (PyCFunction)pybullet_removeUserData, METH_VARARGS | METH_KEYWORDS, - "removeUserData(bodyUniqueId, linkIndex, userDataId, physicsClientId=0)\n" + "removeUserData(userDataId, physicsClientId=0)\n" "Removes a user data entry."}, {"getUserDataId", (PyCFunction)pybullet_getUserDataId, METH_VARARGS | METH_KEYWORDS, - "getUserDataId(bodyUniqueId, linkIndex, key, physicsClientId=0)\n" - "Retrieves the userDataId on a link given the key."}, + "getUserDataId(bodyUniqueId, key, linkIndex=-1, visualShapeIndex=-1, physicsClientId=0)\n" + "Retrieves the userDataId given the key and optionally link and visual shape index."}, {"getNumUserData", (PyCFunction)pybullet_getNumUserData, METH_VARARGS | METH_KEYWORDS, - "getNumUserData(bodyUniqueId, linkIndex, physicsClientId=0)\n" - "Retrieves the number of user data entries in a link."}, + "getNumUserData(bodyUniqueId physicsClientId=0)\n" + "Retrieves the number of user data entries in a body."}, {"getUserDataInfo", (PyCFunction)pybullet_getUserDataInfo, METH_VARARGS | METH_KEYWORDS, - "getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, physicsClientId=0)\n" - "Retrieves the key and the identifier of a user data as (id, key)."}, + "getUserDataInfo(bodyUniqueId, userDataIndex, physicsClientId=0)\n" + "Retrieves the key and the identifier of a user data as (userDataId, key, bodyUniqueId, linkIndex, visualShapeIndex)."}, {"removeBody", (PyCFunction)pybullet_removeBody, METH_VARARGS | METH_KEYWORDS, "Remove a body by its body unique id."}, diff --git a/examples/pybullet/unittests/userDataTest.py b/examples/pybullet/unittests/userDataTest.py index 2b3db2cb4..e16b56958 100644 --- a/examples/pybullet/unittests/userDataTest.py +++ b/examples/pybullet/unittests/userDataTest.py @@ -26,169 +26,170 @@ class TestUserDataMethods(unittest.TestCase): def testAddUserData(self): plane_id = self.client.loadURDF(PLANE_PATH) - uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") - uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") - uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") - uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") + uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1") + uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2") + uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3") + uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4") # Retrieve user data and make sure it's correct. - self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1)) - self.assertEqual(b"MyValue2", self.client.getUserData(plane_id, 0, uid2)) - self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3)) - self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4)) + self.assertEqual(b"MyValue1", self.client.getUserData(uid1)) + self.assertEqual(b"MyValue2", self.client.getUserData(uid2)) + self.assertEqual(b"MyValue3", self.client.getUserData(uid3)) + self.assertEqual(b"MyValue4", self.client.getUserData(uid4)) # Disconnect/reconnect and make sure that the user data is synced back. del self.client self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY) - self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1)) - self.assertEqual(b"MyValue2", self.client.getUserData(plane_id, 0, uid2)) - self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3)) - self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4)) + self.assertEqual(b"MyValue1", self.client.getUserData(uid1)) + self.assertEqual(b"MyValue2", self.client.getUserData(uid2)) + self.assertEqual(b"MyValue3", self.client.getUserData(uid3)) + self.assertEqual(b"MyValue4", self.client.getUserData(uid4)) self.client.resetSimulation() - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid1)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid3)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid4)) + self.assertEqual(None, self.client.getUserData(uid1)) + self.assertEqual(None, self.client.getUserData(uid2)) + self.assertEqual(None, self.client.getUserData(uid3)) + self.assertEqual(None, self.client.getUserData(uid4)) def testGetNumUserData(self): plane_id = self.client.loadURDF(PLANE_PATH) - uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") - uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") - uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") - uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") + uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1") + uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2") + uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3") + uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4") - self.assertEqual(4, self.client.getNumUserData(plane_id, 0)) + self.assertEqual(4, self.client.getNumUserData(plane_id)) del self.client self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY) - self.assertEqual(4, self.client.getNumUserData(plane_id, 0)) + self.assertEqual(4, self.client.getNumUserData(plane_id)) def testReplaceUserData(self): plane_id = self.client.loadURDF(PLANE_PATH) - uid = self.client.addUserData(plane_id, 0, "MyKey", "MyValue") + uid = self.client.addUserData(plane_id, "MyKey", "MyValue") - self.assertEqual(b"MyValue", self.client.getUserData(plane_id, 0, uid)) + self.assertEqual(b"MyValue", self.client.getUserData(uid)) - new_uid = self.client.addUserData(plane_id, 0, "MyKey", "MyNewValue") + new_uid = self.client.addUserData(plane_id, "MyKey", "MyNewValue") self.assertEqual(uid, new_uid) - self.assertEqual(b"MyNewValue", self.client.getUserData(plane_id, 0, uid)) + self.assertEqual(b"MyNewValue", self.client.getUserData(uid)) del self.client self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY) - self.assertEqual(b"MyNewValue", self.client.getUserData(plane_id, 0, uid)) + self.assertEqual(b"MyNewValue", self.client.getUserData(uid)) def testGetUserDataId(self): plane_id = self.client.loadURDF(PLANE_PATH) - uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") - uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") - uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") - uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") + uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1") + uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2") + uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3") + uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4") - self.assertEqual(uid1, self.client.getUserDataId(plane_id, 0, "MyKey1")) - self.assertEqual(uid2, self.client.getUserDataId(plane_id, 0, "MyKey2")) - self.assertEqual(uid3, self.client.getUserDataId(plane_id, 0, "MyKey3")) - self.assertEqual(uid4, self.client.getUserDataId(plane_id, 0, "MyKey4")) + self.assertEqual(uid1, self.client.getUserDataId(plane_id, "MyKey1")) + self.assertEqual(uid2, self.client.getUserDataId(plane_id, "MyKey2")) + self.assertEqual(uid3, self.client.getUserDataId(plane_id, "MyKey3")) + self.assertEqual(uid4, self.client.getUserDataId(plane_id, "MyKey4")) del self.client self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY) - self.assertEqual(uid1, self.client.getUserDataId(plane_id, 0, "MyKey1")) - self.assertEqual(uid2, self.client.getUserDataId(plane_id, 0, "MyKey2")) - self.assertEqual(uid3, self.client.getUserDataId(plane_id, 0, "MyKey3")) - self.assertEqual(uid4, self.client.getUserDataId(plane_id, 0, "MyKey4")) + self.assertEqual(uid1, self.client.getUserDataId(plane_id, "MyKey1")) + self.assertEqual(uid2, self.client.getUserDataId(plane_id, "MyKey2")) + self.assertEqual(uid3, self.client.getUserDataId(plane_id, "MyKey3")) + self.assertEqual(uid4, self.client.getUserDataId(plane_id, "MyKey4")) def testRemoveUserData(self): plane_id = self.client.loadURDF(PLANE_PATH) - uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") - uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") - uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") - uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") + uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1") + uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2") + uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3") + uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4") - self.client.removeUserData(plane_id, 0, uid2) + self.client.removeUserData(uid2) - self.assertEqual(3, self.client.getNumUserData(plane_id, 0)) - self.assertEqual(-1, self.client.getUserDataId(plane_id, 0, "MyKey2")) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2)) - self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1)) - self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3)) - self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4)) + self.assertEqual(3, self.client.getNumUserData(plane_id)) + self.assertEqual(-1, self.client.getUserDataId(plane_id, "MyKey2")) + self.assertEqual(None, self.client.getUserData(uid2)) + self.assertEqual(b"MyValue1", self.client.getUserData(uid1)) + self.assertEqual(b"MyValue3", self.client.getUserData(uid3)) + self.assertEqual(b"MyValue4", self.client.getUserData(uid4)) del self.client self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY) - self.assertEqual(3, self.client.getNumUserData(plane_id, 0)) - self.assertEqual(-1, self.client.getUserDataId(plane_id, 0, "MyKey2")) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2)) - self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1)) - self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3)) - self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4)) + self.assertEqual(3, self.client.getNumUserData(plane_id)) + self.assertEqual(-1, self.client.getUserDataId(plane_id, "MyKey2")) + self.assertEqual(None, self.client.getUserData(uid2)) + self.assertEqual(b"MyValue1", self.client.getUserData(uid1)) + self.assertEqual(b"MyValue3", self.client.getUserData(uid3)) + self.assertEqual(b"MyValue4", self.client.getUserData(uid4)) def testIterateAllUserData(self): plane_id = self.client.loadURDF(PLANE_PATH) - uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") - uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") - uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") - uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") + uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1") + uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2") + uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3") + uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4") entries = set() - for i in range(self.client.getNumUserData(plane_id, 0)): - userDataId, key = self.client.getUserDataInfo(plane_id, 0, i) - value = self.client.getUserData(plane_id, 0, userDataId); - entries.add((userDataId, key, value)) + for i in range(self.client.getNumUserData(plane_id)): + userDataId, key, bodyId, linkIndex, visualShapeIndex = self.client.getUserDataInfo(plane_id, i) + value = self.client.getUserData(userDataId); + entries.add((userDataId, key, value, bodyId, linkIndex, visualShapeIndex)) - self.assertTrue((uid1, b"MyKey1", b"MyValue1") in entries) - self.assertTrue((uid2, b"MyKey2", b"MyValue2") in entries) - self.assertTrue((uid3, b"MyKey3", b"MyValue3") in entries) - self.assertTrue((uid4, b"MyKey4", b"MyValue4") in entries) + self.assertTrue((uid1, b"MyKey1", b"MyValue1", plane_id, -1, -1) in entries) + self.assertTrue((uid2, b"MyKey2", b"MyValue2", plane_id, -1, -1) in entries) + self.assertTrue((uid3, b"MyKey3", b"MyValue3", plane_id, -1, -1) in entries) + self.assertTrue((uid4, b"MyKey4", b"MyValue4", plane_id, -1, -1) in entries) self.assertEqual(4, len(entries)) def testRemoveBody(self): plane_id = self.client.loadURDF(PLANE_PATH) - uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1") - uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2") - uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3") - uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4") + uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1") + uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2") + uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3") + uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4") self.client.removeBody(plane_id) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid1)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid3)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid4)) + self.assertEqual(None, self.client.getUserData(uid1)) + self.assertEqual(None, self.client.getUserData(uid2)) + self.assertEqual(None, self.client.getUserData(uid3)) + self.assertEqual(None, self.client.getUserData(uid4)) del self.client self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid1)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid3)) - self.assertEqual(None, self.client.getUserData(plane_id, 0, uid4)) + self.assertEqual(None, self.client.getUserData(uid1)) + self.assertEqual(None, self.client.getUserData(uid2)) + self.assertEqual(None, self.client.getUserData(uid3)) + self.assertEqual(None, self.client.getUserData(uid4)) + def testMultipleBodies(self): plane1 = self.client.loadURDF(PLANE_PATH) plane2 = self.client.loadURDF(PLANE_PATH) - uid1 = self.client.addUserData(plane1, 0, "MyKey1", "This is plane 1 - 1") - uid2 = self.client.addUserData(plane1, 0, "MyKey2", "This is plane 1 - 2") + uid1 = self.client.addUserData(plane1, "MyKey1", "This is plane 1 - 1") + uid2 = self.client.addUserData(plane1, "MyKey2", "This is plane 1 - 2") - uid3 = self.client.addUserData(plane2, 0, "MyKey1", "This is plane 2 - 1") - uid4 = self.client.addUserData(plane2, 0, "MyKey2", "This is plane 2 - 2") - uid5 = self.client.addUserData(plane2, 0, "MyKey3", "This is plane 2 - 3") + uid3 = self.client.addUserData(plane2, "MyKey1", "This is plane 2 - 1") + uid4 = self.client.addUserData(plane2, "MyKey2", "This is plane 2 - 2") + uid5 = self.client.addUserData(plane2, "MyKey3", "This is plane 2 - 3") - self.assertEqual(b"This is plane 1 - 1", self.client.getUserData(plane1, 0, self.client.getUserDataId(plane1, 0, "MyKey1"))) - self.assertEqual(b"This is plane 1 - 2", self.client.getUserData(plane1, 0, self.client.getUserDataId(plane1, 0, "MyKey2"))) + self.assertEqual(b"This is plane 1 - 1", self.client.getUserData(self.client.getUserDataId(plane1, "MyKey1"))) + self.assertEqual(b"This is plane 1 - 2", self.client.getUserData(self.client.getUserDataId(plane1, "MyKey2"))) - self.assertEqual(b"This is plane 2 - 1", self.client.getUserData(plane2, 0, self.client.getUserDataId(plane2, 0, "MyKey1"))) - self.assertEqual(b"This is plane 2 - 2", self.client.getUserData(plane2, 0, self.client.getUserDataId(plane2, 0, "MyKey2"))) - self.assertEqual(b"This is plane 2 - 3", self.client.getUserData(plane2, 0, self.client.getUserDataId(plane2, 0, "MyKey3"))) + self.assertEqual(b"This is plane 2 - 1", self.client.getUserData(self.client.getUserDataId(plane2, "MyKey1"))) + self.assertEqual(b"This is plane 2 - 2", self.client.getUserData(self.client.getUserDataId(plane2, "MyKey2"))) + self.assertEqual(b"This is plane 2 - 3", self.client.getUserData(self.client.getUserDataId(plane2, "MyKey3"))) def testMultipleLinks(self): @@ -198,14 +199,15 @@ class TestUserDataMethods(unittest.TestCase): self.assertTrue(num_links > 1) for link_index in range(num_links): - uid1 = self.client.addUserData(body_id, link_index, "MyKey1", "Value1 for link %s" % link_index) - uid2 = self.client.addUserData(body_id, link_index, "MyKey2", "Value2 for link %s" % link_index) + uid1 = self.client.addUserData(body_id, "MyKey1", "Value1 for link %s" % link_index, link_index) + uid2 = self.client.addUserData(body_id, "MyKey2", "Value2 for link %s" % link_index, link_index) for link_index in range(num_links): - uid1 = self.client.getUserDataId(body_id, link_index, "MyKey1") - uid2 = self.client.getUserDataId(body_id, link_index, "MyKey2") - self.assertEqual(("Value1 for link %s" % link_index).encode(), self.client.getUserData(body_id, link_index, uid1)) - self.assertEqual(("Value2 for link %s" % link_index).encode(), self.client.getUserData(body_id, link_index, uid2)) + uid1 = self.client.getUserDataId(body_id, "MyKey1", link_index) + uid2 = self.client.getUserDataId(body_id, "MyKey2", link_index) + self.assertEqual(("Value1 for link %s" % link_index).encode(), self.client.getUserData(uid1)) + self.assertEqual(("Value2 for link %s" % link_index).encode(), self.client.getUserData(uid2)) + def testMultipleClients(self): client1 = self.client @@ -215,25 +217,48 @@ class TestUserDataMethods(unittest.TestCase): client2.syncBodyInfo() # Add user data on client 1, check on client 1 - uid = client1.addUserData(plane_id, 0, "MyKey", "MyValue") - self.assertEqual(None, client2.getUserData(plane_id, 0, uid)) + uid = client1.addUserData(plane_id, "MyKey", "MyValue") + self.assertEqual(None, client2.getUserData(uid)) client2.syncUserData() - self.assertEqual(b"MyValue", client2.getUserData(plane_id, 0, uid)) + self.assertEqual(b"MyValue", client2.getUserData(uid)) # Overwrite the value on client 2, check on client 1 - client2.addUserData(plane_id, 0, "MyKey", "MyNewValue") - self.assertEqual(b"MyValue", client1.getUserData(plane_id, 0, uid)) + client2.addUserData(plane_id, "MyKey", "MyNewValue") + self.assertEqual(b"MyValue", client1.getUserData(uid)) client1.syncUserData() - self.assertEqual(b"MyNewValue", client1.getUserData(plane_id, 0, uid)) + self.assertEqual(b"MyNewValue", client1.getUserData(uid)) # Remove user data on client 1, check on client 2 - client1.removeUserData(plane_id, 0, uid) - self.assertEqual(b"MyNewValue", client2.getUserData(plane_id, 0, uid)) + client1.removeUserData(uid) + self.assertEqual(b"MyNewValue", client2.getUserData(uid)) client2.syncUserData() - self.assertEqual(None, client2.getUserData(plane_id, 0, uid)) + self.assertEqual(None, client2.getUserData(uid)) del client2 + def testUserDataOnVisualShapes(self): + body_id = self.client.loadURDF(ROBOT_PATH) + num_links = self.client.getNumJoints(body_id) + visual_shapes = self.client.getVisualShapeData(body_id) + + self.assertTrue(num_links > 0) + self.assertTrue(len(visual_shapes) > 0) + + user_data_entries = set() + for link_index in range(-1, num_links): + num_shapes = sum([1 for shape in visual_shapes if shape[1] == link_index]) + for shape_index in range(num_shapes): + key = "MyKey" + value = "MyValue %s, %s" % (link_index, shape_index) + uid = self.client.addUserData(body_id, key, value, link_index, shape_index) + user_data_entries.add((uid, key, value.encode(), body_id, link_index, shape_index)) + + self.assertEqual(len(visual_shapes), self.client.getNumUserData(body_id)) + for uid, key, value, body_id, link_index, shape_index in user_data_entries: + self.assertEqual(value, self.client.getUserData(uid)) + self.assertEqual(uid, self.client.getUserDataId(body_id, key, link_index, shape_index)) + + if __name__ == "__main__": unittest.main() From 2ce1e65b5367f8f1b9683fa9ca0b3bcbd44a569a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 3 Jul 2018 16:30:54 -0700 Subject: [PATCH 5/9] PyBullet: tweak in TCP server for VR - ROS bridge --- examples/SharedMemory/tcp/main.cpp | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/examples/SharedMemory/tcp/main.cpp b/examples/SharedMemory/tcp/main.cpp index 234b2312e..5bff8b464 100644 --- a/examples/SharedMemory/tcp/main.cpp +++ b/examples/SharedMemory/tcp/main.cpp @@ -177,22 +177,30 @@ int main(int argc, char *argv[]) SharedMemoryCommand cmd; SharedMemoryCommand* cmdPtr = 0; - + + int type = *(int*)&bytesReceived[0]; + //performance test if (numBytesRec == sizeof(int)) { cmdPtr = &cmd; cmd.m_type = *(int*)&bytesReceived[0]; } - - - if (numBytesRec == sizeof(SharedMemoryCommand)) - { - cmdPtr = (SharedMemoryCommand*)&bytesReceived[0]; - } else { - cmdPtr = (SharedMemoryCommand*)&bytesReceived[0]; + + if (numBytesRec == sizeof(SharedMemoryCommand)) + { + cmdPtr = (SharedMemoryCommand*)&bytesReceived[0]; + } + else + { + if (numBytesRec==36) + { + cmdPtr = &cmd; + memcpy(&cmd, &bytesReceived[0], numBytesRec); + } + } } if (cmdPtr) { From 7c7e8af717a3f3ca587810f8d71e04671c3ed8e9 Mon Sep 17 00:00:00 2001 From: Tigran Gasparian Date: Wed, 4 Jul 2018 10:22:44 +0200 Subject: [PATCH 6/9] Increases the shared memory magic number. --- examples/SharedMemory/SharedMemoryPublic.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 86d55db3c..1e4ba1f72 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -7,7 +7,8 @@ //Please don't replace an existing magic number: //instead, only ADD a new one at the top, comment-out previous one -#define SHARED_MEMORY_MAGIC_NUMBER 201806150 +#define SHARED_MEMORY_MAGIC_NUMBER 201807040 +//#define SHARED_MEMORY_MAGIC_NUMBER 201806150 //#define SHARED_MEMORY_MAGIC_NUMBER 201806020 //#define SHARED_MEMORY_MAGIC_NUMBER 201801170 //#define SHARED_MEMORY_MAGIC_NUMBER 201801080 From 8a6db042defbe27d1281edc6f2ac6e1817940d09 Mon Sep 17 00:00:00 2001 From: Tigran Gasparian Date: Wed, 4 Jul 2018 14:25:48 +0200 Subject: [PATCH 7/9] Removes int initializer in SharedMemoryUserDataHashKey, changes '>>' into '> >' for nexted templates. --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 +- examples/SharedMemory/SharedMemoryUserData.h | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 502d757f5..6284451d1 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1538,7 +1538,7 @@ struct PhysicsServerCommandProcessorInternalData b3ResizablePool< InternalBodyHandle > m_bodyHandles; b3ResizablePool m_userCollisionShapeHandles; b3ResizablePool m_userVisualShapeHandles; - b3ResizablePool> m_userDataHandles; + b3ResizablePool > m_userDataHandles; btHashMap m_userDataHandleLookup; b3PluginManager m_pluginManager; diff --git a/examples/SharedMemory/SharedMemoryUserData.h b/examples/SharedMemory/SharedMemoryUserData.h index 991714557..aabc48bff 100644 --- a/examples/SharedMemory/SharedMemoryUserData.h +++ b/examples/SharedMemory/SharedMemoryUserData.h @@ -48,15 +48,14 @@ struct SharedMemoryUserData } }; -class SharedMemoryUserDataHashKey { - unsigned int m_hash = 0; +struct SharedMemoryUserDataHashKey { + unsigned int m_hash; btHashString m_key; btHashInt m_bodyUniqueId; btHashInt m_linkIndex; btHashInt m_visualShapeIndex; -public: SIMD_FORCE_INLINE unsigned int getHash()const { return m_hash; } From 49684144dc18deedfeefe5557aaf26c0bca52c3b Mon Sep 17 00:00:00 2001 From: Tigran Gasparian Date: Thu, 5 Jul 2018 16:01:03 +0200 Subject: [PATCH 8/9] Some minor formatting fixes --- examples/SharedMemory/SharedMemoryCommands.h | 8 ++++---- examples/SharedMemory/SharedMemoryPublic.h | 1 - 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index e529f66b1..e92333b43 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -1004,9 +1004,9 @@ struct UserDataRequestArgs { struct UserDataResponseArgs { int m_userDataId; - int m_bodyUniqueId; - int m_linkIndex; - int m_visualShapeIndex; + int m_bodyUniqueId; + int m_linkIndex; + int m_visualShapeIndex; int m_valueType; int m_valueLength; char m_key[MAX_USER_DATA_KEY_LENGTH]; @@ -1017,7 +1017,7 @@ struct AddUserDataRequestArgs { int m_bodyUniqueId; int m_linkIndex; - int m_visualShapeIndex; + int m_visualShapeIndex; int m_valueType; int m_valueLength; char m_key[MAX_USER_DATA_KEY_LENGTH]; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 1e4ba1f72..0ad8b864f 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -275,7 +275,6 @@ struct b3JointInfo enum UserDataValueType { - USER_DATA_VALUE_TYPE_NOT_USET = -1, // Data represents generic byte array. USER_DATA_VALUE_TYPE_BYTES = 0, // Data represents C-string From 127b82ec1b1fef228eb309de6074fdfb3ce2a2d0 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 8 Jul 2018 11:23:12 +0200 Subject: [PATCH 9/9] expose texture unique id after loading URD file, so you can restore to the original texture after changing it to a custom texture. See also getTextureUid.py example. --- .../PhysicsServerCommandProcessor.cpp | 70 ++++++++++++++- examples/SharedMemory/SharedMemoryPublic.h | 9 ++ .../b3RobotSimulatorClientAPI_NoDirect.cpp | 86 ++++++++++++++++--- .../b3RobotSimulatorClientAPI_NoDirect.h | 30 ++++++- .../TinyRendererVisualShapeConverter.cpp | 18 ++-- examples/pybullet/examples/getTextureUid.py | 19 ++++ .../pybullet/examples/projective_texture.py | 4 +- examples/pybullet/pybullet.c | 17 +++- 8 files changed, 227 insertions(+), 26 deletions(-) create mode 100644 examples/pybullet/examples/getTextureUid.py diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d3c515834..de467657d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2719,7 +2719,8 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, SaveWorldObjectData sd; sd.m_fileName = fileName; - + int currentOpenGLTextureIndex = 0; + for (int m =0; mm_pluginManager.getRenderInterface()) + { + int totalNumVisualShapes = m_data->m_pluginManager.getRenderInterface()->getNumVisualShapes(bodyUniqueId); + //int totalBytesPerVisualShape = sizeof (b3VisualShapeData); + //int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1; + b3VisualShapeData tmpShape; + + int remain = totalNumVisualShapes - startShapeIndex; + int shapeIndex = startShapeIndex; + + int success = m_data->m_pluginManager.getRenderInterface()->getVisualShapesData(bodyUniqueId,shapeIndex,&tmpShape); + if (success) + { + if (tmpShape.m_tinyRendererTextureId>=0) + { + int openglTextureUniqueId = -1; + + //find companion opengl texture unique id and create a 'textureUid' + if (currentOpenGLTextureIndexm_textureHandles.allocHandle(); + InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle); + if(texH) + { + texH->m_tinyRendererTextureId = tmpShape.m_tinyRendererTextureId; + texH->m_openglTextureId = openglTextureUniqueId; + } + } + } + } + } } @@ -2893,8 +2932,16 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, int texId = u2b.getAllocatedTexture(i); m_data->m_allocatedTextures.push_back(texId); } - + +/* + int texHandle = m_data->m_textureHandles.allocHandle(); + InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle); + if(texH) + { + texH->m_tinyRendererTextureId = -1; + texH->m_openglTextureId = -1; + */ for (int i=0;im_tinyRendererTextureId>=0) + { + b3AlignedObjectArray usedHandles; + m_data->m_textureHandles.getUsedHandles(usedHandles); + + for (int i=0;im_textureHandles.getHandle(texHandle); + if (texH && (texH->m_tinyRendererTextureId == visualShapeStoragePtr->m_tinyRendererTextureId)) + { + visualShapeStoragePtr->m_openglTextureId =texH->m_openglTextureId; + visualShapeStoragePtr->m_textureUniqueId = texHandle; + } + } + } + serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1; serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1; serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 7e7338e36..cf6e0318e 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -595,6 +595,11 @@ typedef union { #define MAX_RAY_HITS MAX_RAY_INTERSECTION_BATCH_SIZE #define VISUAL_SHAPE_MAX_PATH_LEN 1024 +enum b3VisualShapeDataFlags +{ + eVISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS = 1, +}; + struct b3VisualShapeData { int m_objectUniqueId; @@ -605,6 +610,10 @@ struct b3VisualShapeData double m_localVisualFrame[7];//pos[3], orn[4] //todo: add more data if necessary (material color etc, although material can be in asset file .obj file) double m_rgbaColor[4]; + int m_tinyRendererTextureId; + int m_textureUniqueId; + int m_openglTextureId; + }; struct b3VisualShapeInformation diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index 496faea40..cf148fce5 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -155,6 +155,69 @@ btVector3 b3RobotSimulatorClientAPI_NoDirect::getEulerFromQuaternion(const btQua return rpy2; } +int b3RobotSimulatorClientAPI_NoDirect::loadTexture(const std::string& fileName) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return -1; + } + btAssert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + { + commandHandle = b3InitLoadTexture(m_data->m_physicsClientHandle, fileName.c_str()); + + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_LOAD_TEXTURE_COMPLETED) + { + return b3GetStatusTextureUniqueId(statusHandle); + } + } + return -1; +} + + +bool b3RobotSimulatorClientAPI_NoDirect::changeVisualShape(const struct b3RobotSimulatorChangeVisualShapeArgs& args) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + int objectUniqueId = args.m_objectUniqueId; + int jointIndex = args.m_linkIndex; + int shapeIndex = args.m_shapeIndex; + int textureUniqueId = args.m_textureUniqueId; + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUpdateVisualShape(m_data->m_physicsClientHandle, objectUniqueId, jointIndex, shapeIndex, textureUniqueId); + + if (args.m_hasSpecularColor) + { + double specularColor[3] = {args.m_specularColor[0],args.m_specularColor[1],args.m_specularColor[2]}; + b3UpdateVisualShapeSpecularColor(commandHandle,specularColor); + } + if (args.m_hasRgbaColor) + { + double rgbaColor[4] = {args.m_rgbaColor[0],args.m_rgbaColor[1],args.m_rgbaColor[2],args.m_rgbaColor[3]}; + b3UpdateVisualShapeRGBAColor(commandHandle,rgbaColor); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + statusType = b3GetStatusType(statusHandle); + + return (statusType == CMD_VISUAL_SHAPE_UPDATE_COMPLETED); +} + int b3RobotSimulatorClientAPI_NoDirect::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args) { int robotUniqueId = -1; @@ -2177,7 +2240,8 @@ void b3RobotSimulatorClientAPI_NoDirect::restoreStateFromMemory(int stateId) bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { + if (sm == 0) + { b3Warning("Not connected"); return false; } @@ -2185,17 +2249,17 @@ bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3 b3SharedMemoryStatusHandle statusHandle; int statusType; - { - commandHandle = b3InitRequestVisualShapeInformation(sm, bodyUniqueId); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); + commandHandle = b3InitRequestVisualShapeInformation(sm, bodyUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); - btAssert(statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED); - if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) { - b3GetVisualShapeInformation(sm, &visualShapeInfo); - } - return true; - } + btAssert(statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED); + if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) + { + b3GetVisualShapeInformation(sm, &visualShapeInfo); + return true; + } + return false; } void b3RobotSimulatorClientAPI_NoDirect::setAdditionalSearchPath(const std::string& path) diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index 5c8a6a424..a58b40d26 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -60,6 +60,30 @@ struct b3RobotSimulatorLoadFileResults } }; +struct b3RobotSimulatorChangeVisualShapeArgs +{ + int m_objectUniqueId; + int m_linkIndex; + int m_shapeIndex; + int m_textureUniqueId; + btVector4 m_rgbaColor; + bool m_hasRgbaColor; + btVector3 m_specularColor; + bool m_hasSpecularColor; + + b3RobotSimulatorChangeVisualShapeArgs() + :m_objectUniqueId(-1), + m_linkIndex(-1), + m_shapeIndex(-1), + m_textureUniqueId(-1), + m_rgbaColor(0,0,0,1), + m_hasRgbaColor(false), + m_specularColor(1,1,1), + m_hasSpecularColor(false) + { + } +}; + struct b3RobotSimulatorJointMotorArgs { int m_controlMode; @@ -505,6 +529,10 @@ public: bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); bool saveBullet(const std::string& fileName); + int loadTexture(const std::string& fileName); + + bool changeVisualShape(const struct b3RobotSimulatorChangeVisualShapeArgs& args); + bool savePythonWorld(const std::string& fileName); bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo); @@ -648,7 +676,7 @@ public: bool getCollisionShapeData(int bodyUniqueId, int linkIndex, b3CollisionShapeInformation &collisionShapeInfo); - bool getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo); + bool getVisualShapeData(int bodyUniqueId, struct b3VisualShapeInformation &visualShapeInfo); int saveStateToMemory(); void restoreStateFromMemory(int stateId); diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index 11f207132..132cc2def 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -671,13 +671,15 @@ void TinyRendererVisualShapeConverter::convertVisualShapes( visualShape.m_rgbaColor[1] = rgbaColor[1]; visualShape.m_rgbaColor[2] = rgbaColor[2]; visualShape.m_rgbaColor[3] = rgbaColor[3]; + visualShape.m_openglTextureId = -1; + visualShape.m_tinyRendererTextureId = -1; + visualShape.m_textureUniqueId = -1; { B3_PROFILE("convertURDFToVisualShape"); convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices, textures, visualShape); } - m_data->m_visualShapes.push_back(visualShape); - + if (vertices.size() && indices.size()) { TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId, linkIndex); @@ -701,13 +703,15 @@ void TinyRendererVisualShapeConverter::convertVisualShapes( } visuals->m_renderObjects.push_back(tinyObj); } + + btAssert(textures.size()<=1); for (int i=0;im_textures.size(); + m_data->m_textures.push_back(textures[i]); } + m_data->m_visualShapes.push_back(visualShape); + } } } @@ -1199,13 +1203,13 @@ void TinyRendererVisualShapeConverter::changeShapeTexture(int objectUniqueId, in for (int v = 0; v < visualArray->m_renderObjects.size(); v++) { TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v]; + if ((shapeIndex < 0) || (shapeIndex == v)) { renderObj->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData1, m_data->m_textures[textureUniqueId].m_width, m_data->m_textures[textureUniqueId].m_height); } } } - } } } diff --git a/examples/pybullet/examples/getTextureUid.py b/examples/pybullet/examples/getTextureUid.py new file mode 100644 index 000000000..50eda65e9 --- /dev/null +++ b/examples/pybullet/examples/getTextureUid.py @@ -0,0 +1,19 @@ +import pybullet as p +p.connect(p.GUI) +plane = p.loadURDF("plane.urdf") +visualData = p.getVisualShapeData(plane, p.VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS) +print(visualData) +curTexUid = visualData[0][8] +print(curTexUid) +texUid = p.loadTexture("tex256.png") +print("texUid=",texUid) + +p.changeVisualShape(plane,-1,textureUniqueId=texUid) + +for i in range (100): + p.getCameraImage(320,200) +p.changeVisualShape(plane,-1,textureUniqueId=curTexUid) + +for i in range (100): + p.getCameraImage(320,200) + diff --git a/examples/pybullet/examples/projective_texture.py b/examples/pybullet/examples/projective_texture.py index c4b21b9d0..5c3f2ffb5 100644 --- a/examples/pybullet/examples/projective_texture.py +++ b/examples/pybullet/examples/projective_texture.py @@ -13,8 +13,8 @@ bearStartPos2 = [0,0,0] bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0]) bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2) textureId = p.loadTexture("checker_grid.jpg") -p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId) -p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId) +#p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId) +#p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId) useRealTimeSimulation = 1 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b7c704305..d4b452ba1 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -5428,8 +5428,10 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO PyObject* pyResultList = 0; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char* kwlist[] = {"objectUniqueId", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &objectUniqueId, &physicsClientId)) + int flags=0; + + static char* kwlist[] = {"objectUniqueId", "flags", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|ii", kwlist, &objectUniqueId, &flags, &physicsClientId)) { return NULL; } @@ -5450,7 +5452,9 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO pyResultList = PyTuple_New(visualShapeInfo.m_numVisualShapes); for (i = 0; i < visualShapeInfo.m_numVisualShapes; i++) { - PyObject* visualShapeObList = PyTuple_New(8); + int numFields = flags&eVISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS ? 9 : 8; + + PyObject* visualShapeObList = PyTuple_New(numFields); PyObject* item; item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_objectUniqueId); PyTuple_SetItem(visualShapeObList, 0, item); @@ -5511,6 +5515,11 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO PyTuple_SetItem(rgba, 3, item); PyTuple_SetItem(visualShapeObList, 7, rgba); } + if (flags&eVISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS) + { + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_textureUniqueId); + PyTuple_SetItem(visualShapeObList, 8, item); + } PyTuple_SetItem(pyResultList, i, visualShapeObList); } @@ -9656,6 +9665,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS", URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS); + PyModule_AddIntConstant(m, "VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS", eVISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS); + PyModule_AddIntConstant(m, "MAX_RAY_INTERSECTION_BATCH_SIZE", MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING); PyModule_AddIntConstant(m, "B3G_F1", B3G_F1);