diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 8cd657d81..afcdcb76d 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -306,7 +306,6 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double { - { b3SharedMemoryCommandHandle cmd_handle = b3RequestActualStateCommandInit(sm, bodyIndex); @@ -463,6 +462,64 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) return NULL; } + PyObject *pyListJointInfo; + + struct b3JointInfo info; + + int bodyIndex = -1; + int jointIndex = -1; + int jointInfoSize = 8; //size of struct b3JointInfo + + int size= PySequence_Size(args); + + if (size==2) // get body index and joint index + { + if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) + { + + // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); + + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + pyListJointInfo = PyTuple_New(jointInfoSize); + + b3GetJointInfo(sm, bodyIndex, jointIndex, &info); + + // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", + // info.m_jointIndex, + // info.m_jointName, + // info.m_jointType, + // info.m_qIndex, + // info.m_uIndex); + // printf(" flags=%d jointDamping=%f jointFriction=%f\n", + // info.m_flags, + // info.m_jointDamping, + // info.m_jointFriction); + + PyTuple_SetItem(pyListJointInfo, 0, + PyInt_FromLong(info.m_jointIndex)); + PyTuple_SetItem(pyListJointInfo, 1, + PyString_FromString(info.m_jointName)); + PyTuple_SetItem(pyListJointInfo, 2, + PyInt_FromLong(info.m_jointType)); + PyTuple_SetItem(pyListJointInfo, 3, + PyInt_FromLong(info.m_qIndex)); + PyTuple_SetItem(pyListJointInfo, 4, + PyInt_FromLong(info.m_uIndex)); + PyTuple_SetItem(pyListJointInfo, 5, + PyInt_FromLong(info.m_flags)); + PyTuple_SetItem(pyListJointInfo, 6, + PyFloat_FromDouble(info.m_jointDamping)); + PyTuple_SetItem(pyListJointInfo, 7, + PyFloat_FromDouble(info.m_jointFriction)); + + return pyListJointInfo; + } + } + Py_INCREF(Py_None); return Py_None; }