From 151bc8e12c7d8cc41ecbaf8af9130e2975458780 Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Tue, 7 Feb 2017 17:19:48 -0800
Subject: [PATCH 1/7] Add depth image when using hardware renderer.

---
 examples/ExampleBrowser/OpenGLGuiHelper.cpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp
index c739d2e47..87bff6e59 100644
--- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp
+++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp
@@ -437,10 +437,15 @@ void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const floa
                         int bytesPerPixel = 4; //RGBA
                         
                         int sourcePixelIndex = (xIndex+yIndex*sourceWidth)*bytesPerPixel;
+                        int sourceDepthIndex = xIndex+yIndex*sourceWidth;
+                        
                         m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+0] = sourceRgbaPixelBuffer[sourcePixelIndex+0];
                         m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+1] = sourceRgbaPixelBuffer[sourcePixelIndex+1];
                         m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+2] = sourceRgbaPixelBuffer[sourcePixelIndex+2];
                         m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+3] = 255;
+                        
+                        m_data->m_depthBuffer1[i+j*destinationWidth] = sourceDepthBuffer[sourceDepthIndex];
+                        
                     }
                 }
             }
@@ -456,7 +461,7 @@ void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const floa
         {
             for (int i=0;i<numRequestedPixels;i++)
             {
-                depthBuffer[i] = m_data->m_depthBuffer1[i];
+                depthBuffer[i] = m_data->m_depthBuffer1[i+startPixelIndex];
             }
         }
 		if (numPixelsCopied)

From 0c464e6848d5fadf1893c9589377ad0dbd4028b9 Mon Sep 17 00:00:00 2001
From: Erwin Coumans <erwincoumans@google.com>
Date: Wed, 8 Feb 2017 09:27:51 -0800
Subject: [PATCH 2/7] [pybullet] add example for roll, pitch, yaw

---
 examples/pybullet/rollPitchYaw.py | 26 ++++++++++++++++++++++++++
 1 file changed, 26 insertions(+)
 create mode 100644 examples/pybullet/rollPitchYaw.py

diff --git a/examples/pybullet/rollPitchYaw.py b/examples/pybullet/rollPitchYaw.py
new file mode 100644
index 000000000..d807fb845
--- /dev/null
+++ b/examples/pybullet/rollPitchYaw.py
@@ -0,0 +1,26 @@
+import pybullet as p
+import time
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid<0):
+	p.connect(p.GUI)
+q = p.loadURDF("quadruped/quadruped.urdf",useFixedBase=True)
+rollId = p.addUserDebugParameter("roll",-1.5,1.5,0)
+pitchId = p.addUserDebugParameter("pitch",-1.5,1.5,0)
+yawId = p.addUserDebugParameter("yaw",-1.5,1.5,0)
+fwdxId = p.addUserDebugParameter("fwd_x",-1,1,0)
+fwdyId = p.addUserDebugParameter("fwd_y",-1,1,0)
+fwdzId = p.addUserDebugParameter("fwd_z",-1,1,0)
+
+while True:
+	roll = p.readUserDebugParameter(rollId)
+	pitch = p.readUserDebugParameter(pitchId)
+	yaw = p.readUserDebugParameter(yawId)
+	x = p.readUserDebugParameter(fwdxId)
+	y = p.readUserDebugParameter(fwdyId)
+	z = p.readUserDebugParameter(fwdzId)
+
+	orn = p.getQuaternionFromEuler([roll,pitch,yaw])
+	p.resetBasePositionAndOrientation(q,[x,y,z],orn)
+	#p.stepSimulation()#not really necessary for this demo, no physics used
+

From ce69f27f321d6d47a1d67f0b0517d88eee25cbe3 Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Wed, 8 Feb 2017 11:34:38 -0800
Subject: [PATCH 3/7] Modify the depth buffer value in TinyRenderer to be
 consistent as in OpenGL.

---
 examples/CommonInterfaces/CommonCameraInterface.h     |  3 +++
 examples/OpenGLWindow/SimpleCamera.cpp                | 10 ++++++++++
 examples/OpenGLWindow/SimpleCamera.h                  |  5 ++++-
 .../SharedMemory/TinyRendererVisualShapeConverter.cpp | 11 ++++++++++-
 4 files changed, 27 insertions(+), 2 deletions(-)

diff --git a/examples/CommonInterfaces/CommonCameraInterface.h b/examples/CommonInterfaces/CommonCameraInterface.h
index 813aa4344..88db58685 100644
--- a/examples/CommonInterfaces/CommonCameraInterface.h
+++ b/examples/CommonInterfaces/CommonCameraInterface.h
@@ -36,6 +36,9 @@ struct CommonCameraInterface
 
 	virtual void	setAspectRatio(float ratio) = 0;
 	virtual float	getAspectRatio() const = 0;
+    
+    virtual float getCameraFrustumFar() const = 0;
+    virtual float getCameraFrustumNear() const = 0;
 };
 
 #endif //COMMON_CAMERA_INTERFACE_H
diff --git a/examples/OpenGLWindow/SimpleCamera.cpp b/examples/OpenGLWindow/SimpleCamera.cpp
index 4b0b27164..93838a212 100644
--- a/examples/OpenGLWindow/SimpleCamera.cpp
+++ b/examples/OpenGLWindow/SimpleCamera.cpp
@@ -383,3 +383,13 @@ float	SimpleCamera::getAspectRatio() const
 {
 	return m_data->m_aspect;
 }
+
+float SimpleCamera::getCameraFrustumFar() const
+{
+    return m_data->m_frustumZFar;
+}
+
+float SimpleCamera::getCameraFrustumNear() const
+{
+    return m_data->m_frustumZNear;
+}
diff --git a/examples/OpenGLWindow/SimpleCamera.h b/examples/OpenGLWindow/SimpleCamera.h
index 5a61a729f..b186759c8 100644
--- a/examples/OpenGLWindow/SimpleCamera.h
+++ b/examples/OpenGLWindow/SimpleCamera.h
@@ -47,6 +47,9 @@ struct SimpleCamera : public CommonCameraInterface
 
 	virtual void	setAspectRatio(float ratio);
 	virtual float	getAspectRatio() const;
+    
+    virtual float getCameraFrustumFar() const;
+    virtual float getCameraFrustumNear() const;
 };
 
-#endif //SIMPLE_CAMERA_H
\ No newline at end of file
+#endif //SIMPLE_CAMERA_H
diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
index 5ae682fc2..cfe25c938 100644
--- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
+++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
@@ -958,7 +958,16 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels
         {
 			if (depthBuffer)
 			{
-				depthBuffer[i] = m_data->m_depthBuffer[i+startPixelIndex];
+                float distance = -m_data->m_depthBuffer[i+startPixelIndex];
+                float farPlane = m_data->m_camera.getCameraFrustumFar();
+                float nearPlane = m_data->m_camera.getCameraFrustumNear();
+                
+                btClamp(distance,nearPlane,farPlane);
+                
+                // the depth buffer value is between 0 and 1
+                float a = farPlane / (farPlane - nearPlane);
+                float b = farPlane * nearPlane / (nearPlane - farPlane);
+                depthBuffer[i] = a + b / distance;
 			}
 			if (segmentationMaskBuffer)
             {

From 0e8bc418d76bd586fc4d328c169e9e4da097b71a Mon Sep 17 00:00:00 2001
From: Erwin Coumans <erwin.coumans@gmail.com>
Date: Thu, 9 Feb 2017 09:48:50 -0800
Subject: [PATCH 4/7] add utility for reading and writing of minitaur quadruped
 robot log files. Example usage: const char* fileName = "D:/LOG00053.TXT";
 btAlignedObjectArray<MinitaurLogRecord> logRecords;
 btAlignedObjectArray<std::string> structNames; std::string structTypes; bool
 verbose = false; //reading int val = readMinitaurLogFile(fileName,
 structNames, structTypes, logRecords, verbose);

//writing
const char* fileNameOut = "D:/LOG00100.TXT";
FILE* f = createMinitaurLogFile(fileNameOut,structNames,structTypes);
for (int i=0;i<logRecords.size();i++)
{
  appendMinitaurLogData(f, structTypes, logRecords[i]);
}
closeMinitaurLogFile(f);
---
 examples/Utils/RobotLoggingUtil.cpp | 253 ++++++++++++++++++++++++++++
 examples/Utils/RobotLoggingUtil.h   |  54 ++++++
 2 files changed, 307 insertions(+)
 create mode 100644 examples/Utils/RobotLoggingUtil.cpp
 create mode 100644 examples/Utils/RobotLoggingUtil.h

diff --git a/examples/Utils/RobotLoggingUtil.cpp b/examples/Utils/RobotLoggingUtil.cpp
new file mode 100644
index 000000000..0499a5aa9
--- /dev/null
+++ b/examples/Utils/RobotLoggingUtil.cpp
@@ -0,0 +1,253 @@
+#include "RobotLoggingUtil.h"
+#include <stdio.h>
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "../Importers/ImportURDFDemo/urdfStringSplit.h"
+
+
+static bool readLine(FILE* file, btAlignedObjectArray<char>& line)
+{
+	int c = 0;
+	for (c=fgetc(file);(c != EOF && c != '\n');c=fgetc(file))
+	{
+		line.push_back(c);
+	}
+	line.push_back(0);
+	return (c == EOF);
+}
+
+
+int readMinitaurLogFile(const char* fileName, btAlignedObjectArray<std::string>& structNames, std::string& structTypes, btAlignedObjectArray<MinitaurLogRecord>& logRecords, bool verbose)
+{
+
+	int retVal = 0;
+
+	FILE* f = fopen(fileName,"rb");
+	if (f)
+	{
+		if (verbose)
+		{
+			printf("Opened file %s\n", fileName);
+		}
+		btAlignedObjectArray<char> line0Buf;
+		bool eof = readLine(f,line0Buf);
+		btAlignedObjectArray<char> line1Buf;
+		eof |= readLine(f,line1Buf);
+		std::string line0 = &line0Buf[0];
+		structTypes = &line1Buf[0];
+		
+		btAlignedObjectArray<std::string> separators;
+		separators.push_back(",");
+		
+		urdfStringSplit(structNames,line0,separators);
+		if (verbose)
+		{
+			printf("Num Fields = %d\n",structNames.size());
+		}
+		btAssert(structTypes.size() == structNames.size());
+		if (structTypes.size() != structNames.size())
+		{
+			retVal = eCorruptHeader;
+		}
+		int numStructsRead  = 0;
+
+		if (structTypes.size() == structNames.size())
+		{
+			while (!eof)
+			{
+				unsigned char blaat[1024];
+				size_t s = fread(blaat,2,1,f);
+				if (s!=1)
+				{
+					eof=true;
+					retVal = eInvalidAABBAlignCheck;
+					break;
+				}
+				if ((blaat[0] != 0xaa) || (blaat[1] != 0xbb))
+				{
+					if (verbose)
+					{
+						printf("Expected 0xaa0xbb, terminating\n");
+					}
+				}
+
+				if (verbose)
+				{
+					printf("Reading structure %d\n",numStructsRead);
+				}
+				MinitaurLogRecord record;
+
+				for (int i=0;i<structNames.size();i++)
+				{
+					
+
+					switch (structTypes[i])
+					{
+					case 'I':
+					{
+						size_t s = fread(blaat,sizeof(int),1,f);
+						if (s != 1)
+						{
+							eof = true;
+							retVal = eCorruptValue;
+							break;
+
+						}
+						int v = *(int*)blaat;
+						if (s==1)
+						{
+							if (verbose)
+							{
+								printf("%s = %d\n",structNames[i].c_str(),v);
+							}
+							record.m_values.push_back(v);
+						}
+						break;
+					}
+					case 'f':
+					{
+						float v;
+						size_t s = fread(&v,sizeof(float),1,f);
+						if (s != 1)
+						{
+							eof = true;
+							break;
+						}
+
+						if (s==1)
+						{
+							if (verbose)
+							{
+								printf("%s = %f\n",structNames[i].c_str(),v);
+							}
+							record.m_values.push_back(v);
+						}
+						break;
+					}
+					case 'B':
+					{
+						char v;
+						size_t s = fread(&v,sizeof(char),1,f);
+						if (s != 1)
+						{
+							eof = true;
+							break;
+						}
+						if (s==1)
+						{
+							if (verbose)
+							{
+								printf("%s = %d\n",structNames[i].c_str(),v);
+							}
+							record.m_values.push_back(v);
+						}
+						break;
+					}
+					default:
+					{
+						if (verbose)
+						{
+							printf("Unknown type\n");
+						}
+						retVal = eUnknownType;
+						btAssert(0);
+					}
+					}
+					
+
+				}
+				logRecords.push_back(record);
+				numStructsRead++;
+			}
+			if (verbose)
+			{
+				printf("numStructsRead = %d\n",numStructsRead);
+			}
+			if (retVal==0)
+			{
+				retVal = numStructsRead;
+			}
+		}
+		
+		//read header and 
+	} else
+	{
+		if (verbose)
+		{
+			printf("Could not open file %s", fileName);
+		}
+		retVal = eMinitaurFileNotFound;
+	}
+	return retVal;
+}
+
+
+FILE* createMinitaurLogFile(const char* fileName, btAlignedObjectArray<std::string>& structNames, std::string& structTypes)
+{
+	FILE* f = fopen(fileName,"wb");
+	if (f)
+	{
+		for (int i=0;i<structNames.size();i++)
+		{
+			int len = strlen(structNames[i].c_str());
+			fwrite(structNames[i].c_str(),len,1,f);
+			if (i<structNames.size()-1)
+			{
+				fwrite(",",1,1,f);
+			}
+		}
+		int sz = sizeof("\n");
+		fwrite("\n",sz-1,1,f);
+		fwrite(structTypes.c_str(),strlen(structTypes.c_str()),1,f);
+		fwrite("\n",sz-1,1,f);
+
+	}
+	
+
+	return f;
+}
+
+void appendMinitaurLogData(FILE* f, std::string& structTypes, const MinitaurLogRecord& logData)
+{
+	if (f)
+	{
+		unsigned char buf[2] = {0xaa,0xbb};
+		fwrite(buf,2,1,f);
+		if (structTypes.length() == logData.m_values.size())
+		{
+			for (int i=0;i<logData.m_values.size();i++)
+			{
+				switch(structTypes[i])
+				{
+				case 'I':
+				{
+					fwrite(&logData.m_values[i].m_intVal,sizeof(int),1,f);
+					break;
+				}
+				case 'f':
+				{
+					fwrite(&logData.m_values[i].m_floatVal,sizeof(float),1,f);
+					break;
+				}
+				case 'B':
+				{
+					fwrite(&logData.m_values[i].m_charVal,sizeof(char),1,f);
+					break;
+				}
+				default:
+				{
+
+				}
+				}
+			}
+		}
+	}
+}
+
+void closeMinitaurLogFile(FILE* f)
+{
+	if (f)
+	{
+		fclose(f);
+	}
+}
diff --git a/examples/Utils/RobotLoggingUtil.h b/examples/Utils/RobotLoggingUtil.h
new file mode 100644
index 000000000..ab88a0dae
--- /dev/null
+++ b/examples/Utils/RobotLoggingUtil.h
@@ -0,0 +1,54 @@
+#ifndef ROBOT_LOGGING_UTIL_H
+#define ROBOT_LOGGING_UTIL_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include <string>
+
+struct MinitaurLogValue
+{
+	MinitaurLogValue()
+	:m_intVal(0xcdcdcdcd)
+	{
+	}
+	MinitaurLogValue(int iv)
+	:m_intVal(iv)
+	{
+	}
+	MinitaurLogValue(float fv)
+		:m_floatVal(fv)
+	{
+	}
+	MinitaurLogValue(char fv)
+		:m_charVal(fv)
+	{
+	}
+
+	union
+	{
+		char m_charVal;
+		int m_intVal;
+		float m_floatVal;
+	};
+};
+
+struct MinitaurLogRecord
+{
+	btAlignedObjectArray<MinitaurLogValue> m_values;
+};
+
+enum MINITAUR_LOG_ERROR
+{
+	eMinitaurFileNotFound = -1,
+	eCorruptHeader = -2,
+	eUnknownType = -3,
+	eCorruptValue = -4,
+	eInvalidAABBAlignCheck = -5,
+};
+
+int readMinitaurLogFile(const char* fileName, btAlignedObjectArray<std::string>& structNames, std::string& structTypes, btAlignedObjectArray<MinitaurLogRecord>& logRecords, bool verbose);
+
+FILE* createMinitaurLogFile(const char* fileName, btAlignedObjectArray<std::string>& structNames, std::string& structTypes);
+void appendMinitaurLogData(FILE* f, std::string& structTypes, const MinitaurLogRecord& logData);
+void closeMinitaurLogFile(FILE* f);
+
+#endif //ROBOT_LOGGING_UTIL_H

From 6db217b36aff4f9231964583b4d36e4b46f0eac2 Mon Sep 17 00:00:00 2001
From: Erwin Coumans <erwin.coumans@gmail.com>
Date: Thu, 9 Feb 2017 18:27:51 -0800
Subject: [PATCH 5/7] remove some sleep delays from PhysicsServerExample
 physics loop.

---
 .../SharedMemory/PhysicsServerExample.cpp     | 21 -------------------
 1 file changed, 21 deletions(-)

diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp
index 1510475c6..4c9ae62fe 100644
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/examples/SharedMemory/PhysicsServerExample.cpp
@@ -299,7 +299,6 @@ void	MotionThreadFunc(void* userPtr,void* lsMemory)
 
 
 		double deltaTimeInSeconds = 0;
-		double sleepCounter = 0;
 		do
 		{
 			BT_PROFILE("loop");
@@ -310,27 +309,7 @@ void	MotionThreadFunc(void* userPtr,void* lsMemory)
 			}
 			double dt = double(clock.getTimeMicroseconds())/1000000.;
 			clock.reset();
-
-			sleepCounter+=dt;
-
-			if (sleepCounter > sleepTimeThreshold)
-			{
-				BT_PROFILE("usleep(100)");
-				sleepCounter = 0;
-				b3Clock::usleep(100);
-
-			}
-
-			{
-				if (gEnableRealTimeSimVR)
-				{
-					BT_PROFILE("usleep(1000)");
-					b3Clock::usleep(1000);
-				}
-			}
 			deltaTimeInSeconds+= dt;
-		
-
 			
 			{
 				

From 63486a712c3d5571c387b61da6fc439282f45e26 Mon Sep 17 00:00:00 2001
From: Erwin Coumans <erwin.coumans@gmail.com>
Date: Thu, 16 Feb 2017 14:19:09 -0800
Subject: [PATCH 6/7] VR video recording, use command-line --mp4=videoname.mp4
 tune gripper grasp example with tefal pan, 800Newton force. URDF importer: if
 using single transform 1 child shape, don't use compound shape. if renderGUI
 is false, don't intercept mouse clicks add manyspheres.py example
 (performance is pretty bad, will look into it) [pybullet] expose
 contactBreakingThreshold

---
 data/gripper/wsg50_one_motor_gripper_new.sdf  |  8 +++
 data/sphere_1cm.urdf                          | 14 +---
 .../CommonInterfaces/CommonExampleInterface.h |  1 +
 .../InProcessExampleBrowser.cpp               |  1 +
 .../ExampleBrowser/OpenGLExampleBrowser.cpp   | 27 +++++--
 .../ExampleBrowser/OpenGLExampleBrowser.h     |  2 +
 .../Importers/ImportURDFDemo/URDF2Bullet.cpp  | 11 ++-
 examples/OpenGLWindow/SimpleOpenGL3App.cpp    |  2 +-
 .../RoboticsLearning/GripperGraspExample.cpp  | 16 ++---
 examples/SharedMemory/PhysicsClientC_API.cpp  | 10 +++
 examples/SharedMemory/PhysicsClientC_API.h    |  2 +-
 .../PhysicsServerCommandProcessor.cpp         |  8 ++-
 .../SharedMemory/PhysicsServerExample.cpp     | 71 ++++++++++---------
 examples/SharedMemory/SharedMemoryCommands.h  |  4 +-
 .../SharedMemoryInProcessPhysicsC_API.cpp     | 17 ++---
 .../StandaloneMain/hellovr_opengl_main.cpp    |  9 ++-
 examples/pybullet/manyspheres.py              |  6 +-
 examples/pybullet/pybullet.c                  | 13 +++-
 18 files changed, 140 insertions(+), 82 deletions(-)

diff --git a/data/gripper/wsg50_one_motor_gripper_new.sdf b/data/gripper/wsg50_one_motor_gripper_new.sdf
index 5bb80d6a6..54f469fe0 100644
--- a/data/gripper/wsg50_one_motor_gripper_new.sdf
+++ b/data/gripper/wsg50_one_motor_gripper_new.sdf
@@ -303,6 +303,10 @@
     </joint>
     
     <link name='finger_right'>
+      <contact>
+        <spinning_friction>.3</spinning_friction>
+        <rolling_friction>0.04</rolling_friction>
+      </contact>
       <pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
       <inertial>
         <mass>0.2</mass>
@@ -343,6 +347,10 @@
     </joint>
     
     <link name='finger_left'>
+      <contact>
+        <spinning_friction>.3</spinning_friction>
+        <rolling_friction>0.04</rolling_friction>
+      </contact>
       <pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
       <inertial>
         <mass>0.2</mass>
diff --git a/data/sphere_1cm.urdf b/data/sphere_1cm.urdf
index b44069877..05a07cde7 100644
--- a/data/sphere_1cm.urdf
+++ b/data/sphere_1cm.urdf
@@ -2,23 +2,15 @@
 <robot name="urdf_robot">
   <link name="base_link">
     <contact>
-      <rolling_friction value="0.0001"/>
-      <spinning_friction value="0.0001"/>
+      <rolling_friction value="0.001"/>
+      <spinning_friction value="0.001"/>
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>
        <mass value=".1"/>
        <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
     </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-				<mesh filename="textured_sphere_smooth.obj" scale="0.005 0.005 0.005"/>      
-      </geometry>
-      <material name="white">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
+    
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
diff --git a/examples/CommonInterfaces/CommonExampleInterface.h b/examples/CommonInterfaces/CommonExampleInterface.h
index df5f1817e..fd0b2651a 100644
--- a/examples/CommonInterfaces/CommonExampleInterface.h
+++ b/examples/CommonInterfaces/CommonExampleInterface.h
@@ -37,6 +37,7 @@ public:
 	
 	virtual void    initPhysics()=0;
 	virtual void    exitPhysics()=0;
+	virtual void	updateGraphics(){}
 	virtual void	stepSimulation(float deltaTime)=0;
 	virtual void	renderScene()=0;
 	virtual void	physicsDebugDraw(int debugFlags)=0;//for now we reuse the flags in Bullet/src/LinearMath/btIDebugDraw.h
diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.cpp b/examples/ExampleBrowser/InProcessExampleBrowser.cpp
index 0b979b361..88a2abd99 100644
--- a/examples/ExampleBrowser/InProcessExampleBrowser.cpp
+++ b/examples/ExampleBrowser/InProcessExampleBrowser.cpp
@@ -266,6 +266,7 @@ void	ExampleBrowserThreadFunc(void* userPtr,void* lsMemory)
 				{
 					B3_PROFILE("clock.usleep");
 					clock.usleep(gMinUpdateTimeMicroSecs/10.);
+					exampleBrowser->updateGraphics();
 				} else
 				{
 					B3_PROFILE("exampleBrowser->update");
diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp
index fb13df898..65464268c 100644
--- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp
+++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp
@@ -309,8 +309,11 @@ static void MyMouseMoveCallback( float x, float y)
   	bool handled = false;
 	if (sCurrentDemo)
 		handled = sCurrentDemo->mouseMoveCallback(x,y);
-	if (!handled && gui2)
-		handled = gui2->mouseMoveCallback(x,y);
+	if (renderGui)
+	{
+		if (!handled && gui2)
+			handled = gui2->mouseMoveCallback(x,y);
+	}
 	if (!handled)
 	{
 		if (prevMouseMoveCallback)
@@ -327,9 +330,11 @@ static void MyMouseButtonCallback(int button, int state, float x, float y)
 	if (sCurrentDemo)
 		handled = sCurrentDemo->mouseButtonCallback(button,state,x,y);
 
-	if (!handled && gui2)
-		handled = gui2->mouseButtonCallback(button,state,x,y);
-
+	if (renderGui)
+	{
+		if (!handled && gui2)
+			handled = gui2->mouseButtonCallback(button,state,x,y);
+	}
 	if (!handled)
 	{
 		if (prevMouseButtonCallback )
@@ -1125,6 +1130,18 @@ bool OpenGLExampleBrowser::requestedExit()
 	return s_window->requestedExit();
 }
 
+void OpenGLExampleBrowser::updateGraphics()
+{
+	if (sCurrentDemo)
+	{
+			if (!pauseSimulation || singleStepSimulation)
+			{
+				B3_PROFILE("sCurrentDemo->updateGraphics");
+				sCurrentDemo->updateGraphics();
+			}
+	}
+}
+
 void OpenGLExampleBrowser::update(float deltaTime)
 {
 	b3ChromeUtilsEnableProfiling();
diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.h b/examples/ExampleBrowser/OpenGLExampleBrowser.h
index 1f68abedb..533b666ff 100644
--- a/examples/ExampleBrowser/OpenGLExampleBrowser.h
+++ b/examples/ExampleBrowser/OpenGLExampleBrowser.h
@@ -19,6 +19,8 @@ public:
 
 	virtual void update(float deltaTime);
 
+	virtual void updateGraphics();
+
 	virtual bool requestedExit();
 
 	virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
index 30b2d90f4..30f35963a 100644
--- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
@@ -71,7 +71,7 @@ struct URDF2BulletCachedData
     }
 
 
-    void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame)
+    void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
     {
         m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
     }
@@ -81,7 +81,7 @@ struct URDF2BulletCachedData
         return m_urdfLink2rigidBodies[urdfLinkIndex];
     }
 
-    void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame)
+    void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
     {
         btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0);
 
@@ -250,7 +250,12 @@ void ConvertURDF2BulletInternal(
     
     
 
-    btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
+    btCompoundShape* tmpShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
+	btCollisionShape* compoundShape = tmpShape;
+	if (tmpShape->getNumChildShapes() == 1 && tmpShape->getChildTransform(0)==btTransform::getIdentity())
+	{
+		compoundShape = tmpShape->getChildShape(0);
+	}
 	
 	int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
 	
diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp
index e7c51fe15..212d80a6f 100644
--- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp
+++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp
@@ -787,7 +787,7 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
 
 #ifdef _WIN32
 	sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
-		"-threads 0 -y -b 50000k  -t 20 -c:v libx264 -preset slow -crf 22 -an   -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName);
+		"-threads 0 -y -b:v 50000k  -t 20 -c:v libx264 -preset slow -crf 22 -an   -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName);
 
     //sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba   -s %dx%d -i - "
     //		"-y -crf 0  -b:v 1500000 -an -vcodec h264 -vf vflip  %s", width, height, mp4FileName);
diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp
index 4b86a567d..5dbbd1562 100644
--- a/examples/RoboticsLearning/GripperGraspExample.cpp
+++ b/examples/RoboticsLearning/GripperGraspExample.cpp
@@ -16,8 +16,8 @@
 #include "b3RobotSimAPI.h"
 #include "../Utils/b3Clock.h"
 
-static btScalar sGripperVerticalVelocity = -0.2f;
-static btScalar sGripperClosingTargetVelocity = 0.5f;
+static btScalar sGripperVerticalVelocity = 0.f;
+static btScalar sGripperClosingTargetVelocity = -0.7f;
 
 class GripperGraspExample : public CommonExampleInterface
 {
@@ -226,9 +226,9 @@ public:
             {
                 b3RobotSimLoadFileArgs args("");
                 b3RobotSimLoadFileResults results;
-                args.m_fileName = "sphere_small.urdf";
-                args.m_startPosition.setValue(0, 0, .107);
-                args.m_startOrientation.setEulerZYX(0, 0, 0);
+                args.m_fileName = "dinnerware/pan_tefal.urdf";
+                args.m_startPosition.setValue(0, -0.2, .47);
+                args.m_startOrientation.setEulerZYX(SIMD_HALF_PI, 0, 0);
                 args.m_useMultiBody = true;
                 m_robotSim.loadFile(args, results);
             }
@@ -492,7 +492,7 @@ public:
             int fingerJointIndices[2]={0,1};
             double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
             };
-            double maxTorqueValues[2]={50.0,50.0};
+            double maxTorqueValues[2]={800.0,800.0};
             for (int i=0;i<2;i++)
             {
                 b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
@@ -550,8 +550,8 @@ public:
 	virtual void resetCamera()
 	{
 		float dist = 1.5;
-        float pitch = 12;
-        float yaw = -10;
+        float pitch = 18;
+        float yaw = 10;
 		float targetPos[3]={-0.2,0.8,0.3};
 		if (m_app->m_renderer  && m_app->m_renderer->getActiveCamera())
 		{
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index f02e332a4..e311cff08 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -338,6 +338,16 @@ int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandl
 	return 0;
 }
 
+int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+
+	command->m_physSimParamArgs.m_contactBreakingThreshold = contactBreakingThreshold;
+	command->m_updateFlags |= SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD;
+	return 0;
+}
+
 int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index febe08396..b150e54d7 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -191,7 +191,7 @@ int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHand
 	
 int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse);
 int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold);
-
+int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold);
 
 //b3PhysicsParamSetInternalSimFlags is for internal/temporary/easter-egg/experimental demo purposes
 //Use at own risk: magic things may or my not happen when calling this API
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index cd4260469..75271eb37 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -12,6 +12,7 @@
 #include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
 #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
 #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 #include "LinearMath/btHashMap.h"
 #include "BulletInverseDynamics/MultiBodyTree.hpp"
 #include "IKTrajectoryHelper.h"
@@ -798,7 +799,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
     m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
     
 	m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
-	m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
+	m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 150;
 	m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
 //	m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2;
 	//todo: islands/constraints are buggy in btMultiBodyDynamicsWorld! (performance + see slipping grasp)
@@ -2724,7 +2725,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					{
 						m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations;
 					}
-
+					if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD)
+					{
+						gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold;
+					}
 					if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
 					{
 						m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode;
diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp
index 4c9ae62fe..520a6d904 100644
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/examples/SharedMemory/PhysicsServerExample.cpp
@@ -553,7 +553,7 @@ public:
 		
 		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
 		{
-			b3Clock::usleep(100);
+			b3Clock::usleep(0);
 		}
 	}
 
@@ -940,6 +940,8 @@ public:
 
 	virtual void	stepSimulation(float deltaTime);
 
+	virtual void updateGraphics();
+
     void enableCommandLogging()
 	{
 		m_physicsServer.enableCommandLogging(true,"BulletPhysicsCommandLog.bin");
@@ -1342,39 +1344,8 @@ bool PhysicsServerExample::wantsTermination()
     return m_wantsShutdown;
 }
 
-
-
-void	PhysicsServerExample::stepSimulation(float deltaTime)
+void	PhysicsServerExample::updateGraphics()
 {
-	BT_PROFILE("PhysicsServerExample::stepSimulation");
-
-	//this->m_physicsServer.processClientCommands();
-
-	for (int i = m_multiThreadedHelper->m_userDebugLines.size()-1;i>=0;i--)
-	{
-		if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime)
-		{
-			m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime -= deltaTime;
-			if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime<=0)
-			{
-				m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1);
-				m_multiThreadedHelper->m_userDebugLines.pop_back();
-			}
-		}
-	}
-
-	for (int i = m_multiThreadedHelper->m_userDebugText.size()-1;i>=0;i--)
-	{
-		if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime)
-		{
-			m_multiThreadedHelper->m_userDebugText[i].m_lifeTime -= deltaTime;
-			if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime<=0)
-			{
-				m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1);
-				m_multiThreadedHelper->m_userDebugText.pop_back();
-			}
-		}
-	}
 	//check if any graphics related tasks are requested
 	
 	switch (m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
@@ -1543,6 +1514,40 @@ void	PhysicsServerExample::stepSimulation(float deltaTime)
 	}
 	
 
+}
+
+void	PhysicsServerExample::stepSimulation(float deltaTime)
+{
+	BT_PROFILE("PhysicsServerExample::stepSimulation");
+
+	//this->m_physicsServer.processClientCommands();
+
+	for (int i = m_multiThreadedHelper->m_userDebugLines.size()-1;i>=0;i--)
+	{
+		if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime)
+		{
+			m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime -= deltaTime;
+			if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime<=0)
+			{
+				m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1);
+				m_multiThreadedHelper->m_userDebugLines.pop_back();
+			}
+		}
+	}
+
+	for (int i = m_multiThreadedHelper->m_userDebugText.size()-1;i>=0;i--)
+	{
+		if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime)
+		{
+			m_multiThreadedHelper->m_userDebugText[i].m_lifeTime -= deltaTime;
+			if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime<=0)
+			{
+				m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1);
+				m_multiThreadedHelper->m_userDebugText.pop_back();
+			}
+		}
+	}
+	updateGraphics();
 
 
 	
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index 847a0de75..94d8b6a9c 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -313,7 +313,8 @@ enum EnumSimParamUpdateFlags
 	SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS=64,
 	SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE=128,
 	SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD = 256,
-	SIM_PARAM_UPDATE_COLLISION_FILTER_MODE=512
+	SIM_PARAM_UPDATE_COLLISION_FILTER_MODE=512,
+	SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD = 1024,
 };
 
 enum EnumLoadBunnyUpdateFlags
@@ -340,6 +341,7 @@ struct SendPhysicsSimulationParameters
 	bool m_allowRealTimeSimulation;
 	int m_useSplitImpulse;
 	double m_splitImpulsePenetrationThreshold;
+	double m_contactBreakingThreshold;
 	int m_internalSimFlags;
 	double m_defaultContactERP;
 	int m_collisionFilterMode;
diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
index b4933aeb4..f1a4b4966 100644
--- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
+++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
@@ -44,16 +44,13 @@ public:
             PhysicsClientSharedMemory::disconnectSharedMemory();
         }
     	unsigned long int ms = m_clock.getTimeMilliseconds();
-	if (ms>20)
-	{ 
-		m_clock.reset(); 
-        	btUpdateInProcessExampleBrowserMainThread(m_data);
-        } else
-	{
-		//b3Clock::usleep(100);
-	}
-	return PhysicsClientSharedMemory::processServerStatus();
-        
+		if (ms>20)
+		{ 
+			m_clock.reset(); 
+        		btUpdateInProcessExampleBrowserMainThread(m_data);
+		}
+		b3Clock::usleep(0);
+		return PhysicsClientSharedMemory::processServerStatus();
         
     }
     
diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp
index 590312503..4c6b16ffe 100644
--- a/examples/StandaloneMain/hellovr_opengl_main.cpp
+++ b/examples/StandaloneMain/hellovr_opengl_main.cpp
@@ -135,6 +135,8 @@ public:
 	void SetupRenderModelForTrackedDevice( vr::TrackedDeviceIndex_t unTrackedDeviceIndex );
 	CGLRenderModel *FindOrLoadRenderModel( const char *pchRenderModelName );
 
+	SimpleOpenGL3App* getApp() { return m_app;}
+
 private: 
 	bool m_bDebugOpenGL;
 	bool m_bVerbose;
@@ -2260,7 +2262,7 @@ int main(int argc, char *argv[])
 		b3ChromeUtilsEnableProfiling();
 	}
 
-
+	
 #ifdef BT_USE_CUSTOM_PROFILER
 	b3SetCustomEnterProfileZoneFunc(dcEnter);
 	b3SetCustomLeaveProfileZoneFunc(dcLeave);
@@ -2287,6 +2289,11 @@ int main(int argc, char *argv[])
 
 	}
 
+	char* gVideoFileName = 0;
+    args.GetCmdLineArgument("mp4",gVideoFileName);
+    if (gVideoFileName)
+        pMainApplication->getApp()->dumpFramesToVideo(gVideoFileName);
+ 
 	//request disable VSYNC
 	typedef bool (APIENTRY *PFNWGLSWAPINTERVALFARPROC)(int);
 	PFNWGLSWAPINTERVALFARPROC wglSwapIntervalEXT = 0;
diff --git a/examples/pybullet/manyspheres.py b/examples/pybullet/manyspheres.py
index c06a477ab..1c4c37b97 100644
--- a/examples/pybullet/manyspheres.py
+++ b/examples/pybullet/manyspheres.py
@@ -7,9 +7,9 @@ p.loadURDF("tray/traybox.urdf",useMaximalCoordinates=True)
 
 gravXid = p.addUserDebugParameter("gravityX",-10,10,0)
 gravYid = p.addUserDebugParameter("gravityY",-10,10,0)
-gravZid = p.addUserDebugParameter("gravityZ",-10,10,0)
+gravZid = p.addUserDebugParameter("gravityZ",-10,10,-10)
 p.setPhysicsEngineParameter(numSolverIterations=10)
-
+p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)
 for i in range (10):
     for j in range (10):
         for k in range (5):
@@ -21,5 +21,5 @@ while True:
     gravY = p.readUserDebugParameter(gravYid)
     gravZ = p.readUserDebugParameter(gravZid)
     p.setGravity(gravX,gravY,gravZ)
-    time.sleep(1)
+    time.sleep(0.01)
 
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 65cc7e108..5b43cf6d7 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -583,14 +583,15 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 	double splitImpulsePenetrationThreshold = -1;
 	int numSubSteps = -1;
 	int collisionFilterMode = -1;
+	double contactBreakingThreshold = -1;
 	
 	b3PhysicsClientHandle sm = 0;
 
 	int physicsClientId = 0;
-	static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps","collisionFilterMode", "physicsClientId", NULL };
+	static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps","collisionFilterMode", "contactBreakingThreshold", "physicsClientId", NULL };
 
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiii", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps,
-									 &collisionFilterMode, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidi", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps,
+									 &collisionFilterMode, &contactBreakingThreshold, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -610,6 +611,7 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 		{
 			b3PhysicsParamSetNumSolverIterations(command, numSolverIterations);
 		}
+
 		if (collisionFilterMode>=0)
 		{
 			b3PhysicsParamSetCollisionFilterMode(command, collisionFilterMode);
@@ -630,6 +632,11 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 		{
 			b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, splitImpulsePenetrationThreshold);
 		}
+		if (contactBreakingThreshold>=0)
+		{
+			b3PhysicsParamSetContactBreakingThreshold(command,contactBreakingThreshold);
+		}
+
 
 		//ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
 

From 65deeee64b2663c907cd9e7b8411b535d96b9e0f Mon Sep 17 00:00:00 2001
From: Erwin Coumans <erwin.coumans@gmail.com>
Date: Thu, 16 Feb 2017 14:29:51 -0800
Subject: [PATCH 7/7] add optimized tray/traybox.urdf

---
 data/tray/tray_textured.mtl |  13 +++
 data/tray/tray_textured.obj | 213 ++++++++++++++++++++++++++++++++++++
 data/tray/traybox.urdf      |  49 +++++++++
 3 files changed, 275 insertions(+)
 create mode 100644 data/tray/tray_textured.mtl
 create mode 100644 data/tray/tray_textured.obj
 create mode 100644 data/tray/traybox.urdf

diff --git a/data/tray/tray_textured.mtl b/data/tray/tray_textured.mtl
new file mode 100644
index 000000000..e3b14fbaf
--- /dev/null
+++ b/data/tray/tray_textured.mtl
@@ -0,0 +1,13 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0.000000
+Ka 0.000000 0.000000 0.000000
+Kd 0.800000 0.800000 0.800000
+Ks 0.800000 0.800000 0.800000
+Ke 0.000000 0.000000 0.000000
+Ni 1.000000
+d 1.000000
+illum 2
+map_Kd tray.jpg
diff --git a/data/tray/tray_textured.obj b/data/tray/tray_textured.obj
new file mode 100644
index 000000000..f3523df4d
--- /dev/null
+++ b/data/tray/tray_textured.obj
@@ -0,0 +1,213 @@
+# Blender v2.78 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib tray_textured.mtl
+o edge_4_Cube.001
+v -0.573309 0.580000 0.261247
+v -0.426691 0.419400 -0.002214
+v -0.590083 0.580000 0.250354
+v -0.573309 -0.580000 0.261247
+v -0.409917 0.419400 0.008679
+v -0.590083 -0.580000 0.250354
+v -0.409917 -0.419400 0.009162
+v -0.426691 -0.419400 -0.001731
+vt 0.9046 0.2397
+vt 0.7929 0.2434
+vt 0.9174 0.2393
+vt 0.9537 0.7559
+vt 0.7801 0.2438
+vt 0.9664 0.7554
+vt 0.8291 0.7599
+vt 0.8419 0.7595
+vn 0.2565 0.8821 -0.3950
+vn 0.8392 0.0002 0.5438
+vn 0.8395 0.0000 0.5433
+vn 0.8396 -0.0000 0.5432
+vn 0.2568 -0.8819 -0.3954
+vn -0.8396 -0.0002 -0.5433
+vn -0.8392 -0.0000 -0.5438
+vn -0.8391 0.0000 -0.5439
+vn 0.5446 -0.0005 -0.8387
+vn -0.5446 -0.0000 0.8387
+vn 0.8391 0.0003 0.5440
+vn -0.8397 -0.0003 -0.5430
+usemtl None
+s 1
+f 1/1/1 2/2/1 3/3/1
+f 4/4/2 5/5/3 1/1/4
+f 6/6/5 7/7/5 4/4/5
+f 3/3/6 8/8/7 6/6/8
+f 5/5/9 8/8/9 2/2/9
+f 4/4/10 3/3/10 6/6/10
+f 1/1/1 5/5/1 2/2/1
+f 4/4/2 7/7/11 5/5/3
+f 6/6/5 8/8/5 7/7/5
+f 3/3/6 2/2/12 8/8/7
+f 5/5/9 7/7/9 8/8/9
+f 4/4/10 1/1/10 3/3/10
+o edge_1_Cube.003
+v 0.580000 0.590083 0.250354
+v -0.419960 0.426691 -0.001860
+v -0.580000 0.590083 0.250354
+v 0.580000 0.573309 0.261247
+v 0.420014 0.426691 -0.001059
+v -0.580000 0.573309 0.261247
+v 0.420014 0.409917 0.009834
+v -0.419960 0.409917 0.009033
+vt 0.8346 0.9187
+vt 0.2203 0.8574
+vt 0.1480 0.9187
+vt 0.8346 0.9129
+vt 0.7623 0.8574
+vt 0.1480 0.9129
+vt 0.7623 0.8511
+vt 0.2203 0.8511
+vn 0.0004 0.8386 -0.5448
+vn 0.0001 0.8391 -0.5439
+vn 0.0000 0.8393 -0.5437
+vn 0.8823 -0.2564 -0.3948
+vn -0.0004 -0.8392 0.5439
+vn -0.0001 -0.8386 0.5447
+vn 0.0000 -0.8385 0.5449
+vn -0.8826 -0.2560 -0.3942
+vn 0.0008 -0.5446 -0.8387
+vn 0.0000 0.5446 0.8387
+vn 0.0005 0.8383 -0.5452
+vn -0.0005 -0.8394 0.5435
+usemtl None
+s 1
+f 9/9/13 10/10/14 11/11/15
+f 12/12/16 13/13/16 9/9/16
+f 14/14/17 15/15/18 12/12/19
+f 11/11/20 16/16/20 14/14/20
+f 13/13/21 16/16/21 10/10/21
+f 12/12/22 11/11/22 14/14/22
+f 9/9/13 13/13/23 10/10/14
+f 12/12/16 15/15/16 13/13/16
+f 14/14/17 16/16/24 15/15/18
+f 11/11/20 10/10/20 16/16/20
+f 13/13/21 15/15/21 16/16/21
+f 12/12/22 9/9/22 11/11/22
+o edge_2_Cube
+v 0.590083 0.580000 0.250354
+v 0.409917 0.420060 0.009390
+v 0.573309 0.580000 0.261247
+v 0.590083 -0.580000 0.250354
+v 0.426691 0.420060 -0.001503
+v 0.573309 -0.580000 0.261247
+v 0.426691 -0.419158 -0.002053
+v 0.409917 -0.419158 0.008840
+vt 0.9410 0.8520
+vt 0.7523 0.8566
+vt 0.9234 0.8524
+vt 0.8896 0.1426
+vt 0.7698 0.8562
+vt 0.8721 0.1430
+vt 0.7185 0.1468
+vt 0.7009 0.1472
+vn -0.2561 0.8826 -0.3943
+vn 0.8394 0.0003 -0.5435
+vn 0.8390 0.0001 -0.5441
+vn 0.8389 0.0000 -0.5443
+vn -0.2569 -0.8818 -0.3956
+vn -0.8390 -0.0003 0.5441
+vn -0.8394 -0.0001 0.5436
+vn -0.8395 -0.0000 0.5434
+vn -0.5446 0.0005 -0.8387
+vn 0.5446 -0.0000 0.8387
+vn 0.8396 0.0004 -0.5433
+vn -0.8388 -0.0004 0.5444
+usemtl None
+s 1
+f 17/17/25 18/18/25 19/19/25
+f 20/20/26 21/21/27 17/17/28
+f 22/22/29 23/23/29 20/20/29
+f 19/19/30 24/24/31 22/22/32
+f 21/21/33 24/24/33 18/18/33
+f 20/20/34 19/19/34 22/22/34
+f 17/17/25 21/21/25 18/18/25
+f 20/20/26 23/23/35 21/21/27
+f 22/22/29 24/24/29 23/23/29
+f 19/19/30 18/18/36 24/24/31
+f 21/21/33 23/23/33 24/24/33
+f 20/20/34 17/17/34 19/19/34
+o base_Cube.004
+v 0.420000 0.420000 0.010000
+v -0.420000 0.420000 -0.010000
+v -0.420000 0.420000 0.010000
+v 0.420000 -0.420000 0.010000
+v 0.420000 0.420000 -0.010000
+v -0.420000 -0.420000 0.010000
+v 0.420000 -0.420000 -0.010000
+v -0.420000 -0.420000 -0.010000
+vt 0.7524 0.8072
+vt -0.3038 0.8371
+vt -0.3038 0.8371
+vt 0.7012 0.1905
+vt 0.7524 0.8072
+vt -0.3550 0.2204
+vt 0.7012 0.1905
+vt -0.3550 0.2204
+vn 0.0000 1.0000 0.0000
+vn 1.0000 -0.0000 0.0000
+vn 0.0000 -1.0000 0.0000
+vn -1.0000 -0.0000 0.0000
+vn 0.0000 0.0000 -1.0000
+vn 0.0000 -0.0000 1.0000
+usemtl None
+s 1
+f 25/25/37 26/26/37 27/27/37
+f 28/28/38 29/29/38 25/25/38
+f 30/30/39 31/31/39 28/28/39
+f 27/27/40 32/32/40 30/30/40
+f 29/29/41 32/32/41 26/26/41
+f 28/28/42 27/27/42 30/30/42
+f 25/25/37 29/29/37 26/26/37
+f 28/28/38 31/31/38 29/29/38
+f 30/30/39 32/32/39 31/31/39
+f 27/27/40 26/26/40 32/32/40
+f 29/29/41 31/31/41 32/32/41
+f 28/28/42 25/25/42 27/27/42
+o edge_3_Cube.002
+v 0.580000 -0.573309 0.261247
+v -0.419400 -0.409917 0.008678
+v -0.580000 -0.573309 0.261247
+v 0.580000 -0.590083 0.250354
+v 0.419883 -0.409917 0.009162
+v -0.580000 -0.590083 0.250354
+v 0.419883 -0.426691 -0.001731
+v -0.419400 -0.426691 -0.002215
+vt 0.8690 0.1040
+vt 0.1365 0.1739
+vt 0.0188 0.1040
+vt 0.8690 0.0968
+vt 0.7517 0.1739
+vt 0.0188 0.0968
+vt 0.7517 0.1668
+vt 0.1365 0.1668
+vn -0.0002 0.8392 0.5438
+vn -0.0000 0.8395 0.5433
+vn 0.0000 0.8396 0.5432
+vn 0.8825 0.2562 -0.3945
+vn 0.0002 -0.8396 -0.5433
+vn 0.0000 -0.8392 -0.5438
+vn 0.0000 -0.8391 -0.5439
+vn -0.8821 0.2565 -0.3950
+vn -0.8822 0.2565 -0.3950
+vn 0.0005 0.5446 -0.8387
+vn 0.0000 -0.5446 0.8387
+vn -0.0003 0.8391 0.5440
+vn 0.0003 -0.8397 -0.5430
+usemtl None
+s 1
+f 33/33/43 34/34/44 35/35/45
+f 36/36/46 37/37/46 33/33/46
+f 38/38/47 39/39/48 36/36/49
+f 35/35/50 40/40/51 38/38/51
+f 37/37/52 40/40/52 34/34/52
+f 36/36/53 35/35/53 38/38/53
+f 33/33/43 37/37/54 34/34/44
+f 36/36/46 39/39/46 37/37/46
+f 38/38/47 40/40/55 39/39/48
+f 35/35/50 34/34/51 40/40/51
+f 37/37/52 39/39/52 40/40/52
+f 36/36/53 33/33/53 35/35/53
diff --git a/data/tray/traybox.urdf b/data/tray/traybox.urdf
new file mode 100644
index 000000000..58e578f11
--- /dev/null
+++ b/data/tray/traybox.urdf
@@ -0,0 +1,49 @@
+<robot name="tabletop">
+  <link name="base_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="tray_textured.obj" scale="0.5 0.5 0.5"/>
+      </geometry>
+      <material name="tray_material">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+     <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.005"/>
+      <geometry>
+	 			<box size=".6 .6 .02"/>
+      </geometry>
+    </collision>
+     <collision>
+      <origin rpy="0 0.575469961 0" xyz="0.25 0 0.059"/>
+      <geometry>
+	 			<box size=".02 .6 .15"/>
+      </geometry>
+    </collision>
+		<collision>
+      <origin rpy="0 -0.575469961 0" xyz="-0.25 0 0.059"/>
+      <geometry>
+	 			<box size=".02 .6 .15"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0.575469961 0 0" xyz="0 -0.25 0.059"/>
+      <geometry>
+	 			<box size=".6 .02 .15"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="-0.575469961 0 0" xyz="0 0.25 0.059"/>
+      <geometry>
+	 			<box size=".6 .02 .15"/>
+      </geometry>
+    </collision>
+
+  </link>
+</robot>