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

allow to enable/disable implicit cylinder conversion through an API

Browse Source 
p.loadURDF("r2d2.urdf", flags=p.URDF_USE_IMPLICIT_CYLINDER)
allow to enable/disable deterministicOverlappingPairs through an API
p.setPhysicsEngineParameter(deterministicOverlappingPairs = False)
This commit is contained in:
erwincoumans
2018-01-09 10:10:36 -08:00
parent bf6d805228
commit e138e85bca
11 changed files with 85 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -25,7 +25,7 @@ subject to the following restrictions:
#include "Bullet3Common/b3FileUtils.h"
#include <string>
#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<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
int numSteps = 32;
for (int i=0;i<numSteps;i++)
{
if (m_data->m_flags & CUF_USE_IMPLICIT_CYLINDER)
{
btVector3 halfExtents(cylRadius,cylRadius, cylHalfLength);
btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
shape = cylZShape;
} else
{
btAlignedObjectArray<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
int numSteps = 32;
for (int i=0;i<numSteps;i++)
{
btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i)/numSteps)),cylRadius*btCos(SIMD_2_PI*(float(i)/numSteps)),cylLength/2.);
vertices.push_back(vert);
vert[2] = -cylLength/2.;
vertices.push_back(vert);
btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i)/numSteps)),cylRadius*btCos(SIMD_2_PI*(float(i)/numSteps)),cylHalfLength);
vertices.push_back(vert);
vert[2] = -cylHalfLength;
vertices.push_back(vert);
}
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->setMargin(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:

2
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

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

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

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

9
examples/SharedMemory/PhysicsClientC_API.cpp
View File

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

2
examples/SharedMemory/PhysicsClientC_API.h
View File

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

20
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

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

1
examples/SharedMemory/SharedMemoryCommands.h
View File

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

3
examples/SharedMemory/SharedMemoryPublic.h
View File

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

14
examples/pybullet/pybullet.c
View File

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

4
src/BulletCollision/BroadphaseCollision/btDispatcher.h
View File

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

18
src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
View File

@@ -334,8 +334,11 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
for (i=0;i<maxNumManifolds ;i++)
{
btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
if (manifold->getNumContacts() == 0)
continue;
if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
{
if (manifold->getNumContacts() == 0)
continue;
}
const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(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());