[MJCF import] add custom broadphase collision filter, for MJCF/MuJoCo compatibility

[MJCF import] improve MuJoCo importer, support collision filters
fixed a few more warnings
This commit is contained in:
Erwin Coumans
2017-01-16 08:23:49 -08:00
parent c0c4c8ba3f
commit 8e9181f85c
8 changed files with 156 additions and 24 deletions

View File

@@ -1137,7 +1137,7 @@ bool BulletMJCFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
int BulletMJCFImporter::getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const
{
int flags = 0;
/*
const UrdfLink* link = m_data->getLink(m_data->m_activeModel,linkIndex);
if (link)
{
@@ -1151,7 +1151,7 @@ int BulletMJCFImporter::getCollisionGroupAndMask(int linkIndex, int& colGroup, i
}
}
*/
return flags;
}

View File

@@ -51,6 +51,8 @@ static btAlignedObjectArray<std::string> gMCFJFileNameArray;
#define MAX_NUM_MOTORS 1024
struct ImportMJCFInternalData
{
ImportMJCFInternalData()
@@ -171,13 +173,19 @@ struct MyMJCFLogger : public MJCFErrorLogger
}
};
void ImportMJCFSetup::initPhysics()
{
m_guiHelper->setUpAxis(m_upAxis);
this->createEmptyDynamicsWorld();
createEmptyDynamicsWorld();
//MuJoCo uses a slightly different collision filter mode, use the FILTER_GROUPAMASKB_OR_GROUPBMASKA2
//@todo also use the modified collision filter for raycast and other collision related queries
m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2;
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(