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

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

Browse Source
This commit is contained in:
Erwin Coumans
2017-05-10 15:01:25 -07:00
parent d54eab16e1
commit 53a82819a0
10 changed files with 89 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -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];

4
examples/Importers/ImportURDFDemo/URDF2Bullet.h
View File

@@ -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,

10
examples/SharedMemory/PhysicsClientC_API.cpp
View File

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

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -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

7
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

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

3
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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

2
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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

16
examples/pybullet/pybullet.c
View File

@@ -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);

3
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@@ -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

40
src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
View File

@@ -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;
}