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

PyBullet.loadURDF, expose flags=URDF_INITIALIZE_SAT_FEATURES

Browse Source
This commit is contained in:
erwincoumans
2018-06-13 15:35:56 -07:00
parent b75c63f2a2
commit 25c5e87dc2
5 changed files with 28 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -474,7 +474,7 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor
return true; return true;
} }
static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale) static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, int flags)
{ {
B3_PROFILE("createConvexHullFromShapes"); B3_PROFILE("createConvexHullFromShapes");
btCompoundShape* compound = new btCompoundShape(); btCompoundShape* compound = new btCompoundShape();
@@ -513,7 +513,11 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
convexHull->recalcLocalAabb(); convexHull->recalcLocalAabb();
convexHull->optimizeConvexHull(); convexHull->optimizeConvexHull();
convexHull->initializePolyhedralFeatures(); if (flags & CUF_INITIALIZE_SAT_FEATURES)
{
convexHull->initializePolyhedralFeatures();
}
compound->addChildShape(identity,convexHull); compound->addChildShape(identity,convexHull);
} }
@@ -683,7 +687,10 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->setMargin(gUrdfDefaultCollisionMargin); cylZShape->setMargin(gUrdfDefaultCollisionMargin);
cylZShape->recalcLocalAabb(); cylZShape->recalcLocalAabb();
cylZShape->initializePolyhedralFeatures(); if (m_data->m_flags & CUF_INITIALIZE_SAT_FEATURES)
{
cylZShape->initializePolyhedralFeatures();
}
cylZShape->optimizeConvexHull(); cylZShape->optimizeConvexHull();
shape = cylZShape; shape = cylZShape;
} }
@@ -695,7 +702,10 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
btVector3 extents = collision->m_geometry.m_boxSize; btVector3 extents = collision->m_geometry.m_boxSize;
btBoxShape* boxShape = new btBoxShape(extents*0.5f); btBoxShape* boxShape = new btBoxShape(extents*0.5f);
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); //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 = boxShape;
shape ->setMargin(gUrdfDefaultCollisionMargin); shape ->setMargin(gUrdfDefaultCollisionMargin);
break; break;
@@ -779,7 +789,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str()); 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 //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); m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, *collision);
return shape; return shape;
} }
@@ -907,7 +917,10 @@ upAxisMat.setIdentity();
BT_PROFILE("convert btConvexHullShape"); BT_PROFILE("convert btConvexHullShape");
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
convexHull->optimizeConvexHull(); convexHull->optimizeConvexHull();
convexHull->initializePolyhedralFeatures(); if (m_data->m_flags & CUF_INITIALIZE_SAT_FEATURES)
{
convexHull->initializePolyhedralFeatures();
}
convexHull->setMargin(gUrdfDefaultCollisionMargin); convexHull->setMargin(gUrdfDefaultCollisionMargin);
convexHull->recalcLocalAabb(); convexHull->recalcLocalAabb();
//convexHull->setLocalScaling(collision->m_geometry.m_meshScale); //convexHull->setLocalScaling(collision->m_geometry.m_meshScale);

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

@@ -30,6 +30,7 @@ enum ConvertURDFFlags {
CUF_MJCF_COLORS_FROM_FILE=512, CUF_MJCF_COLORS_FROM_FILE=512,
CUF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024, CUF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024,
CUF_ENABLE_SLEEPING=2048, CUF_ENABLE_SLEEPING=2048,
CUF_INITIALIZE_SAT_FEATURES=4096,
}; };
struct UrdfVisualShapeCache struct UrdfVisualShapeCache

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -723,6 +723,7 @@ enum eURDF_Flags
MJCF_COLORS_FROM_FILE=512, MJCF_COLORS_FROM_FILE=512,
URDF_ENABLE_CACHED_GRAPHICS_SHAPES=1024, URDF_ENABLE_CACHED_GRAPHICS_SHAPES=1024,
URDF_ENABLE_SLEEPING=2048, URDF_ENABLE_SLEEPING=2048,
URDF_INITIALIZE_SAT_FEATURES = 4096,
}; };
enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes

8
examples/pybullet/examples/satCollision.py
View File

@@ -4,11 +4,13 @@ import time
p.connect(p.GUI) p.connect(p.GUI)
p.setGravity(0,0,-10) p.setGravity(0,0,-10)
p.setPhysicsEngineParameter(enableSAT=1) p.setPhysicsEngineParameter(enableSAT=1)
p.loadURDF("cube_concave.urdf",[0,0,-25], globalScaling=50, useFixedBase=True) 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) 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) p.loadURDF("duck_vhacd.urdf",[1,0,1], globalScaling=1,flags=p.URDF_INITIALIZE_SAT_FEATURES)
while (p.isConnected()): while (p.isConnected()):
p.stepSimulation() p.stepSimulation()
pts = p.getContactPoints()
#print("num contacts = ", len(pts))
time.sleep(1./240.) time.sleep(1./240.)

3
examples/pybullet/pybullet.c
View File

@@ -9616,7 +9616,8 @@ initpybullet(void)
PyModule_AddIntConstant(m, "MJCF_COLORS_FROM_FILE", MJCF_COLORS_FROM_FILE); 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_CACHED_GRAPHICS_SHAPES", URDF_ENABLE_CACHED_GRAPHICS_SHAPES);
PyModule_AddIntConstant(m, "URDF_ENABLE_SLEEPING", URDF_ENABLE_SLEEPING); 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", 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_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT);