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/setup.py b/setup.py index bf36944a9..42ff89b26 100644 --- a/setup.py +++ b/setup.py @@ -417,7 +417,7 @@ else: setup( name = 'pybullet', - version='1.0.1', + version='1.0.2', description='Official Python Interface for the Bullet Physics SDK Robotics Simulator', long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', 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; }