Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.

example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
This commit is contained in:
Erwin Coumans
2017-05-23 22:05:26 -07:00
parent 19933a9454
commit db008ab3c2
30 changed files with 1195 additions and 517 deletions

View File

@@ -1612,6 +1612,9 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle p
command->m_userDebugDrawArgs.m_lineWidth = lineWidth;
command->m_userDebugDrawArgs.m_lifeTime = lifeTime;
command->m_userDebugDrawArgs.m_trackingObjectUniqueId = -1;
command->m_userDebugDrawArgs.m_trackingLinkIndex = -1;
command->m_userDebugDrawArgs.m_optionFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
@@ -1646,10 +1649,51 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle p
command->m_userDebugDrawArgs.m_textSize = textSize;
command->m_userDebugDrawArgs.m_lifeTime = lifeTime;
command->m_userDebugDrawArgs.m_trackingObjectUniqueId = -1;
command->m_userDebugDrawArgs.m_trackingLinkIndex = -1;
command->m_userDebugDrawArgs.m_optionFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags)
{
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_optionFlags = optionFlags;
command->m_updateFlags |= USER_DEBUG_HAS_OPTION_FLAGS;
}
void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, double orientation[4])
{
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_textOrientation[0] = orientation[0];
command->m_userDebugDrawArgs.m_textOrientation[1] = orientation[1];
command->m_userDebugDrawArgs.m_textOrientation[2] = orientation[2];
command->m_userDebugDrawArgs.m_textOrientation[3] = orientation[3];
command->m_updateFlags |= USER_DEBUG_HAS_TEXT_ORIENTATION;
}
void b3UserDebugItemSetTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
command->m_updateFlags |= USER_DEBUG_HAS_TRACKING_OBJECT;
command->m_userDebugDrawArgs.m_trackingObjectUniqueId = objectUniqueId;
command->m_userDebugDrawArgs.m_trackingLinkIndex = linkIndex;
}
b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -1670,6 +1714,8 @@ b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle ph
command->m_userDebugDrawArgs.m_rangeMin = rangeMin;
command->m_userDebugDrawArgs.m_rangeMax = rangeMax;
command->m_userDebugDrawArgs.m_startValue = startValue;
command->m_userDebugDrawArgs.m_trackingObjectUniqueId = -1;
command->m_userDebugDrawArgs.m_optionFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}

View File

@@ -122,7 +122,13 @@ int b3GetStatusOpenGLVisualizerCamera(b3SharedMemoryStatusHandle statusHandle, s
/// Add/remove user-specific debug lines and debug text messages
b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime);
b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime);
void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags);
void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, double orientation[4]);
void b3UserDebugItemSetTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex);
b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue);
b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId);
int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue);

View File

@@ -280,7 +280,15 @@ bool PhysicsClientSharedMemory::connect() {
if (m_data->m_testBlock1) {
if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) {
b3Error("Error: please start server before client\n");
//there is no chance people are still using this software 100 years from now ;-)
if ((m_data->m_testBlock1->m_magicId < 211705023) &&
(m_data->m_testBlock1->m_magicId >=201705023))
{
b3Error("Error: physics server version mismatch (expected %d got %d)\n",SHARED_MEMORY_MAGIC_NUMBER, m_data->m_testBlock1->m_magicId);
} else
{
b3Error("Error connecting to shared memory: please start server before client\n");
}
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey,
SHARED_MEMORY_SIZE);
m_data->m_testBlock1 = 0;

View File

@@ -5816,6 +5816,37 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED;
hasStatus = true;
int trackingVisualShapeIndex = -1;
if (clientCmd.m_userDebugDrawArgs.m_trackingObjectUniqueId>=0)
{
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_userDebugDrawArgs.m_trackingObjectUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
{
int linkIndex = clientCmd.m_userDebugDrawArgs.m_trackingLinkIndex;
if (linkIndex ==-1)
{
if (bodyHandle->m_multiBody->getBaseCollider())
{
trackingVisualShapeIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex();
}
} else
{
if (linkIndex >=0 && linkIndex < bodyHandle->m_multiBody->getNumLinks())
{
if (bodyHandle->m_multiBody->getLink(linkIndex).m_collider)
{
trackingVisualShapeIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex();
}
}
}
}
}
if (clientCmd.m_updateFlags & USER_DEBUG_ADD_PARAMETER)
{
int uid = m_data->m_guiHelper->addUserDebugParameter(
@@ -5888,11 +5919,27 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT)
{
//addUserDebugText3D( const double orientation[4], const double textColorRGB[3], double size, double lifeTime, int trackingObjectUniqueId, int optionFlags){return -1;}
int optionFlags = clientCmd.m_userDebugDrawArgs.m_optionFlags;
if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT_ORIENTATION)
{
optionFlags |= DEB_DEBUG_TEXT_USE_ORIENTATION;
}
int uid = m_data->m_guiHelper->addUserDebugText3D(clientCmd.m_userDebugDrawArgs.m_text,
clientCmd.m_userDebugDrawArgs.m_textPositionXYZ,
clientCmd.m_userDebugDrawArgs.m_textOrientation,
clientCmd.m_userDebugDrawArgs.m_textColorRGB,
clientCmd.m_userDebugDrawArgs.m_textSize,
clientCmd.m_userDebugDrawArgs.m_lifeTime);
clientCmd.m_userDebugDrawArgs.m_lifeTime,
trackingVisualShapeIndex,
optionFlags);
if (uid>=0)
{
@@ -5908,7 +5955,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_userDebugDrawArgs.m_debugLineToXYZ,
clientCmd.m_userDebugDrawArgs.m_debugLineColorRGB,
clientCmd.m_userDebugDrawArgs.m_lineWidth,
clientCmd.m_userDebugDrawArgs.m_lifeTime);
clientCmd.m_userDebugDrawArgs.m_lifeTime,
trackingVisualShapeIndex);
if (uid>=0)
{
@@ -5953,14 +6001,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//static int skip=1;
void PhysicsServerCommandProcessor::syncPhysicsToGraphics()
{
m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld);
}
void PhysicsServerCommandProcessor::renderScene(int renderFlags)
{
if (m_data->m_guiHelper)
{
if (0==(renderFlags&COV_DISABLE_SYNC_RENDERING))
{
m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld);
}
m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
}
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD

View File

@@ -81,7 +81,9 @@ public:
virtual void renderScene(int renderFlags);
virtual void physicsDebugDraw(int debugDrawFlags);
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper);
virtual void syncPhysicsToGraphics();
//@todo(erwincoumans) Should we have shared memory commands for picking objects?
///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);

View File

@@ -541,6 +541,7 @@ struct UserDebugDrawLine
double m_lifeTime;
int m_itemUniqueId;
int m_trackingVisualShapeIndex;
};
struct UserDebugParameter
@@ -555,12 +556,16 @@ struct UserDebugParameter
struct UserDebugText
{
char m_text[1024];
double m_textPositionXYZ[3];
double m_textPositionXYZ1[3];
double m_textColorRGB[3];
double textSize;
double m_lifeTime;
int m_itemUniqueId;
double m_textOrientation[4];
int m_trackingVisualShapeIndex;
int m_optionFlags;
};
@@ -783,7 +788,7 @@ public:
m_texels(0),
m_textureId(-1)
{
m_childGuiHelper = guiHelper;;
m_childGuiHelper = guiHelper;
}
@@ -1078,12 +1083,16 @@ public:
{
}
virtual void drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag)
{
}
btAlignedObjectArray<UserDebugText> m_userDebugText;
UserDebugText m_tmpText;
virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double textColorRGB[3], double size, double lifeTime)
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)
{
m_tmpText.m_itemUniqueId = m_uidGenerator++;
@@ -1091,13 +1100,27 @@ public:
m_tmpText.textSize = size;
//int len = strlen(txt);
strcpy(m_tmpText.m_text,txt);
m_tmpText.m_textPositionXYZ[0] = positionXYZ[0];
m_tmpText.m_textPositionXYZ[1] = positionXYZ[1];
m_tmpText.m_textPositionXYZ[2] = positionXYZ[2];
m_tmpText.m_textPositionXYZ1[0] = positionXYZ[0];
m_tmpText.m_textPositionXYZ1[1] = positionXYZ[1];
m_tmpText.m_textPositionXYZ1[2] = positionXYZ[2];
m_tmpText.m_textOrientation[0] = orientation[0];
m_tmpText.m_textOrientation[1] = orientation[1];
m_tmpText.m_textOrientation[2] = orientation[2];
m_tmpText.m_textOrientation[3] = orientation[3];
m_tmpText.m_textColorRGB[0] = textColorRGB[0];
m_tmpText.m_textColorRGB[1] = textColorRGB[1];
m_tmpText.m_textColorRGB[2] = textColorRGB[2];
m_tmpText.m_trackingVisualShapeIndex = trackingVisualShapeIndex;
m_tmpText.m_optionFlags = optionFlags;
m_tmpText.m_textOrientation[0] = orientation[0];
m_tmpText.m_textOrientation[1] = orientation[1];
m_tmpText.m_textOrientation[2] = orientation[2];
m_tmpText.m_textOrientation[3] = orientation[3];
m_cs->lock();
m_cs->setSharedParam(1, eGUIUserDebugAddText);
workerThreadWait();
@@ -1140,7 +1163,7 @@ public:
btAlignedObjectArray<UserDebugDrawLine> m_userDebugLines;
UserDebugDrawLine m_tmpLine;
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)
{
m_tmpLine.m_lifeTime = lifeTime;
m_tmpLine.m_lineWidth = lineWidth;
@@ -1156,7 +1179,7 @@ public:
m_tmpLine.m_debugLineColorRGB[0] = debugLineColorRGB[0];
m_tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1];
m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2];
m_tmpLine.m_trackingVisualShapeIndex = trackingVisualShapeIndex;
m_cs->lock();
m_cs->setSharedParam(1, eGUIUserDebugAddLine);
workerThreadWait();
@@ -2004,6 +2027,11 @@ void PhysicsServerExample::drawUserDebugLines()
for (int i = 0; i<m_multiThreadedHelper->m_userDebugLines.size(); i++)
{
btVector3 from;
from.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[0],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[1],
@@ -2013,6 +2041,26 @@ void PhysicsServerExample::drawUserDebugLines()
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[1],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[2]);
int graphicsIndex = m_multiThreadedHelper->m_userDebugLines[i].m_trackingVisualShapeIndex;
if (graphicsIndex>=0)
{
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
if (renderer)
{
float parentPos[3];
float parentOrn[4];
if (renderer->readSingleInstanceTransformToCPU(parentPos,parentOrn,graphicsIndex))
{
btTransform parentTrans;
parentTrans.setOrigin(btVector3(parentPos[0],parentPos[1],parentPos[2]));
parentTrans.setRotation(btQuaternion(parentOrn[0],parentOrn[1],parentOrn[2],parentOrn[3]));
from = parentTrans*from;
toX = parentTrans*toX;
}
}
}
btVector3 color;
color.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0],
m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1],
@@ -2071,11 +2119,77 @@ void PhysicsServerExample::drawUserDebugLines()
for (int i = 0; i<m_multiThreadedHelper->m_userDebugText.size(); i++)
{
// int optionFlag = CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera|CommonGraphicsApp::eDrawText3D_TrueType;
//int optionFlag = CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera;
int optionFlag = 0;
float orientation[4] = {0,0,0,1};
if (m_multiThreadedHelper->m_userDebugText[i].m_optionFlags&CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera)
{
orientation[0] = m_multiThreadedHelper->m_userDebugText[i].m_textOrientation[0];
orientation[1] = m_multiThreadedHelper->m_userDebugText[i].m_textOrientation[1];
orientation[2] = m_multiThreadedHelper->m_userDebugText[i].m_textOrientation[2];
orientation[3] = m_multiThreadedHelper->m_userDebugText[i].m_textOrientation[3];
optionFlag |= CommonGraphicsApp::eDrawText3D_TrueType;
} else
{
optionFlag |= CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera;
}
float colorRGBA[4] = {
m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[0],
m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[1],
m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[2],
1.};
float pos[3] = {m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[0],
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[1],
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[2]};
int graphicsIndex = m_multiThreadedHelper->m_userDebugText[i].m_trackingVisualShapeIndex;
if (graphicsIndex>=0)
{
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
if (renderer)
{
float parentPos[3];
float parentOrn[4];
if (renderer->readSingleInstanceTransformToCPU(parentPos,parentOrn,graphicsIndex))
{
btTransform parentTrans;
parentTrans.setOrigin(btVector3(parentPos[0],parentPos[1],parentPos[2]));
parentTrans.setRotation(btQuaternion(parentOrn[0],parentOrn[1],parentOrn[2],parentOrn[3]));
btTransform childTr;
childTr.setOrigin(btVector3(pos[0],pos[1],pos[2]));
childTr.setRotation(btQuaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
btTransform siteTr = parentTrans*childTr;
pos[0] = siteTr.getOrigin()[0];
pos[1] = siteTr.getOrigin()[1];
pos[2] = siteTr.getOrigin()[2];
btQuaternion siteOrn = siteTr.getRotation();
orientation[0] = siteOrn[0];
orientation[1] = siteOrn[1];
orientation[2] = siteOrn[2];
orientation[3] = siteOrn[3];
}
}
}
m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text,
pos,orientation,colorRGBA,
m_multiThreadedHelper->m_userDebugText[i].textSize,optionFlag);
/*m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text,
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[0],
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[1],
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[2],
m_multiThreadedHelper->m_userDebugText[i].textSize);
*/
}
}
@@ -2260,6 +2374,11 @@ void PhysicsServerExample::renderScene()
void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags)
{
if (gEnableSyncPhysicsRendering)
{
m_physicsServer.syncPhysicsToGraphics();
}
drawUserDebugLines();
if (gEnableRendering)

View File

@@ -278,8 +278,11 @@ void PhysicsServerSharedMemory::renderScene(int renderFlags)
{
m_data->m_commandProcessor->renderScene(renderFlags);
}
void PhysicsServerSharedMemory::syncPhysicsToGraphics()
{
m_data->m_commandProcessor->syncPhysicsToGraphics();
}
void PhysicsServerSharedMemory::physicsDebugDraw(int debugDrawFlags)

View File

@@ -43,6 +43,7 @@ public:
//to a physics client, over shared memory
void physicsDebugDraw(int debugDrawFlags);
void renderScene(int renderFlags);
void syncPhysicsToGraphics();
void enableCommandLogging(bool enable, const char* fileName);
void replayFromLogFile(const char* fileName);

View File

@@ -625,6 +625,9 @@ enum EnumUserDebugDrawFlags
USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR = 32,
USER_DEBUG_ADD_PARAMETER=64,
USER_DEBUG_READ_PARAMETER=128,
USER_DEBUG_HAS_OPTION_FLAGS=256,
USER_DEBUG_HAS_TEXT_ORIENTATION = 512,
USER_DEBUG_HAS_TRACKING_OBJECT=1024,
};
@@ -640,8 +643,13 @@ struct UserDebugDrawArgs
char m_text[MAX_FILENAME_LENGTH];
double m_textPositionXYZ[3];
double m_textOrientation[4];
int m_trackingObjectUniqueId;
int m_trackingLinkIndex;
double m_textColorRGB[3];
double m_textSize;
int m_optionFlags;
double m_rangeMin;
double m_rangeMax;

View File

@@ -4,7 +4,8 @@
#define SHARED_MEMORY_KEY 12347
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
///my convention is year/month/day/rev
#define SHARED_MEMORY_MAGIC_NUMBER 201703024
#define SHARED_MEMORY_MAGIC_NUMBER 201705023
//#define SHARED_MEMORY_MAGIC_NUMBER 201703024
enum EnumSharedMemoryClientCommand
{
@@ -507,6 +508,13 @@ enum b3ConfigureDebugVisualizerEnum
COV_ENABLE_SYNC_RENDERING_INTERNAL,
};
enum b3AddUserDebugItemEnum
{
DEB_DEBUG_TEXT_USE_ORIENTATION=1,
DEB_DEBUG_TEXT_USE_TRUE_TYPE_FONTS=2,
DEB_DEBUG_TEXT_HAS_TRACKING_OBJECT=4,
};
enum eCONNECT_METHOD {
eCONNECT_GUI = 1,
eCONNECT_DIRECT = 2,