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

Add preliminary PhysX 4.0 backend for PyBullet

Browse Source 
Add inverse dynamics / mass matrix code from DeepMimic, thanks to Xue Bin (Jason) Peng
Add example how to use stable PD control for humanoid with spherical joints (see humanoidMotionCapture.py)
Fix related to TinyRenderer object transforms not updating when using collision filtering
This commit is contained in:
erwincoumans
2019-01-22 21:08:37 -08:00
parent 80684f44ea
commit ae8e83988b
366 changed files with 131855 additions and 359 deletions
Download Patch File Download Diff File Expand all files Collapse all files

167
examples/pybullet/pybullet.c
View File

@@ -10,6 +10,11 @@
#include "../SharedMemory/dart/DARTPhysicsC_API.h"
#endif
#ifdef BT_ENABLE_PHYSX
#include "../SharedMemory/physx/PhysXC_API.h"
#endif
#ifdef BT_ENABLE_MUJOCO
#include "../SharedMemory/mujoco/MuJoCoPhysicsC_API.h"
#endif
@@ -420,6 +425,13 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
break;
}
#endif
#ifdef BT_ENABLE_PHYSX
case eCONNECT_PHYSX:
{
sm = b3ConnectPhysX();
break;
}
#endif
#ifdef BT_ENABLE_MUJOCO
case eCONNECT_MUJOCO:
@@ -2395,12 +2407,14 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
double targetPositionArray[4] = { 0, 0, 0, 1 };
double targetVelocityArray[3] = { 0, 0, 0 };
double targetForceArray[3] = { 100000.0, 100000.0, 100000.0 };
int targetPositionSize = 0;
int targetVelocitySize = 0;
int targetForceSize = 0;
PyObject* targetPositionObj = 0;
PyObject* targetVelocityObj = 0;
double force = 100000.0;
PyObject* targetForceObj = 0;
double kp = 0.1;
double kd = 1.0;
double maxVelocity = -1;
@@ -2408,8 +2422,8 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
int physicsClientId = 0;
static char* kwlist[] = { "bodyUniqueId", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "maxVelocity", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|OOddddi", kwlist, &bodyUniqueId, &jointIndex, &controlMode,
&targetPositionObj, &targetVelocityObj, &force, &kp, &kd, &maxVelocity, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|OOOdddi", kwlist, &bodyUniqueId, &jointIndex, &controlMode,
&targetPositionObj, &targetVelocityObj, &targetForceObj, &kp, &kd, &maxVelocity, &physicsClientId))
{
return NULL;
}
@@ -2463,6 +2477,29 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
}
if (targetForceObj)
{
int i = 0;
PyObject* targetForceSeq = 0;
targetForceSeq = PySequence_Fast(targetForceObj, "expected a force sequence");
targetForceSize = PySequence_Size(targetForceObj);
if (targetForceSize < 0)
{
targetForceSize = 0;
}
if (targetForceSize >3)
{
targetForceSize = 3;
}
for (i = 0; i < targetForceSize; i++)
{
targetForceArray[i] = pybullet_internalGetFloatFromSequence(targetForceSeq, i);
}
}
//if (targetPositionSize == 0 && targetVelocitySize == 0)
//{
@@ -2480,7 +2517,7 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
}
if (//(controlMode != CONTROL_MODE_VELOCITY)&&
//(controlMode != CONTROL_MODE_TORQUE) &&
(controlMode != CONTROL_MODE_TORQUE) &&
(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)//&&
//(controlMode != CONTROL_MODE_PD)
)
@@ -2505,13 +2542,17 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
break;
}
#endif
case CONTROL_MODE_TORQUE:
{
b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex,
force);
if (info.m_uSize == targetForceSize)
{
b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_uIndex,
targetForceArray, targetForceSize);
}
break;
}
#endif
case CONTROL_MODE_POSITION_VELOCITY_PD:
case CONTROL_MODE_PD:
{
@@ -2544,7 +2585,11 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
//printf("Warning: targetVelocity array size doesn't match joint dimentions (got %d, expected %d).", targetVelocitySize, info.m_uSize);
}
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force);
if (info.m_uSize == targetForceSize || targetForceSize==1)
{
b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_uIndex,
targetForceArray, targetForceSize);
}
break;
}
default:
@@ -9051,6 +9096,54 @@ static PyObject* pybullet_getQuaternionFromAxisAngle(PyObject* self, PyObject* a
}
static PyObject* pybullet_getAxisDifferenceQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
{
PyObject* quatStartObj;
PyObject* quatEndObj;
double quatStart[4];
double quatEnd[4];
int physicsClientId = 0;
int hasQuatStart = 0;
int hasQuatEnd = 0;
static char* kwlist[] = { "quaternionStart", "quaternionEnd", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, &quatStartObj, &quatEndObj, &physicsClientId))
{
return NULL;
}
if (quatStartObj)
{
hasQuatStart = pybullet_internalSetVector4d(quatStartObj, quatStart);
}
if (quatEndObj)
{
hasQuatEnd = pybullet_internalSetVector4d(quatEndObj, quatEnd);
}
if (hasQuatStart && hasQuatEnd)
{
double axisOut[3];
b3GetAxisDifferenceQuaternion(quatStart, quatEnd, axisOut);
{
PyObject* pylist;
int i;
pylist = PyTuple_New(3);
for (i = 0; i < 3; i++)
PyTuple_SetItem(pylist, i, PyFloat_FromDouble(axisOut[i]));
return pylist;
}
}
else
{
PyErr_SetString(SpamError, "Require start and end quaternion, each with 4 components [x,y,z,w].");
return NULL;
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_getDifferenceQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
{
PyObject* quatStartObj;
@@ -9565,13 +9658,16 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
PyObject* objVelocitiesQdot;
PyObject* objAccelerations;
int physicsClientId = 0;
int flags = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"bodyUniqueId", "objPositions",
"objVelocities", "objAccelerations",
"flags",
"physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist,
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|ii", kwlist,
&bodyUniqueId, &objPositionsQ,
&objVelocitiesQdot, &objAccelerations,
&flags,
&physicsClientId))
{
static char* kwlist2[] = {"bodyIndex", "objPositions",
@@ -9607,7 +9703,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
double* jointPositionsQ = (double*)malloc(szInBytesQ);
double* jointVelocitiesQdot = (double*)malloc(szInBytesQdot);
double* jointAccelerations = (double*)malloc(szInBytesQdot);
double* jointForcesOutput = (double*)malloc(szInBytesQdot);
for (i = 0; i < szObPos; i++)
{
@@ -9625,10 +9721,12 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle commandHandle =
b3CalculateInverseDynamicsCommandInit2(
sm, bodyUniqueId, jointPositionsQ, szObPos, jointVelocitiesQdot,
jointAccelerations, szObVel);
b3CalculateInverseDynamicsSetFlags(commandHandle, flags);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
@@ -9637,14 +9735,13 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
{
int bodyUniqueId;
int dofCount;
b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId,
&dofCount, 0);
b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId,&dofCount, 0);
if (dofCount)
{
b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0,
jointForcesOutput);
double* jointForcesOutput = (double*)malloc(sizeof(double) * dofCount);
b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0,jointForcesOutput);
{
{
int i;
@@ -9654,6 +9751,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
PyFloat_FromDouble(jointForcesOutput[i]));
}
}
free(jointForcesOutput);
}
}
else
@@ -9665,7 +9763,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
free(jointPositionsQ);
free(jointVelocitiesQdot);
free(jointAccelerations);
free(jointForcesOutput);
if (pylist) return pylist;
}
else
@@ -9879,9 +9977,10 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py
PyObject* objPositions;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"bodyUniqueId", "objPositions", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iO|i", kwlist,
&bodyUniqueId, &objPositions, &physicsClientId))
int flags = 0;
static char* kwlist[] = {"bodyUniqueId", "objPositions", "flags", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iO|ii", kwlist,
&bodyUniqueId, &objPositions, &flags, &physicsClientId))
{
return NULL;
}
@@ -9894,15 +9993,16 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py
{
int szObPos = PySequence_Size(objPositions);
int numJoints = b3GetNumJoints(sm, bodyUniqueId);
if (numJoints && (szObPos == numJoints))
///int dofCountQ = b3GetNumJoints(sm, bodyUniqueId);
if (szObPos>=0)//(szObPos == dofCountQ))
{
int byteSizeJoints = sizeof(double) * numJoints;
PyObject* pyResultList;
int byteSizeJoints = sizeof(double) * szObPos;
PyObject* pyResultList=NULL;
double* jointPositions = (double*)malloc(byteSizeJoints);
double* massMatrix = NULL;
int i;
for (i = 0; i < numJoints; i++)
for (i = 0; i < szObPos; i++)
{
jointPositions[i] =
pybullet_internalGetFloatFromSequence(objPositions, i);
@@ -9911,17 +10011,19 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle commandHandle =
b3CalculateMassMatrixCommandInit(sm, bodyUniqueId, jointPositions);
b3CalculateMassMatrixCommandInit(sm, bodyUniqueId, jointPositions, szObPos);
b3CalculateMassMatrixSetFlags(commandHandle, flags);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CALCULATED_MASS_MATRIX_COMPLETED)
{
int dofCount;
b3GetStatusMassMatrix(sm, statusHandle, &dofCount, NULL);
pyResultList = PyTuple_New(dofCount);
if (dofCount)
{
int byteSizeDofCount = sizeof(double) * dofCount;
pyResultList = PyTuple_New(dofCount);
massMatrix = (double*)malloc(dofCount * byteSizeDofCount);
b3GetStatusMassMatrix(sm, statusHandle, NULL, massMatrix);
@@ -10368,6 +10470,11 @@ static PyMethodDef SpamMethods[] = {
{ "getDifferenceQuaternion", (PyCFunction)pybullet_getDifferenceQuaternion, METH_VARARGS | METH_KEYWORDS,
"Compute the quaternion difference from two quaternions." },
{ "getAxisDifferenceQuaternion", (PyCFunction)pybullet_getAxisDifferenceQuaternion, METH_VARARGS | METH_KEYWORDS,
"Compute the velocity axis difference from two quaternions." },
{ "calculateVelocityQuaternion", (PyCFunction)pybullet_calculateVelocityQuaternion, METH_VARARGS | METH_KEYWORDS,
"Compute the angular velocity given start and end quaternion and delta time." },
@@ -10555,6 +10662,10 @@ initpybullet(void)
PyModule_AddIntConstant(m, "DART", eCONNECT_DART); // user read
#endif
#ifdef BT_ENABLE_PHYSX
PyModule_AddIntConstant(m, "PhysX", eCONNECT_PHYSX);
#endif
#ifdef BT_ENABLE_MUJOCO
PyModule_AddIntConstant(m, "MuJoCo", eCONNECT_MUJOCO); // user read
#endif
@@ -10667,6 +10778,8 @@ initpybullet(void)
PyModule_AddIntConstant(m, "URDF_INITIALIZE_SAT_FEATURES", URDF_INITIALIZE_SAT_FEATURES);
PyModule_AddIntConstant(m, "URDF_USE_MATERIAL_COLORS_FROM_MTL", URDF_USE_MATERIAL_COLORS_FROM_MTL);
PyModule_AddIntConstant(m, "URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL", URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL);
PyModule_AddIntConstant(m, "URDF_MAINTAIN_LINK_ORDER", URDF_MAINTAIN_LINK_ORDER);
PyModule_AddIntConstant(m, "ACTIVATION_STATE_ENABLE_SLEEPING", eActivationStateEnableSleeping);
PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_SLEEPING", eActivationStateDisableSleeping);