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/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/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/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..ab0314a1a 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; @@ -2719,7 +2650,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 +2863,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;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; 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 +5035,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 +5067,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 +5118,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 +8245,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); } @@ -9284,6 +9257,25 @@ bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const s shapeIndex, visualShapeStoragePtr); if (success) { + + //find the matching texture unique ids. + if (visualShapeStoragePtr->m_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; @@ -10495,6 +10487,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..e92333b43 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..9ad100bdd 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 @@ -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; @@ -595,6 +589,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 +604,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 @@ -743,6 +746,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/SharedMemoryUserData.h b/examples/SharedMemory/SharedMemoryUserData.h index a29f59eb0..aabc48bff 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,43 @@ struct SharedMemoryUserData } }; +struct SharedMemoryUserDataHashKey { + unsigned int m_hash; + + btHashString m_key; + btHashInt m_bodyUniqueId; + btHashInt m_linkIndex; + btHashInt m_visualShapeIndex; + + 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/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/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/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/SharedMemory/tcp/main.cpp b/examples/SharedMemory/tcp/main.cpp index 6c2df155d..5bff8b464 100644 --- a/examples/SharedMemory/tcp/main.cpp +++ b/examples/SharedMemory/tcp/main.cpp @@ -177,17 +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)) + 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) { @@ -207,7 +220,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 +247,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() ); } } 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/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/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) 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/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/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..836f06312 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); @@ -689,18 +713,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 +737,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 +755,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 +773,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 +793,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 +813,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 +821,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 +840,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 +859,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 +877,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 +886,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 +907,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; } } @@ -5428,8 +5453,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 +5477,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 +5540,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); } @@ -9131,28 +9165,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."}, @@ -9549,6 +9583,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); @@ -9656,6 +9698,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); 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()