From 5d66ce20e0d8dfec2ad03aa331155347405205f0 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 4 Nov 2016 17:06:55 -0700 Subject: [PATCH] network UDP: transmit structural DNA to deal with version/platform differences. pybullet: allow to specify shared memory key and hostname/port for UDP. --- examples/SharedMemory/PhysicsDirect.cpp | 51 ++++++++++++++++++- .../PhysicsServerCommandProcessor.cpp | 22 ++++++++ examples/SharedMemory/SharedMemoryPublic.h | 3 ++ examples/pybullet/pybullet.c | 49 +++++++++++++++--- src/LinearMath/btSerializer.h | 20 ++++++++ 5 files changed, 135 insertions(+), 10 deletions(-) diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 8b21eeaa3..f513aaeaa 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -23,6 +23,7 @@ struct PhysicsDirectInternalData { DummyGUIHelper m_noGfx; + btAlignedObjectArray m_serverDNA; SharedMemoryCommand m_command; SharedMemoryStatus m_serverStatus; bool m_hasStatus; @@ -83,6 +84,31 @@ bool PhysicsDirect::connect() { bool connected = m_data->m_commandProcessor->connect(); m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx); + + + //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 + { + int timeout = 1024 * 1024 * 1024; + while ((!hasStatus) && (timeout-- > 0)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + } + } + return connected; } @@ -444,7 +470,14 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta bParse::btBulletFile bf( &m_data->m_bulletStreamDataServerToClient[0], serverCmd.m_numDataStreamBytes); - bf.setFileDNAisMemoryDNA(); + 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; @@ -469,7 +502,8 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); } } - if (bf.ok()) { + if (bf.ok()) + { if (m_data->m_verboseOutput) { b3Printf("Received robot description ok!\n"); @@ -484,6 +518,19 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd { switch (serverCmd.m_type) { + 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: { m_data->m_debugLinesFrom.clear(); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index c6f0cf2cb..4df28832c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2178,6 +2178,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } + case CMD_REQUEST_INTERNAL_DATA: + { + //todo: also check version etc? + + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_FAILED; + hasStatus = true; + + int sz = btDefaultSerializer::getMemoryDnaSizeInBytes(); + const char* memDna = btDefaultSerializer::getMemoryDna(); + if (sz < bufferSizeInBytes) + { + for (int i = 0; i < bufferSizeInBytes; i++) + { + bufferServerToClient[i] = memDna[i]; + } + serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_COMPLETED; + serverCmd.m_numDataStreamBytes = bufferSizeInBytes; + } + + break; + }; case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index b435a38a5..7b0a967bc 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -21,6 +21,7 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_ACTUAL_STATE, CMD_REQUEST_DEBUG_LINES, CMD_REQUEST_BODY_INFO, + CMD_REQUEST_INTERNAL_DATA, CMD_STEP_FORWARD_SIMULATION, CMD_RESET_SIMULATION, CMD_PICK_BODY, @@ -54,6 +55,8 @@ enum EnumSharedMemoryServerStatus CMD_SDF_LOADING_FAILED, CMD_URDF_LOADING_COMPLETED, CMD_URDF_LOADING_FAILED, + CMD_REQUEST_INTERNAL_DATA_COMPLETED, + CMD_REQUEST_INTERNAL_DATA_FAILED, CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED, CMD_BULLET_DATA_STREAM_RECEIVED_FAILED, CMD_BOX_COLLISION_SHAPE_CREATION_COMPLETED, diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index ec69952a6..af8eacdf7 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -61,12 +61,45 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args) { { int method = eCONNECT_GUI; - if (!PyArg_ParseTuple(args, "i", &method)) { - PyErr_SetString(SpamError, - "connectPhysicsServer expected argument GUI, " - "DIRECT, SHARED_MEMORY or UDP"); - return NULL; - } + int key = SHARED_MEMORY_KEY; + int port = 1234; + const char* hostName = "localhost"; + + int size = PySequence_Size(args); + if (size == 1) + { + if (!PyArg_ParseTuple(args, "i", &method)) { + PyErr_SetString(SpamError, + "connectPhysicsServer expected argument GUI, " + "DIRECT, SHARED_MEMORY or UDP"); + return NULL; + } + } + + if (size == 2) + { + if (!PyArg_ParseTuple(args, "ii", &method, &key)) + { + if (!PyArg_ParseTuple(args, "is", &method, &hostName)) + { + PyErr_SetString(SpamError, + "connectPhysicsServer cannot parse second argument (either integer or string)"); + return NULL; + + } + } + } + + if (size == 3) + { + if (!PyArg_ParseTuple(args, "isi", &method, &hostName, &port)) + { + PyErr_SetString(SpamError, + "connectPhysicsServer 3 arguments: method, hostname, port"); + return NULL; + } + } + switch (method) { case eCONNECT_GUI: { @@ -85,14 +118,14 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args) { break; } case eCONNECT_SHARED_MEMORY: { - sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY); + sm = b3ConnectSharedMemory(key); break; } case eCONNECT_UDP: { #ifdef BT_ENABLE_ENET - sm = b3ConnectPhysicsUDP("localhost", 1234); + sm = b3ConnectPhysicsUDP(hostName, port); #else PyErr_SetString(SpamError, "UDP is not enabled in this pybullet build"); return NULL; diff --git a/src/LinearMath/btSerializer.h b/src/LinearMath/btSerializer.h index 454068d6a..424e0b99c 100644 --- a/src/LinearMath/btSerializer.h +++ b/src/LinearMath/btSerializer.h @@ -446,6 +446,26 @@ public: btAlignedFree(m_dna); } + static int getMemoryDnaSizeInBytes() + { + const bool VOID_IS_8 = ((sizeof(void*) == 8)); + + if (VOID_IS_8) + { + return sBulletDNAlen64; + } + return sBulletDNAlen; + } + static const char* getMemoryDna() + { + const bool VOID_IS_8 = ((sizeof(void*) == 8)); + if (VOID_IS_8) + { + return (const char*)sBulletDNAstr64; + } + return (const char*)sBulletDNAstr; + } + void insertHeader() { writeHeader(m_buffer);