expose b3LoadMJCFCommandSetFlags / pybullet.pybullet_loadMJCF(fileName,flags=pybullet.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)

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);
+				int collisionFlags = u2b.getCollisionGroupAndMask(urdfLinkIndex,colGroup, colMask);
-				if (flags & URDF_HAS_COLLISION_GROUP)
+				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];

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,

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)
 {

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

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)
                         {

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

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

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};
+	int flags = -1;
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &mjcfFileName, &physicsClientId))
+
+	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;
+//	b3SharedMemoryStatusHandle statusHandle;
-	int statusType;
+//	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);
 

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

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)
+			if (link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
-				return false;
+			{
+				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)
+			if (otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
-				return false;
+			{
+				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;
 	}