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

PyBullet: allow to replace existing debug lines in addUserDebugLine through the replaceItemUniqueId argument

Browse Source 
See batchRayCast.py for example usage
This commit is contained in:
erwincoumans
2018-06-22 16:47:20 -07:00
parent 6b2cae1b1d
commit 49b0ec08e1
7 changed files with 66 additions and 19 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)