further work on saveRestoreState.py

Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp

@@ -80,9 +80,42 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 
 				for (int i = 0; i < mbd->m_numLinks; i++)
 				{
+					switch (mbd->m_links[i].m_jointType)
+					{
+					case btMultibodyLink::eFixed:
+					{
+						break;
+					}
+					case btMultibodyLink::ePrismatic:
+					{
+						mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
+						mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
+						break;
+					}
+					case btMultibodyLink::eRevolute:
+					{
+						mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
+						mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
+						break;
+					}
+					case btMultibodyLink::eSpherical:
+					{
+						btScalar jointPos[3] = { mbd->m_links[i].m_jointPos[0], mbd->m_links[i].m_jointPos[1], mbd->m_links[i].m_jointPos[2] };
+						btScalar jointVel[3] = { mbd->m_links[i].m_jointVel[0], mbd->m_links[i].m_jointVel[1], mbd->m_links[i].m_jointVel[2] };
+						mb->setJointPosMultiDof(i, jointPos);
+						mb->setJointVelMultiDof(i, jointVel);
+
+						break;
+					}
+					case btMultibodyLink::ePlanar:
+					{
+						break;
+					}
+					default:
+					{
+					}
+					}
 				}
-				btVector3 linvel = mb->getBaseVel();
-				btVector3 angvel = mb->getBaseOmega();
 				mb->forwardKinematics(scratchQ, scratchM);
 				mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM);
 			}
@@ -145,7 +178,8 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 								}
 								else
 								{
-									printf("Issue: cannot find maching contact manifold (%d, %d), may cause issues in determinism.\n", uid0, uid1);
+									existingManifold->setNumContacts(0);
+									//printf("Issue: cannot find maching contact manifold (%d, %d), may cause issues in determinism.\n", uid0, uid1);
 								}
 
 								manifoldArray.clear();

examples/pybullet/examples/saveRestoreState.py

@@ -3,16 +3,20 @@ import math, time
 import difflib,sys
 
 numSteps = 500
-numSteps2 = 3
+numSteps2 = 30
 p.connect(p.GUI, options="--width=1024 --height=768")
-numObjects = 10
+numObjects = 50
 verbose = 0
 
 def setupWorld():
 	p.resetSimulation()
 	p.loadURDF("planeMesh.urdf")
+	kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10])
+	for i in range (p.getNumJoints(kukaId)):
+		p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0)
 	for i in range (numObjects):
-		cube = p.loadURDF("cube_small.urdf",0,0,(i+1)*0.5)
+		cube = p.loadURDF("cube_small.urdf",0,i*0.02,(i+1)*0.2)
+		#p.changeDynamics(cube,-1,mass=100)
 	p.stepSimulation()
 	p.setGravity(0,0,-10)
 
@@ -80,15 +84,35 @@ file = open("restoreFile.txt","w")
 dumpStateToFile(file)
 file.close() 
 
+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("restoreFile2.txt","w")
+dumpStateToFile(file)
+file.close() 
+
 file1 = open("saveFile.txt","r") 
 file2 = open("restoreFile.txt","r") 
-
-
 compareFiles(file1,file2)
-            
+file1.close()
+file2.close()
+
+file1 = open("saveFile.txt","r") 
+file2 = open("restoreFile2.txt","r") 
+compareFiles(file1,file2)
+file1.close()
+file2.close()        
 
 while (p.getConnectionInfo()["isConnected"]):
 	time.sleep(1)
 
-file1.close()
-file2.close()

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -1665,8 +1665,8 @@ void btCollisionWorld::serializeContactManifolds(btSerializer* serializer)
 			const btPersistentManifold* manifold = getDispatcher()->getInternalManifoldPointer()[i];
 			//don't serialize empty manifolds, they just take space 
 			//(may have to do it anyway if it destroys determinism)
-			if (manifold->getNumContacts() == 0)
+			//if (manifold->getNumContacts() == 0)
-				continue;
+			//	continue;
 
 			btChunk* chunk = serializer->allocate(manifold->calculateSerializeBufferSize(), 1);
 			const char* structType = manifold->serialize(manifold, chunk->m_oldPtr, serializer);