From 34db76d2635cd4a127a3b8e80733ff551fd795de Mon Sep 17 00:00:00 2001 From: Tigran Gasparian Date: Thu, 23 Jan 2020 14:46:33 +0100 Subject: [PATCH 1/2] Adds an option to syncUserData to specify the bodies for which to sync. --- examples/SharedMemory/PhysicsClientC_API.cpp | 11 +++++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../PhysicsServerCommandProcessor.cpp | 19 +++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 9 ++++++ examples/SharedMemory/SharedMemoryPublic.h | 1 + examples/pybullet/pybullet.c | 31 +++++++++++++++---- examples/pybullet/unittests/userDataTest.py | 2 +- 7 files changed, 66 insertions(+), 8 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index ac863421d..687ee105a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3714,9 +3714,20 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsCli b3Assert(command); command->m_type = CMD_SYNC_USER_DATA; + command->m_syncUserDataRequestArgs.m_numRequestedBodies = 0; return (b3SharedMemoryCommandHandle)command; } +B3_SHARED_API void b3AddBodyToSyncUserDataRequest(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_SYNC_USER_DATA); + + command->m_syncUserDataRequestArgs.m_requestedBodyIds[command->m_syncUserDataRequestArgs.m_numRequestedBodies++] = bodyUniqueId; +} + + 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; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 52df10696..529edd2e8 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -130,6 +130,7 @@ extern "C" ///user data handling B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsClientHandle physClient); + B3_SHARED_API void b3AddBodyToSyncUserDataRequest(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId); 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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 3fe9940dc..e51ebfcd6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6139,7 +6139,24 @@ bool PhysicsServerCommandProcessor::processSyncUserDataCommand(const struct Shar BT_PROFILE("CMD_SYNC_USER_DATA"); b3AlignedObjectArray userDataHandles; - m_data->m_userDataHandles.getUsedHandles(userDataHandles); + if (clientCmd.m_syncUserDataRequestArgs.m_numRequestedBodies == 0) + { + m_data->m_userDataHandles.getUsedHandles(userDataHandles); + } + else + { + for (int i=0; im_bodyHandles.getHandle(bodyUniqueId); + if (!body) + { + return hasStatus; + } + for (int j=0; j < body->m_userDataHandles.size(); ++j) { + userDataHandles.push_back(body->m_userDataHandles[j]); + } + } + } if (userDataHandles.size()) { memcpy(bufferServerToClient, &userDataHandles[0], sizeof(int) * userDataHandles.size()); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 223a0a5d5..778baec8d 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -1044,6 +1044,14 @@ struct b3StateSerializationArguments int m_stateId; }; +struct SyncUserDataRequestArgs +{ + // The number of bodies for which we'd like to sync the user data of. When 0, all bodies are synced. + int m_numRequestedBodies; + // The body IDs for which we'd like to sync the user data of. + int m_requestedBodyIds[MAX_REQUESTED_BODIES_LENGTH]; +}; + struct SyncUserDataArgs { // User data identifiers stored in m_bulletStreamDataServerToClientRefactor @@ -1148,6 +1156,7 @@ struct SharedMemoryCommand struct b3CustomCommand m_customCommandArgs; struct b3StateSerializationArguments m_loadStateArguments; struct RequestCollisionShapeDataArgs m_requestCollisionShapeDataArguments; + struct SyncUserDataRequestArgs m_syncUserDataRequestArgs; struct UserDataRequestArgs m_userDataRequestArgs; struct AddUserDataRequestArgs m_addUserDataRequestArgs; struct UserDataRequestArgs m_removeUserDataRequestArgs; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 63a7cfa55..f0596ca6e 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -494,6 +494,7 @@ enum b3VREventType #define MAX_SDF_BODIES 512 #define MAX_USER_DATA_KEY_LENGTH 256 +#define MAX_REQUESTED_BODIES_LENGTH 256 enum b3VRButtonInfo { diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index ffffb4289..27ad500d9 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -752,14 +752,19 @@ static PyObject* pybullet_syncUserData(PyObject* self, PyObject* args, PyObject* { b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"physicsClientId", NULL}; + static char* kwlistSingleBody[] = {"bodyUniqueId", "physicsClientId", NULL}; + static char* kwlistMultipleBodies[] = {"bodyUniqueIds", "physicsClientId", NULL}; b3SharedMemoryCommandHandle command; b3SharedMemoryStatusHandle statusHandle; int statusType; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) - { - return NULL; + PyObject* bodyUniqueIdsObj = 0; + int requestedBodyUniqueId = -1; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|ii", kwlistSingleBody, &requestedBodyUniqueId, &physicsClientId)) { + PyErr_Clear(); + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|Oi", kwlistMultipleBodies, &bodyUniqueIdsObj, &physicsClientId)) + { + return NULL; + } } sm = getPhysicsClient(physicsClientId); if (sm == 0) @@ -769,6 +774,20 @@ static PyObject* pybullet_syncUserData(PyObject* self, PyObject* args, PyObject* } command = b3InitSyncUserDataCommand(sm); + if (bodyUniqueIdsObj) + { + PyObject *seq = PySequence_Fast(bodyUniqueIdsObj, "expected a sequence"); + int len = PySequence_Size(bodyUniqueIdsObj); + for (int i=0; i < len; ++i) + { + b3AddBodyToSyncUserDataRequest(command, pybullet_internalGetIntFromSequence(seq, i)); + } + } + else if (requestedBodyUniqueId != -1) + { + b3AddBodyToSyncUserDataRequest(command, requestedBodyUniqueId); + + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); @@ -11932,7 +11951,7 @@ static PyMethodDef SpamMethods[] = { "Update body and constraint/joint information, in case other clients made changes."}, {"syncUserData", (PyCFunction)pybullet_syncUserData, METH_VARARGS | METH_KEYWORDS, - "syncUserData(physicsClientId=0)\n" + "syncUserData(bodyUniqueIds=[], physicsClientId=0)\n" "Update user data, in case other clients made changes."}, {"addUserData", (PyCFunction)pybullet_addUserData, METH_VARARGS | METH_KEYWORDS, diff --git a/examples/pybullet/unittests/userDataTest.py b/examples/pybullet/unittests/userDataTest.py index f6dd5f443..1a10fcb90 100644 --- a/examples/pybullet/unittests/userDataTest.py +++ b/examples/pybullet/unittests/userDataTest.py @@ -28,7 +28,7 @@ class TestUserDataMethods(unittest.TestCase): def testLoadingUserDataFromURDF(self): body_id = self.client.loadURDF(OBJECT_WITH_USER_DATA_PATH) - self.client.syncUserData() + self.client.syncUserData(body_id) num_user_data = self.client.getNumUserData(body_id) self.assertEqual(num_user_data, 7) From 2403a22ebf0399949700f3b6c0b0de55929f8fcc Mon Sep 17 00:00:00 2001 From: Tigran Gasparian Date: Thu, 23 Jan 2020 16:58:32 +0100 Subject: [PATCH 2/2] Bump up shared memory magic number. --- examples/SharedMemory/SharedMemoryPublic.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index f0596ca6e..9fddb0fb9 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 201911280 +#define SHARED_MEMORY_MAGIC_NUMBER 202001230 +//#define SHARED_MEMORY_MAGIC_NUMBER 201911280 //#define SHARED_MEMORY_MAGIC_NUMBER 201911180 //#define SHARED_MEMORY_MAGIC_NUMBER 201909030 //#define SHARED_MEMORY_MAGIC_NUMBER 201908110