diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 317902254..e622cf716 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -39,7 +39,6 @@ struct PhysicsClientSharedMemoryInternalData { btAlignedObjectArray m_cachedSegmentationMaskBuffer; btAlignedObjectArray m_cachedContactPoints; - btAlignedObjectArraym_cachedContactPointDynamics; btAlignedObjectArray 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; } diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index e465aa6d9..6ac5d6212 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -39,6 +39,8 @@ struct PhysicsDirectInternalData btAlignedObjectArray m_cachedCameraDepthBuffer; btAlignedObjectArray m_cachedSegmentationMask; + btAlignedObjectArray 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;im_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; + } diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index ee3709ecf..eae128998 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -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: diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 7bfd03cc1..03357e59f 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2151,7 +2151,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm for (int i=0;im_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;pgetNumContacts();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); } } diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 231929ab6..28038a3ad 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -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 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f7f062073..75e4a02a5 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -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;i0.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(); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 08bee49c4..b40c6d72b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -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();