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

add user debug line/text features in pybullet + shared memory API:

Browse Source 
addUserDebugLine,
addUserDebugText
removeUserDebugItem
removeAllUserDebugItems
This commit is contained in:
Erwin Coumans
2016-11-14 07:39:34 -08:00
parent d49e3d787a
commit c521d816c6
17 changed files with 809 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

261
examples/pybullet/pybullet.c
View File

@@ -199,7 +199,7 @@ static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args) {
}
#define MAX_SDF_BODIES 512
static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args)
{
int size = PySequence_Size(args);
@@ -207,8 +207,9 @@ static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args)
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command;
int i,numBodies;
int bodyIndicesOut[MAX_SDF_BODIES];
PyObject* pylist = 0;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
@@ -225,8 +226,24 @@ static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args)
PyErr_SetString(SpamError, "Couldn't load .bullet file.");
return NULL;
}
Py_INCREF(Py_None);
return Py_None;
numBodies =
b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES);
if (numBodies > MAX_SDF_BODIES) {
PyErr_SetString(SpamError, "loadBullet exceeds body capacity");
return NULL;
}
pylist = PyTuple_New(numBodies);
if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) {
for (i = 0; i < numBodies; i++) {
PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i]));
}
}
return pylist;
}
static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args)
@@ -366,7 +383,7 @@ static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) {
return v;
}
#define MAX_SDF_BODIES 512
static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args) {
const char* sdfFileName = "";
@@ -1376,12 +1393,14 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) {
//
// // Args:
// vector - float[3] which will be set by values from objMat
static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) {
static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) {
int i, len;
PyObject* seq;
if (objVec==NULL)
return 0;
seq = PySequence_Fast(objMat, "expected a sequence");
len = PySequence_Size(objMat);
seq = PySequence_Fast(objVec, "expected a sequence");
len = PySequence_Size(objVec);
if (len == 3) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
@@ -1397,7 +1416,9 @@ static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) {
static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
int i, len;
PyObject* seq;
if (obVec==NULL)
return 0;
seq = PySequence_Fast(obVec, "expected a sequence");
len = PySequence_Size(obVec);
if (len == 3) {
@@ -1415,7 +1436,9 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) {
int i, len;
PyObject* seq;
if (obVec==NULL)
return 0;
seq = PySequence_Fast(obVec, "expected a sequence");
len = PySequence_Size(obVec);
if (len == 4) {
@@ -1429,6 +1452,196 @@ static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) {
return 0;
}
static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject *keywds)
{
int size = PySequence_Size(args);
b3SharedMemoryCommandHandle commandHandle;
struct b3ContactInformation contactPointData;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int res = 0;
PyObject* pyResultList = 0;
char* text;
double posXYZ[3];
double colorRGB[3]={1,1,1};
PyObject* textPositionObj=0;
PyObject* textColorRGBObj=0;
double textSize = 1.f;
double lifeTime = 0.f;
static char *kwlist[] = { "text", "textPosition", "textColorRGB", "textSize", "lifeTime", NULL };
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (!PyArg_ParseTupleAndKeywords(args, keywds, "sO|Odd", kwlist, &text, &textPositionObj, &textColorRGBObj,&textSize, &lifeTime))
{
return NULL;
}
res = pybullet_internalSetVectord(textPositionObj,posXYZ);
if (!res)
{
PyErr_SetString(SpamError, "Error converting lineFrom[3]");
return NULL;
}
if (textColorRGBObj)
{
res = pybullet_internalSetVectord(textColorRGBObj,colorRGB);
if (!res)
{
PyErr_SetString(SpamError, "Error converting lineTo[3]");
return NULL;
}
}
commandHandle = b3InitUserDebugDrawAddText3D(sm,text,posXYZ,colorRGB,textSize,lifeTime);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)
{
int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
PyObject* item = PyInt_FromLong(debugItemUniqueId);
return item;
}
PyErr_SetString(SpamError, "Error in addUserDebugText.");
return NULL;
}
static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObject *keywds)
{
int size = PySequence_Size(args);
b3SharedMemoryCommandHandle commandHandle;
struct b3ContactInformation contactPointData;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int res = 0;
PyObject* pyResultList = 0;
double fromXYZ[3];
double toXYZ[3];
double colorRGB[3]={1,1,1};
PyObject* lineFromObj=0;
PyObject* lineToObj=0;
PyObject* lineColorRGBObj=0;
double lineWidth = 1.f;
double lifeTime = 0.f;
static char *kwlist[] = { "lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", NULL };
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Odd", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj,&lineWidth, &lifeTime))
{
return NULL;
}
res = pybullet_internalSetVectord(lineFromObj,fromXYZ);
if (!res)
{
PyErr_SetString(SpamError, "Error converting lineFrom[3]");
return NULL;
}
res = pybullet_internalSetVectord(lineToObj,toXYZ);
if (!res)
{
PyErr_SetString(SpamError, "Error converting lineTo[3]");
return NULL;
}
if (lineColorRGBObj)
{
res = pybullet_internalSetVectord(lineColorRGBObj,colorRGB);
}
commandHandle = b3InitUserDebugDrawAddLine3D(sm,fromXYZ,toXYZ,colorRGB,lineWidth,lifeTime);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)
{
int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
PyObject* item = PyInt_FromLong(debugItemUniqueId);
return item;
}
PyErr_SetString(SpamError, "Error in addUserDebugLine.");
return NULL;
}
static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject *keywds)
{
b3SharedMemoryCommandHandle commandHandle;
struct b3ContactInformation contactPointData;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int itemUniqueId;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (!PyArg_ParseTuple(args, "i", &itemUniqueId)) {
PyErr_SetString(SpamError, "Error parsing user debug item unique id");
return NULL;
}
commandHandle = b3InitUserDebugDrawRemove(sm,itemUniqueId);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args, PyObject *keywds)
{
b3SharedMemoryCommandHandle commandHandle;
struct b3ContactInformation contactPointData;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
commandHandle = b3InitUserDebugDrawRemoveAll(sm);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args)
{
int size = PySequence_Size(args);
@@ -1509,7 +1722,7 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args)
PyTuple_SetItem(visualShapeObList, 6, vec);
}
visualShapeInfo.m_visualShapeData[0].m_rgbaColor[0];
PyTuple_SetItem(pyResultList, i, visualShapeObList);
}
@@ -2687,6 +2900,30 @@ static PyMethodDef SpamMethods[] = {
"axis-aligned bounding box volume (AABB)."
"Input are two vectors defining the AABB in world space [min_x,min_y,min_z],[max_x,max_y,max_z]."
},
{ "addUserDebugLine", (PyCFunction)pybullet_addUserDebugLine, METH_VARARGS | METH_KEYWORDS,
"Add a user debug draw line with lineFrom[3], lineTo[3], lineColorRGB[3], lineWidth, lifeTime. "
"A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."
},
{ "addUserDebugText", (PyCFunction)pybullet_addUserDebugText, METH_VARARGS | METH_KEYWORDS,
"Add a user debug draw line with text, textPosition[3], textSize and lifeTime in seconds "
"A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."
},
{ "removeUserDebugItem", (PyCFunction)pybullet_removeUserDebugItem, METH_VARARGS | METH_KEYWORDS,
"remove a user debug draw item, giving its unique id"
},
{ "removeAllUserDebugItems", (PyCFunction)pybullet_removeAllUserDebugItems, METH_VARARGS | METH_KEYWORDS,
"remove all user debug draw items"
},
{"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS,
"Return the visual shape information for one object." },