diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 0af4728bd..48c451bf3 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -474,7 +474,7 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor return true; } -static btCollisionShape* createConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale) +static btCollisionShape* createConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale, int flags) { B3_PROFILE("createConvexHullFromShapes"); btCompoundShape* compound = new btCompoundShape(); @@ -513,7 +513,11 @@ static btCollisionShape* createConvexHullFromShapes(std::vectorrecalcLocalAabb(); convexHull->optimizeConvexHull(); - convexHull->initializePolyhedralFeatures(); + if (flags & CUF_INITIALIZE_SAT_FEATURES) + { + convexHull->initializePolyhedralFeatures(); + } + compound->addChildShape(identity,convexHull); } @@ -683,7 +687,10 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); cylZShape->setMargin(gUrdfDefaultCollisionMargin); cylZShape->recalcLocalAabb(); - cylZShape->initializePolyhedralFeatures(); + if (m_data->m_flags & CUF_INITIALIZE_SAT_FEATURES) + { + cylZShape->initializePolyhedralFeatures(); + } cylZShape->optimizeConvexHull(); shape = cylZShape; } @@ -695,7 +702,10 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl btVector3 extents = collision->m_geometry.m_boxSize; btBoxShape* boxShape = new btBoxShape(extents*0.5f); //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); - boxShape->initializePolyhedralFeatures(); + if (m_data->m_flags & CUF_INITIALIZE_SAT_FEATURES) + { + boxShape->initializePolyhedralFeatures(); + } shape = boxShape; shape ->setMargin(gUrdfDefaultCollisionMargin); break; @@ -779,7 +789,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str()); //create a convex hull for each shape, and store it in a btCompoundShape - shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); + shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale, m_data->m_flags); m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, *collision); return shape; } @@ -907,7 +917,10 @@ upAxisMat.setIdentity(); BT_PROFILE("convert btConvexHullShape"); btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); convexHull->optimizeConvexHull(); - convexHull->initializePolyhedralFeatures(); + if (m_data->m_flags & CUF_INITIALIZE_SAT_FEATURES) + { + convexHull->initializePolyhedralFeatures(); + } convexHull->setMargin(gUrdfDefaultCollisionMargin); convexHull->recalcLocalAabb(); //convexHull->setLocalScaling(collision->m_geometry.m_meshScale); diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index deca1c3b9..87fd698b9 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -30,6 +30,7 @@ enum ConvertURDFFlags { CUF_MJCF_COLORS_FROM_FILE=512, CUF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024, CUF_ENABLE_SLEEPING=2048, + CUF_INITIALIZE_SAT_FEATURES=4096, }; struct UrdfVisualShapeCache diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 933d5b266..f5682b1a2 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -723,6 +723,7 @@ enum eURDF_Flags MJCF_COLORS_FROM_FILE=512, URDF_ENABLE_CACHED_GRAPHICS_SHAPES=1024, URDF_ENABLE_SLEEPING=2048, + URDF_INITIALIZE_SAT_FEATURES = 4096, }; enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes diff --git a/examples/pybullet/examples/satCollision.py b/examples/pybullet/examples/satCollision.py index 84111bf6c..847d2b8e6 100644 --- a/examples/pybullet/examples/satCollision.py +++ b/examples/pybullet/examples/satCollision.py @@ -4,11 +4,13 @@ import time p.connect(p.GUI) p.setGravity(0,0,-10) p.setPhysicsEngineParameter(enableSAT=1) -p.loadURDF("cube_concave.urdf",[0,0,-25], globalScaling=50, useFixedBase=True) -p.loadURDF("cube.urdf",[0,0,1], globalScaling=1) -p.loadURDF("duck_vhacd.urdf",[1,0,1], globalScaling=1) +p.loadURDF("cube_concave.urdf",[0,0,-25], globalScaling=50, useFixedBase=True, flags=p.URDF_INITIALIZE_SAT_FEATURES) +p.loadURDF("cube.urdf",[0,0,1], globalScaling=1,flags=p.URDF_INITIALIZE_SAT_FEATURES) +p.loadURDF("duck_vhacd.urdf",[1,0,1], globalScaling=1,flags=p.URDF_INITIALIZE_SAT_FEATURES) while (p.isConnected()): p.stepSimulation() + pts = p.getContactPoints() + #print("num contacts = ", len(pts)) time.sleep(1./240.) \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 5d027125d..09bd9cef8 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -9616,7 +9616,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "MJCF_COLORS_FROM_FILE", MJCF_COLORS_FROM_FILE); PyModule_AddIntConstant(m, "URDF_ENABLE_CACHED_GRAPHICS_SHAPES", URDF_ENABLE_CACHED_GRAPHICS_SHAPES); PyModule_AddIntConstant(m, "URDF_ENABLE_SLEEPING", URDF_ENABLE_SLEEPING); - + PyModule_AddIntConstant(m, "URDF_INITIALIZE_SAT_FEATURES", URDF_INITIALIZE_SAT_FEATURES); + 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);