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

Merge pull request #1774 from erwincoumans/master

Browse Source 
PyBullet: allow to replace existing debug lines in addUserDebugLine t…
This commit is contained in:
erwincoumans
2018-06-26 10:40:40 -07:00
committed by GitHub
parent e023d94b32 6549b0a586
commit c531234f3a
9 changed files with 83 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/CommonInterfaces/CommonGUIHelperInterface.h
View File

@@ -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;}

3
examples/ExampleBrowser/OpenGLGuiHelper.h
View File

@@ -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;
}

1
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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;
}

10
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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)
{

28
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -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<UserDebugDrawLine> 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;i<m_multiThreadedHelper->m_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:
{

30
examples/pybullet/examples/batchRayCast.py
View File

@@ -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.)

11
examples/pybullet/pybullet.c
View File

@@ -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)

2
setup.py
View File

@@ -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',

16
src/LinearMath/btAlignedAllocator.cpp
View File

@@ -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