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

Add pybullet API for logging contacts.

Browse Source
This commit is contained in:
yunfeibai
2017-04-02 16:03:20 -07:00
parent 0d83667817
commit 1e91e78469
3 changed files with 33 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -363,10 +363,10 @@ b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle phys
int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName);
int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof);
int b3StateLoggingSetLinkIndexA(b3SharedMemoryStatusHandle commandHandle, int linkIndexA);
int b3StateLoggingSetLinkIndexB(b3SharedMemoryStatusHandle commandHandle, int linkIndexB);
int b3StateLoggingSetBodyIndexA(b3SharedMemoryStatusHandle commandHandle, int bodyIndexA);
int b3StateLoggingSetBodyIndexB(b3SharedMemoryStatusHandle commandHandle, int bodyIndexB);
int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
int b3StateLoggingSetBodyIndexA(b3SharedMemoryCommandHandle commandHandle, int bodyIndexA);
int b3StateLoggingSetBodyIndexB(b3SharedMemoryCommandHandle commandHandle, int bodyIndexB);
int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle);
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId);

3
examples/pybullet/examples/kuka_with_cube.py
View File

@@ -47,7 +47,8 @@ p.setRealTimeSimulation(useRealTimeSimulation)
#use 0 for no-removal
trailDuration = 15
logId = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2])
logId1 = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2])
logId2 = p.startStateLogging(p.STATE_LOGGING_CONTACT_POINTS,"LOG0002.txt",bodyIndexA=1)
for i in xrange(5):
print "Body %d's name is %s." % (i, p.getBodyInfo(i)[1])

32
examples/pybullet/pybullet.c
View File

@@ -2678,12 +2678,16 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb
char* fileName = 0;
PyObject* objectUniqueIdsObj = 0;
int maxLogDof = -1;
int bodyIndexA = -1;
int bodyIndexB = -1;
int linkIndexA = -2;
int linkIndexB = -2;
static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "physicsClientId", NULL};
static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "bodyIndexA", "bodyIndexB", "linkIndexA", "linkIndexB", "physicsClientId", NULL};
int physicsClientId = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oii", kwlist,
&loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oiiiiii", kwlist,
&loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &bodyIndexA, &bodyIndexB, &linkIndexA, &linkIndexB, &physicsClientId))
return NULL;
sm = getPhysicsClient(physicsClientId);
@@ -2718,6 +2722,23 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb
{
b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof);
}
if (bodyIndexA > -1)
{
b3StateLoggingSetLinkIndexA(commandHandle, bodyIndexA);
}
if (bodyIndexB > -1)
{
b3StateLoggingSetBodyIndexB(commandHandle, bodyIndexB);
}
if (linkIndexA > -2)
{
b3StateLoggingSetLinkIndexA(commandHandle, linkIndexA);
}
if (linkIndexB > -2)
{
b3StateLoggingSetLinkIndexB(commandHandle, linkIndexB);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
@@ -5293,8 +5314,8 @@ static PyMethodDef SpamMethods[] = {
{"startStateLogging", (PyCFunction)pybullet_startStateLogging, METH_VARARGS | METH_KEYWORDS,
"Start logging of state, such as robot base position, orientation, joint positions etc. "
"Specify loggingType (STATE_LOG_MINITAUR, STATE_LOG_GENERIC_ROBOT, STATE_LOG_VR_CONTROLLERS etc), "
"fileName, optional objectUniqueId. Function returns int loggingUniqueId"},
"Specify loggingType (STATE_LOGGING_MINITAUR, STATE_LOGGING_GENERIC_ROBOT, STATE_LOGGING_VR_CONTROLLERS, STATE_LOGGING_CONTACT_POINTS, etc), "
"fileName, optional objectUniqueId, maxLogDof, bodyIndexA, bodyIndexB, linkIndexA, linkIndexB. Function returns int loggingUniqueId"},
{"stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS,
"Stop logging of robot state, given a loggingUniqueId."},
{"rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS,
@@ -5425,6 +5446,7 @@ initpybullet(void)
PyModule_AddIntConstant(m, "STATE_LOGGING_GENERIC_ROBOT", STATE_LOGGING_GENERIC_ROBOT);
PyModule_AddIntConstant(m, "STATE_LOGGING_VR_CONTROLLERS", STATE_LOGGING_VR_CONTROLLERS);
PyModule_AddIntConstant(m, "STATE_LOGGING_VIDEO_MP4", STATE_LOGGING_VIDEO_MP4);
PyModule_AddIntConstant(m, "STATE_LOGGING_CONTACT_POINTS", STATE_LOGGING_CONTACT_POINTS);
PyModule_AddIntConstant(m, "COV_ENABLE_GUI", COV_ENABLE_GUI);
PyModule_AddIntConstant(m, "COV_ENABLE_SHADOWS", COV_ENABLE_SHADOWS);