From 49b0ec08e1e7cc7e738443a1cd22764e9a3f0eeb Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 22 Jun 2018 16:47:20 -0700 Subject: [PATCH 1/3] PyBullet: allow to replace existing debug lines in addUserDebugLine through the replaceItemUniqueId argument See batchRayCast.py for example usage --- .../CommonGUIHelperInterface.h | 2 +- examples/ExampleBrowser/OpenGLGuiHelper.h | 3 +- examples/SharedMemory/PhysicsClientC_API.cpp | 1 - .../PhysicsServerCommandProcessor.cpp | 10 ++++++- .../SharedMemory/PhysicsServerExample.cpp | 28 +++++++++++++---- examples/pybullet/examples/batchRayCast.py | 30 ++++++++++++++----- examples/pybullet/pybullet.c | 11 +++++-- 7 files changed, 66 insertions(+), 19 deletions(-) diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index a9e273f02..de3172d18 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -103,7 +103,7 @@ struct GUIHelperInterface virtual void drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag){} virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double orientation[4], const double textColorRGB[3], double size, double lifeTime, int trackingVisualShapeIndex, int optionFlags, int replaceItemUid){return -1;} - virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex){return -1;}; + virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex, int replaceItemUid){return -1;}; virtual int addUserDebugParameter(const char* txt, double rangeMin, double rangeMax, double startValue){return -1;}; virtual int readUserDebugParameter(int itemUniqueId, double* value) { return 0;} diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index 3a43a0221..cf5a4c4f4 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -75,7 +75,8 @@ struct OpenGLGuiHelper : public GUIHelperInterface { return -1; } - virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime ) + + virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex, int replaceItemUid ) { return -1; } diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 35f487d7b..e5abaa26a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3063,7 +3063,6 @@ B3_SHARED_API void b3UserDebugItemSetReplaceItemUniqueId(b3SharedMemoryCommandHa struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); b3Assert(command->m_type == CMD_USER_DEBUG_DRAW); - b3Assert(command->m_updateFlags & USER_DEBUG_HAS_TEXT); command->m_userDebugDrawArgs.m_replaceItemUniqueId = replaceItemUniqueId; command->m_updateFlags |= USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index ee134e9cd..f89135df2 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4582,13 +4582,21 @@ bool PhysicsServerCommandProcessor::processUserDebugDrawCommand(const struct Sha if (clientCmd.m_updateFlags & USER_DEBUG_HAS_LINE) { + int replaceItemUid = -1; + if (clientCmd.m_updateFlags&USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID) + { + replaceItemUid = clientCmd.m_userDebugDrawArgs.m_replaceItemUniqueId; + } + int uid = m_data->m_guiHelper->addUserDebugLine( clientCmd.m_userDebugDrawArgs.m_debugLineFromXYZ, clientCmd.m_userDebugDrawArgs.m_debugLineToXYZ, clientCmd.m_userDebugDrawArgs.m_debugLineColorRGB, clientCmd.m_userDebugDrawArgs.m_lineWidth, clientCmd.m_userDebugDrawArgs.m_lifeTime, - trackingVisualShapeIndex); + trackingVisualShapeIndex, + replaceItemUid + ); if (uid>=0) { diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index fd4b3bd10..5260d6356 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -515,6 +515,7 @@ struct UserDebugDrawLine double m_lifeTime; int m_itemUniqueId; int m_trackingVisualShapeIndex; + int m_replaceItemUid; }; struct UserDebugParameter @@ -1275,11 +1276,14 @@ public: btAlignedObjectArray m_userDebugLines; UserDebugDrawLine m_tmpLine; int m_resultDebugLineUid; - virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex) + + + virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex, int replaceItemUid) { m_tmpLine.m_lifeTime = lifeTime; m_tmpLine.m_lineWidth = lineWidth; - m_tmpLine.m_itemUniqueId = m_uidGenerator++; + + m_tmpLine.m_itemUniqueId = replaceItemUid<0? m_uidGenerator++ : replaceItemUid; m_tmpLine.m_debugLineFromXYZ[0] = debugLineFromXYZ[0]; m_tmpLine.m_debugLineFromXYZ[1] = debugLineFromXYZ[1]; m_tmpLine.m_debugLineFromXYZ[2] = debugLineFromXYZ[2]; @@ -1292,6 +1296,7 @@ public: m_tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1]; m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2]; m_tmpLine.m_trackingVisualShapeIndex = trackingVisualShapeIndex; + m_tmpLine.m_replaceItemUid = replaceItemUid; m_cs->lock(); m_cs->setSharedParam(1, eGUIUserDebugAddLine); m_resultDebugLineUid=-1; @@ -2365,10 +2370,23 @@ void PhysicsServerExample::updateGraphics() { B3_PROFILE("eGUIUserDebugAddLine"); - m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine); - m_multiThreadedHelper->m_resultDebugLineUid = m_multiThreadedHelper->m_userDebugLines[m_multiThreadedHelper->m_userDebugLines.size()-1].m_itemUniqueId; + if (m_multiThreadedHelper->m_tmpLine.m_replaceItemUid>=0) + { + for (int i=0;im_userDebugLines.size();i++) + { + if (m_multiThreadedHelper->m_userDebugLines[i].m_itemUniqueId == m_multiThreadedHelper->m_tmpLine.m_replaceItemUid) + { + m_multiThreadedHelper->m_userDebugLines[i] = m_multiThreadedHelper->m_tmpLine; + m_multiThreadedHelper->m_resultDebugLineUid = m_multiThreadedHelper->m_tmpLine.m_replaceItemUid; + } + } + } else + { + m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine); + m_multiThreadedHelper->m_resultDebugLineUid = m_multiThreadedHelper->m_userDebugLines[m_multiThreadedHelper->m_userDebugLines.size()-1].m_itemUniqueId; + } m_multiThreadedHelper->mainThreadRelease(); - break; + break; } case eGUIUserDebugRemoveItem: { diff --git a/examples/pybullet/examples/batchRayCast.py b/examples/pybullet/examples/batchRayCast.py index f25cf4cff..44b262dba 100644 --- a/examples/pybullet/examples/batchRayCast.py +++ b/examples/pybullet/examples/batchRayCast.py @@ -9,22 +9,34 @@ if (useGui): else: p.connect(p.DIRECT) +p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) -p.loadURDF("samurai.urdf") +#p.loadURDF("samurai.urdf") p.loadURDF("r2d2.urdf",[3,3,1]) rayFrom=[] rayTo=[] +rayIds=[] numRays = 1024 + rayLen = 13 +rayHitColor = [1,0,0] +rayMissColor = [0,1,0] + +replaceLines = True + for i in range (numRays): rayFrom.append([0,0,1]) rayTo.append([rayLen*math.sin(2.*math.pi*float(i)/numRays), rayLen*math.cos(2.*math.pi*float(i)/numRays),1]) + if (replaceLines): + rayIds.append(p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor)) + else: + rayIds.append(-1) if (not useGui): timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"rayCastBench.json") @@ -38,24 +50,26 @@ for i in range (numSteps): for j in range (8): results = p.rayTestBatch(rayFrom,rayTo,j+1) - for i in range (10): - p.removeAllUserDebugItems() + #for i in range (10): + # p.removeAllUserDebugItems() - rayHitColor = [1,0,0] - rayMissColor = [0,1,0] + if (useGui): - p.removeAllUserDebugItems() + if (not replaceLines): + p.removeAllUserDebugItems() + for i in range (numRays): hitObjectUid=results[i][0] if (hitObjectUid<0): - p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor) + hitPosition =[0,0,0] + p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor,replaceItemUniqueId=rayIds[i]) else: hitPosition = results[i][3] - p.addUserDebugLine(rayFrom[i],hitPosition, rayHitColor) + p.addUserDebugLine(rayFrom[i],hitPosition, rayHitColor,replaceItemUniqueId=rayIds[i]) #time.sleep(1./240.) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index fb6078865..b7c704305 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4261,10 +4261,11 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj double lifeTime = 0.f; int physicsClientId = 0; int debugItemUniqueId = -1; + int replaceItemUniqueId = -1; b3PhysicsClientHandle sm = 0; - static char* kwlist[] = {"lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", "parentObjectUniqueId", "parentLinkIndex", "physicsClientId", NULL}; + static char* kwlist[] = {"lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", "parentObjectUniqueId", "parentLinkIndex", "replaceItemUniqueId", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Oddiii", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj, &lineWidth, &lifeTime, &parentObjectUniqueId, &parentLinkIndex, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Oddiiii", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj, &lineWidth, &lifeTime, &parentObjectUniqueId, &parentLinkIndex, &replaceItemUniqueId, &physicsClientId)) { return NULL; } @@ -4293,6 +4294,7 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj res = pybullet_internalSetVectord(lineColorRGBObj, colorRGB); } + commandHandle = b3InitUserDebugDrawAddLine3D(sm, fromXYZ, toXYZ, colorRGB, lineWidth, lifeTime); if (parentObjectUniqueId>=0) @@ -4300,6 +4302,11 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj b3UserDebugItemSetParentObject(commandHandle, parentObjectUniqueId,parentLinkIndex); } + if (replaceItemUniqueId>=0) + { + b3UserDebugItemSetReplaceItemUniqueId(commandHandle,replaceItemUniqueId); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) From 2c881d0b74c87e4155152f29e73acf4b0a0be39d Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 23 Jun 2018 11:53:12 -0700 Subject: [PATCH 2/3] bump up pybullet version to 2.0.9 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 6022d0724..cfb5f5b8e 100644 --- a/setup.py +++ b/setup.py @@ -452,7 +452,7 @@ print("-----") setup( name = 'pybullet', - version='2.0.8', + version='2.0.9', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From 6549b0a5862dc6ba37d45000af327d013d49f256 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 26 Jun 2018 09:19:10 -0700 Subject: [PATCH 3/3] re-enable optional tracking of memory allocations (disabled by default) --- src/LinearMath/btAlignedAllocator.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/LinearMath/btAlignedAllocator.cpp b/src/LinearMath/btAlignedAllocator.cpp index 927500b2f..0526a4228 100644 --- a/src/LinearMath/btAlignedAllocator.cpp +++ b/src/LinearMath/btAlignedAllocator.cpp @@ -15,6 +15,12 @@ subject to the following restrictions: #include "btAlignedAllocator.h" +#ifdef BT_DEBUG_MEMORY_ALLOCATIONS +int gNumAlignedAllocs = 0; +int gNumAlignedFree = 0; +int gTotalBytesAlignedAllocs = 0;//detect memory leaks +#endif //BT_DEBUG_MEMORY_ALLOCATIONST_DEBUG_ALLOCATIONS + static void *btAllocDefault(size_t size) { return malloc(size); @@ -159,6 +165,9 @@ void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filen // printf("big alloc!%d\n", size); // } + gTotalBytesAlignedAllocs += size; + gNumAlignedAllocs++; + int sz4prt = 4*sizeof(void *); @@ -183,6 +192,7 @@ int sz4prt = 4*sizeof(void *); ret = (void *)(real);//?? } + printf("allocation %d at address %x, from %s,line %d, size %d (total allocated = %d)\n",allocId,real, filename,line,size,gTotalBytesAlignedAllocs); allocId++; int* ptr = (int*)ret; @@ -196,6 +206,7 @@ void btAlignedFreeInternal (void* ptr,int line,char* filename) void* real; if (ptr) { + gNumAlignedFree++; btDebugPtrMagic p; p.vptr = ptr; @@ -220,6 +231,11 @@ void btAlignedFreeInternal (void* ptr,int line,char* filename) } } + + gTotalBytesAlignedAllocs -= size; + + int diff = gNumAlignedAllocs-gNumAlignedFree; + printf("free %d at address %x, from %s,line %d, size %d (total remain = %d in %d non-freed allocations)\n",allocId,real, filename,line,size, gTotalBytesAlignedAllocs, diff); sFreeFunc(real); } else