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

initial implementation of state logging.

Browse Source 
see examples/pybullet/logMinitaur.py for example. Other state logging will include general robot states and VR controllers state.
This commit is contained in:
Erwin Coumans
2017-02-17 14:25:53 -08:00
parent 2b27ab2463
commit cfd35840f0
9 changed files with 402 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
examples/pybullet/pybullet.c
View File

@@ -2614,6 +2614,40 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
b3SharedMemoryCommandHandle commandHandle;
commandHandle = b3StateLoggingCommandInit(sm);
b3StateLoggingStart(commandHandle,loggingType,fileName);
if (objectUniqueIdsObj)
{
PyObject* seq = PySequence_Fast(objectUniqueIdsObj, "expected a sequence of object unique ids");
if (seq)
{
int len = PySequence_Size(objectUniqueIdsObj);
for (int i=0;i<len;i++)
{
int objectUid = pybullet_internalGetFloatFromSequence(seq, i);
b3StateLoggingAddLoggingObjectUniqueId(commandHandle,objectUid);
}
Py_DECREF(seq);
}
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType==CMD_STATE_LOGGING_START_COMPLETED)
{
int loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle);
PyObject* loggingUidObj = PyInt_FromLong(loggingUniqueId);
return loggingUidObj;
}
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_stopStateLogging(PyObject* self, PyObject* args, PyObject *keywds)
@@ -2621,16 +2655,14 @@ static PyObject* pybullet_stopStateLogging(PyObject* self, PyObject* args, PyObj
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
PyObject* rayFromObj=0;
PyObject* rayToObj=0;
double from[3];
double to[3];
int loggingId=-1;
b3PhysicsClientHandle sm = 0;
static char *kwlist[] = { "loggingId", "physicsClientId", NULL };
int physicsClientId = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist,
&rayFromObj, &rayToObj,&physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,
&loggingId,&physicsClientId))
return NULL;
sm = getPhysicsClient(physicsClientId);
@@ -2639,6 +2671,17 @@ static PyObject* pybullet_stopStateLogging(PyObject* self, PyObject* args, PyObj
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (loggingId>=0)
{
b3SharedMemoryCommandHandle commandHandle;
commandHandle = b3StateLoggingCommandInit(sm);
b3StateLoggingStop(commandHandle, loggingId);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
}
Py_INCREF(Py_None);
return Py_None;
}
@@ -5019,12 +5062,12 @@ static PyMethodDef SpamMethods[] = {
"Set properties of the VR Camera such as its root transform "
"for teleporting or to track objects (camera inside a vehicle for example)."
},
{ "startRobotStateLogging", (PyCFunction)pybullet_startStateLogging, METH_VARARGS | METH_KEYWORDS,
{ "startStateLogging", (PyCFunction)pybullet_startStateLogging, METH_VARARGS | METH_KEYWORDS,
"Start logging of state, such as robot base position, orientation, joint positions etc. "
"Specify loggingType (ROBOT_LOG_MINITAUR, ROBOT_LOG_GENERIC_ROBOT, ROBOT_LOG_VR_CONTROLLERS etc), "
"Specify loggingType (STATE_LOG_MINITAUR, STATE_LOG_GENERIC_ROBOT, STATE_LOG_VR_CONTROLLERS etc), "
"fileName, optional objectUniqueId. Function returns int loggingUniqueId"
},
{ "stopRobotStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS,
{ "stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS,
"Stop logging of robot state, given a loggingUniqueId."
},
{ "rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS,
@@ -5112,9 +5155,9 @@ initpybullet(void)
PyModule_AddIntConstant(m, "VR_MAX_CONTROLLERS", MAX_VR_CONTROLLERS);
PyModule_AddIntConstant(m, "VR_MAX_BUTTONS", MAX_VR_BUTTONS);
PyModule_AddIntConstant(m, "ROBOT_LOGGING_MINITAUR", ROBOT_LOGGING_MINITAUR);
PyModule_AddIntConstant(m, "ROBOT_LOGGING_GENERIC_ROBOT", ROBOT_LOGGING_GENERIC_ROBOT);
PyModule_AddIntConstant(m, "ROBOT_LOGGING_VR_CONTROLLERS", ROBOT_LOGGING_VR_CONTROLLERS);
PyModule_AddIntConstant(m, "STATE_LOGGING_MINITAUR", STATE_LOGGING_MINITAUR);
PyModule_AddIntConstant(m, "STATE_LOGGING_GENERIC_ROBOT", STATE_LOGGING_GENERIC_ROBOT);
PyModule_AddIntConstant(m, "STATE_LOGGING_VR_CONTROLLERS", STATE_LOGGING_VR_CONTROLLERS);
SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
Py_INCREF(SpamError);