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

trackObject -> parentObject

Browse Source 
trackLinkIndex -> parentLinkIndex
add example debugDrawItems.py
This commit is contained in:
Erwin Coumans
2017-05-24 09:06:15 -07:00
parent 9f7d7fecd5
commit 5e2599863d
6 changed files with 44 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1612,8 +1612,8 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle p
command->m_userDebugDrawArgs.m_lineWidth = lineWidth; command->m_userDebugDrawArgs.m_lineWidth = lineWidth;
command->m_userDebugDrawArgs.m_lifeTime = lifeTime; command->m_userDebugDrawArgs.m_lifeTime = lifeTime;
command->m_userDebugDrawArgs.m_trackingObjectUniqueId = -1; command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
command->m_userDebugDrawArgs.m_trackingLinkIndex = -1; command->m_userDebugDrawArgs.m_parentLinkIndex = -1;
command->m_userDebugDrawArgs.m_optionFlags = 0; command->m_userDebugDrawArgs.m_optionFlags = 0;
return (b3SharedMemoryCommandHandle) command; return (b3SharedMemoryCommandHandle) command;
@@ -1649,8 +1649,8 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle p
command->m_userDebugDrawArgs.m_textSize = textSize; command->m_userDebugDrawArgs.m_textSize = textSize;
command->m_userDebugDrawArgs.m_lifeTime = lifeTime; command->m_userDebugDrawArgs.m_lifeTime = lifeTime;
command->m_userDebugDrawArgs.m_trackingObjectUniqueId = -1; command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
command->m_userDebugDrawArgs.m_trackingLinkIndex = -1; command->m_userDebugDrawArgs.m_parentLinkIndex = -1;
command->m_userDebugDrawArgs.m_optionFlags = 0; 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; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_USER_DEBUG_DRAW); b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
command->m_updateFlags |= USER_DEBUG_HAS_TRACKING_OBJECT; command->m_updateFlags |= USER_DEBUG_HAS_PARENT_OBJECT;
command->m_userDebugDrawArgs.m_trackingObjectUniqueId = objectUniqueId; command->m_userDebugDrawArgs.m_parentObjectUniqueId = objectUniqueId;
command->m_userDebugDrawArgs.m_trackingLinkIndex = linkIndex; command->m_userDebugDrawArgs.m_parentLinkIndex = linkIndex;
} }
@@ -1714,7 +1714,7 @@ b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle ph
command->m_userDebugDrawArgs.m_rangeMin = rangeMin; command->m_userDebugDrawArgs.m_rangeMin = rangeMin;
command->m_userDebugDrawArgs.m_rangeMax = rangeMax; command->m_userDebugDrawArgs.m_rangeMax = rangeMax;
command->m_userDebugDrawArgs.m_startValue = startValue; command->m_userDebugDrawArgs.m_startValue = startValue;
command->m_userDebugDrawArgs.m_trackingObjectUniqueId = -1; command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
command->m_userDebugDrawArgs.m_optionFlags = 0; command->m_userDebugDrawArgs.m_optionFlags = 0;
return (b3SharedMemoryCommandHandle)command; return (b3SharedMemoryCommandHandle)command;
} }

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -127,7 +127,7 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle p
void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags); void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags);
void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, double orientation[4]); 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 b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue);
b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId); b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId);

6
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -5820,12 +5820,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int trackingVisualShapeIndex = -1; 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) if (bodyHandle && bodyHandle->m_multiBody)
{ {
int linkIndex = clientCmd.m_userDebugDrawArgs.m_trackingLinkIndex; int linkIndex = clientCmd.m_userDebugDrawArgs.m_parentLinkIndex;
if (linkIndex ==-1) if (linkIndex ==-1)
{ {
if (bodyHandle->m_multiBody->getBaseCollider()) if (bodyHandle->m_multiBody->getBaseCollider())

6
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -627,7 +627,7 @@ enum EnumUserDebugDrawFlags
USER_DEBUG_READ_PARAMETER=128, USER_DEBUG_READ_PARAMETER=128,
USER_DEBUG_HAS_OPTION_FLAGS=256, USER_DEBUG_HAS_OPTION_FLAGS=256,
USER_DEBUG_HAS_TEXT_ORIENTATION = 512, 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]; char m_text[MAX_FILENAME_LENGTH];
double m_textPositionXYZ[3]; double m_textPositionXYZ[3];
double m_textOrientation[4]; double m_textOrientation[4];
int m_trackingObjectUniqueId; int m_parentObjectUniqueId;
int m_trackingLinkIndex; int m_parentLinkIndex;
double m_textColorRGB[3]; double m_textColorRGB[3];
double m_textSize; double m_textSize;
int m_optionFlags; int m_optionFlags;

16
examples/pybullet/examples/debugDrawItems.py Normal file
View File

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

24
examples/pybullet/pybullet.c
View File

@@ -3079,16 +3079,16 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
PyObject* textColorRGBObj = 0; PyObject* textColorRGBObj = 0;
PyObject* textOrientationObj = 0; PyObject* textOrientationObj = 0;
double textOrientation[4]; double textOrientation[4];
int trackingObjectUniqueId=-1; int parentObjectUniqueId=-1;
int trackingLinkIndex=-1; int parentLinkIndex=-1;
double textSize = 1.f; double textSize = 1.f;
double lifeTime = 0.f; double lifeTime = 0.f;
int physicsClientId = 0; int physicsClientId = 0;
b3PhysicsClientHandle sm = 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; return NULL;
} }
@@ -3118,9 +3118,9 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
commandHandle = b3InitUserDebugDrawAddText3D(sm, text, posXYZ, colorRGB, textSize, lifeTime); commandHandle = b3InitUserDebugDrawAddText3D(sm, text, posXYZ, colorRGB, textSize, lifeTime);
if (trackingObjectUniqueId>=0) if (parentObjectUniqueId>=0)
{ {
b3UserDebugItemSetTrackingObject(commandHandle, trackingObjectUniqueId,trackingLinkIndex); b3UserDebugItemSetParentObject(commandHandle, parentObjectUniqueId,parentLinkIndex);
} }
if (textOrientationObj) if (textOrientationObj)
{ {
@@ -3158,8 +3158,8 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
double fromXYZ[3]; double fromXYZ[3];
double toXYZ[3]; double toXYZ[3];
double colorRGB[3] = {1, 1, 1}; double colorRGB[3] = {1, 1, 1};
int trackingObjectUniqueId=-1; int parentObjectUniqueId=-1;
int trackingLinkIndex=-1; int parentLinkIndex=-1;
PyObject* lineFromObj = 0; PyObject* lineFromObj = 0;
PyObject* lineToObj = 0; PyObject* lineToObj = 0;
@@ -3168,9 +3168,9 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
double lifeTime = 0.f; double lifeTime = 0.f;
int physicsClientId = 0; int physicsClientId = 0;
b3PhysicsClientHandle sm = 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; return NULL;
} }
@@ -3201,9 +3201,9 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
commandHandle = b3InitUserDebugDrawAddLine3D(sm, fromXYZ, toXYZ, colorRGB, lineWidth, lifeTime); 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); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);