preliminary work towards saveState/restoreState and saveRestoreState.py example (not implemented yet)

allow multiple options in connect, for example: p.connect(p.GUI, options="--width=640, --height=480")

Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp

@@ -36,45 +36,87 @@ void btMultiBodyWorldImporter::deleteAllData()
 
 bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletFile2)
 {
-	bool result = btBulletWorldImporter::convertAllObjects(bulletFile2);
+	bool result = false;
-	
+	btAlignedObjectArray<btQuaternion> scratchQ;
-	
+	btAlignedObjectArray<btVector3> scratchM;
-	//convert all multibodies
+
-	for (int i=0;i<bulletFile2->m_multiBodies.size();i++)
+	if (m_importerFlags&eRESTORE_EXISTING_OBJECTS)
 	{
-		
+		//check if the snapshot is valid for the existing world
-		if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
+		//equal number of objects, # links etc
+		if (bulletFile2->m_multiBodies.size() != m_data->m_mbDynamicsWorld->getNumMultibodies())
 		{
-			btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*) bulletFile2->m_multiBodies[i];
+			result = false;
-			bool isFixedBase = mbd->m_baseMass==0;
+			return result;
-			bool canSleep = false;
+		}
-			btVector3 baseInertia;
+		result = true;
-			baseInertia.deSerializeDouble(mbd->m_baseInertia);
-			btMultiBody* mb = new btMultiBody(mbd->m_numLinks,mbd->m_baseMass,baseInertia,isFixedBase,canSleep);
-			mb->setHasSelfCollision(false);
-			btTransform tr; tr.deSerializeDouble(mbd->m_baseWorldTransform);
-			mb->setBaseWorldTransform(tr);
 
-			m_data->m_mbMap.insert(mbd,mb);
+		//convert all multibodies
-			for (int i=0;i<mbd->m_numLinks;i++)
+		for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
+		{
+
+			if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
 			{
-				btVector3 localInertiaDiagonal;
+				btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
-				localInertiaDiagonal.deSerializeDouble(mbd->m_links[i].m_linkInertia);
+				bool isFixedBase = mbd->m_baseMass == 0;
-				btQuaternion parentRotToThis;
+				bool canSleep = false;
-				parentRotToThis.deSerializeDouble(mbd->m_links[i].m_zeroRotParentToThis);
+				btVector3 baseInertia;
-				btVector3 parentComToThisPivotOffset;
+				baseInertia.deSerializeDouble(mbd->m_baseInertia);
-				parentComToThisPivotOffset.deSerializeDouble(mbd->m_links[i].m_parentComToThisComOffset);
-				btVector3 thisPivotToThisComOffset;
-				thisPivotToThisComOffset.deSerializeDouble(mbd->m_links[i].m_thisPivotToThisComOffset);
 
-				switch (mbd->m_links[i].m_jointType)
+				btTransform tr; tr.deSerializeDouble(mbd->m_baseWorldTransform);
+				btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
+				mb->setBaseWorldTransform(tr);
+				for (int i = 0; i < mbd->m_numLinks; i++)
 				{
+				}
+				btVector3 linvel = mb->getBaseVel();
+				btVector3 angvel = mb->getBaseOmega();
+				mb->forwardKinematics(scratchQ, scratchM);
+				mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM);
+			}
+		}
+	}
+	else
+	{
+		result = btBulletWorldImporter::convertAllObjects(bulletFile2);
+
+
+		//convert all multibodies
+		for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
+		{
+
+			if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
+			{
+				btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
+				bool isFixedBase = mbd->m_baseMass == 0;
+				bool canSleep = false;
+				btVector3 baseInertia;
+				baseInertia.deSerializeDouble(mbd->m_baseInertia);
+				btMultiBody* mb = new btMultiBody(mbd->m_numLinks, mbd->m_baseMass, baseInertia, isFixedBase, canSleep);
+				mb->setHasSelfCollision(false);
+				btTransform tr; tr.deSerializeDouble(mbd->m_baseWorldTransform);
+				mb->setBaseWorldTransform(tr);
+
+				m_data->m_mbMap.insert(mbd, mb);
+				for (int i = 0; i < mbd->m_numLinks; i++)
+				{
+					btVector3 localInertiaDiagonal;
+					localInertiaDiagonal.deSerializeDouble(mbd->m_links[i].m_linkInertia);
+					btQuaternion parentRotToThis;
+					parentRotToThis.deSerializeDouble(mbd->m_links[i].m_zeroRotParentToThis);
+					btVector3 parentComToThisPivotOffset;
+					parentComToThisPivotOffset.deSerializeDouble(mbd->m_links[i].m_parentComToThisComOffset);
+					btVector3 thisPivotToThisComOffset;
+					thisPivotToThisComOffset.deSerializeDouble(mbd->m_links[i].m_thisPivotToThisComOffset);
+
+					switch (mbd->m_links[i].m_jointType)
+					{
 					case btMultibodyLink::eFixed:
 					{
-						
+
-						
+
 						mb->setupFixed(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
-                                                            parentRotToThis, parentComToThisPivotOffset,thisPivotToThisComOffset);
+							parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset);
 						//search for the collider
 						//mbd->m_links[i].m_linkCollider
 						break;
@@ -84,8 +126,8 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 						btVector3 jointAxis;
 						jointAxis.deSerializeDouble(mbd->m_links[i].m_jointAxisBottom[0]);
 						bool disableParentCollision = true;//todo
-						mb->setupPrismatic(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex,
+						mb->setupPrismatic(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
-							parentRotToThis,jointAxis, parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision);
+							parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
 						mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
 						mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
 						break;
@@ -95,8 +137,8 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 						btVector3 jointAxis;
 						jointAxis.deSerializeDouble(mbd->m_links[i].m_jointAxisTop[0]);
 						bool disableParentCollision = true;//todo
-						mb->setupRevolute(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex,
+						mb->setupRevolute(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
-							parentRotToThis, jointAxis,parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision);
+							parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
 						mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
 						mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
 						break;
@@ -105,8 +147,8 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 					{
 						btAssert(0);
 						bool disableParentCollision = true;//todo
-						mb->setupSpherical(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex,
+						mb->setupSpherical(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
-							parentRotToThis,parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision);
+							parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
 						mb->setJointPosMultiDof(i, mbd->m_links[i].m_jointPos);
 						mb->setJointVelMultiDof(i, mbd->m_links[i].m_jointVel);
 
@@ -121,109 +163,110 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 					{
 						btAssert(0);
 					}
+					}
 				}
 			}
 		}
-	}
-	
-	//forward kinematics, so that the link world transforms are valid, for collision detection
-	btAlignedObjectArray<btQuaternion> scratchQ;
-	btAlignedObjectArray<btVector3> scratchM;
-	for (int i = 0; i<m_data->m_mbMap.size(); i++)
-	{
-		btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
-		if (ptr)
-		{
-			btMultiBody* mb = *ptr;
-			mb->finalizeMultiDof();
-			btVector3 linvel = mb->getBaseVel();
-			btVector3 angvel = mb->getBaseOmega();
-			mb->forwardKinematics(scratchQ, scratchM);
-		}
-	}
 
-	//convert all multibody link colliders
+		//forward kinematics, so that the link world transforms are valid, for collision detection
-	for (int i=0;i<bulletFile2->m_multiBodyLinkColliders.size();i++)
+		for (int i = 0; i < m_data->m_mbMap.size(); i++)
-	{
-		if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
 		{
-			btMultiBodyLinkColliderDoubleData* mblcd = (btMultiBodyLinkColliderDoubleData*) bulletFile2->m_multiBodyLinkColliders[i];
+			btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
-
-			btMultiBody** ptr = m_data->m_mbMap[mblcd->m_multiBody];
 			if (ptr)
 			{
-				btMultiBody* multiBody = *ptr;
+				btMultiBody* mb = *ptr;
-				
+				mb->finalizeMultiDof();
-
+				btVector3 linvel = mb->getBaseVel();
-				btCollisionShape** shapePtr = m_shapeMap.find(mblcd->m_colObjData.m_collisionShape);
+				btVector3 angvel = mb->getBaseOmega();
-				if (shapePtr && *shapePtr)
+				mb->forwardKinematics(scratchQ, scratchM);
-				{
-					btTransform startTransform;
-					mblcd->m_colObjData.m_worldTransform.m_origin.m_floats[3] = 0.f;
-					startTransform.deSerializeDouble(mblcd->m_colObjData.m_worldTransform);
-				
-					btCollisionShape* shape = (btCollisionShape*)*shapePtr;
-					if (shape)
-					{
-						btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(multiBody, mblcd->m_link);
-						col->setCollisionShape( shape );
-						//btCollisionObject* body = createCollisionObject(startTransform,shape,mblcd->m_colObjData.m_name);
-						col->setFriction(btScalar(mblcd->m_colObjData.m_friction));
-						col->setRestitution(btScalar(mblcd->m_colObjData.m_restitution));
-						//m_bodyMap.insert(colObjData,body);
-						if (mblcd->m_link==-1)
-						{
-							col->setWorldTransform(multiBody->getBaseWorldTransform());
-							multiBody->setBaseCollider(col);
-						} else
-						{
-							col->setWorldTransform(multiBody->getLink(mblcd->m_link).m_cachedWorldTransform);
-							multiBody->getLink(mblcd->m_link).m_collider = col;
-						}
-						int mbLinkIndex = mblcd->m_link;
-
-						bool isDynamic = (mbLinkIndex<0 && multiBody->hasFixedBase())? false : true;
-						int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
-						int collisionFilterMask = isDynamic? 	int(btBroadphaseProxy::AllFilter) : 	int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
-
-	#if 0
-						int colGroup=0, colMask=0;
-						int collisionFlags = mblcd->m_colObjData.m_collisionFlags;
-						if (collisionFlags & URDF_HAS_COLLISION_GROUP)
-						{
-							collisionFilterGroup = colGroup;
-						}
-						if (collisionFlags & URDF_HAS_COLLISION_MASK)
-						{
-							collisionFilterMask = colMask;
-						}
-	#endif
-						m_data->m_mbDynamicsWorld->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
-					}
-					
-				} else
-				{
-					printf("error: no shape found\n");
-				}
-#if 0
-				//base and fixed? -> static, otherwise flag as dynamic
-				
-				world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
-#endif
 			}
-
 		}
-	}
 
-	for (int i=0;i<m_data->m_mbMap.size();i++)
+		//convert all multibody link colliders
-	{
+		for (int i = 0; i < bulletFile2->m_multiBodyLinkColliders.size(); i++)
-		btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
-		if (ptr)
 		{
-			btMultiBody* mb = *ptr;
+			if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
-			mb->finalizeMultiDof();
+			{
-			
+				btMultiBodyLinkColliderDoubleData* mblcd = (btMultiBodyLinkColliderDoubleData*)bulletFile2->m_multiBodyLinkColliders[i];
-			m_data->m_mbDynamicsWorld->addMultiBody(mb);
+
+				btMultiBody** ptr = m_data->m_mbMap[mblcd->m_multiBody];
+				if (ptr)
+				{
+					btMultiBody* multiBody = *ptr;
+
+
+					btCollisionShape** shapePtr = m_shapeMap.find(mblcd->m_colObjData.m_collisionShape);
+					if (shapePtr && *shapePtr)
+					{
+						btTransform startTransform;
+						mblcd->m_colObjData.m_worldTransform.m_origin.m_floats[3] = 0.f;
+						startTransform.deSerializeDouble(mblcd->m_colObjData.m_worldTransform);
+
+						btCollisionShape* shape = (btCollisionShape*)*shapePtr;
+						if (shape)
+						{
+							btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(multiBody, mblcd->m_link);
+							col->setCollisionShape(shape);
+							//btCollisionObject* body = createCollisionObject(startTransform,shape,mblcd->m_colObjData.m_name);
+							col->setFriction(btScalar(mblcd->m_colObjData.m_friction));
+							col->setRestitution(btScalar(mblcd->m_colObjData.m_restitution));
+							//m_bodyMap.insert(colObjData,body);
+							if (mblcd->m_link == -1)
+							{
+								col->setWorldTransform(multiBody->getBaseWorldTransform());
+								multiBody->setBaseCollider(col);
+							}
+							else
+							{
+								col->setWorldTransform(multiBody->getLink(mblcd->m_link).m_cachedWorldTransform);
+								multiBody->getLink(mblcd->m_link).m_collider = col;
+							}
+							int mbLinkIndex = mblcd->m_link;
+
+							bool isDynamic = (mbLinkIndex < 0 && multiBody->hasFixedBase()) ? false : true;
+							int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
+							int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+#if 0
+							int colGroup = 0, colMask = 0;
+							int collisionFlags = mblcd->m_colObjData.m_collisionFlags;
+							if (collisionFlags & URDF_HAS_COLLISION_GROUP)
+							{
+								collisionFilterGroup = colGroup;
+							}
+							if (collisionFlags & URDF_HAS_COLLISION_MASK)
+							{
+								collisionFilterMask = colMask;
+							}
+#endif
+							m_data->m_mbDynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
+						}
+
+					}
+					else
+					{
+						printf("error: no shape found\n");
+					}
+#if 0
+					//base and fixed? -> static, otherwise flag as dynamic
+
+					world1->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
+#endif
+				}
+
+			}
+		}
+
+		for (int i = 0; i < m_data->m_mbMap.size(); i++)
+		{
+			btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
+			if (ptr)
+			{
+				btMultiBody* mb = *ptr;
+				mb->finalizeMultiDof();
+
+				m_data->m_mbDynamicsWorld->addMultiBody(mb);
+			}
 		}
 	}
 	return result;

Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp

@@ -20,7 +20,8 @@ subject to the following restrictions:
 #endif
 btWorldImporter::btWorldImporter(btDynamicsWorld* world)
 :m_dynamicsWorld(world),
-m_verboseMode(0)
+m_verboseMode(0),
+m_importerFlags(0)
 {
 
 }

Extras/Serialize/BulletWorldImporter/btWorldImporter.h

@@ -59,6 +59,10 @@ struct btRigidBodyFloatData;
 #define btRigidBodyData btRigidBodyFloatData
 #endif//BT_USE_DOUBLE_PRECISION
 
+enum btWorldImporterFlags
+{
+	eRESTORE_EXISTING_OBJECTS=1,//don't create new objects
+};
 
 class btWorldImporter
 {
@@ -66,6 +70,7 @@ protected:
 	btDynamicsWorld* m_dynamicsWorld;
 	
 	int m_verboseMode;
+	int m_importerFlags;
 
 	btAlignedObjectArray<btCollisionShape*>  m_allocatedCollisionShapes;
 	btAlignedObjectArray<btCollisionObject*> m_allocatedRigidBodies;
@@ -131,6 +136,18 @@ public:
 		return m_verboseMode;
 	}
 
+	void	setImporterFlags(int importerFlags)
+	{
+		m_importerFlags = importerFlags;
+	}
+
+	int getImporterFlags() const
+	{
+		return m_importerFlags;
+	}
+
+	
+
 		// query for data
 	int	getNumCollisionShapes() const;
 	btCollisionShape* getCollisionShapeByIndex(int index);

examples/Importers/ImportURDFDemo/urdfStringSplit.h

@@ -2,14 +2,20 @@
 #ifndef STRING_SPLIT_H
 #define STRING_SPLIT_H
 
+#ifdef __cplusplus
+
 #include <cstring>
 #include <string>
 #include "LinearMath/btAlignedObjectArray.h"
 
-
 void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, const btAlignedObjectArray<std::string>& separators);
 
 void urdfIsAnyOf(const char* seps, btAlignedObjectArray<std::string>& strArray);
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
 
 ///The string split C code is by Lars Wirzenius
 ///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char
@@ -26,5 +32,9 @@ void urdfStrArrayFree(char** array);
 /* Return length of a NULL-delimited array of strings. */
 size_t urdfStrArrayLen(char** array);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif //STRING_SPLIT_H
 

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -107,6 +107,91 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClien
 	return 0;
 }
 
+
+B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadStateCommandInit(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+
+	if (cl->canSubmitCommand())
+	{
+		struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+		b3Assert(command);
+		command->m_type = CMD_RESTORE_STATE;
+		command->m_updateFlags = 0;
+		command->m_loadStateArguments.m_fileName[0] = 0;
+		command->m_loadStateArguments.m_stateId = -1;
+		return (b3SharedMemoryCommandHandle)command;
+	}
+	return 0;
+}
+
+B3_SHARED_API int	b3LoadStateSetStateId(b3SharedMemoryCommandHandle commandHandle, int stateId)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_RESTORE_STATE);
+	if (command->m_type == CMD_RESTORE_STATE)
+	{
+		command->m_loadStateArguments.m_stateId = stateId;
+		command->m_updateFlags |= CMD_LOAD_STATE_HAS_STATEID;
+	}
+	return 0;
+
+}
+
+B3_SHARED_API int	b3LoadStateSetFileName(b3SharedMemoryCommandHandle commandHandle, const char* fileName)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_RESTORE_STATE);
+	if (command->m_type == CMD_RESTORE_STATE)
+	{
+		int len = strlen(fileName);
+		if (len < MAX_URDF_FILENAME_LENGTH)
+		{
+			strcpy(command->m_loadStateArguments.m_fileName, fileName);
+		}
+		else
+		{
+			command->m_loadStateArguments.m_fileName[0] = 0;
+		}
+		command->m_updateFlags |= CMD_LOAD_STATE_HAS_FILENAME;
+	}
+	return 0;
+}
+
+
+B3_SHARED_API b3SharedMemoryCommandHandle b3SaveStateCommandInit(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+
+	if (cl->canSubmitCommand())
+	{
+		struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+		b3Assert(command);
+		command->m_type = CMD_SAVE_STATE;
+		command->m_updateFlags = 0;
+		return (b3SharedMemoryCommandHandle)command;
+	}
+	return 0;
+}
+
+B3_SHARED_API int b3GetStatusGetStateId(b3SharedMemoryStatusHandle statusHandle)
+{
+	int stateId = -1;
+
+	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	b3Assert(status);
+	b3Assert(status->m_type == CMD_SAVE_STATE_COMPLETED);
+	if (status && status->m_type == CMD_SAVE_STATE_COMPLETED)
+	{
+		stateId =  status->m_saveStateResultArgs.m_stateId;
+	}
+	return stateId;
+}
+
 B3_SHARED_API b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
 {
 	PhysicsClient* cl = (PhysicsClient*)physClient;

examples/SharedMemory/PhysicsClientC_API.h

@@ -313,7 +313,16 @@ B3_SHARED_API int	b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle
 
 
 
+
+B3_SHARED_API b3SharedMemoryCommandHandle b3SaveStateCommandInit(b3PhysicsClientHandle physClient);
+B3_SHARED_API int b3GetStatusGetStateId(b3SharedMemoryStatusHandle statusHandle);
+
+B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadStateCommandInit(b3PhysicsClientHandle physClient);
+B3_SHARED_API int	b3LoadStateSetStateId(b3SharedMemoryCommandHandle commandHandle, int stateId);
+B3_SHARED_API int	b3LoadStateSetFileName(b3SharedMemoryCommandHandle commandHandle, const char* fileName);
+
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
+
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
 B3_SHARED_API	void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);

examples/SharedMemory/PhysicsClientSharedMemory.cpp

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

examples/SharedMemory/PhysicsDirect.cpp

@@ -1007,6 +1007,19 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 		{
 			break;
 		}
+	case CMD_RESTORE_STATE_FAILED:
+	{
+		b3Warning("restoreState failed");
+		break;
+	}
+	case CMD_RESTORE_STATE_COMPLETED:
+	{
+		break;
+	}
+	case CMD_BULLET_SAVING_COMPLETED:
+	{
+		break;
+	}
 	default:
 	{
 		//b3Warning("Unknown server status type");

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -8121,6 +8121,62 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share
 	return hasStatus;
 }
 
+bool PhysicsServerCommandProcessor::processRestoreStateCommand(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_RESTORE_STATE_FAILED;
+
+	btMultiBodyWorldImporter* importer = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
+	importer->setImporterFlags(eRESTORE_EXISTING_OBJECTS);
+
+	bool ok = false;
+
+	if (clientCmd.m_loadStateArguments.m_stateId >= 0)
+	{
+		char* memoryBuffer = 0;
+		int len = 0;
+		ok = importer->loadFileFromMemory(memoryBuffer, len);
+	}
+	else
+	{
+		const char* prefix[] = { "", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/" };
+		int numPrefixes = sizeof(prefix) / sizeof(const char*);
+		char relativeFileName[1024];
+		FILE* f = 0;
+		bool found = false;
+
+		for (int i = 0; !f && i<numPrefixes; i++)
+		{
+			sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName);
+			f = fopen(relativeFileName, "rb");
+			if (f)
+			{
+				found = true;
+				break;
+			}
+		}
+		if (f)
+		{
+			fclose(f);
+		}
+
+		if (found)
+		{
+			ok = importer->loadFile(relativeFileName);
+		}
+	}
+
+	if (ok)
+	{
+		serverCmd.m_type = CMD_RESTORE_STATE_COMPLETED;
+	}
+	
+
+	return hasStatus;
+}
+
 bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
 {
 	BT_PROFILE("CMD_LOAD_BULLET");
@@ -8541,6 +8597,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 			hasStatus = processLoadTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
 			break;
 		}
+	case CMD_RESTORE_STATE:
+	{
+		hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
+		break;
+	}
+
 	case CMD_LOAD_BULLET:
 		{
 			hasStatus = processLoadBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -80,6 +80,7 @@ protected:
 	bool processLoadBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	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 loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling);

examples/SharedMemory/SharedMemoryCommands.h

@@ -69,6 +69,13 @@ struct SdfArgs
 struct FileArgs
 {
 	char m_fileName[MAX_URDF_FILENAME_LENGTH];
+	int m_stateId;
+};
+
+enum EnumLoadStateArgsUpdateFlags
+{
+	CMD_LOAD_STATE_HAS_STATEID=1,
+	CMD_LOAD_STATE_HAS_FILENAME=2,
 };
 
 enum EnumUrdfArgsUpdateFlags
@@ -922,6 +929,12 @@ struct b3ChangeTextureArgs
 	int m_height;
 };
 
+struct b3StateSerializationArguments
+{
+	char m_fileName[MAX_URDF_FILENAME_LENGTH];
+	int m_stateId;
+};
+
 struct SharedMemoryCommand
 {
 	int m_type;
@@ -976,6 +989,7 @@ struct SharedMemoryCommand
 		struct b3ChangeTextureArgs m_changeTextureArgs;
 		struct b3SearchPathfArgs m_searchPathArgs;
 		struct b3CustomCommand m_customCommandArgs;
+		struct b3StateSerializationArguments m_loadStateArguments;
     };
 };
 
@@ -1050,6 +1064,7 @@ struct SharedMemoryStatus
 		struct b3LoadTextureResultArgs m_loadTextureResultArguments;
 		struct b3CustomCommandResultArgs m_customCommandResultArgs;
 		struct b3PhysicsSimulationParameters m_simulationParameterResultArgs;
+		struct b3StateSerializationArguments m_saveStateResultArgs;
 	};
 };
 

examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp

@@ -112,12 +112,13 @@ public:
 	{
 		int newargc = argc+2;
 		m_newargv = (char**)malloc(sizeof(void*)*newargc);
-		for (int i=0;i<argc;i++)
+		char* t0 = (char*)"--unused";
-			m_newargv[i] = argv[i];
+		m_newargv[0] = t0;
 
-		char* t0 = (char*)"--logtostderr";
+		for (int i=0;i<argc;i++)
+			m_newargv[i+1] = argv[i];
+		
 		char* t1 = (char*)"--start_demo_name=Physics Server";
-		m_newargv[argc] = t0;
 		m_newargv[argc+1] = t1;
 		m_data = btCreateInProcessExampleBrowser(newargc,m_newargv, useInProcessMemory);
 		SharedMemoryInterface* shMem = btGetSharedMemoryInterface(m_data);

examples/SharedMemory/SharedMemoryPublic.h

@@ -77,6 +77,8 @@ enum EnumSharedMemoryClientCommand
 	CMD_SET_ADDITIONAL_SEARCH_PATH,
 	CMD_CUSTOM_COMMAND,
 	CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS,
+	CMD_SAVE_STATE,
+	CMD_RESTORE_STATE,
     //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,
     
@@ -178,7 +180,11 @@ enum EnumSharedMemoryServerStatus
 		CMD_CUSTOM_COMMAND_COMPLETED,
 		CMD_CUSTOM_COMMAND_FAILED,
 		CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED,
-        //don't go beyond 'CMD_MAX_SERVER_COMMANDS!
+		CMD_SAVE_STATE_FAILED,
+		CMD_SAVE_STATE_COMPLETED,
+		CMD_RESTORE_STATE_FAILED,
+		CMD_RESTORE_STATE_COMPLETED,
+		//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
         CMD_MAX_SERVER_COMMANDS
 };
 

examples/pybullet/examples/saveRestoreState.py

@@ -0,0 +1,29 @@
+import pybullet as p
+import math, time
+
+p.connect(p.GUI, options="--width=1024 --height=768")
+
+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)
+
+
+for i in range (500):
+	p.stepSimulation()
+
+for i in range (10):
+	print("pos[",i,"]=",p.getBasePositionAndOrientation(i))
+
+#saveState = 0
+
+#if saveState:
+#	for i in range (500):
+#		p.stepSimulation()
+#	p.saveBullet("state.bullet")
+#else:
+#	p.restoreState(fileName="state.bullet")
+
+
+while (p.getConnectionInfo()["isConnected"]):
+	time.sleep(1)

examples/pybullet/pybullet.c

@@ -15,6 +15,8 @@
 #include <Python.h>
 #endif
 
+#include "../Importers/ImportURDFDemo/urdfStringSplit.h"
+
 #ifdef B3_DUMP_PYTHON_VERSION
 #define B3_VALUE_TO_STRING(x) #x
 #define B3_VALUE(x) B3_VALUE_TO_STRING(x)
@@ -298,7 +300,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
 	int freeIndex = -1;
 	int method = eCONNECT_GUI;
 	int i;
-	char* options="";
+	char* options=0;
 
 	b3PhysicsClientHandle sm = 0;
 
@@ -352,12 +354,22 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
 			}
 		}
 
+		int argc = 0;
+		char** argv=0;
+		if (options)
+		{
+			argv = urdfStrSplit(options, " ");
+			argc = urdfStrArrayLen(argv);
+			for (int i = 0; i < argc; i++)
+			{
+				printf("argv[%d]=%s\n", i, argv[i]);
+			}
+		}
 		switch (method)
 		{
 			case eCONNECT_GUI:
 			{
-				int argc = 2;
+				
-				char* argv[2] = {"unused",options};
 
 #ifdef __APPLE__
 				sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
@@ -368,9 +380,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
 			}
 			case eCONNECT_GUI_SERVER:
 			{
-				int argc = 2;
+				
-				char* argv[2] = {"unused",options};
-
 #ifdef __APPLE__
 				sm = b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(argc, argv);
 #else
@@ -419,6 +429,11 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
 				return NULL;
 			}
 		};
+
+		if (options)
+		{
+			urdfStrArrayFree(argv);
+		}
 	}
 
 	if (sm && b3CanSubmitCommand(sm))
@@ -588,7 +603,7 @@ static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args, PyObject* k
 	b3PhysicsClientHandle sm = 0;
 
 	int physicsClientId = 0;
-	static char* kwlist[] = {"bulletFileName", "physicsClientId", NULL};
+	static char* kwlist[] = {"bulletFileName",  "physicsClientId", NULL};
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &bulletFileName, &physicsClientId))
 	{
 		return NULL;
@@ -662,6 +677,93 @@ static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args, PyObject* k
 	return Py_None;
 }
 
+
+static PyObject* pybullet_restoreState(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	const char* fileName = "";
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	int stateId = -1;
+	b3SharedMemoryCommandHandle command;
+
+	PyObject* pylist = 0;
+	b3PhysicsClientHandle sm = 0;
+
+	int physicsClientId = 0;
+	static char* kwlist[] = { "stateId", "fileName", "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|isi", kwlist, &stateId, &fileName, &physicsClientId))
+	{
+		return NULL;
+	}
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	command = b3LoadStateCommandInit(sm);
+	if (stateId >= 0)
+	{
+		b3LoadStateSetStateId(command, stateId);
+	}
+	if (fileName)
+	{
+		b3LoadStateSetFileName(command, fileName);
+	}
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType != CMD_RESTORE_STATE_COMPLETED)
+	{
+		PyErr_SetString(SpamError, "Couldn't restore state.");
+		return NULL;
+	}
+
+	Py_INCREF(Py_None);
+	return Py_None;
+
+}
+
+static PyObject* pybullet_saveState(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	b3SharedMemoryCommandHandle command;
+	b3PhysicsClientHandle sm = 0;
+	int stateId = -1;
+
+	int physicsClientId = 0;
+	static char* kwlist[] = { "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId))
+	{
+		return NULL;
+	}
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	command = b3SaveStateCommandInit(sm);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusType(statusHandle);
+
+	if (statusType != CMD_SAVE_STATE_COMPLETED)
+	{
+		PyErr_SetString(SpamError, "Couldn't save state");
+		return NULL;
+	}
+
+	{
+		stateId = b3GetStatusGetStateId(statusHandle);
+		PyObject* pylist = 0;
+		pylist = PyTuple_New(1);
+		PyTuple_SetItem(pylist, 0, PyInt_FromLong(stateId));
+		return pylist;
+	}
+}
+
 static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	const char* mjcfFileName = "";
@@ -7761,11 +7863,17 @@ static PyMethodDef SpamMethods[] = {
 	 "Load multibodies from an SDF file."},
 
 	{"loadBullet", (PyCFunction)pybullet_loadBullet, METH_VARARGS | METH_KEYWORDS,
-	 "Restore the full state of the world from a .bullet file."},
+	 "Load a world from a .bullet file."},
 
 	{"saveBullet", (PyCFunction)pybullet_saveBullet, METH_VARARGS | METH_KEYWORDS,
 	 "Save the full state of the world to a .bullet file."},
 
+	 { "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." },
+
 	{"loadMJCF", (PyCFunction)pybullet_loadMJCF, METH_VARARGS | METH_KEYWORDS,
 	 "Load multibodies from an MJCF file."},