diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 90e340890..d438a906f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1612,8 +1612,8 @@ 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_parentObjectUniqueId = -1; + command->m_userDebugDrawArgs.m_parentLinkIndex = -1; command->m_userDebugDrawArgs.m_optionFlags = 0; return (b3SharedMemoryCommandHandle) command; @@ -1649,8 +1649,8 @@ 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_parentObjectUniqueId = -1; + command->m_userDebugDrawArgs.m_parentLinkIndex = -1; command->m_userDebugDrawArgs.m_optionFlags = 0; @@ -1682,15 +1682,15 @@ void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, do } -void b3UserDebugItemSetTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex) +void b3UserDebugItemSetParentObject(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; + command->m_updateFlags |= USER_DEBUG_HAS_PARENT_OBJECT; + command->m_userDebugDrawArgs.m_parentObjectUniqueId = objectUniqueId; + command->m_userDebugDrawArgs.m_parentLinkIndex = linkIndex; } @@ -1714,7 +1714,7 @@ 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_parentObjectUniqueId = -1; command->m_userDebugDrawArgs.m_optionFlags = 0; return (b3SharedMemoryCommandHandle)command; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index f36560508..6dfe21718 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -127,7 +127,7 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle p void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags); void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, double orientation[4]); -void b3UserDebugItemSetTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex); +void b3UserDebugItemSetParentObject(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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 54e1063df..02635029e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5820,12 +5820,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int trackingVisualShapeIndex = -1; - if (clientCmd.m_userDebugDrawArgs.m_trackingObjectUniqueId>=0) + if (clientCmd.m_userDebugDrawArgs.m_parentObjectUniqueId>=0) { - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_userDebugDrawArgs.m_trackingObjectUniqueId); + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_userDebugDrawArgs.m_parentObjectUniqueId); if (bodyHandle && bodyHandle->m_multiBody) { - int linkIndex = clientCmd.m_userDebugDrawArgs.m_trackingLinkIndex; + int linkIndex = clientCmd.m_userDebugDrawArgs.m_parentLinkIndex; if (linkIndex ==-1) { if (bodyHandle->m_multiBody->getBaseCollider()) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 6a584f86d..0c3a44de1 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -627,7 +627,7 @@ enum EnumUserDebugDrawFlags USER_DEBUG_READ_PARAMETER=128, USER_DEBUG_HAS_OPTION_FLAGS=256, USER_DEBUG_HAS_TEXT_ORIENTATION = 512, - USER_DEBUG_HAS_TRACKING_OBJECT=1024, + USER_DEBUG_HAS_PARENT_OBJECT=1024, }; @@ -644,8 +644,8 @@ struct UserDebugDrawArgs char m_text[MAX_FILENAME_LENGTH]; double m_textPositionXYZ[3]; double m_textOrientation[4]; - int m_trackingObjectUniqueId; - int m_trackingLinkIndex; + int m_parentObjectUniqueId; + int m_parentLinkIndex; double m_textColorRGB[3]; double m_textSize; int m_optionFlags; diff --git a/examples/pybullet/examples/debugDrawItems.py b/examples/pybullet/examples/debugDrawItems.py new file mode 100644 index 000000000..3aa1e44ae --- /dev/null +++ b/examples/pybullet/examples/debugDrawItems.py @@ -0,0 +1,16 @@ +import pybullet as p +import time +p.connect(p.GUI) +p.loadURDF("plane.urdf") +kuka = p.loadURDF("kuka_iiwa/model.urdf") +p.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textSize=1.5,parentObjectUniqueId=kuka, parentLinkIndex=6) +p.addUserDebugLine([0,0,0],[0.1,0,0],[1,0,0],parentObjectUniqueId=kuka, parentLinkIndex=6) +p.addUserDebugLine([0,0,0],[0,0.1,0],[0,1,0],parentObjectUniqueId=kuka, parentLinkIndex=6) +p.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],parentObjectUniqueId=kuka, parentLinkIndex=6) +p.setRealTimeSimulation(0) +angle=0 +while (True): + time.sleep(0.01) + p.resetJointState(kuka,2,angle) + p.resetJointState(kuka,3,angle) + angle+=0.01 \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 43d0a1992..e70395e7e 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -3079,16 +3079,16 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj PyObject* textColorRGBObj = 0; PyObject* textOrientationObj = 0; double textOrientation[4]; - int trackingObjectUniqueId=-1; - int trackingLinkIndex=-1; + int parentObjectUniqueId=-1; + int parentLinkIndex=-1; double textSize = 1.f; double lifeTime = 0.f; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char* kwlist[] = {"text", "textPosition", "textColorRGB", "textSize", "lifeTime", "textOrientation", "trackObjectUniqueId", "trackLinkIndex", "physicsClientId", NULL}; + static char* kwlist[] = {"text", "textPosition", "textColorRGB", "textSize", "lifeTime", "textOrientation", "parentObjectUniqueId", "parentLinkIndex", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "sO|OddOiii", kwlist, &text, &textPositionObj, &textColorRGBObj, &textSize, &lifeTime, &textOrientationObj, &trackingObjectUniqueId, &trackingLinkIndex, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "sO|OddOiii", kwlist, &text, &textPositionObj, &textColorRGBObj, &textSize, &lifeTime, &textOrientationObj, &parentObjectUniqueId, &parentLinkIndex, &physicsClientId)) { return NULL; } @@ -3118,9 +3118,9 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj commandHandle = b3InitUserDebugDrawAddText3D(sm, text, posXYZ, colorRGB, textSize, lifeTime); - if (trackingObjectUniqueId>=0) + if (parentObjectUniqueId>=0) { - b3UserDebugItemSetTrackingObject(commandHandle, trackingObjectUniqueId,trackingLinkIndex); + b3UserDebugItemSetParentObject(commandHandle, parentObjectUniqueId,parentLinkIndex); } if (textOrientationObj) { @@ -3158,8 +3158,8 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj double fromXYZ[3]; double toXYZ[3]; double colorRGB[3] = {1, 1, 1}; - int trackingObjectUniqueId=-1; - int trackingLinkIndex=-1; + int parentObjectUniqueId=-1; + int parentLinkIndex=-1; PyObject* lineFromObj = 0; PyObject* lineToObj = 0; @@ -3168,9 +3168,9 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj double lifeTime = 0.f; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char* kwlist[] = {"lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", "trackObjectUniqueId", "trackLinkIndex", "physicsClientId", NULL}; + static char* kwlist[] = {"lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", "parentObjectUniqueId", "parentLinkIndex", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Oddiii", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj, &lineWidth, &lifeTime, &trackingObjectUniqueId, &trackingLinkIndex, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Oddiii", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj, &lineWidth, &lifeTime, &parentObjectUniqueId, &parentLinkIndex, &physicsClientId)) { return NULL; } @@ -3201,9 +3201,9 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj commandHandle = b3InitUserDebugDrawAddLine3D(sm, fromXYZ, toXYZ, colorRGB, lineWidth, lifeTime); - if (trackingObjectUniqueId>=0) + if (parentObjectUniqueId>=0) { - b3UserDebugItemSetTrackingObject(commandHandle, trackingObjectUniqueId,trackingLinkIndex); + b3UserDebugItemSetParentObject(commandHandle, parentObjectUniqueId,parentLinkIndex); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);