From 1e91e784691b08c3e0b28612e8d9c3ae039d7a77 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Sun, 2 Apr 2017 16:03:20 -0700 Subject: [PATCH] Add pybullet API for logging contacts. --- examples/SharedMemory/PhysicsClientC_API.h | 8 ++--- examples/pybullet/examples/kuka_with_cube.py | 3 +- examples/pybullet/pybullet.c | 32 +++++++++++++++++--- 3 files changed, 33 insertions(+), 10 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 29efca7d4..b59029553 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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); diff --git a/examples/pybullet/examples/kuka_with_cube.py b/examples/pybullet/examples/kuka_with_cube.py index aad32f489..db6ceb247 100644 --- a/examples/pybullet/examples/kuka_with_cube.py +++ b/examples/pybullet/examples/kuka_with_cube.py @@ -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]) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b041f88d8..467bbdcb1 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -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);