made some progress in saving and restoring the state during the simulation, with identical results.

Option to de/serialize btPersistentContactManifolds and fix lossy conversion during btMultiBody de/serialization of base world transform
(serialize the quaternion, not the converted 3x3 matrix)
There are still several caches not taken into account, and btMultiBody links/constraints are not deserialized yet etc.
See examples\pybullet\examples\saveRestoreState.py for an example.

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -2172,9 +2172,12 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 	m_data->m_pairCache = new btHashedOverlappingPairCache();
 	
 	m_data->m_pairCache->setOverlapFilterCallback(m_data->m_broadphaseCollisionFilterCallback);
-	
+
+	//int maxProxies = 32768;
+	//m_data->m_broadphase = new btSimpleBroadphase(maxProxies, m_data->m_pairCache);
     m_data->m_broadphase = new btDbvtBroadphase(m_data->m_pairCache);
     
+
     m_data->m_solver = new btMultiBodyConstraintSolver;
     
 #ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
@@ -2183,7 +2186,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
     m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
 #endif
     
-    //Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
+    //Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
     m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(32768);
     
     m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
@@ -8305,6 +8308,9 @@ bool PhysicsServerCommandProcessor::processSaveBulletCommand(const struct Shared
 	if (f)
 	{
 		btDefaultSerializer* ser = new btDefaultSerializer();
+		int currentFlags = ser->getSerializationFlags();
+		ser->setSerializationFlags(currentFlags | BT_SERIALIZE_CONTACT_MANIFOLDS);
+
 		m_data->m_dynamicsWorld->serialize(ser);
 		fwrite(ser->getBufferPointer(), ser->getCurrentBufferSize(), 1, f);
 		fclose(f);

examples/pybullet/examples/saveRestoreState.py

@@ -1,30 +1,95 @@
 import pybullet as p
 import math, time
+import difflib,sys
 
+numSteps = 500
+numSteps2 = 3
 p.connect(p.GUI, options="--width=1024 --height=768")
+numObjects = 10
+verbose = 0
 
-p.loadURDF("plane.urdf")
-for i in range (10):
-	cube = p.loadURDF("cube_small.urdf",0,i*0.01,i*0.5)
-p.setGravity(0,0,-10)
+def setupWorld():
+	p.resetSimulation()
+	p.loadURDF("planeMesh.urdf")
+	for i in range (numObjects):
+		cube = p.loadURDF("cube_small.urdf",0,0,(i+1)*0.5)
+	p.stepSimulation()
+	p.setGravity(0,0,-10)
 
 
-#for i in range (500):
-#	p.stepSimulation()
-
-
-
-saveState = 1
-
-if saveState:
-	for i in range (500):
-		p.stepSimulation()
-	p.saveBullet("state.bullet")
-else:
-	p.restoreState(fileName="state.bullet")
-
-for i in range (10):
-	print("pos[",i,"]=",p.getBasePositionAndOrientation(i))
+def dumpStateToFile(file):
+	for i in  range (p.getNumBodies()):
+		pos,orn = p.getBasePositionAndOrientation(i)
+		linVel,angVel = p.getBaseVelocity(i)
+		txtPos = "pos="+str(pos)+"\n"
+		txtOrn = "orn="+str(orn)+"\n"
+		txtLinVel = "linVel"+str(linVel)+"\n"
+		txtAngVel = "angVel"+str(angVel)+"\n"
+		file.write(txtPos)
+		file.write(txtOrn)
+		file.write(txtLinVel)
+		file.write(txtAngVel)
+		
+def compareFiles(file1,file2):
+	diff = difflib.unified_diff(
+            file1.readlines(),
+            file2.readlines(),
+            fromfile='saveFile.txt',
+            tofile='restoreFile.txt',
+        )
+	for line in diff:
+		sys.stdout.write(line)
 	
+setupWorld()
+for i in range (numSteps):
+	p.stepSimulation()	
+p.saveBullet("state.bullet")
+if verbose:
+	p.setInternalSimFlags(1)
+p.stepSimulation()
+if verbose:
+	p.setInternalSimFlags(0)
+	print("contact points=")
+	for q in p.getContactPoints():
+		print(q)
+
+for i in range (numSteps2):
+	p.stepSimulation()
+
+
+file = open("saveFile.txt","w") 
+dumpStateToFile(file)
+file.close() 
+
+#################################
+setupWorld()
+p.restoreState(fileName="state.bullet")
+if verbose:
+	p.setInternalSimFlags(1)
+p.stepSimulation()
+if verbose:
+	p.setInternalSimFlags(0)
+	print("contact points restored=")
+	for q in p.getContactPoints():
+		print(q)
+for i in range (numSteps2):
+	p.stepSimulation()
+
+	
+file = open("restoreFile.txt","w")
+dumpStateToFile(file)
+file.close() 
+
+file1 = open("saveFile.txt","r") 
+file2 = open("restoreFile.txt","r") 
+#file3 = open("saveFile.txt","r")#saveFileReorder.txt","r") 
+
+compareFiles(file1,file2)
+#compareFiles(file1,file3)
+            
+
 while (p.getConnectionInfo()["isConnected"]):
-	time.sleep(1)
+	time.sleep(1)
+
+file1.close()
+file2.close()

examples/pybullet/pybullet.c

@@ -7876,8 +7876,8 @@ static PyMethodDef SpamMethods[] = {
 	 { "restoreState", (PyCFunction)pybullet_restoreState, METH_VARARGS | METH_KEYWORDS,
 	"Restore the full state of an existing world." },
 
-//	{ "saveState", (PyCFunction)pybullet_saveState, METH_VARARGS | METH_KEYWORDS,
-//	"Save the full state of the world to memory." },
+	{ "saveState", (PyCFunction)pybullet_saveState, METH_VARARGS | METH_KEYWORDS,
+	"Save the full state of the world to memory." },
 
 	{"loadMJCF", (PyCFunction)pybullet_loadMJCF, METH_VARARGS | METH_KEYWORDS,
 	 "Load multibodies from an MJCF file."},