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!