diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 20b1e4370..3ddcfe080 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -1302,6 +1302,11 @@ bool BulletURDFImporter::getLinkColor2(int linkIndex, UrdfMaterialColor& matCol) return false; } +void BulletURDFImporter::setLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const +{ + m_data->m_linkColors.insert(linkIndex, matCol); +} + bool BulletURDFImporter::getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo ) const { UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdflinkIndex); diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index e058c56ea..efab36121 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -52,6 +52,8 @@ public: virtual bool getLinkColor2(int linkIndex, UrdfMaterialColor& matCol) const; + virtual void setLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const; + virtual bool getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo ) const; virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const; diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 8bf13d865..7bcbea20c 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -185,7 +185,7 @@ void ConvertURDF2BulletInternal( URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, bool createMultiBody, const char* pathPrefix, - int flags = 0) + int flags = 0, UrdfVisualShapeCache& cachedLinkGraphicsShapesIn= UrdfVisualShapeCache(), UrdfVisualShapeCache& cachedLinkGraphicsShapesOut = UrdfVisualShapeCache()) { B3_PROFILE("ConvertURDF2BulletInternal2"); //b3Printf("start converting/extracting data from URDF interface\n"); @@ -273,7 +273,20 @@ void ConvertURDF2BulletInternal( int graphicsIndex; { B3_PROFILE("convertLinkVisualShapes"); - graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex, pathPrefix, localInertialFrame); + if (cachedLinkGraphicsShapesIn.m_cachedUrdfLinkVisualShapeIndices.size() > (mbLinkIndex+1)) + { + graphicsIndex = cachedLinkGraphicsShapesIn.m_cachedUrdfLinkVisualShapeIndices[mbLinkIndex+1]; + UrdfMaterialColor matColor = cachedLinkGraphicsShapesIn.m_cachedUrdfLinkColors[mbLinkIndex + 1]; + u2b.setLinkColor2(urdfLinkIndex, matColor); + } + else + { + graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex, pathPrefix, localInertialFrame); + cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.push_back(graphicsIndex); + UrdfMaterialColor matColor; + u2b.getLinkColor2(urdfLinkIndex, matColor); + cachedLinkGraphicsShapesOut.m_cachedUrdfLinkColors.push_back(matColor); + } } @@ -594,7 +607,7 @@ void ConvertURDF2BulletInternal( { int urdfChildLinkIndex = urdfChildIndices[i]; - ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags); + ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut); } } @@ -602,14 +615,21 @@ void ConvertURDF2Bullet( const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, - bool createMultiBody, const char* pathPrefix, int flags) + bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache& cachedLinkGraphicsShapes) { - URDF2BulletCachedData cache; + URDF2BulletCachedData cache; InitURDF2BulletCache(u2b,cache); int urdfLinkIndex = u2b.getRootLinkIndex(); B3_PROFILE("ConvertURDF2Bullet"); - ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags); + + UrdfVisualShapeCache cachedLinkGraphicsShapesOut; + + ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags, cachedLinkGraphicsShapes, cachedLinkGraphicsShapesOut); + if (cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.size() > cachedLinkGraphicsShapes.m_cachedUrdfLinkVisualShapeIndices.size()) + { + cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut; + } if (world1 && cache.m_bulletMultiBody) { diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index a3a79be1e..964a8a12a 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -3,6 +3,8 @@ #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btTransform.h" #include +#include "UrdfJointTypes.h"//for UrdfMaterialColor cache + class btVector3; class btTransform; class btMultiBodyDynamicsWorld; @@ -26,15 +28,25 @@ enum ConvertURDFFlags { CUF_USE_IMPLICIT_CYLINDER=128, CUF_GLOBAL_VELOCITIES_MB=256, CUF_MJCF_COLORS_FROM_FILE=512, + CUF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024, }; +struct UrdfVisualShapeCache +{ + btAlignedObjectArray m_cachedUrdfLinkColors; + btAlignedObjectArray m_cachedUrdfLinkVisualShapeIndices; +}; + + void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creationCallback, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world, bool createMultiBody, const char* pathPrefix, - int flags = 0); + int flags = 0, + UrdfVisualShapeCache& cachedLinkGraphicsShapes= UrdfVisualShapeCache() + ); #endif //_URDF2BULLET_H diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 703303dfe..611967d20 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -37,7 +37,7 @@ public: virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;} virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const { return false;} - + virtual void setLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const {} virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const { return 0;} ///this API will likely change, don't override it! diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index 1a66c32df..b2b1f93e2 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -861,7 +861,10 @@ void GLInstancingRenderer::rebuildGraphicsInstances() { int srcIndex2 = usedObjects[i]; b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2); - m_graphicsInstances[pg->m_shapeIndex]->m_tempObjectUids.push_back(srcIndex2); + if (pg && pg->m_shapeIndex < m_graphicsInstances.size() && pg->m_shapeIndex >=0) + { + m_graphicsInstances[pg->m_shapeIndex]->m_tempObjectUids.push_back(srcIndex2); + } } int curOffset = 0; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index c3bd61296..eae4255a6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -593,6 +593,26 @@ B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHand return 0; } +B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double solverResidualThreshold) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_physSimParamArgs.m_solverResidualThreshold = solverResidualThreshold; + command->m_updateFlags |= SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD; + return 0; +} + +B3_SHARED_API int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle commandHandle, double contactSlop) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_physSimParamArgs.m_contactSlop = contactSlop; + command->m_updateFlags |= SIM_PARAM_UPDATE_CONTACT_SLOP; + return 0; +} + + + B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode) { @@ -2354,6 +2374,19 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryComm return 0; } +B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactProcessingThreshold) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex; + command->m_changeDynamicsInfoArgs.m_contactProcessingThreshold = contactProcessingThreshold; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD; + return 0; +} + + + B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 05a96f601..712340dc9 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -126,6 +126,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHan B3_SHARED_API int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping); B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor); B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, double ccdSweptSphereRadius); +B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactProcessingThreshold); B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); @@ -299,6 +300,11 @@ B3_SHARED_API int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandl B3_SHARED_API int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMemoryCommandHandle commandHandle, int deterministicOverlappingPairs); B3_SHARED_API int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryCommandHandle commandHandle, double allowedCcdPenetration); B3_SHARED_API int b3PhysicsParameterSetJointFeedbackMode(b3SharedMemoryCommandHandle commandHandle, int jointFeedbackMode); +B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double solverResidualThreshold); +B3_SHARED_API int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle commandHandle, double contactSlop); + + + diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index cc4f7586f..36efbd68c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1484,6 +1484,7 @@ struct SaveStateData btSerializer* m_serializer; }; + struct PhysicsServerCommandProcessorInternalData { ///handle management @@ -1581,6 +1582,7 @@ struct PhysicsServerCommandProcessorInternalData #endif b3HashMap m_profileEvents; + b3HashMap m_cachedVUrdfisualShapes; PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc) :m_pluginManager(proc), @@ -2618,7 +2620,22 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, u2b.getRootTransformInWorld(rootTrans); //CUF_RESERVED is a temporary flag, for backward compatibility purposes flags |= CUF_RESERVED; - ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags); + + if (flags & CUF_ENABLE_CACHED_GRAPHICS_SHAPES) + { + { + UrdfVisualShapeCache* tmpPtr = m_data->m_cachedVUrdfisualShapes[fileName]; + if (tmpPtr==0) + { + m_data->m_cachedVUrdfisualShapes.insert(fileName, UrdfVisualShapeCache()); + } + } + UrdfVisualShapeCache* cachedVisualShapesPtr = m_data->m_cachedVUrdfisualShapes[fileName]; + ConvertURDF2Bullet(u2b, creation, rootTrans, m_data->m_dynamicsWorld, useMultiBody, u2b.getPathPrefix(), flags, *cachedVisualShapesPtr); + } else + { + ConvertURDF2Bullet(u2b, creation, rootTrans, m_data->m_dynamicsWorld, useMultiBody, u2b.getPathPrefix(), flags); + } @@ -6483,6 +6500,11 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc } } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD) + { + body->m_rigidBody->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS) { body->m_rigidBody->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius); @@ -6696,11 +6718,24 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st { m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations; } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD) + { + m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = clientCmd.m_physSimParamArgs.m_solverResidualThreshold; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD) { gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold; } - + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_SLOP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = clientCmd.m_physSimParamArgs.m_contactSlop; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE) { m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode; @@ -9714,6 +9749,8 @@ void PhysicsServerCommandProcessor::resetSimulation() { //clean up all data + m_data->m_cachedVUrdfisualShapes.clear(); + #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD if (m_data && m_data->m_dynamicsWorld) { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 75edd0be8..bd3285351 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -164,6 +164,7 @@ enum EnumChangeDynamicsInfoFlags CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR = 512, CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL = 1024, CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS = 2048, + CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD = 4096, }; struct ChangeDynamicsInfoArgs @@ -183,6 +184,7 @@ struct ChangeDynamicsInfoArgs double m_localInertiaDiagonal[3]; int m_frictionAnchor; double m_ccdSweptSphereRadius; + double m_contactProcessingThreshold; }; struct GetDynamicsInfoArgs @@ -450,6 +452,8 @@ enum EnumSimParamUpdateFlags SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE = 262144, SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM = 524288, SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM = 1048576, + SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD = 2097152, + SIM_PARAM_UPDATE_CONTACT_SLOP = 4194304, }; enum EnumLoadSoftBodyUpdateFlags diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index eb89b24af..80bbb3c30 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -681,6 +681,7 @@ enum eURDF_Flags URDF_USE_IMPLICIT_CYLINDER =128, URDF_GLOBAL_VELOCITIES_MB =256, MJCF_COLORS_FROM_FILE=512, + URDF_ENABLE_CACHED_GRAPHICS_SHAPES=1024, }; enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes @@ -755,6 +756,8 @@ struct b3PhysicsSimulationParameters int m_deterministicOverlappingPairs; double m_allowedCcdPenetration; int m_jointFeedbackMode; + double m_solverResidualThreshold; + double m_contactSlop; }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 1fae95a6d..31b201ea0 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -919,13 +919,15 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO double contactDamping = -1; double ccdSweptSphereRadius=-1; int frictionAnchor = -1; + double contactProcessingThreshold = -1; + PyObject* localInertiaDiagonalObj=0; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOdi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &physicsClientId)) { return NULL; } @@ -995,6 +997,10 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO { b3ChangeDynamicsInfoSetCcdSweptSphereRadius(command,bodyUniqueId,linkIndex, ccdSweptSphereRadius); } + if (contactProcessingThreshold >= 0) + { + b3ChangeDynamicsInfoSetContactProcessingThreshold(command, bodyUniqueId, linkIndex, contactProcessingThreshold); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } @@ -1163,12 +1169,14 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar b3PhysicsClientHandle sm = 0; int deterministicOverlappingPairs = -1; int jointFeedbackMode =-1; + double solverResidualThreshold = -1; + double contactSlop = -1; int physicsClientId = 0; - static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "physicsClientId", NULL}; + static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiidii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, - &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, + &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &physicsClientId)) { return NULL; } @@ -1189,6 +1197,11 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar b3PhysicsParamSetNumSolverIterations(command, numSolverIterations); } + if (solverResidualThreshold) + { + b3PhysicsParamSetSolverResidualThreshold(command, solverResidualThreshold); + } + if (collisionFilterMode >= 0) { b3PhysicsParamSetCollisionFilterMode(command, collisionFilterMode); @@ -1213,6 +1226,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar { b3PhysicsParamSetContactBreakingThreshold(command, contactBreakingThreshold); } + if (contactSlop >= 0) + { + b3PhysicsParamSetContactSlop(command, contactSlop); + } + + //-1 is disables the maxNumCmdPer1ms feature, allow it if (maxNumCmdPer1ms >= -1) { @@ -6832,6 +6851,29 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py return Py_None; } + +static PyObject* pybullet_isNumpyEnabled(PyObject* self, PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + int isNumpyEnabled = 0; + int method = 0; + PyObject* pylist = 0; + PyObject* val = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = { "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) + { + return NULL; + } + +#ifdef PYBULLET_USE_NUMPY + isNumpyEnabled = 1; +#endif + return PyLong_FromLong(isNumpyEnabled); +} + + + /// Render an image from the current timestep of the simulation, width, height are required, other args are optional // getCameraImage(w, h, view[16], projection[16], lightDir[3], lightColor[3], lightDist, hasShadow, lightAmbientCoeff, lightDiffuseCoeff, lightSpecularCoeff, renderer) static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObject* keywds) @@ -8897,6 +8939,10 @@ static PyMethodDef SpamMethods[] = { #endif }, + { "isNumpyEnabled", (PyCFunction)pybullet_isNumpyEnabled, METH_VARARGS | METH_KEYWORDS, + "return True if PyBullet was compiled with NUMPY support. This makes the getCameraImage API faster" + }, + {"computeViewMatrix", (PyCFunction)pybullet_computeViewMatrix, METH_VARARGS | METH_KEYWORDS, "Compute a camera viewmatrix from camera eye, target position and up vector "}, @@ -9261,6 +9307,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "URDF_USE_IMPLICIT_CYLINDER", URDF_USE_IMPLICIT_CYLINDER); PyModule_AddIntConstant(m, "URDF_GLOBAL_VELOCITIES_MB", URDF_GLOBAL_VELOCITIES_MB); 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_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT);