From b6120e760ac2719f3fbc9f0f7aeeda00097fec09 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 2 Jun 2018 13:40:08 -0700 Subject: [PATCH] PyBullet.addUserData / getUserData / removeUserData / getUserDataId / getNumUserData / getUserDataInfo See examples/pybullet/examples/userData.py how to use it. TODO: add to PyBullet Quickstart Guide. Thanks to Tigran Gasparian for the contribution! --- examples/SharedMemory/PhysicsClient.h | 6 + examples/SharedMemory/PhysicsClientC_API.cpp | 97 ++++++ examples/SharedMemory/PhysicsClientC_API.h | 11 + .../PhysicsClientSharedMemory.cpp | 243 +++++++++++++-- .../SharedMemory/PhysicsClientSharedMemory.h | 5 + examples/SharedMemory/PhysicsDirect.cpp | 183 +++++++++++ examples/SharedMemory/PhysicsDirect.h | 9 +- examples/SharedMemory/PhysicsLoopBack.cpp | 16 + examples/SharedMemory/PhysicsLoopBack.h | 5 + .../PhysicsServerCommandProcessor.cpp | 228 +++++++++++++- .../PhysicsServerCommandProcessor.h | 4 + examples/SharedMemory/SharedMemoryCommands.h | 38 ++- examples/SharedMemory/SharedMemoryPublic.h | 40 ++- examples/pybullet/examples/userData.py | 110 +++++++ examples/pybullet/pybullet.c | 286 ++++++++++++++++++ src/LinearMath/btHashMap.h | 5 + 16 files changed, 1257 insertions(+), 29 deletions(-) create mode 100644 examples/pybullet/examples/userData.py diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 785e01e7b..7c97996df 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -73,6 +73,12 @@ 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; }; + + #endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 515fbe2da..0dc931a93 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2791,6 +2791,103 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsCli return (b3SharedMemoryCommandHandle) command; } +B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsClientHandle physClient) { + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_SYNC_USER_DATA; + return (b3SharedMemoryCommandHandle) command; +} + +B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char* key, UserDataValueType valueType, int valueLength, const void *valueData) { + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(strlen(key) < MAX_USER_DATA_KEY_LENGTH); + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + + command->m_type = CMD_ADD_USER_DATA; + command->m_addUserDataRequestArgs.m_bodyUniqueId = bodyUniqueId; + command->m_addUserDataRequestArgs.m_linkIndex = linkIndex; + command->m_addUserDataRequestArgs.m_valueType = valueType; + command->m_addUserDataRequestArgs.m_valueLength = valueLength; + strcpy(command->m_addUserDataRequestArgs.m_key, key); + cl->uploadBulletFileToSharedMemory((const char *)valueData, valueLength); + + return (b3SharedMemoryCommandHandle) command; +} + +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId) { + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + 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) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + if (cl) + { + return cl->getCachedUserData(bodyUniqueId, linkIndex, userDataId, *valueOut); + } + return false; +} + +B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char *key) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + if (cl) + { + return cl->getCachedUserDataId(bodyUniqueId, linkIndex, key); + } + return -1; +} + +B3_SHARED_API int b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHandle) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + if (status) + { + btAssert(status->m_type == CMD_ADD_USER_DATA_COMPLETED); + return status->m_userDataResponseArgs.m_userDataGlobalId.m_userDataId; + } + return -1; +} + +B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + if (cl) + { + return cl->getNumUserData(bodyUniqueId, linkIndex); + } + return 0; +} + +B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + if (cl) + { + cl->getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, keyOut, userDataIdOut); + } +} + + B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 341e04dbc..1776c1da6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -113,6 +113,17 @@ B3_SHARED_API int b3ComputeDofCount(b3PhysicsClientHandle physClient, int bodyUn ///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, struct b3JointInfo* info); +///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 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 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 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 B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index c78eff91c..0f2ec78c0 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -11,16 +11,34 @@ #include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h" #include "SharedMemoryBlock.h" #include "BodyJointInfoUtility.h" +#include "SharedMemoryUserData.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; + + ~BodyJointInfoCache() + { + } }; struct PhysicsClientSharedMemoryInternalData { @@ -28,32 +46,34 @@ struct PhysicsClientSharedMemoryInternalData { bool m_ownsSharedMemory; SharedMemoryBlock* m_testBlock1; - b3HashMap m_bodyJointMap; - b3HashMap m_userConstraintInfoMap; + btHashMap m_bodyJointMap; + btHashMap m_userConstraintInfoMap; - b3AlignedObjectArray m_debugLinesFrom; - b3AlignedObjectArray m_debugLinesTo; - b3AlignedObjectArray m_debugLinesColor; + btAlignedObjectArray m_debugLinesFrom; + btAlignedObjectArray m_debugLinesTo; + btAlignedObjectArray m_debugLinesColor; int m_cachedCameraPixelsWidth; int m_cachedCameraPixelsHeight; - b3AlignedObjectArray m_cachedCameraPixelsRGBA; - b3AlignedObjectArray m_cachedCameraDepthBuffer; - b3AlignedObjectArray m_cachedSegmentationMaskBuffer; + btAlignedObjectArray m_cachedCameraPixelsRGBA; + btAlignedObjectArray m_cachedCameraDepthBuffer; + btAlignedObjectArray m_cachedSegmentationMaskBuffer; - b3AlignedObjectArray m_cachedContactPoints; - b3AlignedObjectArray m_cachedOverlappingObjects; - b3AlignedObjectArray m_cachedVisualShapes; - b3AlignedObjectArray m_cachedCollisionShapes; + btAlignedObjectArray m_cachedContactPoints; + btAlignedObjectArray m_cachedOverlappingObjects; + btAlignedObjectArray m_cachedVisualShapes; + btAlignedObjectArray m_cachedCollisionShapes; - b3AlignedObjectArray m_cachedVREvents; - b3AlignedObjectArray m_cachedKeyboardEvents; - b3AlignedObjectArray m_cachedMouseEvents; - b3AlignedObjectArray m_cachedMassMatrix; - b3AlignedObjectArray m_raycastHits; + btAlignedObjectArray m_cachedVREvents; + btAlignedObjectArray m_cachedKeyboardEvents; + btAlignedObjectArray m_cachedMouseEvents; + btAlignedObjectArray m_cachedMassMatrix; + btAlignedObjectArray m_raycastHits; - b3AlignedObjectArray m_bodyIdsRequestInfo; - b3AlignedObjectArray m_constraintIdsRequestInfo; + btAlignedObjectArray m_bodyIdsRequestInfo; + btAlignedObjectArray m_constraintIdsRequestInfo; + + btAlignedObjectArray m_userDataIdsRequestInfo; SharedMemoryStatus m_tempBackupServerStatus; @@ -1285,6 +1305,33 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { { break; } + case CMD_SYNC_USER_DATA_FAILED: + { + b3Warning("Synchronizing user data failed."); + break; + } + case CMD_REQUEST_USER_DATA_FAILED: + { + b3Warning("Requesting 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_REQUEST_USER_DATA_COMPLETED: + case CMD_SYNC_USER_DATA_COMPLETED: + case CMD_REMOVE_USER_DATA_COMPLETED: + case CMD_ADD_USER_DATA_COMPLETED: + { + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); @@ -1336,6 +1383,94 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { } } + if (serverCmd.m_type == CMD_SYNC_USER_DATA_COMPLETED) { + B3_PROFILE("CMD_SYNC_USER_DATA_COMPLETED"); + 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_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(); + + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + command.m_type = CMD_REQUEST_USER_DATA; + command.m_userDataRequestArgs = userDataGlobalId; + submitClientCommand(command); + return 0; + } + } + + 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]; + 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) { + // Only replace the value. + userDataPtr->replaceValue(dataStream, serverCmd.m_userDataResponseArgs.m_valueLength, serverCmd.m_userDataResponseArgs.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); + } + } + } + + if (serverCmd.m_type == CMD_REQUEST_USER_DATA_COMPLETED) { + + 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]; + 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; + submitClientCommand(command); + return 0; + } + m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus; + } + + 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); + } + } + } + } + if (serverCmd.m_type == CMD_REMOVE_BODY_COMPLETED) { for (int i=0;im_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]; + 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 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]; + if (!userDataId) { + return -1; + } + return *userDataId; +} + +int PhysicsClientSharedMemory::getNumUserData(int bodyUniqueId, int linkIndex) 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(); +} + +void PhysicsClientSharedMemory::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const { + BodyJointInfoCache** 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()) + { + *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(); +} diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index ccc8c07ed..441fceb8d 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -83,6 +83,11 @@ 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; + }; #endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 969604f8c..6bf1004a0 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -13,11 +13,27 @@ #include "BodyJointInfoUtility.h" #include +#include "SharedMemoryUserData.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; + + ~BodyJointInfoCache2() { + } }; @@ -651,6 +667,38 @@ 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 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); + } + } +} + void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd) { switch (serverCmd.m_type) @@ -1056,6 +1104,73 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd { 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"); + 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) { + 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); + } + } + } + break; + } default: { //b3Warning("Unknown server status type"); @@ -1331,3 +1446,71 @@ 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]; + 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 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]; + if (!userDataId) { + return -1; + } + return *userDataId; +} + +int PhysicsDirect::getNumUserData(int bodyUniqueId, int linkIndex) 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(); +} + +void PhysicsDirect::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; + } + 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(); +} diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index f2699a6f5..b02c10da8 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -25,7 +25,9 @@ protected: 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(); @@ -110,6 +112,11 @@ 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; }; #endif //PHYSICS_DIRECT_H diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 0606588e6..b7a2dd13c 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -225,3 +225,19 @@ 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); +} + +int PhysicsLoopBack::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const { + return m_data->m_physicsClient->getCachedUserDataId(bodyUniqueId, linkIndex, key); +} + +int PhysicsLoopBack::getNumUserData(int bodyUniqueId, int linkIndex) const { + return m_data->m_physicsClient->getNumUserData(bodyUniqueId, linkIndex); +} + +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); +} + diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index f4591ed99..afa42817b 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -86,6 +86,11 @@ 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; }; #endif //PHYSICS_LOOP_BACK_H diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0978ab122..0e1501b8a 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -176,6 +176,74 @@ 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; @@ -191,7 +259,7 @@ struct InternalBodyData btAlignedObjectArray m_rigidBodyJoints; btAlignedObjectArray m_rigidBodyJointNames; btAlignedObjectArray m_rigidBodyLinkNames; - + btHashMap m_linkUserDataMap; #ifdef B3_ENABLE_TINY_AUDIO b3HashMap m_audioSources; @@ -216,6 +284,10 @@ struct InternalBodyData m_rigidBodyJoints.clear(); m_rigidBodyJointNames.clear(); m_rigidBodyLinkNames.clear(); + for(int i=0; i 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; km_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); + if (!userData) { + return hasStatus; + } + btAssert(bufferSizeInBytes >= userData->m_bytes.size()) + serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId = clientCmd.m_userDataRequestArgs; + 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; + + strcpy(serverStatusOut.m_userDataResponseArgs.m_key, userData->m_key.c_str()); + if (userData->m_bytes.size()) + { + memcpy(bufferServerToClient, &userData->m_bytes[0], userData->m_bytes.size()); + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processAddUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_ADD_USER_DATA"); + serverStatusOut.m_type = CMD_ADD_USER_DATA_FAILED; + + if (clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId< 0 || clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId >= m_data->m_bodyHandles.getNumHandles()) + { + 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); + } + + const int userDataId = (*userDataPtr)->add(clientCmd.m_addUserDataRequestArgs.m_key, + 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_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); + + // Keep bufferServerToClient as-is. + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRemoveUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + 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); + 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; + } + serverStatusOut.m_removeUserDataResponseArgs = clientCmd.m_removeUserDataRequestArgs; + serverStatusOut.m_type = CMD_REMOVE_USER_DATA_COMPLETED; + return hasStatus; +} + bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; @@ -9467,6 +9671,26 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = processCustomCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); break; } + case CMD_SYNC_USER_DATA: + { + hasStatus = processSyncUserDataCommand(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; + } default: { BT_PROFILE("CMD_UNKNOWN"); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 24f9f2401..903f2e4e1 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -83,6 +83,10 @@ protected: bool processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processSaveStateCommand(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 processRequestUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processAddUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRemoveUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); int extractCollisionShapes(const class btCollisionShape* colShape, const class btTransform& transform, struct b3CollisionShapeData* collisionShapeBuffer, int maxCollisionShapes); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index db7f1eac1..dd2434124 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -36,6 +36,7 @@ #define MAX_SDF_FILENAME_LENGTH 1024 #define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH #define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM +#define MAX_USER_DATA_KEY_LENGTH MAX_URDF_FILENAME_LENGTH struct TmpFloat3 { @@ -977,6 +978,32 @@ struct b3StateSerializationArguments int m_stateId; }; +struct SyncUserDataArgs +{ + // User data identifiers stored in m_bulletStreamDataServerToClientRefactor + // as as array of b3UserDataGlobalIdentifier objects + int m_numUserDataIdentifiers; +}; + +struct UserDataResponseArgs +{ + b3UserDataGlobalIdentifier m_userDataGlobalId; + int m_valueType; + int m_valueLength; + char m_key[MAX_USER_DATA_KEY_LENGTH]; + // Value data stored in m_bulletStreamDataServerToClientRefactor. +}; + +struct AddUserDataRequestArgs +{ + int m_bodyUniqueId; + int m_linkIndex; + int m_valueType; + int m_valueLength; + char m_key[MAX_USER_DATA_KEY_LENGTH]; + // Value data stored in m_bulletStreamDataServerToClientRefactor. +}; + struct SharedMemoryCommand { int m_type; @@ -1033,6 +1060,9 @@ struct SharedMemoryCommand struct b3CustomCommand m_customCommandArgs; struct b3StateSerializationArguments m_loadStateArguments; struct RequestCollisionShapeDataArgs m_requestCollisionShapeDataArguments; + struct b3UserDataGlobalIdentifier m_userDataRequestArgs; + struct AddUserDataRequestArgs m_addUserDataRequestArgs; + struct b3UserDataGlobalIdentifier m_removeUserDataRequestArgs; }; }; @@ -1055,11 +1085,6 @@ struct SendOverlappingObjectsArgs int m_numRemainingOverlappingObjects; }; - - - - - struct SharedMemoryStatus { int m_type; @@ -1110,6 +1135,9 @@ struct SharedMemoryStatus struct b3StateSerializationArguments m_saveStateResultArgs; struct b3LoadSoftBodyResultArgs m_loadSoftBodyResultArguments; struct SendCollisionShapeDataArgs m_sendCollisionShapeArgs; + struct SyncUserDataArgs m_syncUserDataArgs; + struct UserDataResponseArgs m_userDataResponseArgs; + struct b3UserDataGlobalIdentifier m_removeUserDataResponseArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index bfabbfb0c..4648314c7 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -5,7 +5,8 @@ ///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures ///my convention is year/month/day/rev -#define SHARED_MEMORY_MAGIC_NUMBER 201801170 +#define SHARED_MEMORY_MAGIC_NUMBER 201806020 +//#define SHARED_MEMORY_MAGIC_NUMBER 201801170 //#define SHARED_MEMORY_MAGIC_NUMBER 201801080 //#define SHARED_MEMORY_MAGIC_NUMBER 201801010 //#define SHARED_MEMORY_MAGIC_NUMBER 201710180 @@ -83,9 +84,14 @@ enum EnumSharedMemoryClientCommand CMD_SAVE_STATE, CMD_RESTORE_STATE, CMD_REQUEST_COLLISION_SHAPE_INFO, + + CMD_SYNC_USER_DATA, + CMD_REQUEST_USER_DATA, + CMD_ADD_USER_DATA, + CMD_REMOVE_USER_DATA, + //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, - }; enum EnumSharedMemoryServerStatus @@ -192,6 +198,15 @@ enum EnumSharedMemoryServerStatus CMD_COLLISION_SHAPE_INFO_FAILED, CMD_LOAD_SOFT_BODY_FAILED, CMD_LOAD_SOFT_BODY_COMPLETED, + + CMD_SYNC_USER_DATA_COMPLETED, + CMD_SYNC_USER_DATA_FAILED, + CMD_REQUEST_USER_DATA_COMPLETED, + CMD_REQUEST_USER_DATA_FAILED, + CMD_ADD_USER_DATA_COMPLETED, + CMD_ADD_USER_DATA_FAILED, + CMD_REMOVE_USER_DATA_COMPLETED, + CMD_REMOVE_USER_DATA_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; @@ -254,6 +269,27 @@ struct b3JointInfo }; +enum UserDataValueType { + // Data represents generic byte array. + USER_DATA_VALUE_TYPE_BYTES = 0, + // Data represents C-string + USER_DATA_VALUE_TYPE_STRING = 1, +}; + +struct b3UserDataValue +{ + int m_type; + int m_length; + char* m_data1; +}; + +struct b3UserDataGlobalIdentifier +{ + int m_bodyUniqueId; + int m_linkIndex; + int m_userDataId; +}; + struct b3UserConstraint { int m_parentBodyIndex; diff --git a/examples/pybullet/examples/userData.py b/examples/pybullet/examples/userData.py new file mode 100644 index 000000000..17080a4e5 --- /dev/null +++ b/examples/pybullet/examples/userData.py @@ -0,0 +1,110 @@ +import pybullet as pb +import time +from pybullet_utils import bullet_client + + +server = bullet_client.BulletClient(connection_mode=pb.GUI_SERVER) + + +print ("Connecting to bullet server") +CONNECTION_METHOD = pb.SHARED_MEMORY +client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD) + + +PLANE_PATH = "plane.urdf" +client.loadURDF(PLANE_PATH) + +client.setGravity(0, 0, -10) + +print ("Adding plane object") +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") + +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 ("Disconnecting") +del client + +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 ("Number of user data entries: %s" % client.getNumUserData(plane_id, 0)) + +print ("Overriding user data") +client.addUserData(plane_id, 0, "MyKey1", "MyNewValue") + +print ("Cached overridden data") +print (client.getUserData(plane_id, 0, MyKey1)) + + +print ("Disconnecting") +del client + +print ("Reconnecting") +client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD) + + +print ("Synced overridden data") +print (client.getUserData(plane_id, 0, MyKey1)) + +print ("Getting user data ID") +print ("Retrieved ID: %s, ID retrieved from addUserData: %s" % (client.getUserDataId(plane_id, 0, "MyKey2"), MyKey2)) + +print ("Removing user data") +client.removeUserData(plane_id, 0, MyKey2) + +print ("Retrieving cached removed data") +print (client.getUserData(plane_id, 0, MyKey2)) + +print ("Syncing") +client.syncUserData() + +print ("Retrieving removed removed data") +print (client.getUserData(plane_id, 0, 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)) + +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 ("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)) + +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)) + diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 7c4839308..bef819a0f 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -487,6 +487,21 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P sNumPhysicsClients++; return PyInt_FromLong(-1); } + + command = b3InitSyncUserDataCommand(sm); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + + if (statusType != CMD_SYNC_USER_DATA_COMPLETED) + { + printf("Connection terminated, couldn't get user data\n"); + b3DisconnectSharedMemory(sm); + sm = 0; + sPhysicsClients1[freeIndex] = 0; + sPhysicsClientsGUI[freeIndex] = 0; + sNumPhysicsClients++; + return PyInt_FromLong(-1); + } } } else { @@ -635,6 +650,249 @@ static PyObject* pybullet_syncBodyInfo(PyObject* self, PyObject* args, PyObject* return Py_None; } +static PyObject* pybullet_syncUserData(PyObject* self, PyObject* args, PyObject* keywds) +{ + b3PhysicsClientHandle sm = 0; + int physicsClientId = 0; + static char* kwlist[] = {"physicsClientId", NULL}; + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + command = b3InitSyncUserDataCommand(sm); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + + if (statusType != CMD_SYNC_USER_DATA_COMPLETED) + { + PyErr_SetString(SpamError, "Error in syncUserInfo command."); + return NULL; + } + + Py_RETURN_NONE; +} + +static PyObject* pybullet_addUserData(PyObject* self, PyObject* args, PyObject* keywds) +{ + b3PhysicsClientHandle sm = 0; + int physicsClientId = 0; + int bodyUniqueId = -1; + int linkIndex = -2; + 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}; + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int userDataId; + int valueLen=-1; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiss|i", kwlist, &bodyUniqueId, &linkIndex, &key, &value, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + valueLen = strlen(value)+1; + command = b3InitAddUserDataCommand(sm, bodyUniqueId, linkIndex, key, USER_DATA_VALUE_TYPE_STRING, valueLen, value); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + + if (statusType != CMD_ADD_USER_DATA_COMPLETED) + { + PyErr_SetString(SpamError, "Error in addUserData command."); + return NULL; + } + + userDataId = b3GetUserDataIdFromStatus(statusHandle); + return PyInt_FromLong(userDataId); +} + +static PyObject* pybullet_removeUserData(PyObject* self, PyObject* args, PyObject* keywds) +{ + b3PhysicsClientHandle sm = 0; + int physicsClientId = 0; + int bodyUniqueId = -1; + int linkIndex = -1; + int userDataId = -1; + + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataId", "physicsClientId", NULL}; + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataId, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + command = b3InitRemoveUserDataCommand(sm, bodyUniqueId, linkIndex, userDataId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + + if (statusType != CMD_REMOVE_USER_DATA_COMPLETED) + { + PyErr_SetString(SpamError, "Error in removeUserData command."); + Py_RETURN_FALSE; + } + Py_RETURN_TRUE; +} + + +static PyObject* pybullet_getUserDataId(PyObject* self, PyObject* args, PyObject* keywds) +{ + b3PhysicsClientHandle sm = 0; + int physicsClientId = 0; + int bodyUniqueId = -1; + int linkIndex = -1; + const char* key = ""; + int userDataId; + + + + + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "key", "physicsClientId", NULL}; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iis|i", kwlist, &bodyUniqueId, &linkIndex, &key, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + userDataId = b3GetUserDataId(sm, bodyUniqueId, linkIndex, key); + return PyInt_FromLong(userDataId); +} + +static PyObject* pybullet_getUserData(PyObject* self, PyObject* args, PyObject* keywds) +{ + b3PhysicsClientHandle sm = 0; + int physicsClientId = 0; + int bodyUniqueId = -1; + int linkIndex = -1; + int userDataId = -1; + + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataId", "physicsClientId", NULL}; + + + + struct b3UserDataValue value; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataId, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + + if (!b3GetUserData(sm, bodyUniqueId, linkIndex, userDataId, &value)) { + Py_RETURN_NONE; + } + switch (value.m_type) { + case USER_DATA_VALUE_TYPE_STRING: + return PyString_FromString((const char *)value.m_data1); + default: + PyErr_SetString(SpamError, "User data value has unknown type"); + return NULL; + } +} + +static PyObject* pybullet_getNumUserData(PyObject* self, PyObject* args, PyObject* keywds) +{ + b3PhysicsClientHandle sm = 0; + int physicsClientId = 0; + int bodyUniqueId = -1; + int linkIndex = -1; + + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL}; + + + + int numUserData; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + numUserData = b3GetNumUserData(sm, bodyUniqueId, linkIndex); + return PyInt_FromLong(numUserData); +} + +static PyObject* pybullet_getUserDataInfo(PyObject* self, PyObject* args, PyObject* keywds) +{ + b3PhysicsClientHandle sm = 0; + int physicsClientId = 0; + int bodyUniqueId = -1; + int linkIndex = -1; + int userDataIndex = -1; + + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataIndex", "physicsClientId", NULL}; + + + const char* key = 0; + int userDataId = -1; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataIndex, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + b3GetUserDataInfo(sm, bodyUniqueId, linkIndex, userDataIndex, &key, &userDataId); + if (key == 0 || userDataId == -1) { + PyErr_SetString(SpamError, "Could not get user data info."); + Py_RETURN_NONE; + } + { + PyObject *userDataInfoTuple = PyTuple_New(2); + PyTuple_SetItem(userDataInfoTuple, 0, PyInt_FromLong(userDataId)); + PyTuple_SetItem(userDataInfoTuple, 1, PyString_FromString(key)); + return userDataInfoTuple; + } +} static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args, PyObject* keywds) { @@ -8827,6 +9085,34 @@ static PyMethodDef SpamMethods[] = { "syncBodyInfo(physicsClientId=0)\n" "Update body and constraint/joint information, in case other clients made changes."}, + {"syncUserData", (PyCFunction)pybullet_syncUserData, METH_VARARGS | METH_KEYWORDS, + "syncUserData(physicsClientId=0)\n" + "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."}, + + {"getUserData", (PyCFunction)pybullet_getUserData, METH_VARARGS | METH_KEYWORDS, + "getUserData(bodyUniqueId, linkIndex, userDataId, physicsClientId=0)\n" + "Returns the user data value."}, + + {"removeUserData", (PyCFunction)pybullet_removeUserData, METH_VARARGS | METH_KEYWORDS, + "removeUserData(bodyUniqueId, linkIndex, 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."}, + + {"getNumUserData", (PyCFunction)pybullet_getNumUserData, METH_VARARGS | METH_KEYWORDS, + "getNumUserData(bodyUniqueId, linkIndex, physicsClientId=0)\n" + "Retrieves the number of user data entries in a link."}, + + {"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)."}, + {"removeBody", (PyCFunction)pybullet_removeBody, METH_VARARGS | METH_KEYWORDS, "Remove a body by its body unique id."}, diff --git a/src/LinearMath/btHashMap.h b/src/LinearMath/btHashMap.h index 5e9cdb605..f43e76a80 100644 --- a/src/LinearMath/btHashMap.h +++ b/src/LinearMath/btHashMap.h @@ -30,6 +30,11 @@ struct btHashString return m_hash; } + btHashString() + { + m_string=""; + m_hash=0; + } btHashString(const char* name) :m_string(name) {