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

implement pybullet.getContactPointData(), two optional object unique ids as filter

Browse Source 
returns a pylist of contact points. Each point has the following data:

	0     int m_contactFlags;//unused for now
         1     int m_bodyUniqueIdA;
         2     int m_bodyUniqueIdB;
         3     int m_linkIndexA;
         4     int m_linkIndexB;
         5-6-7     double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates
         8-9-10     double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates
         11-12-13     double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A
         14     double m_contactDistance;//negative number is penetration, positive is distance.

         15    double m_normalForce;
This commit is contained in:
Erwin Coumans
2016-09-01 18:28:39 -07:00
parent a370c3bbac
commit e98fca1e5e
8 changed files with 205 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -39,7 +39,6 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<int> m_cachedSegmentationMaskBuffer;
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
btAlignedObjectArray<b3ContactPointDynamicsData>m_cachedContactPointDynamics;
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
SharedMemoryStatus m_tempBackupServerStatus;
@@ -572,7 +571,6 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied);
m_data->m_cachedContactPointDynamics.resize(0);
b3ContactPointData* contactData = (b3ContactPointData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
@@ -653,8 +651,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
{
//todo: request remaining points, if needed
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied)
{
command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION;
@@ -765,7 +762,6 @@ void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* c
void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
{
contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size();
contactPointData->m_contactDynamicsData = 0;
contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0;
}

58
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -39,6 +39,8 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
btAlignedObjectArray<int> m_cachedSegmentationMask;
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
PhysicsServerCommandProcessor* m_commandProcessor;
@@ -193,6 +195,51 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma
return m_data->m_hasStatus;
}
bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& orgCommand)
{
SharedMemoryCommand command = orgCommand;
const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
do
{
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
if (hasStatus)
{
if (m_data->m_verboseOutput)
{
b3Printf("Contact Point Information Request OK\n");
}
int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex;
int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied);
b3ContactPointData* contactData = (b3ContactPointData*)&m_data->m_bulletStreamDataServerToClient[0];
for (int i=0;i<numContactsCopied;i++)
{
m_data->m_cachedContactPoints[startContactIndex+i] = contactData[i];
}
if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied)
{
command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION;
command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex+serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
command.m_requestContactPointArguments.m_objectAIndexFilter = -1;
command.m_requestContactPointArguments.m_objectBIndexFilter = -1;
}
}
} while (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied);
return m_data->m_hasStatus;
}
bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
{
SharedMemoryCommand command = orgCommand;
@@ -326,6 +373,10 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
{
return processCamera(command);
}
if (command.m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION)
{
return processContactPointData(command);
}
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
@@ -490,9 +541,8 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
{
if (contactPointData)
{
contactPointData->m_numContactPoints = 0;
}
contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size();
contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0;
}

2
examples/SharedMemory/PhysicsDirect.h
View File

@@ -21,6 +21,8 @@ protected:
bool processCamera(const struct SharedMemoryCommand& orgCommand);
bool processContactPointData(const struct SharedMemoryCommand& orgCommand);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
public:

35
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2151,7 +2151,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
for (int i=0;i<numContactManifolds;i++)
{
const btPersistentManifold* manifold = m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i];
int linkIndexA = -1;
int linkIndexB = -1;
int objectIndexB = -1;
const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1());
@@ -2162,6 +2164,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
if (mblB && mblB->m_multiBody)
{
linkIndexB = mblB->m_link;
objectIndexB = mblB->m_multiBody->getUserIndex2();
}
@@ -2174,12 +2177,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
if (mblA && mblA->m_multiBody)
{
linkIndexA = mblA->m_link;
objectIndexA = mblA->m_multiBody->getUserIndex2();
}
btAssert(bodyA || mblA);
//apply the filter, if any
//apply the filter, if the user provides it
if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0)
{
if ((clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexA) &&
@@ -2187,6 +2192,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
continue;
}
//apply the second object filter, if the user provides it
if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter>=0)
{
if ((clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexA) &&
@@ -2196,29 +2202,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
for (int p=0;p<manifold->getNumContacts();p++)
{
//if the point passes the optional filter, add it
if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0)
{
//one of the two unique Ids has to match...
}
b3ContactPointData pt;
pt.m_bodyUniqueIdA = -1;
pt.m_bodyUniqueIdB = -1;
pt.m_bodyUniqueIdA = objectIndexA;
pt.m_bodyUniqueIdB = objectIndexB;
const btManifoldPoint& srcPt = manifold->getContactPoint(p);
pt.m_contactDistance = srcPt.getDistance();
pt.m_contactFlags = 0;
pt.m_contactPointDynamicsIndex = -1;
pt.m_linkIndexA = -1;
pt.m_linkIndexB = -1;
pt.m_linkIndexA = linkIndexA;
pt.m_linkIndexB = linkIndexB;
for (int j=0;j<3;j++)
{
pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j];
pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j];
pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
}
pt.m_normalForce = srcPt.getAppliedImpulse()/m_data->m_physicsDeltaTime;
// pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
m_data->m_cachedContactPoints.push_back (pt);
}
}
@@ -2298,8 +2298,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
// Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force) &&
-1 != tree->setGravityInWorldFrame(id_grav))
if (-1 != tree->setGravityInWorldFrame(id_grav) &&
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs;
@@ -2649,7 +2650,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size());
}
m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,1./240.);
m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,m_data->m_physicsDeltaTime);
}
}

23
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -139,7 +139,8 @@ struct b3CameraImageData
struct b3ContactPointData
{
int m_contactFlags;//flag wether certain fields below are valid
//todo: expose some contact flags, such as telling which fields below are valid
int m_contactFlags;
int m_bodyUniqueIdA;
int m_bodyUniqueIdB;
int m_linkIndexA;
@@ -148,23 +149,23 @@ struct b3ContactPointData
double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates
double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A
double m_contactDistance;//negative number is penetration, positive is distance.
int m_contactPointDynamicsIndex;
double m_normalForce;
//todo: expose the friction forces as well
// double m_linearFrictionDirection0[3];
// double m_linearFrictionForce0;
// double m_linearFrictionDirection1[3];
// double m_linearFrictionForce1;
// double m_angularFrictionDirection[3];
// double m_angularFrictionForce;
};
struct b3ContactPointDynamicsData
{
double m_normalForce;
double m_linearFrictionDirection[3];
double m_linearFrictionForce;
double m_angularFrictionDirection[3];
double m_angularFrictionForce;
};
struct b3ContactInformation
{
int m_numContactPoints;
struct b3ContactPointData* m_contactPointData;
struct b3ContactPointDynamicsData* m_contactDynamicsData;
};
///b3LinkState provides extra information such as the Cartesian world coordinates

114
examples/pybullet/pybullet.c
View File

@@ -1113,6 +1113,116 @@ static int pybullet_internalSetVector(PyObject* objMat, float vector[3])
return 0;
}
static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args)
{
int size= PySequence_Size(args);
int objectUniqueIdA = -1;
int objectUniqueIdB = -1;
b3SharedMemoryCommandHandle commandHandle;
struct b3ContactInformation contactPointData;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
PyObject* pyResultList=0;
if (size==1)
{
if (!PyArg_ParseTuple(args, "i", &objectUniqueIdA))
{
PyErr_SetString(SpamError, "Error parsing object unique id");
return NULL;
}
}
if (size==2)
{
if (!PyArg_ParseTuple(args, "ii", &objectUniqueIdA,&objectUniqueIdB))
{
PyErr_SetString(SpamError, "Error parsing object unique id");
return NULL;
}
}
commandHandle = b3InitRequestContactPointInformation(sm);
b3SetContactFilterBodyA(commandHandle,objectUniqueIdA);
b3SetContactFilterBodyB(commandHandle,objectUniqueIdB);
b3SubmitClientCommand(sm, commandHandle);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType==CMD_CONTACT_POINT_INFORMATION_COMPLETED)
{
/*
0 int m_contactFlags;
1 int m_bodyUniqueIdA;
2 int m_bodyUniqueIdB;
3 int m_linkIndexA;
4 int m_linkIndexB;
5-6-7 double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates
8-9-10 double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates
11-12-13 double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A
14 double m_contactDistance;//negative number is penetration, positive is distance.
15 double m_normalForce;
*/
b3GetContactPointInformation(sm, &contactPointData);
pyResultList = PyTuple_New(contactPointData.m_numContactPoints);
for (int i=0;i<contactPointData.m_numContactPoints;i++)
{
PyObject* contactObList = PyTuple_New(16);//see above 16 fields
PyObject* item;
item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_contactFlags);
PyTuple_SetItem(contactObList,0,item);
item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_bodyUniqueIdA);
PyTuple_SetItem(contactObList,1,item);
item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_bodyUniqueIdB);
PyTuple_SetItem(contactObList,2,item);
item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexA);
PyTuple_SetItem(contactObList,3,item);
item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexB);
PyTuple_SetItem(contactObList,4,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnAInWS[0]);
PyTuple_SetItem(contactObList,5,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnAInWS[1]);
PyTuple_SetItem(contactObList,6,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnAInWS[2]);
PyTuple_SetItem(contactObList,7,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnBInWS[0]);
PyTuple_SetItem(contactObList,8,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnBInWS[1]);
PyTuple_SetItem(contactObList,9,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnBInWS[2]);
PyTuple_SetItem(contactObList,10,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[0]);
PyTuple_SetItem(contactObList,11,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[1]);
PyTuple_SetItem(contactObList,12,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[2]);
PyTuple_SetItem(contactObList,13,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_contactDistance);
PyTuple_SetItem(contactObList,14,item);
item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_normalForce);
PyTuple_SetItem(contactObList,15,item);
PyTuple_SetItem(pyResultList, i, contactObList);
}
return pyResultList;
}
Py_INCREF(Py_None);
return Py_None;
}
// Render an image from the current timestep of the simulation
//
// Examples:
@@ -1794,6 +1904,10 @@ static PyMethodDef SpamMethods[] = {
{"renderImage", pybullet_renderImage, METH_VARARGS,
"Render an image (given the pixel resolution width, height, camera view matrix, projection matrix, near, and far values), and return the 8-8-8bit RGB pixel data and floating point depth values"},
{"getContactPointData", pybullet_getContactPointData, METH_VARARGS,
"Return the contact point information for all or some of pairwise object-object collisions. Optional arguments one or two object unique ids, that need to be involved in the contact."},
{"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS,
"Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to quaternion [x,y,z,w]"},

4
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -1053,8 +1053,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
{
//only a single rollingFriction per manifold
rollingFriction--;
//disabled: only a single rollingFriction per manifold
// rollingFriction--;
if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
{
relAngVel.normalize();

4
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -963,8 +963,8 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
{
//only a single rollingFriction per manifold
rollingFriction--;
//disabled: only a single rollingFriction per manifold
//rollingFriction--;
if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
{
relAngVel.normalize();