implement pybullet.saveState command, for in-memory storage of state.

bump up pybullet API version (SHARED_MEMORY_MAGIC_NUMBER) to 201801010

Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp

@@ -268,7 +268,7 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 	{
 		//check if the snapshot is valid for the existing world
 		//equal number of objects, # links etc
-		if (bulletFile2->m_multiBodies.size() != m_data->m_mbDynamicsWorld->getNumMultibodies())
+		if ((bulletFile2->m_multiBodies.size() != m_data->m_mbDynamicsWorld->getNumMultibodies()))
 		{
 			result = false;
 			return result;
@@ -287,6 +287,44 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 				syncMultiBody(mbd, mb, m_data, scratchQ, scratchM);
 			}
 
+			for (int i = bulletFile2->m_rigidBodies.size() - 1; i >= 0; i--)
+			{
+				btRigidBodyDoubleData* rbd = (btRigidBodyDoubleData*)bulletFile2->m_rigidBodies[i];
+				int foundRb = -1;
+				int uid = rbd->m_collisionObjectData.m_uniqueId;
+				for (int i = 0; i < m_data->m_mbDynamicsWorld->getNumCollisionObjects(); i++)
+				{
+					if (uid == m_data->m_mbDynamicsWorld->getCollisionObjectArray()[i]->getBroadphaseHandle()->m_uniqueId)
+					{
+						foundRb = i;
+						break;
+					}
+				}
+				if (foundRb >= 0)
+				{
+					btRigidBody* rb = btRigidBody::upcast(m_data->m_mbDynamicsWorld->getCollisionObjectArray()[foundRb]);
+					if (rb)
+					{
+						btTransform tr;
+						tr.deSerializeDouble(rbd->m_collisionObjectData.m_worldTransform);
+						rb->setWorldTransform(tr);
+						btVector3 linVel, angVel;
+						linVel.deSerializeDouble(rbd->m_linearVelocity);
+						angVel.deSerializeDouble(rbd->m_angularVelocity);
+						rb->setLinearVelocity(linVel);
+						rb->setAngularVelocity(angVel);
+					}
+					else
+					{
+						result = false;
+					}
+				}
+				else
+				{
+					result = false;
+				}
+			}
+
 			//todo: check why body1 pointer is not properly deserialized
 			for (int i = 0; i < bulletFile2->m_contactManifolds.size(); i++)
 			{

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -1236,6 +1236,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
 			{
 				break;
 			}
+			case CMD_SAVE_STATE_COMPLETED:
+			{
+				break;
+			}
 			case CMD_RESTORE_STATE_FAILED:
 			{
 				b3Warning("restoreState failed");

examples/SharedMemory/PhysicsDirect.cpp

@@ -1007,6 +1007,10 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 		{
 			break;
 		}
+	case CMD_SAVE_STATE_COMPLETED:
+	{
+		break;
+	}
 	case CMD_RESTORE_STATE_FAILED:
 	{
 		b3Warning("restoreState failed");

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -39,6 +39,7 @@
 #include "Bullet3Common/b3ResizablePool.h"
 #include "../Utils/b3Clock.h"
 #include "b3PluginManager.h"
+#include "../Extras/Serialize/BulletFileLoader/btBulletFile.h"
 
 
 #ifdef STATIC_LINK_VR_PLUGIN
@@ -1459,6 +1460,12 @@ struct ContactPointsStateLogger : public InternalStateLogger
 	}
 };
 
+struct SaveStateData
+{
+	bParse::btBulletFile* m_bulletFile;
+	btSerializer* m_serializer;
+};
+
 struct PhysicsServerCommandProcessorInternalData
 {
 	///handle management
@@ -1476,6 +1483,8 @@ struct PhysicsServerCommandProcessorInternalData
 
 	b3VRControllerEvents m_vrControllerEvents;
 
+	btAlignedObjectArray<SaveStateData> m_savedStates;
+
 
 	btAlignedObjectArray<b3KeyboardEvent> m_keyboardEvents;
 	btAlignedObjectArray<b3MouseEvent> m_mouseEvents;
@@ -8177,6 +8186,32 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share
 	return hasStatus;
 }
 
+bool PhysicsServerCommandProcessor::processSaveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
+{
+	BT_PROFILE("CMD_RESTORE_STATE");
+	bool hasStatus = true;
+	SharedMemoryStatus& serverCmd = serverStatusOut;
+	serverCmd.m_type = CMD_SAVE_STATE_FAILED;
+
+	btDefaultSerializer* ser = new btDefaultSerializer();
+	int currentFlags = ser->getSerializationFlags();
+	ser->setSerializationFlags(currentFlags | BT_SERIALIZE_CONTACT_MANIFOLDS);
+	m_data->m_dynamicsWorld->serialize(ser);
+	bParse::btBulletFile* bulletFile = new bParse::btBulletFile((char*)ser->getBufferPointer(), ser->getCurrentBufferSize());
+	bulletFile->parse(false);
+	if (bulletFile->ok())
+	{
+		serverCmd.m_type = CMD_SAVE_STATE_COMPLETED;
+		serverCmd.m_saveStateResultArgs.m_stateId = m_data->m_savedStates.size();
+		SaveStateData sd;
+		sd.m_bulletFile = bulletFile;
+		sd.m_serializer = ser;
+		m_data->m_savedStates.push_back(sd);
+	}
+	return hasStatus;
+}
+
+
 bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
 {
 	BT_PROFILE("CMD_RESTORE_STATE");
@@ -8191,9 +8226,11 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar
 
 	if (clientCmd.m_loadStateArguments.m_stateId >= 0)
 	{
-		char* memoryBuffer = 0;
+		if (clientCmd.m_loadStateArguments.m_stateId < m_data->m_savedStates.size())
-		int len = 0;
+		{
-		ok = importer->loadFileFromMemory(memoryBuffer, len);
+			bParse::btBulletFile* bulletFile = m_data->m_savedStates[clientCmd.m_loadStateArguments.m_stateId].m_bulletFile;
+			ok = importer->convertAllObjects(bulletFile);
+		}
 	}
 	else
 	{
@@ -8229,7 +8266,6 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar
 		serverCmd.m_type = CMD_RESTORE_STATE_COMPLETED;
 	}	
 
-
 	return hasStatus;
 }
 
@@ -8662,6 +8698,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 		break;
 	}
 
+	case CMD_SAVE_STATE:
+	{
+		hasStatus = processSaveStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
+		break;
+	}
+
 	case CMD_LOAD_BULLET:
 		{
 			hasStatus = processLoadBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
@@ -9060,6 +9102,12 @@ void PhysicsServerCommandProcessor::resetSimulation()
 	if (m_data)
 	{
 		m_data->m_visualConverter.resetAll();
+		for (int i = 0; i < m_data->m_savedStates.size(); i++)
+		{
+			delete m_data->m_savedStates[i].m_bulletFile;
+			delete m_data->m_savedStates[i].m_serializer;
+		}
+		m_data->m_savedStates.clear();
 	}
 
 	removePickingConstraint();

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -81,7 +81,7 @@ protected:
 	bool processSaveBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
-
+	bool processSaveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 
 	bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling);
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -5,7 +5,8 @@
 ///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
 ///my convention is year/month/day/rev
 
-#define SHARED_MEMORY_MAGIC_NUMBER 201710180
+#define SHARED_MEMORY_MAGIC_NUMBER 201801010
+//#define SHARED_MEMORY_MAGIC_NUMBER 201710180
 //#define SHARED_MEMORY_MAGIC_NUMBER 201710050
 //#define SHARED_MEMORY_MAGIC_NUMBER 201708270
 //#define SHARED_MEMORY_MAGIC_NUMBER 201707140

examples/pybullet/examples/saveRestoreState.py

@@ -8,6 +8,8 @@ p.connect(p.GUI, options="--width=1024 --height=768")
 numObjects = 50
 verbose = 0
 
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "saveRestoreTimings.log")
+
 def setupWorld():
 	p.resetSimulation()
 	p.loadURDF("planeMesh.urdf")
@@ -15,7 +17,7 @@ def setupWorld():
 	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,i*0.02,(i+1)*0.2)
+		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)
@@ -41,8 +43,14 @@ def compareFiles(file1,file2):
             fromfile='saveFile.txt',
             tofile='restoreFile.txt',
         )
+	numDifferences = 0
 	for line in diff:
+		numDifferences = numDifferences+1
 		sys.stdout.write(line)
+	if (numDifferences>0):
+		print("Error:", numDifferences, " lines are different between files.")
+	else:
+		print("OK, files are identical")
 	
 setupWorld()
 for i in range (numSteps):
@@ -67,7 +75,11 @@ file.close()
 
 #################################
 setupWorld()
+
+#both restore from file or from in-memory state should work
 p.restoreState(fileName="state.bullet")
+stateId = p.saveState()
+
 if verbose:
 	p.setInternalSimFlags(1)
 p.stepSimulation()
@@ -84,7 +96,7 @@ file = open("restoreFile.txt","w")
 dumpStateToFile(file)
 file.close() 
 
-p.restoreState(fileName="state.bullet")
+p.restoreState(stateId)
 if verbose:
 	p.setInternalSimFlags(1)
 p.stepSimulation()
@@ -113,6 +125,8 @@ compareFiles(file1,file2)
 file1.close()
 file2.close()        
 
-while (p.getConnectionInfo()["isConnected"]):
+p.stopStateLogging(logId)
-	time.sleep(1)
+
+#while (p.getConnectionInfo()["isConnected"]):
+#	time.sleep(1)
 

examples/pybullet/pybullet.c

@@ -756,13 +756,8 @@ static PyObject* pybullet_saveState(PyObject* self, PyObject* args, PyObject* ke
 		return NULL;
 	}
 
-	{
 	stateId = b3GetStatusGetStateId(statusHandle);
-		PyObject* pylist = 0;
+	return PyInt_FromLong(stateId);		
-		pylist = PyTuple_New(1);
-		PyTuple_SetItem(pylist, 0, PyInt_FromLong(stateId));
-		return pylist;
-	}
 }
 
 static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* keywds)