PyBullet: add option to cache graphics shapes for URDF files, handy for benchmarks with many duplicate robots

See https://github.com/erwincoumans/pybullet_robots ANYmal.py for an example.
PyBullet: Expose p.setPhysicsEngineParameter(solverResidualThreshold=1e-2) (b3PhysicsParamSetSolverResidualThreshold), increases solver performance a lot
PyBullet: Expose p.setPhysicsEngineParameter(contactSlop) Set it to zero, to avoid issues with restitution.
PyBullet: Expose isNumpyEnabled, return True is PyBullet was compiled with NUMPY support for 'getCameraImage'.
PyBullet: Expose p.ChangeDynamics(objectUid, linkIndex, contactProcessingThreshold), to avoid issues of speculative/predictive contacts with restitution.
See also http://twvideo01.ubm-us.net/o1/vault/gdc2012/slides/Programming%20Track/Vincent_ROBERT_Track_ADifferentApproach.pdf

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

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;

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

examples/Importers/ImportURDFDemo/URDF2Bullet.h

@@ -3,6 +3,8 @@
 #include "LinearMath/btAlignedObjectArray.h"
 #include "LinearMath/btTransform.h"
 #include <string>
+#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<UrdfMaterialColor> m_cachedUrdfLinkColors;
+	btAlignedObjectArray<int> 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

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!

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;

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)

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

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<b3HashString,  char*> m_profileEvents;
+	b3HashMap<b3HashString, UrdfVisualShapeCache> 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)
 	{

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

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

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};
+	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|dddddddddiOdi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &physicsClientId))
+	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,
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
-									 &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &physicsClientId))
+									 &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);