Add rudimentary 'saveWorld' command in shared memory API and pybullet, see examples/pybullet/saveWorld.py

Use trilinear filtering instead of bilinear

examples/OpenGLWindow/GLInstancingRenderer.cpp

@@ -1465,7 +1465,9 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode)
 #endif//OLD_SHADOWMAP_INIT
 
 			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
-			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
+
+
 
 			float l_ClampColor[] = {1.0, 1.0, 1.0, 1.0};
 			glTexParameterfv(GL_TEXTURE_2D, GL_TEXTURE_BORDER_COLOR, l_ClampColor);

examples/OpenGLWindow/GLPrimitiveRenderer.cpp

@@ -223,7 +223,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
 	bool useFiltering = false;
 	if (useFiltering)
 	{
-		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
 	} else
 	{

examples/OpenGLWindow/SimpleOpenGL3App.cpp

@@ -107,7 +107,7 @@ static GLuint BindFont(const CTexFont *_Font)
     glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
     glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
     glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER,GL_NEAREST);
-    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,GL_NEAREST);
+    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,GL_LINEAR_MIPMAP_LINEAR);
     glBindTexture(GL_TEXTURE_2D, 0);
 
     return TexID;
@@ -830,7 +830,7 @@ void SimpleOpenGL3App::dumpNextFrameToPng(const char* filename)
                          , 0,GL_RGBA, GL_FLOAT, 0);
 
             glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
             //glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
             //glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
 

examples/OpenGLWindow/opengl_fontstashcallbacks.cpp

@@ -126,7 +126,7 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly
 			texture->m_texels = (unsigned char*)malloc(textureWidth*textureHeight);
 			memset(texture->m_texels,0,textureWidth*textureHeight);
 			glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, textureWidth, textureHeight, 0, GL_RED, GL_UNSIGNED_BYTE, texture->m_texels);
-			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
 			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
 	    assert(glGetError()==GL_NO_ERROR);
 
@@ -187,7 +187,7 @@ void InternalOpenGL2RenderCallbacks::render(sth_texture* texture)
 	bool useFiltering = false;
 	if (useFiltering)
 	{
-		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
 	} else
 	{

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -30,7 +30,27 @@ b3SharedMemoryCommandHandle	b3LoadSdfCommandInit(b3PhysicsClientHandle physClien
 	return (b3SharedMemoryCommandHandle) command;
 }
 
+b3SharedMemoryCommandHandle	b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
     
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+	command->m_type = CMD_SAVE_WORLD;
+	int len = strlen(sdfFileName);
+	if (len<MAX_SDF_FILENAME_LENGTH)
+	{
+		strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
+	} else
+	{
+		command->m_sdfArguments.m_sdfFileName[0] = 0;
+	}
+	command->m_updateFlags = SDF_ARGS_FILE_NAME;
+	
+	return (b3SharedMemoryCommandHandle) command;
+}
 
 b3SharedMemoryCommandHandle	b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
 {

examples/SharedMemory/PhysicsClientC_API.h

@@ -146,6 +146,9 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
 b3SharedMemoryCommandHandle	b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
 int	b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
 
+b3SharedMemoryCommandHandle	b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
+
+
 ///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
 b3SharedMemoryCommandHandle  b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
 

examples/SharedMemory/PhysicsClientExample.cpp

@@ -451,6 +451,12 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
                 b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
                 break;
             }
+		case CMD_SAVE_WORLD:
+		{
+                b3SharedMemoryCommandHandle commandHandle = b3SaveWorldCommandInit(m_physicsClientHandle, "saveWorld.py");
+                b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
+			break;
+		}
         default:
         {
             b3Error("Unknown buttonId");
@@ -525,6 +531,7 @@ void	PhysicsClientExample::createButtons()
 
         createButton("Load URDF",CMD_LOAD_URDF,  isTrigger);
         createButton("Load SDF",CMD_LOAD_SDF,  isTrigger);
+		createButton("Save World",CMD_SAVE_WORLD,  isTrigger);
         createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger);
         createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION,  isTrigger);
         createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM,  isTrigger);

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -615,7 +615,14 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                     b3Warning("Contact Point Information Request failed");
                     break;
                 }
+			case CMD_SAVE_WORLD_COMPLETED:
+  				break;
 
+			case CMD_SAVE_WORLD_FAILED:
+				{
+					b3Warning("Saving world  failed");
+					break;
+				}
             default: {
                 b3Error("Unknown server status\n");
                 btAssert(0);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -301,6 +301,12 @@ struct CommandLogPlayback
 	}
 };
 
+struct SaveWorldObjectData
+{
+	b3AlignedObjectArray<int> m_bodyUniqueIds;
+	std::string	m_fileName;
+};
+
 struct PhysicsServerCommandProcessorInternalData
 {
 	///handle management
@@ -410,7 +416,7 @@ struct PhysicsServerCommandProcessorInternalData
 	btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
 	btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
 	btHashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
-
+	b3AlignedObjectArray<SaveWorldObjectData> m_saveWorldBodyData;
 
 
 	btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
@@ -811,6 +817,9 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
         {
             b3Printf("loaded %s OK!", fileName);
         }
+		SaveWorldObjectData sd;
+		sd.m_fileName = fileName;
+
 
         for (int m =0; m<u2b.getNumModels();m++)
         {
@@ -824,6 +833,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
 
             InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
 			
+			sd.m_bodyUniqueIds.push_back(bodyUniqueId);
 
             u2b.setBodyUniqueId(bodyUniqueId);
             {
@@ -854,6 +864,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
 			if (mb)
 				mb->setUserIndex2(bodyUniqueId);
 
+			
             if (mb)
             {
                 bodyHandle->m_multiBody = mb;
@@ -898,6 +909,9 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
 			}
 
         }
+		
+		m_data->m_saveWorldBodyData.push_back(sd);
+
     }
     return loadOk;
 }
@@ -930,6 +944,14 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
 		if (bodyUniqueIdPtr)
 			*bodyUniqueIdPtr= bodyUniqueId;
 
+		//quick prototype of 'save world' for crude world editing
+		{
+			SaveWorldObjectData sd;
+			sd.m_fileName = fileName;
+			sd.m_bodyUniqueIds.push_back(bodyUniqueId);
+			m_data->m_saveWorldBodyData.push_back(sd);
+		}
+
         u2b.setBodyUniqueId(bodyUniqueId);
 		InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
 		
@@ -1336,6 +1358,154 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                         hasStatus = true;
                         break;
                     }
+				case CMD_SAVE_WORLD:
+				{
+					///this is a very rudimentary way to save the state of the world, for scene authoring
+					///many todo's, for example save the state of motor controllers etc.
+
+					if (clientCmd.m_sdfArguments.m_sdfFileName)
+					{
+						//saveWorld(clientCmd.m_sdfArguments.m_sdfFileName);
+
+						FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName,"w");
+						if (f)
+						{
+							char line[1024];
+							{
+								sprintf(line,"import pybullet as p\n");
+								int len = strlen(line);
+								fwrite(line,len,1,f);
+							}
+							{
+								sprintf(line,"p.connect(p.SHARED_MEMORY)\n");
+								int len = strlen(line);
+								fwrite(line,len,1,f);
+							}
+							//for each objects ...
+							for (int i=0;i<m_data->m_saveWorldBodyData.size();i++)
+							{
+								SaveWorldObjectData& sd = m_data->m_saveWorldBodyData[i];
+
+								for (int i=0;i<sd.m_bodyUniqueIds.size();i++)
+								{
+									{
+										int bodyUniqueId = sd.m_bodyUniqueIds[i];
+										InteralBodyData* body = m_data->getHandle(bodyUniqueId);
+										if (body)
+										{
+											 if (body->m_multiBody)
+											 {
+												btMultiBody* mb = body->m_multiBody;
+												
+												btTransform comTr = mb->getBaseWorldTransform();
+												btTransform tr = comTr * body->m_rootLocalInertialFrame.inverse();
+												
+												if (strstr(sd.m_fileName.c_str(),".urdf"))
+												{
+													sprintf(line,"objects = [p.loadURDF(\"%s\", %f,%f,%f,%f,%f,%f,%f)]\n",sd.m_fileName.c_str(),
+														tr.getOrigin()[0],tr.getOrigin()[1],tr.getOrigin()[2],
+														tr.getRotation()[0],tr.getRotation()[1],tr.getRotation()[2],tr.getRotation()[3]);
+													int len = strlen(line);
+													fwrite(line,len,1,f);
+												}
+
+												if (strstr(sd.m_fileName.c_str(),".sdf") && i==0)
+												{
+													sprintf(line,"objects = p.loadSDF(\"%s\")\n",sd.m_fileName.c_str());
+													int len = strlen(line);
+													fwrite(line,len,1,f);
+												}
+
+												if (strstr(sd.m_fileName.c_str(),".sdf") || ((strstr(sd.m_fileName.c_str(),".urdf")) && mb->getNumLinks()) )
+												{
+													sprintf(line,"ob = objects[%d]\n",i);
+													int len = strlen(line);
+													fwrite(line,len,1,f);
+												}
+
+												if (strstr(sd.m_fileName.c_str(),".sdf"))
+												{
+													sprintf(line,"p.resetBasePositionAndOrientation(ob,[%f,%f,%f],[%f,%f,%f,%f])\n",
+														comTr.getOrigin()[0],comTr.getOrigin()[1],comTr.getOrigin()[2],
+														comTr.getRotation()[0],comTr.getRotation()[1],comTr.getRotation()[2],comTr.getRotation()[3]);
+													int len = strlen(line);
+													fwrite(line,len,1,f);
+												}
+
+												if (mb->getNumLinks())
+												{
+													{
+														sprintf(line,"jointPositions=[");
+														int len = strlen(line);
+														fwrite(line,len,1,f);
+													}
+
+													for (int i=0;i<mb->getNumLinks();i++)
+													{
+														btScalar jointPos = mb->getJointPosMultiDof(i)[0];
+														if (i<mb->getNumLinks()-1)
+														{
+															sprintf(line," %f,",jointPos);
+															int len = strlen(line);
+															fwrite(line,len,1,f);
+														} else
+														{
+															sprintf(line," %f ",jointPos);
+															int len = strlen(line);
+															fwrite(line,len,1,f);
+														}
+													}
+
+													{
+														sprintf(line,"]\nfor jointIndex in range (p.getNumJoints(ob)):\n\tp.resetJointState(ob,jointIndex,jointPositions[jointIndex])\n\n");
+														int len = strlen(line);
+														fwrite(line,len,1,f);
+													}
+												}
+											 } else
+											 {
+												 //todo: btRigidBody/btSoftBody etc case
+											 }
+										}
+									}
+									
+								}
+								
+								//for URDF, load at origin, then reposition...
+								
+
+								struct SaveWorldObjectData
+								{
+									b3AlignedObjectArray<int> m_bodyUniqueIds;
+									std::string	m_fileName;
+								};
+							}
+
+							{
+								btVector3 grav=this->m_data->m_dynamicsWorld->getGravity();
+								sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]);
+								int len = strlen(line);
+								fwrite(line,len,1,f);
+							}
+														
+
+							{
+									sprintf(line,"p.stepSimulation()\np.disconnect()\n");
+									int len = strlen(line);
+									fwrite(line,len,1,f);
+							}
+							fclose(f);
+						}
+
+
+						serverStatusOut.m_type = CMD_SAVE_WORLD_COMPLETED;
+						hasStatus = true;
+						break;
+					}
+					serverStatusOut.m_type = CMD_SAVE_WORLD_FAILED;
+					hasStatus = true;
+					break;
+				}
                 case CMD_LOAD_SDF:
                     {
                         const SdfArgs& sdfArgs = clientCmd.m_sdfArguments;

examples/SharedMemory/SharedMemoryPublic.h

@@ -32,6 +32,7 @@ enum EnumSharedMemoryClientCommand
     CMD_CALCULATE_JACOBIAN,
     CMD_CREATE_JOINT,
     CMD_REQUEST_CONTACT_POINT_INFORMATION,
+	CMD_SAVE_WORLD,
     //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,
     
@@ -74,6 +75,10 @@ enum EnumSharedMemoryServerStatus
 		CMD_CONTACT_POINT_INFORMATION_FAILED,
 		CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
 		CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
+		CMD_SAVE_WORLD_COMPLETED,
+		CMD_SAVE_WORLD_FAILED,
+
+		//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
         CMD_MAX_SERVER_COMMANDS
 };
 

examples/pybullet/pybullet.c

@@ -112,6 +112,48 @@ static PyObject* pybullet_disconnectPhysicsServer(PyObject* self,
   return Py_None;
 }
 
+static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args) {
+	int size = PySequence_Size(args);
+	const char* worldFileName = "";
+
+	if (0 == sm) {
+    PyErr_SetString(SpamError, "Not connected to physics server.");
+    return NULL;
+  }
+
+   if (size == 1) {
+    if (!PyArg_ParseTuple(args, "s", &worldFileName))
+	{
+		return NULL;
+	}
+	else
+	{
+		b3SharedMemoryCommandHandle	command;
+		b3SharedMemoryStatusHandle statusHandle;
+		int statusType;
+
+		command = b3SaveWorldCommandInit(sm, worldFileName);
+		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+		statusType = b3GetStatusType(statusHandle);
+		if (statusType != CMD_SAVE_WORLD_COMPLETED) {
+			PyErr_SetString(SpamError, "saveWorld command execution failed.");
+			return NULL;
+		}
+		Py_INCREF(Py_None);
+		return Py_None;
+	}
+  }
+
+
+     
+	PyErr_SetString(SpamError, "Cannot execute saveWorld command.");
+	return NULL;
+    
+    return NULL;
+}
+
+
+
 // Load a URDF file indicating the links and joints of an object
 // function can be called without arguments and will default
 // to position (0,0,1) with orientation(0,0,0,1)
@@ -2082,6 +2124,10 @@ static PyMethodDef SpamMethods[] = {
     {"loadSDF", pybullet_loadSDF, METH_VARARGS,
      "Load multibodies from an SDF file."},
 
+	{"saveWorld", pybullet_saveWorld, METH_VARARGS,
+     "Save an approximate Python file to reproduce the current state of the world: saveWorld"
+	"(filename). (very preliminary and approximately)"},
+
 	{"getNumBodies", pybullet_getNumBodies, METH_VARARGS,
      "Get the number of bodies in the simulation."},
 

examples/pybullet/saveWorld.py

@@ -0,0 +1,8 @@
+import pybullet as p
+import time
+
+p.connect(p.SHARED_MEMORY)
+timestr = time.strftime("%Y%m%d-%H%M%S")
+filename = "saveWorld" + timestr + ".py"
+p.saveWorld(filename)
+p.disconnect()