From e138e85bcaa90d87a056b6d5e0484e93ee3dc4b0 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 9 Jan 2018 10:10:36 -0800 Subject: [PATCH] allow to enable/disable implicit cylinder conversion through an API p.loadURDF("r2d2.urdf", flags=p.URDF_USE_IMPLICIT_CYLINDER) allow to enable/disable deterministicOverlappingPairs through an API p.setPhysicsEngineParameter(deterministicOverlappingPairs = False) --- .../ImportURDFDemo/BulletUrdfImporter.cpp | 56 ++++++++++--------- .../ImportURDFDemo/BulletUrdfImporter.h | 2 +- .../Importers/ImportURDFDemo/URDF2Bullet.h | 3 +- examples/SharedMemory/PhysicsClientC_API.cpp | 9 +++ examples/SharedMemory/PhysicsClientC_API.h | 2 + .../PhysicsServerCommandProcessor.cpp | 20 ++++--- examples/SharedMemory/SharedMemoryCommands.h | 1 + examples/SharedMemory/SharedMemoryPublic.h | 3 +- examples/pybullet/pybullet.c | 14 ++++- .../BroadphaseCollision/btDispatcher.h | 4 +- .../btSimulationIslandManager.cpp | 18 ++++-- 11 files changed, 85 insertions(+), 47 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 40b84cc6a..0c50643ab 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -25,7 +25,7 @@ subject to the following restrictions: #include "Bullet3Common/b3FileUtils.h" #include #include "../../Utils/b3ResourcePath.h" - +#include "URDF2Bullet.h"//for flags #include "../ImportMeshUtility/b3ImportMeshUtility.h" static btScalar gUrdfDefaultCollisionMargin = 0.001; @@ -53,6 +53,7 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData LinkVisualShapesConverter* m_customVisualShapesConverter; bool m_enableTinyRenderer; + int m_flags; void setSourceFile(const std::string& relativeFileName, const std::string& prefix) { @@ -66,6 +67,7 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData { m_enableTinyRenderer = true; m_pathPrefix[0] = 0; + m_flags=0; } void setGlobalScaling(btScalar scaling) @@ -80,10 +82,11 @@ void BulletURDFImporter::printTree() // btAssert(0); } -BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, double globalScaling) +BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, double globalScaling, int flags) { m_data = new BulletURDFInternalData; m_data->setGlobalScaling(globalScaling); + m_data->m_flags = flags; m_data->m_guiHelper = helper; m_data->m_customVisualShapesConverter = customConverter; @@ -612,32 +615,33 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl { btScalar cylRadius = collision->m_geometry.m_capsuleRadius; btScalar cylHalfLength = 0.5*collision->m_geometry.m_capsuleHeight; -#if 0 - btAlignedObjectArray vertices; - //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); - int numSteps = 32; - for (int i=0;im_flags & CUF_USE_IMPLICIT_CYLINDER) + { + btVector3 halfExtents(cylRadius,cylRadius, cylHalfLength); + btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); + shape = cylZShape; + } else + { + btAlignedObjectArray vertices; + //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); + int numSteps = 32; + for (int i=0;isetMargin(gUrdfDefaultCollisionMargin); - cylZShape->recalcLocalAabb(); - cylZShape->initializePolyhedralFeatures(); - cylZShape->optimizeConvexHull(); - - //btConvexShape* cylZShape = new btConeShapeZ(cylRadius,cylLength);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); -#else - btVector3 halfExtents(cylRadius,cylRadius, cylHalfLength); - btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); -#endif - - shape = cylZShape; + } + btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); + cylZShape->setMargin(gUrdfDefaultCollisionMargin); + cylZShape->recalcLocalAabb(); + cylZShape->initializePolyhedralFeatures(); + cylZShape->optimizeConvexHull(); + shape = cylZShape; + } + break; } case URDF_GEOM_BOX: diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 81b28f7fd..8bfa98f13 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -23,7 +23,7 @@ class BulletURDFImporter : public URDFImporterInterface public: - BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, double globalScaling=1); + BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, double globalScaling, int flags); virtual ~BulletURDFImporter(); diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index 68acf497d..ed7292b41 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -13,7 +13,7 @@ class URDFImporterInterface; class MultiBodyCreationInterface; - +//manually sync with eURDF_Flags in SharedMemoryPublic.h! enum ConvertURDFFlags { CUF_USE_SDF = 1, // Use inertia values in URDF instead of recomputing them from collision shape. @@ -23,6 +23,7 @@ enum ConvertURDFFlags { CUF_USE_SELF_COLLISION_EXCLUDE_PARENT=16, CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32, CUF_RESERVED=64, + CUF_USE_IMPLICIT_CYLINDER=128, }; void ConvertURDF2Bullet(const URDFImporterInterface& u2b, diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index b8d8b7359..15f56f5bc 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -557,6 +557,15 @@ B3_SHARED_API int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandl return 0; } +B3_SHARED_API int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMemoryCommandHandle commandHandle, int deterministicOverlappingPairs) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_physSimParamArgs.m_deterministicOverlappingPairs = deterministicOverlappingPairs; + command->m_updateFlags |= SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS; + return 0; +} + B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 78591f3b5..e7f283026 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -291,6 +291,8 @@ B3_SHARED_API int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHan B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching); B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold); B3_SHARED_API int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandle commandHandle, int enableConeFriction); +B3_SHARED_API int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMemoryCommandHandle commandHandle, int deterministicOverlappingPairs); + diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index cc5111ed3..86abefc62 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2727,7 +2727,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe m_data->m_sdfRecentLoadedBodies.clear(); - BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling); + BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling, flags); u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); bool forceFixedBase = false; @@ -2759,7 +2759,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto - BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling); + BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling, flags); u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); bool loadOk = u2b.loadURDF(fileName, useFixedBase); @@ -3770,12 +3770,12 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str btAlignedObjectArray convertedVerts; convertedVerts.reserve(glmesh->m_numvertices); - for (int i = 0; i < glmesh->m_numvertices; i++) + for (int v = 0; v < glmesh->m_numvertices; v++) { convertedVerts.push_back(btVector3( - glmesh->m_vertices->at(i).xyzw[0] * meshScale[0], - glmesh->m_vertices->at(i).xyzw[1] * meshScale[1], - glmesh->m_vertices->at(i).xyzw[2] * meshScale[2])); + glmesh->m_vertices->at(v).xyzw[0] * meshScale[0], + glmesh->m_vertices->at(v).xyzw[1] * meshScale[1], + glmesh->m_vertices->at(v).xyzw[2] * meshScale[2])); } if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH) @@ -3868,8 +3868,8 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct bool hasStatus = true; serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED; double globalScaling = 1.f; - - BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling); + int flags=0; + BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling, flags); u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); btTransform localInertiaFrame; localInertiaFrame.setIdentity(); @@ -6502,6 +6502,10 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |=SOLVER_DISABLE_IMPLICIT_CONE_FRICTION; } } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS) + { + m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs = clientCmd.m_physSimParamArgs.m_deterministicOverlappingPairs; + } if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) { m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 4fff75859..99089ce7e 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -437,6 +437,7 @@ enum EnumSimParamUpdateFlags SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD = 8192, SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP=16384, SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 32768, + SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS = 65536, }; enum EnumLoadSoftBodyUpdateFlags diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 9fcfc191a..78ec82e2b 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -670,7 +670,7 @@ enum eURDF_Flags URDF_USE_SELF_COLLISION_EXCLUDE_PARENT=16, URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32, URDF_RESERVED=64, - + URDF_USE_IMPLICIT_CYLINDER =128, }; enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes @@ -733,6 +733,7 @@ struct b3PhysicsSimulationParameters double m_defaultNonContactERP; double m_frictionERP; int m_enableConeFriction; + int m_deterministicOverlappingPairs; }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 7028789d1..f66dbec89 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1068,12 +1068,13 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar double frictionERP = -1; int enableConeFriction = -1; b3PhysicsClientHandle sm = 0; + int deterministicOverlappingPairs = -1; int physicsClientId = 0; - static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "physicsClientId", NULL}; + static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, - &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, + &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &physicsClientId)) { return NULL; } @@ -1149,6 +1150,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar { b3PhysicsParamSetEnableConeFriction(command, enableConeFriction); } + if (deterministicOverlappingPairs>=0) + { + b3PhysicsParameterSetDeterministicOverlappingPairs(command,deterministicOverlappingPairs); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } @@ -8811,6 +8816,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "IK_HAS_JOINT_DAMPING", IK_HAS_JOINT_DAMPING); PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE); + PyModule_AddIntConstant(m, "URDF_USE_IMPLICIT_CYLINDER", URDF_USE_IMPLICIT_CYLINDER); + 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); @@ -8861,6 +8868,7 @@ initpybullet(void) PyModule_AddIntConstant(m, "STATE_LOG_JOINT_USER_TORQUES", STATE_LOG_JOINT_USER_TORQUES); PyModule_AddIntConstant(m, "STATE_LOG_JOINT_TORQUES", STATE_LOG_JOINT_USER_TORQUES+STATE_LOG_JOINT_MOTOR_TORQUES); + SpamError = PyErr_NewException("pybullet.error", NULL, NULL); Py_INCREF(SpamError); PyModule_AddObject(m, "error", SpamError); diff --git a/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/src/BulletCollision/BroadphaseCollision/btDispatcher.h index 7b0f9489a..d7a97e8e0 100644 --- a/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -46,7 +46,8 @@ struct btDispatcherInfo m_useEpa(true), m_allowedCcdPenetration(btScalar(0.04)), m_useConvexConservativeDistanceUtil(false), - m_convexConservativeDistanceThreshold(0.0f) + m_convexConservativeDistanceThreshold(0.0f), + m_deterministicOverlappingPairs(true) { } @@ -62,6 +63,7 @@ struct btDispatcherInfo btScalar m_allowedCcdPenetration; bool m_useConvexConservativeDistanceUtil; btScalar m_convexConservativeDistanceThreshold; + bool m_deterministicOverlappingPairs; }; enum ebtDispatcherQueryType diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 1f0067d3e..f849e31e3 100644 --- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -334,8 +334,11 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio for (i=0;igetManifoldByIndexInternal(i); - if (manifold->getNumContacts() == 0) - continue; + if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs) + { + if (manifold->getNumContacts() == 0) + continue; + } const btCollisionObject* colObj0 = static_cast(manifold->getBody0()); const btCollisionObject* colObj1 = static_cast(manifold->getBody1()); @@ -397,12 +400,15 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, //tried a radix sort, but quicksort/heapsort seems still faster //@todo rewrite island management - - //m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); - //btPersistentManifoldSortPredicateDeterministic sorts contact manifolds based on islandid, //but also based on object0 unique id and object1 unique id - m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic()); + if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs) + { + m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic()); + } else + { + m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); + } //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate());