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

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
yunfeibai
2017-02-03 12:32:08 -08:00
parent ac5a8aa2d6 c1eabcf059
commit 3442b6524a
10 changed files with 170 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

111
examples/pybullet/pybullet.c
View File

@@ -2668,7 +2668,7 @@ static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keyw
return Py_None;
}
static PyObject* pybullet_getMatrixFromQuaterion(PyObject* self, PyObject* args)
static PyObject* pybullet_getMatrixFromQuaternion(PyObject* self, PyObject* args)
{
PyObject* quatObj;
double quat[4];
@@ -2907,7 +2907,7 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
pyResultList = PyTuple_New(visualShapeInfo.m_numVisualShapes);
for (i = 0; i < visualShapeInfo.m_numVisualShapes; i++)
{
PyObject* visualShapeObList = PyTuple_New(7);
PyObject* visualShapeObList = PyTuple_New(8);
PyObject* item;
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_objectUniqueId);
PyTuple_SetItem(visualShapeObList, 0, item);
@@ -2920,11 +2920,11 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
{
PyObject* vec = PyTuple_New(3);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[0]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_dimensions[0]);
PyTuple_SetItem(vec, 0, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[1]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_dimensions[1]);
PyTuple_SetItem(vec, 1, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[2]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_dimensions[2]);
PyTuple_SetItem(vec, 2, item);
PyTuple_SetItem(visualShapeObList, 3, vec);
}
@@ -2934,28 +2934,41 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
{
PyObject* vec = PyTuple_New(3);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[0]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[0]);
PyTuple_SetItem(vec, 0, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[1]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[1]);
PyTuple_SetItem(vec, 1, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[2]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[2]);
PyTuple_SetItem(vec, 2, item);
PyTuple_SetItem(visualShapeObList, 5, vec);
}
{
PyObject* vec = PyTuple_New(4);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[3]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[3]);
PyTuple_SetItem(vec, 0, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[4]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[4]);
PyTuple_SetItem(vec, 1, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[5]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[5]);
PyTuple_SetItem(vec, 2, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[6]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[6]);
PyTuple_SetItem(vec, 3, item);
PyTuple_SetItem(visualShapeObList, 6, vec);
}
{
PyObject* rgba = PyTuple_New(4);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[0]);
PyTuple_SetItem(rgba, 0, item);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[1]);
PyTuple_SetItem(rgba, 1, item);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[2]);
PyTuple_SetItem(rgba, 2, item);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[3]);
PyTuple_SetItem(rgba, 3, item);
PyTuple_SetItem(visualShapeObList, 7, rgba);
}
PyTuple_SetItem(pyResultList, i, visualShapeObList);
}
@@ -3362,6 +3375,63 @@ static PyObject* pybullet_updateUserConstraint(PyObject* self, PyObject* args, P
}
*/
static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject* args, PyObject *keywds)
{
int bodyUniqueId = -1;
int jointIndex = -1;
int enableSensor = 1;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
int numJoints = -1;
static char *kwlist[] = {"bodyUniqueId", "jointIndex" ,"enableSensor", "physicsClientId" };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &jointIndex, &enableSensor,&physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (bodyUniqueId < 0)
{
PyErr_SetString(SpamError, "Error: invalid bodyUniqueId");
return NULL;
}
numJoints = b3GetNumJoints(sm, bodyUniqueId);
if ((jointIndex < 0) || (jointIndex >= numJoints))
{
PyErr_SetString(SpamError, "Error: invalid jointIndex.");
return NULL;
}
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
commandHandle = b3CreateSensorCommandInit(sm, bodyUniqueId);
b3CreateSensorEnable6DofJointForceTorqueSensor(commandHandle, jointIndex, enableSensor);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CLIENT_COMMAND_COMPLETED)
{
Py_INCREF(Py_None);
return Py_None;
}
}
PyErr_SetString(SpamError, "Error creating sensor.");
return NULL;
}
static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, PyObject *keywds)
{
@@ -3455,15 +3525,11 @@ static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, P
int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle);
PyObject* ob = PyLong_FromLong(userConstraintUid);
return ob;
} else
{
PyErr_SetString(SpamError, "createConstraint failed.");
return NULL;
}
Py_INCREF(Py_None);
return Py_None;
PyErr_SetString(SpamError, "createConstraint failed.");
return NULL;
}
static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, PyObject *keywds) {
@@ -4690,6 +4756,10 @@ static PyMethodDef SpamMethods[] = {
"Remove a constraint using its unique id."
},
{ "enableJointForceTorqueSensor", (PyCFunction)pybullet_enableJointForceTorqueSensor, METH_VARARGS | METH_KEYWORDS,
"Enable or disable a joint force/torque sensor measuring the joint reaction forces."
},
{"saveWorld", (PyCFunction)pybullet_saveWorld, METH_VARARGS| METH_KEYWORDS,
"Save a approximate Python file to reproduce the current state of the world: saveWorld"
"(filename). (very preliminary and approximately)"},
@@ -4868,7 +4938,7 @@ static PyMethodDef SpamMethods[] = {
"Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF "
"convention"},
{"getMatrixFromQuaterion", pybullet_getMatrixFromQuaterion,METH_VARARGS,
{"getMatrixFromQuaternion", pybullet_getMatrixFromQuaternion,METH_VARARGS,
"Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major)"},
{"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS| METH_KEYWORDS,
@@ -4954,6 +5024,9 @@ initpybullet(void)
PyModule_AddIntConstant(m, "JOINT_FIXED", eFixedType); // user read
PyModule_AddIntConstant(m, "JOINT_POINT2POINT", ePoint2PointType); // user read
PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType); // user read
PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE);
PyModule_AddIntConstant(m, "VELOCITY_CONTROL",
CONTROL_MODE_VELOCITY); // user read