From 53a82819a0b09731fd718a5bea00e33b3a16a4de Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 10 May 2017 15:01:25 -0700 Subject: [PATCH] expose b3LoadMJCFCommandSetFlags / pybullet.pybullet_loadMJCF(fileName,flags=pybullet.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) --- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 18 +++++++-- .../Importers/ImportURDFDemo/URDF2Bullet.h | 4 +- examples/SharedMemory/PhysicsClientC_API.cpp | 10 +++++ examples/SharedMemory/PhysicsClientC_API.h | 2 +- .../PhysicsServerCommandProcessor.cpp | 7 +++- examples/SharedMemory/SharedMemoryCommands.h | 3 ++ examples/SharedMemory/SharedMemoryPublic.h | 2 + examples/pybullet/pybullet.c | 16 ++++++-- .../Featherstone/btMultiBodyLink.h | 3 +- .../Featherstone/btMultiBodyLinkCollider.h | 40 +++++++++++++++++-- 10 files changed, 89 insertions(+), 16 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index fcee5a150..d7ed716b0 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -473,12 +473,12 @@ void ConvertURDF2BulletInternal( int collisionFilterMask = isDynamic? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); int colGroup=0, colMask=0; - int flags = u2b.getCollisionGroupAndMask(urdfLinkIndex,colGroup, colMask); - if (flags & URDF_HAS_COLLISION_GROUP) + int collisionFlags = u2b.getCollisionGroupAndMask(urdfLinkIndex,colGroup, colMask); + if (collisionFlags & URDF_HAS_COLLISION_GROUP) { collisionFilterGroup = colGroup; } - if (flags & URDF_HAS_COLLISION_MASK) + if (collisionFlags & URDF_HAS_COLLISION_MASK) { collisionFilterMask = colMask; } @@ -498,6 +498,14 @@ void ConvertURDF2BulletInternal( if (mbLinkIndex>=0) //???? double-check +/- 1 { cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider=col; + if (flags&CUF_USE_SELF_COLLISION_EXCLUDE_PARENT) + { + cache.m_bulletMultiBody->getLink(mbLinkIndex).m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + } + if (flags&CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) + { + cache.m_bulletMultiBody->getLink(mbLinkIndex).m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION; + } } else { cache.m_bulletMultiBody->setBaseCollider(col); @@ -539,7 +547,9 @@ void ConvertURDF2Bullet( if (world1 && cache.m_bulletMultiBody) { btMultiBody* mb = cache.m_bulletMultiBody; - mb->setHasSelfCollision(false); + + mb->setHasSelfCollision((flags&CUF_USE_SELF_COLLISION)!=0); + mb->finalizeMultiDof(); btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex]; diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index de8978faf..ccc63c4d9 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -19,7 +19,9 @@ enum ConvertURDFFlags { // Use inertia values in URDF instead of recomputing them from collision shape. CUF_USE_URDF_INERTIA = 2, CUF_USE_MJCF = 4, - CUF_USE_SELF_COLLISION=8 + CUF_USE_SELF_COLLISION=8, + CUF_USE_SELF_COLLISION_EXCLUDE_PARENT=16, + CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32, }; void ConvertURDF2Bullet(const URDFImporterInterface& u2b, diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 5c235125f..f75e2c5e7 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -160,6 +160,16 @@ b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClie return 0; } +void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_LOAD_MJCF); + if (command->m_type == CMD_LOAD_MJCF) + { + command->m_mjcfArguments.m_flags = flags; + command->m_updateFlags |= URDF_ARGS_HAS_CUSTOM_URDF_FLAGS; + } +} b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 021bcf493..382e144d8 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -237,7 +237,7 @@ int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int fla b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName); b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName); b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName); - +void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); ///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index bd18e3cf7..6e98ad191 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5693,7 +5693,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Printf("Processed CMD_LOAD_MJCF:%s", mjcfArgs.m_mjcfFileName); } bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (mjcfArgs.m_useMultiBody!=0) : true; - int flags = CUF_USE_MJCF;//CUF_USE_URDF_INERTIA + int flags = CUF_USE_MJCF; + if (clientCmd.m_updateFlags&URDF_ARGS_HAS_CUSTOM_URDF_FLAGS) + { + flags |= clientCmd.m_mjcfArguments.m_flags; + } + bool completedOk = loadMjcf(mjcfArgs.m_mjcfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags); if (completedOk) { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index e1a494fb8..df9dbc14e 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -93,10 +93,13 @@ struct UrdfArgs int m_urdfFlags; }; + + struct MjcfArgs { char m_mjcfFileName[MAX_URDF_FILENAME_LENGTH]; int m_useMultiBody; + int m_flags; }; struct BulletDataStreamArgs diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 9a373b115..eccda8cfc 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -509,6 +509,8 @@ enum eURDF_Flags { URDF_USE_INERTIA_FROM_FILE=2,//sync with URDF2Bullet.h 'ConvertURDFFlags' URDF_USE_SELF_COLLISION=8,//see CUF_USE_SELF_COLLISION + URDF_USE_SELF_COLLISION_EXCLUDE_PARENT=16, + URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32, }; #endif//SHARED_MEMORY_PUBLIC_H diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 726abebac..372b281a2 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -563,8 +563,10 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key int bodyIndicesOut[MAX_SDF_BODIES]; PyObject* pylist = 0; int physicsClientId = 0; - static char* kwlist[] = {"mjcfFileName", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &mjcfFileName, &physicsClientId)) + int flags = -1; + + static char* kwlist[] = {"mjcfFileName", "flags", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|ii", kwlist, &mjcfFileName, &flags, &physicsClientId)) { return NULL; } @@ -576,6 +578,10 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key } command = b3LoadMJCFCommandInit(sm, mjcfFileName); + if (flags>=0) + { + b3LoadMJCFCommandSetFlags(command,flags); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType != CMD_MJCF_LOADING_COMPLETED) @@ -3206,8 +3212,8 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb static PyObject* pybullet_submitProfileTiming(PyObject* self, PyObject* args, PyObject* keywds) { - b3SharedMemoryStatusHandle statusHandle; - int statusType; +// b3SharedMemoryStatusHandle statusHandle; +// int statusType; char* eventName = 0; int duractionInMicroSeconds=-1; @@ -6208,6 +6214,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION); + PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT); + PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS", URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS); PyModule_AddIntConstant(m, "MAX_RAY_INTERSECTION_BATCH_SIZE", MAX_RAY_INTERSECTION_BATCH_SIZE); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 2734bf4bd..6cd8c69d3 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -22,7 +22,8 @@ subject to the following restrictions: enum btMultiBodyLinkFlags { - BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1 + BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1, + BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2, }; //both defines are now permanently enabled diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h index 5080ea874..9a375e3e4 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -74,15 +74,47 @@ public: if (m_link>=0) { const btMultibodyLink& link = m_multiBody->getLink(this->m_link); - if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link) - return false; + if (link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) + { + if ( link.m_parent == other->m_link) + return false; + } + if (link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION) + { + int parent_of_this = m_link; + while (1) + { + if (parent_of_this==-1) + break; + parent_of_this = m_multiBody->getLink(parent_of_this).m_parent; + if (parent_of_this==other->m_link) + { + return false; + } + } + } } if (other->m_link>=0) { const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link); - if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link) - return false; + if (otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) + { + if (otherLink.m_parent == this->m_link) + return false; + } + if (otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION) + { + int parent_of_other = other->m_link; + while (1) + { + if (parent_of_other==-1) + break; + parent_of_other = m_multiBody->getLink(parent_of_other).m_parent; + if (parent_of_other==this->m_link) + return false; + } + } } return true; }