Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

PyBullet.addUserData / getUserData / removeUserData / getUserDataId / getNumUserData / getUserDataInfo

Browse Source 
See examples/pybullet/examples/userData.py how to use it. TODO: add to PyBullet Quickstart Guide.
Thanks to Tigran Gasparian for the contribution!
This commit is contained in:
erwincoumans
2018-06-02 13:40:08 -07:00
parent cb6b7a7c38
commit b6120e760a
16 changed files with 1257 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/SharedMemory/PhysicsClient.h
View File

@@ -73,6 +73,12 @@ public:
virtual void setTimeOut(double timeOutInSeconds) = 0;
virtual double getTimeOut() const = 0;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const = 0;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const = 0;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const = 0;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const = 0;
};
#endif // BT_PHYSICS_CLIENT_API_H

97
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2791,6 +2791,103 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsCli
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsClientHandle physClient) {
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_SYNC_USER_DATA;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char* key, UserDataValueType valueType, int valueLength, const void *valueData) {
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(strlen(key) < MAX_USER_DATA_KEY_LENGTH);
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_ADD_USER_DATA;
command->m_addUserDataRequestArgs.m_bodyUniqueId = bodyUniqueId;
command->m_addUserDataRequestArgs.m_linkIndex = linkIndex;
command->m_addUserDataRequestArgs.m_valueType = valueType;
command->m_addUserDataRequestArgs.m_valueLength = valueLength;
strcpy(command->m_addUserDataRequestArgs.m_key, key);
cl->uploadBulletFileToSharedMemory((const char *)valueData, valueLength);
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId) {
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REMOVE_USER_DATA;
command->m_removeUserDataRequestArgs.m_bodyUniqueId = bodyUniqueId;
command->m_removeUserDataRequestArgs.m_linkIndex = linkIndex;
command->m_removeUserDataRequestArgs.m_userDataId = userDataId;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue *valueOut)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
return cl->getCachedUserData(bodyUniqueId, linkIndex, userDataId, *valueOut);
}
return false;
}
B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char *key)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
return cl->getCachedUserDataId(bodyUniqueId, linkIndex, key);
}
return -1;
}
B3_SHARED_API int b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status)
{
btAssert(status->m_type == CMD_ADD_USER_DATA_COMPLETED);
return status->m_userDataResponseArgs.m_userDataGlobalId.m_userDataId;
}
return -1;
}
B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
return cl->getNumUserData(bodyUniqueId, linkIndex);
}
return 0;
}
B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
cl->getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, keyOut, userDataIdOut);
}
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;

11
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -113,6 +113,17 @@ B3_SHARED_API int b3ComputeDofCount(b3PhysicsClientHandle physClient, int bodyUn
///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, struct b3JointInfo* info);
///user data handling
B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char* key, enum UserDataValueType valueType, int valueLength, const void *valueData);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId);
B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue *valueOut);
B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char *key);
B3_SHARED_API int b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut);
B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h
B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info);

243
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -11,16 +11,34 @@
#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h"
#include "SharedMemoryBlock.h"
#include "BodyJointInfoUtility.h"
#include "SharedMemoryUserData.h"
struct UserDataCache
{
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
btHashMap<btHashString, int> m_keyToUserDataIdMap;
UserDataCache()
{
}
~UserDataCache()
{
}
};
struct BodyJointInfoCache
{
std::string m_baseName;
b3AlignedObjectArray<b3JointInfo> m_jointInfo;
std::string m_bodyName;
// Joint index -> user data.
btHashMap<btHashInt, UserDataCache> m_jointToUserDataMap;
~BodyJointInfoCache()
{
}
};
struct PhysicsClientSharedMemoryInternalData {
@@ -28,32 +46,34 @@ struct PhysicsClientSharedMemoryInternalData {
bool m_ownsSharedMemory;
SharedMemoryBlock* m_testBlock1;
b3HashMap<b3HashInt,BodyJointInfoCache*> m_bodyJointMap;
b3HashMap<b3HashInt,b3UserConstraint> m_userConstraintInfoMap;
btHashMap<btHashInt,BodyJointInfoCache*> m_bodyJointMap;
btHashMap<btHashInt,b3UserConstraint> m_userConstraintInfoMap;
b3AlignedObjectArray<TmpFloat3> m_debugLinesFrom;
b3AlignedObjectArray<TmpFloat3> m_debugLinesTo;
b3AlignedObjectArray<TmpFloat3> m_debugLinesColor;
btAlignedObjectArray<TmpFloat3> m_debugLinesFrom;
btAlignedObjectArray<TmpFloat3> m_debugLinesTo;
btAlignedObjectArray<TmpFloat3> m_debugLinesColor;
int m_cachedCameraPixelsWidth;
int m_cachedCameraPixelsHeight;
b3AlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
b3AlignedObjectArray<float> m_cachedCameraDepthBuffer;
b3AlignedObjectArray<int> m_cachedSegmentationMaskBuffer;
btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
btAlignedObjectArray<int> m_cachedSegmentationMaskBuffer;
b3AlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
b3AlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
b3AlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
b3AlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
btAlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;
b3AlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
b3AlignedObjectArray<b3KeyboardEvent> m_cachedKeyboardEvents;
b3AlignedObjectArray<b3MouseEvent> m_cachedMouseEvents;
b3AlignedObjectArray<double> m_cachedMassMatrix;
b3AlignedObjectArray<b3RayHitInfo> m_raycastHits;
btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
btAlignedObjectArray<b3KeyboardEvent> m_cachedKeyboardEvents;
btAlignedObjectArray<b3MouseEvent> m_cachedMouseEvents;
btAlignedObjectArray<double> m_cachedMassMatrix;
btAlignedObjectArray<b3RayHitInfo> m_raycastHits;
b3AlignedObjectArray<int> m_bodyIdsRequestInfo;
b3AlignedObjectArray<int> m_constraintIdsRequestInfo;
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
btAlignedObjectArray<int> m_constraintIdsRequestInfo;
btAlignedObjectArray<b3UserDataGlobalIdentifier> m_userDataIdsRequestInfo;
SharedMemoryStatus m_tempBackupServerStatus;
@@ -1285,6 +1305,33 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
{
break;
}
case CMD_SYNC_USER_DATA_FAILED:
{
b3Warning("Synchronizing user data failed.");
break;
}
case CMD_REQUEST_USER_DATA_FAILED:
{
b3Warning("Requesting user data failed");
break;
}
case CMD_ADD_USER_DATA_FAILED:
{
b3Warning("Adding user data failed (do the specified body and link exist?)");
break;
}
case CMD_REMOVE_USER_DATA_FAILED:
{
b3Warning("Removing user data failed");
break;
}
case CMD_REQUEST_USER_DATA_COMPLETED:
case CMD_SYNC_USER_DATA_COMPLETED:
case CMD_REMOVE_USER_DATA_COMPLETED:
case CMD_ADD_USER_DATA_COMPLETED:
{
break;
}
default: {
b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0);
@@ -1336,6 +1383,94 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
}
if (serverCmd.m_type == CMD_SYNC_USER_DATA_COMPLETED) {
B3_PROFILE("CMD_SYNC_USER_DATA_COMPLETED");
const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers;
if (numIdentifiers > 0) {
m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus;
const b3UserDataGlobalIdentifier *identifiers = (b3UserDataGlobalIdentifier *)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
for (int i=0; i<numIdentifiers; i++) {
m_data->m_userDataIdsRequestInfo.push_back(identifiers[i]);
}
// Request individual user data entries.
const b3UserDataGlobalIdentifier userDataGlobalId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size()-1];
m_data->m_userDataIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
command.m_type = CMD_REQUEST_USER_DATA;
command.m_userDataRequestArgs = userDataGlobalId;
submitClientCommand(command);
return 0;
}
}
if (serverCmd.m_type == CMD_ADD_USER_DATA_COMPLETED || serverCmd.m_type == CMD_REQUEST_USER_DATA_COMPLETED) {
B3_PROFILE("CMD_ADD_USER_DATA_COMPLETED");
const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_userDataResponseArgs.m_userDataGlobalId;
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) {
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
if (!userDataCachePtr)
{
UserDataCache cache;
(*bodyJointsPtr)->m_jointToUserDataMap.insert(userDataGlobalId.m_linkIndex, cache);
}
userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
const char *dataStream = m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
if (userDataPtr) {
// Only replace the value.
userDataPtr->replaceValue(dataStream, serverCmd.m_userDataResponseArgs.m_valueLength, serverCmd.m_userDataResponseArgs.m_valueType);
}
else {
// Add a new user data entry.
const char *key = serverCmd.m_userDataResponseArgs.m_key;
(userDataCachePtr)->m_userDataMap.insert(userDataGlobalId.m_userDataId, SharedMemoryUserData(key));
(userDataCachePtr)->m_keyToUserDataIdMap.insert(key, userDataGlobalId.m_userDataId);
userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
userDataPtr->replaceValue(dataStream, serverCmd.m_userDataResponseArgs.m_valueLength, serverCmd.m_userDataResponseArgs.m_valueType);
}
}
}
if (serverCmd.m_type == CMD_REQUEST_USER_DATA_COMPLETED) {
if (m_data->m_userDataIdsRequestInfo.size() > 0) {
// Request individual user data entries.
const b3UserDataGlobalIdentifier userDataGlobalId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size()-1];
m_data->m_userDataIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
command.m_type = CMD_REQUEST_USER_DATA;
command.m_userDataRequestArgs = userDataGlobalId;
submitClientCommand(command);
return 0;
}
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
}
if (serverCmd.m_type == CMD_REMOVE_USER_DATA_COMPLETED) {
B3_PROFILE("CMD_REMOVE_USER_DATA_COMPLETED");
const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_removeUserDataResponseArgs;
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) {
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
if (userDataCachePtr)
{
SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
if (userDataPtr)
{
(userDataCachePtr)->m_keyToUserDataIdMap.remove((userDataPtr)->m_key.c_str());
(userDataCachePtr)->m_userDataMap.remove(userDataGlobalId.m_userDataId);
}
}
}
}
if (serverCmd.m_type == CMD_REMOVE_BODY_COMPLETED)
{
for (int i=0;i<serverCmd.m_removeObjectArgs.m_numBodies;i++)
@@ -1663,3 +1798,73 @@ 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];
if (!userDataPtr)
{
return false;
}
valueOut.m_type = (userDataPtr)->m_type;
valueOut.m_length = userDataPtr->m_bytes.size();
valueOut.m_data1 = userDataPtr->m_bytes.size()? &userDataPtr->m_bytes[0] : 0;
return true;
}
int PhysicsClientSharedMemory::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return -1;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr)
{
return -1;
}
int *userDataId = (userDataCachePtr)->m_keyToUserDataIdMap[key];
if (!userDataId) {
return -1;
}
return *userDataId;
}
int PhysicsClientSharedMemory::getNumUserData(int bodyUniqueId, int linkIndex) const {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return 0;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr)
{
return 0;
}
return (userDataCachePtr)->m_userDataMap.size();
}
void PhysicsClientSharedMemory::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr))
{
*keyOut = 0;
*userDataIdOut = -1;
return;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr || userDataIndex >= (userDataCachePtr)->m_userDataMap.size())
{
*keyOut = 0;
*userDataIdOut = -1;
return;
}
*userDataIdOut = (userDataCachePtr)->m_userDataMap.getKeyAtIndex(userDataIndex).getUid1();
SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex);
*keyOut = (userDataPtr)->m_key.c_str();
}

5
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -83,6 +83,11 @@ public:
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
};
#endif // BT_PHYSICS_CLIENT_API_H

183
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -13,11 +13,27 @@
#include "BodyJointInfoUtility.h"
#include <string>
#include "SharedMemoryUserData.h"
struct UserDataCache {
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
btHashMap<btHashString, int> m_keyToUserDataIdMap;
~UserDataCache() {
}
};
struct BodyJointInfoCache2
{
std::string m_baseName;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
std::string m_bodyName;
// Joint index -> user data.
btHashMap<btHashInt, UserDataCache> m_jointToUserDataMap;
~BodyJointInfoCache2() {
}
};
@@ -651,6 +667,38 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
}
}
void PhysicsDirect::processAddUserData(const struct SharedMemoryStatus& serverCmd) {
const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_userDataResponseArgs.m_userDataGlobalId;
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
if (!userDataCachePtr)
{
UserDataCache cache;
(*bodyJointsPtr)->m_jointToUserDataMap.insert(userDataGlobalId.m_linkIndex, cache);
}
userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
const char *dataStream = m_data->m_bulletStreamDataServerToClient;
b3UserDataValue userDataValue;
userDataValue.m_type = serverCmd.m_userDataResponseArgs.m_valueType;
userDataValue.m_length = serverCmd.m_userDataResponseArgs.m_valueLength;
SharedMemoryUserData *userDataPtr = userDataCachePtr->m_userDataMap[userDataGlobalId.m_userDataId];
if (userDataPtr) {
// Only replace the value.
(userDataPtr)->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type);
}
else {
// Add a new user data entry.
(userDataCachePtr)->m_userDataMap.insert(userDataGlobalId.m_userDataId, SharedMemoryUserData(serverCmd.m_userDataResponseArgs.m_key));
userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
userDataPtr->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type);
}
}
}
void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd)
{
switch (serverCmd.m_type)
@@ -1056,6 +1104,73 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
{
break;
}
case CMD_SYNC_USER_DATA_FAILED:
{
b3Warning("Synchronizing user data failed.");
break;
}
case CMD_ADD_USER_DATA_FAILED:
{
b3Warning("Adding user data failed (do the specified body and link exist?)");
break;
}
case CMD_REMOVE_USER_DATA_FAILED:
{
b3Warning("Removing user data failed");
break;
}
case CMD_ADD_USER_DATA_COMPLETED:
{
processAddUserData(serverCmd);
break;
}
case CMD_SYNC_USER_DATA_COMPLETED:
{
B3_PROFILE("CMD_SYNC_USER_DATA_COMPLETED");
const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers;
b3UserDataGlobalIdentifier *identifiers = new b3UserDataGlobalIdentifier[numIdentifiers];
memcpy(identifiers, &m_data->m_bulletStreamDataServerToClient[0], numIdentifiers * sizeof(b3UserDataGlobalIdentifier));
for (int i=0; i<numIdentifiers; i++) {
m_data->m_tmpInfoRequestCommand.m_type = CMD_REQUEST_USER_DATA;
m_data->m_tmpInfoRequestCommand.m_userDataRequestArgs = identifiers[i];
bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
b3Clock clock;
double startTime = clock.getTimeInSeconds();
double timeOutInSeconds = m_data->m_timeOutInSeconds;
while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
{
hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
}
if (hasStatus)
{
processAddUserData(m_data->m_tmpInfoStatus);
}
}
delete[] identifiers;
break;
}
case CMD_REMOVE_USER_DATA_COMPLETED:
{
const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_removeUserDataResponseArgs;
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) {
UserDataCache *userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
if (userDataCachePtr)
{
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
if (userDataPtr) {
(userDataCachePtr)->m_keyToUserDataIdMap.remove((userDataPtr)->m_key.c_str());
(userDataCachePtr)->m_userDataMap.remove(userDataGlobalId.m_userDataId);
}
}
}
break;
}
default:
{
//b3Warning("Unknown server status type");
@@ -1331,3 +1446,71 @@ double PhysicsDirect::getTimeOut() const
{
return m_data->m_timeOutInSeconds;
}
bool PhysicsDirect::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return false;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr)
{
return false;
}
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataId];
if (!userDataPtr)
{
return false;
}
valueOut.m_type = userDataPtr->m_type;
valueOut.m_length = userDataPtr->m_bytes.size();
valueOut.m_data1 = userDataPtr->m_bytes.size()? &userDataPtr->m_bytes[0] : 0;
return true;
}
int PhysicsDirect::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return -1;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr) {
return -1;
}
int *userDataId = (userDataCachePtr)->m_keyToUserDataIdMap[key];
if (!userDataId) {
return -1;
}
return *userDataId;
}
int PhysicsDirect::getNumUserData(int bodyUniqueId, int linkIndex) const {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return 0;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr) {
return 0;
}
return (userDataCachePtr)->m_userDataMap.size();
}
void PhysicsDirect::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
*keyOut = 0;
*userDataIdOut = -1;
return;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr || userDataIndex >= (userDataCachePtr)->m_userDataMap.size())
{
*keyOut = 0;
*userDataIdOut = -1;
return;
}
*userDataIdOut = (userDataCachePtr)->m_userDataMap.getKeyAtIndex(userDataIndex).getUid1();
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex);
*keyOut = (userDataPtr)->m_key.c_str();
}

9
examples/SharedMemory/PhysicsDirect.h
View File

@@ -25,7 +25,9 @@ protected:
bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void processAddUserData(const struct SharedMemoryStatus& serverCmd);
void postProcessStatus(const struct SharedMemoryStatus& serverCmd);
void resetData();
@@ -110,6 +112,11 @@ public:
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
};
#endif //PHYSICS_DIRECT_H

16
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -225,3 +225,19 @@ double PhysicsLoopBack::getTimeOut() const
return m_data->m_physicsClient->getTimeOut();
}
bool PhysicsLoopBack::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const {
return m_data->m_physicsClient->getCachedUserData(bodyUniqueId, linkIndex, userDataId, valueOut);
}
int PhysicsLoopBack::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const {
return m_data->m_physicsClient->getCachedUserDataId(bodyUniqueId, linkIndex, key);
}
int PhysicsLoopBack::getNumUserData(int bodyUniqueId, int linkIndex) const {
return m_data->m_physicsClient->getNumUserData(bodyUniqueId, linkIndex);
}
void PhysicsLoopBack::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const {
m_data->m_physicsClient->getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, keyOut, userDataIdOut);
}

5
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -86,6 +86,11 @@ public:
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
};
#endif //PHYSICS_LOOP_BACK_H

228
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -176,6 +176,74 @@ struct InternalCollisionShapeData
}
};
#include "SharedMemoryUserData.h"
/**
* Holds all custom user data entries for a link.
*/
struct InternalLinkUserData {
// Used to look up user data entry handles for string keys.
btHashMap<btHashString, int> m_keyToHandleMap;
b3ResizablePool<b3PoolBodyHandle<SharedMemoryUserData> > 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<int> &userDataIds) const
{
m_userData.getUsedHandles(userDataIds);
}
};
struct InternalBodyData
{
btMultiBody* m_multiBody;
@@ -191,7 +259,7 @@ struct InternalBodyData
btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_rigidBodyJoints;
btAlignedObjectArray<std::string> m_rigidBodyJointNames;
btAlignedObjectArray<std::string> m_rigidBodyLinkNames;
btHashMap<btHashInt, InternalLinkUserData*> m_linkUserDataMap;
#ifdef B3_ENABLE_TINY_AUDIO
b3HashMap<btHashInt, SDFAudioSource> m_audioSources;
@@ -216,6 +284,10 @@ struct InternalBodyData
m_rigidBodyJoints.clear();
m_rigidBodyJointNames.clear();
m_rigidBodyLinkNames.clear();
for(int i=0; i<m_linkUserDataMap.size(); i++) {
delete *m_linkUserDataMap.getAtIndex(i);
}
m_linkUserDataMap.clear();
}
};
@@ -1484,7 +1556,6 @@ struct SaveStateData
btSerializer* m_serializer;
};
struct PhysicsServerCommandProcessorInternalData
{
///handle management
@@ -4726,6 +4797,139 @@ bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct Shar
return hasStatus;
}
bool PhysicsServerCommandProcessor::processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
BT_PROFILE("CMD_SYNC_USER_DATA");
b3UserDataGlobalIdentifier *userDataIdentifiers = (b3UserDataGlobalIdentifier *)bufferServerToClient;
int numIdentifiers = 0;
b3AlignedObjectArray<int> bodyHandles;
m_data->m_bodyHandles.getUsedHandles(bodyHandles);
for (int i=0; i<bodyHandles.size(); i++) {
int bodyHandle = bodyHandles[i];
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyHandle);
if (!body) {
continue;
}
for (int j=0; j<body->m_linkUserDataMap.size(); j++) {
const int linkIndex = body->m_linkUserDataMap.getKeyAtIndex(j).getUid1();
InternalLinkUserData **userDataPtr = body->m_linkUserDataMap.getAtIndex(j);
if (!userDataPtr) {
continue;
}
b3AlignedObjectArray<int> userDataIds;
(*userDataPtr)->getUserDataIds(userDataIds);
for (int k=0; k<userDataIds.size(); k++) {
b3UserDataGlobalIdentifier &identifier = userDataIdentifiers[numIdentifiers++];
identifier.m_bodyUniqueId = bodyHandle;
identifier.m_linkIndex = linkIndex;
identifier.m_userDataId = userDataIds[k];
}
}
}
serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = numIdentifiers;
serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED;
return hasStatus;
}
bool PhysicsServerCommandProcessor::processRequestUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
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);
if (!userData) {
return hasStatus;
}
btAssert(bufferSizeInBytes >= userData->m_bytes.size())
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId = clientCmd.m_userDataRequestArgs;
serverStatusOut.m_userDataResponseArgs.m_valueType = userData->m_type;
serverStatusOut.m_userDataResponseArgs.m_valueLength = userData->m_bytes.size();
serverStatusOut.m_type = CMD_REQUEST_USER_DATA_COMPLETED;
strcpy(serverStatusOut.m_userDataResponseArgs.m_key, userData->m_key.c_str());
if (userData->m_bytes.size())
{
memcpy(bufferServerToClient, &userData->m_bytes[0], userData->m_bytes.size());
}
return hasStatus;
}
bool PhysicsServerCommandProcessor::processAddUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
BT_PROFILE("CMD_ADD_USER_DATA");
serverStatusOut.m_type = CMD_ADD_USER_DATA_FAILED;
if (clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId< 0 || clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId >= m_data->m_bodyHandles.getNumHandles())
{
return hasStatus;
}
InternalBodyData *body = m_data->m_bodyHandles.getHandle(clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId);
if (!body) {
return hasStatus;
}
const int linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex;
InternalLinkUserData **userDataPtr = body->m_linkUserDataMap[linkIndex];
if (!userDataPtr) {
InternalLinkUserData *userData = new InternalLinkUserData;
userDataPtr = &userData;
body->m_linkUserDataMap.insert(linkIndex, userData);
}
const int userDataId = (*userDataPtr)->add(clientCmd.m_addUserDataRequestArgs.m_key,
bufferServerToClient,clientCmd.m_addUserDataRequestArgs.m_valueLength,
clientCmd.m_addUserDataRequestArgs.m_valueType);
serverStatusOut.m_type = CMD_ADD_USER_DATA_COMPLETED;
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_userDataId = userDataId;
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_bodyUniqueId = clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId;
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex;
serverStatusOut.m_userDataResponseArgs.m_valueLength = clientCmd.m_addUserDataRequestArgs.m_valueLength;
serverStatusOut.m_userDataResponseArgs.m_valueType = clientCmd.m_addUserDataRequestArgs.m_valueType;
strcpy(serverStatusOut.m_userDataResponseArgs.m_key, clientCmd.m_addUserDataRequestArgs.m_key);
// Keep bufferServerToClient as-is.
return hasStatus;
}
bool PhysicsServerCommandProcessor::processRemoveUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
BT_PROFILE("CMD_REMOVE_USER_DATA");
serverStatusOut.m_type = CMD_REMOVE_USER_DATA_FAILED;
InternalBodyData *body = m_data->m_bodyHandles.getHandle(clientCmd.m_removeUserDataRequestArgs.m_bodyUniqueId);
if (!body) {
return hasStatus;
}
const int linkIndex = clientCmd.m_removeUserDataRequestArgs.m_linkIndex;
InternalLinkUserData **userDataPtr = body->m_linkUserDataMap[linkIndex];
if (!userDataPtr) {
return hasStatus;
}
const bool removed = (*userDataPtr)->remove(clientCmd.m_removeUserDataRequestArgs.m_userDataId);
if (!removed) {
return hasStatus;
}
serverStatusOut.m_removeUserDataResponseArgs = clientCmd.m_removeUserDataRequestArgs;
serverStatusOut.m_type = CMD_REMOVE_USER_DATA_COMPLETED;
return hasStatus;
}
bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@@ -9467,6 +9671,26 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = processCustomCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_SYNC_USER_DATA:
{
hasStatus = processSyncUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REQUEST_USER_DATA:
{
hasStatus = processRequestUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_ADD_USER_DATA:
{
hasStatus = processAddUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REMOVE_USER_DATA:
{
hasStatus = processRemoveUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
default:
{
BT_PROFILE("CMD_UNKNOWN");

4
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -83,6 +83,10 @@ protected:
bool processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processSaveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processAddUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRemoveUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
int extractCollisionShapes(const class btCollisionShape* colShape, const class btTransform& transform, struct b3CollisionShapeData* collisionShapeBuffer, int maxCollisionShapes);

38
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -36,6 +36,7 @@
#define MAX_SDF_FILENAME_LENGTH 1024
#define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
#define MAX_USER_DATA_KEY_LENGTH MAX_URDF_FILENAME_LENGTH
struct TmpFloat3
{
@@ -977,6 +978,32 @@ struct b3StateSerializationArguments
int m_stateId;
};
struct SyncUserDataArgs
{
// User data identifiers stored in m_bulletStreamDataServerToClientRefactor
// as as array of b3UserDataGlobalIdentifier objects
int m_numUserDataIdentifiers;
};
struct UserDataResponseArgs
{
b3UserDataGlobalIdentifier m_userDataGlobalId;
int m_valueType;
int m_valueLength;
char m_key[MAX_USER_DATA_KEY_LENGTH];
// Value data stored in m_bulletStreamDataServerToClientRefactor.
};
struct AddUserDataRequestArgs
{
int m_bodyUniqueId;
int m_linkIndex;
int m_valueType;
int m_valueLength;
char m_key[MAX_USER_DATA_KEY_LENGTH];
// Value data stored in m_bulletStreamDataServerToClientRefactor.
};
struct SharedMemoryCommand
{
int m_type;
@@ -1033,6 +1060,9 @@ struct SharedMemoryCommand
struct b3CustomCommand m_customCommandArgs;
struct b3StateSerializationArguments m_loadStateArguments;
struct RequestCollisionShapeDataArgs m_requestCollisionShapeDataArguments;
struct b3UserDataGlobalIdentifier m_userDataRequestArgs;
struct AddUserDataRequestArgs m_addUserDataRequestArgs;
struct b3UserDataGlobalIdentifier m_removeUserDataRequestArgs;
};
};
@@ -1055,11 +1085,6 @@ struct SendOverlappingObjectsArgs
int m_numRemainingOverlappingObjects;
};
struct SharedMemoryStatus
{
int m_type;
@@ -1110,6 +1135,9 @@ struct SharedMemoryStatus
struct b3StateSerializationArguments m_saveStateResultArgs;
struct b3LoadSoftBodyResultArgs m_loadSoftBodyResultArguments;
struct SendCollisionShapeDataArgs m_sendCollisionShapeArgs;
struct SyncUserDataArgs m_syncUserDataArgs;
struct UserDataResponseArgs m_userDataResponseArgs;
struct b3UserDataGlobalIdentifier m_removeUserDataResponseArgs;
};
};

40
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -5,7 +5,8 @@
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
///my convention is year/month/day/rev
#define SHARED_MEMORY_MAGIC_NUMBER 201801170
#define SHARED_MEMORY_MAGIC_NUMBER 201806020
//#define SHARED_MEMORY_MAGIC_NUMBER 201801170
//#define SHARED_MEMORY_MAGIC_NUMBER 201801080
//#define SHARED_MEMORY_MAGIC_NUMBER 201801010
//#define SHARED_MEMORY_MAGIC_NUMBER 201710180
@@ -83,9 +84,14 @@ enum EnumSharedMemoryClientCommand
CMD_SAVE_STATE,
CMD_RESTORE_STATE,
CMD_REQUEST_COLLISION_SHAPE_INFO,
CMD_SYNC_USER_DATA,
CMD_REQUEST_USER_DATA,
CMD_ADD_USER_DATA,
CMD_REMOVE_USER_DATA,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
};
enum EnumSharedMemoryServerStatus
@@ -192,6 +198,15 @@ enum EnumSharedMemoryServerStatus
CMD_COLLISION_SHAPE_INFO_FAILED,
CMD_LOAD_SOFT_BODY_FAILED,
CMD_LOAD_SOFT_BODY_COMPLETED,
CMD_SYNC_USER_DATA_COMPLETED,
CMD_SYNC_USER_DATA_FAILED,
CMD_REQUEST_USER_DATA_COMPLETED,
CMD_REQUEST_USER_DATA_FAILED,
CMD_ADD_USER_DATA_COMPLETED,
CMD_ADD_USER_DATA_FAILED,
CMD_REMOVE_USER_DATA_COMPLETED,
CMD_REMOVE_USER_DATA_FAILED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};
@@ -254,6 +269,27 @@ struct b3JointInfo
};
enum UserDataValueType {
// Data represents generic byte array.
USER_DATA_VALUE_TYPE_BYTES = 0,
// Data represents C-string
USER_DATA_VALUE_TYPE_STRING = 1,
};
struct b3UserDataValue
{
int m_type;
int m_length;
char* m_data1;
};
struct b3UserDataGlobalIdentifier
{
int m_bodyUniqueId;
int m_linkIndex;
int m_userDataId;
};
struct b3UserConstraint
{
int m_parentBodyIndex;

110
examples/pybullet/examples/userData.py Normal file
View File

@@ -0,0 +1,110 @@
import pybullet as pb
import time
from pybullet_utils import bullet_client
server = bullet_client.BulletClient(connection_mode=pb.GUI_SERVER)
print ("Connecting to bullet server")
CONNECTION_METHOD = pb.SHARED_MEMORY
client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD)
PLANE_PATH = "plane.urdf"
client.loadURDF(PLANE_PATH)
client.setGravity(0, 0, -10)
print ("Adding plane object")
plane_id = client.loadURDF(PLANE_PATH)
print ("Plane ID: %s" % plane_id)
print ("Adding user data to plane")
MyKey1 = client.addUserData(plane_id, 0, "MyKey1", "MyValue1")
MyKey2 = client.addUserData(plane_id, 0, "MyKey2", "MyValue2")
MyKey3 = client.addUserData(plane_id, 0, "MyKey3", "MyValue3")
MyKey4 = client.addUserData(plane_id, 0, "MyKey4", "MyValue4")
print ("Retrieving cached user data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(plane_id, 0, MyKey2))
print (client.getUserData(plane_id, 0, MyKey3))
print (client.getUserData(plane_id, 0, MyKey4))
print ("Disconnecting")
del client
print ("Reconnecting")
client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD)
print ("Retrieving synced user data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(plane_id, 0, MyKey2))
print (client.getUserData(plane_id, 0, MyKey3))
print (client.getUserData(plane_id, 0, MyKey4))
print ("Number of user data entries: %s" % client.getNumUserData(plane_id, 0))
print ("Overriding user data")
client.addUserData(plane_id, 0, "MyKey1", "MyNewValue")
print ("Cached overridden data")
print (client.getUserData(plane_id, 0, MyKey1))
print ("Disconnecting")
del client
print ("Reconnecting")
client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD)
print ("Synced overridden data")
print (client.getUserData(plane_id, 0, MyKey1))
print ("Getting user data ID")
print ("Retrieved ID: %s, ID retrieved from addUserData: %s" % (client.getUserDataId(plane_id, 0, "MyKey2"), MyKey2))
print ("Removing user data")
client.removeUserData(plane_id, 0, MyKey2)
print ("Retrieving cached removed data")
print (client.getUserData(plane_id, 0, MyKey2))
print ("Syncing")
client.syncUserData()
print ("Retrieving removed removed data")
print (client.getUserData(plane_id, 0, MyKey2))
print ("Iterating over all user data entries and printing results")
for i in range(client.getNumUserData(plane_id, 0)):
userDataId, key = client.getUserDataInfo(plane_id, 0, i)
print ("Info: (%s, %s)" % (userDataId, key))
print ("Value: %s" % client.getUserData(plane_id, 0, userDataId))
print ("Removing body")
client.removeBody(plane_id)
print ("Retrieving user data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(plane_id, 0, MyKey3))
print (client.getUserData(plane_id, 0, MyKey4))
print ("Syncing")
client.syncUserData()
print ("Retrieving user data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(plane_id, 0, MyKey3))
print (client.getUserData(plane_id, 0, MyKey4))
plane_id2 = client.loadURDF(PLANE_PATH)
print ("Plane1: %s, plane2: %s" % (plane_id, plane_id2))
print ("Retrieving user data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(plane_id, 0, MyKey3))
print (client.getUserData(plane_id, 0, MyKey4))

286
examples/pybullet/pybullet.c
View File

@@ -487,6 +487,21 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
sNumPhysicsClients++;
return PyInt_FromLong(-1);
}
command = b3InitSyncUserDataCommand(sm);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_SYNC_USER_DATA_COMPLETED)
{
printf("Connection terminated, couldn't get user data\n");
b3DisconnectSharedMemory(sm);
sm = 0;
sPhysicsClients1[freeIndex] = 0;
sPhysicsClientsGUI[freeIndex] = 0;
sNumPhysicsClients++;
return PyInt_FromLong(-1);
}
}
} else
{
@@ -635,6 +650,249 @@ static PyObject* pybullet_syncBodyInfo(PyObject* self, PyObject* args, PyObject*
return Py_None;
}
static PyObject* pybullet_syncUserData(PyObject* self, PyObject* args, PyObject* keywds)
{
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"physicsClientId", NULL};
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
command = b3InitSyncUserDataCommand(sm);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_SYNC_USER_DATA_COMPLETED)
{
PyErr_SetString(SpamError, "Error in syncUserInfo command.");
return NULL;
}
Py_RETURN_NONE;
}
static PyObject* pybullet_addUserData(PyObject* self, PyObject* args, PyObject* keywds)
{
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -2;
const char* key = "";
const char* value = ""; // TODO: Change this to a PyObject and detect the type dynamically.
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "key", "value", "physicsClientId", NULL};
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int userDataId;
int valueLen=-1;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiss|i", kwlist, &bodyUniqueId, &linkIndex, &key, &value, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
valueLen = strlen(value)+1;
command = b3InitAddUserDataCommand(sm, bodyUniqueId, linkIndex, key, USER_DATA_VALUE_TYPE_STRING, valueLen, value);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_ADD_USER_DATA_COMPLETED)
{
PyErr_SetString(SpamError, "Error in addUserData command.");
return NULL;
}
userDataId = b3GetUserDataIdFromStatus(statusHandle);
return PyInt_FromLong(userDataId);
}
static PyObject* pybullet_removeUserData(PyObject* self, PyObject* args, PyObject* keywds)
{
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
int userDataId = -1;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataId", "physicsClientId", NULL};
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataId, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
command = b3InitRemoveUserDataCommand(sm, bodyUniqueId, linkIndex, userDataId);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_REMOVE_USER_DATA_COMPLETED)
{
PyErr_SetString(SpamError, "Error in removeUserData command.");
Py_RETURN_FALSE;
}
Py_RETURN_TRUE;
}
static PyObject* pybullet_getUserDataId(PyObject* self, PyObject* args, PyObject* keywds)
{
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
const char* key = "";
int userDataId;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "key", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iis|i", kwlist, &bodyUniqueId, &linkIndex, &key, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
userDataId = b3GetUserDataId(sm, bodyUniqueId, linkIndex, key);
return PyInt_FromLong(userDataId);
}
static PyObject* pybullet_getUserData(PyObject* self, PyObject* args, PyObject* keywds)
{
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
int userDataId = -1;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataId", "physicsClientId", NULL};
struct b3UserDataValue value;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataId, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (!b3GetUserData(sm, bodyUniqueId, linkIndex, userDataId, &value)) {
Py_RETURN_NONE;
}
switch (value.m_type) {
case USER_DATA_VALUE_TYPE_STRING:
return PyString_FromString((const char *)value.m_data1);
default:
PyErr_SetString(SpamError, "User data value has unknown type");
return NULL;
}
}
static PyObject* pybullet_getNumUserData(PyObject* self, PyObject* args, PyObject* keywds)
{
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL};
int numUserData;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
numUserData = b3GetNumUserData(sm, bodyUniqueId, linkIndex);
return PyInt_FromLong(numUserData);
}
static PyObject* pybullet_getUserDataInfo(PyObject* self, PyObject* args, PyObject* keywds)
{
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
int userDataIndex = -1;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataIndex", "physicsClientId", NULL};
const char* key = 0;
int userDataId = -1;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataIndex, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
b3GetUserDataInfo(sm, bodyUniqueId, linkIndex, userDataIndex, &key, &userDataId);
if (key == 0 || userDataId == -1) {
PyErr_SetString(SpamError, "Could not get user data info.");
Py_RETURN_NONE;
}
{
PyObject *userDataInfoTuple = PyTuple_New(2);
PyTuple_SetItem(userDataInfoTuple, 0, PyInt_FromLong(userDataId));
PyTuple_SetItem(userDataInfoTuple, 1, PyString_FromString(key));
return userDataInfoTuple;
}
}
static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args, PyObject* keywds)
{
@@ -8827,6 +9085,34 @@ static PyMethodDef SpamMethods[] = {
"syncBodyInfo(physicsClientId=0)\n"
"Update body and constraint/joint information, in case other clients made changes."},
{"syncUserData", (PyCFunction)pybullet_syncUserData, METH_VARARGS | METH_KEYWORDS,
"syncUserData(physicsClientId=0)\n"
"Update user data, in case other clients made changes."},
{"addUserData", (PyCFunction)pybullet_addUserData, METH_VARARGS | METH_KEYWORDS,
"addUserData(bodyUniqueId, linkIndex, key, value, physicsClientId=0)\n"
"Adds or updates a user data entry to a link. Returns user data identifier."},
{"getUserData", (PyCFunction)pybullet_getUserData, METH_VARARGS | METH_KEYWORDS,
"getUserData(bodyUniqueId, linkIndex, userDataId, physicsClientId=0)\n"
"Returns the user data value."},
{"removeUserData", (PyCFunction)pybullet_removeUserData, METH_VARARGS | METH_KEYWORDS,
"removeUserData(bodyUniqueId, linkIndex, userDataId, physicsClientId=0)\n"
"Removes a user data entry."},
{"getUserDataId", (PyCFunction)pybullet_getUserDataId, METH_VARARGS | METH_KEYWORDS,
"getUserDataId(bodyUniqueId, linkIndex, key, physicsClientId=0)\n"
"Retrieves the userDataId on a link given the key."},
{"getNumUserData", (PyCFunction)pybullet_getNumUserData, METH_VARARGS | METH_KEYWORDS,
"getNumUserData(bodyUniqueId, linkIndex, physicsClientId=0)\n"
"Retrieves the number of user data entries in a link."},
{"getUserDataInfo", (PyCFunction)pybullet_getUserDataInfo, METH_VARARGS | METH_KEYWORDS,
"getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, physicsClientId=0)\n"
"Retrieves the key and the identifier of a user data as (id, key)."},
{"removeBody", (PyCFunction)pybullet_removeBody, METH_VARARGS | METH_KEYWORDS,
"Remove a body by its body unique id."},

5
src/LinearMath/btHashMap.h
View File

@@ -30,6 +30,11 @@ struct btHashString
return m_hash;
}
btHashString()
{
m_string="";
m_hash=0;
}
btHashString(const char* name)
:m_string(name)
{