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 0dd61fcad..d3c515834 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) 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', 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