diff options
author | erwincoumans <erwin.coumans@gmail.com> | 2016-10-13 00:00:22 -0700 |
---|---|---|
committer | GitHub <noreply@github.com> | 2016-10-13 00:00:22 -0700 |
commit | 4ebc3271511161ee0e504ca7e0e66e01454a66dd (patch) | |
tree | 993db1ba41f057f06ad11f80e0ae67ee38101a89 | |
parent | 729ae8d3d64a540a77c56539312b5b70e7e330db (diff) | |
parent | 1a62f21143a7e910278f08724f4d2b82f4ede825 (diff) | |
download | bullet3-2.85.tar.gz |
Merge pull request #834 from erwincoumans/master2.85
Add rudimentary 'saveWorld' command in shared memory API and pybullet…
-rw-r--r-- | examples/OpenGLWindow/GLInstancingRenderer.cpp | 4 | ||||
-rw-r--r-- | examples/OpenGLWindow/GLPrimitiveRenderer.cpp | 2 | ||||
-rw-r--r-- | examples/OpenGLWindow/SimpleOpenGL3App.cpp | 4 | ||||
-rw-r--r-- | examples/OpenGLWindow/opengl_fontstashcallbacks.cpp | 4 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsClientC_API.cpp | 22 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsClientC_API.h | 3 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsClientExample.cpp | 7 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsClientSharedMemory.cpp | 14 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 172 | ||||
-rw-r--r-- | examples/SharedMemory/SharedMemoryPublic.h | 5 | ||||
-rw-r--r-- | examples/pybullet/pybullet.c | 46 | ||||
-rw-r--r-- | examples/pybullet/saveWorld.py | 8 |
12 files changed, 281 insertions, 10 deletions
diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index 1a8ee2063..8ca3f6dc8 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/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); diff --git a/examples/OpenGLWindow/GLPrimitiveRenderer.cpp b/examples/OpenGLWindow/GLPrimitiveRenderer.cpp index 49cdbaed1..314066851 100644 --- a/examples/OpenGLWindow/GLPrimitiveRenderer.cpp +++ b/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 { diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index f7980109a..f94465655 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/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); diff --git a/examples/OpenGLWindow/opengl_fontstashcallbacks.cpp b/examples/OpenGLWindow/opengl_fontstashcallbacks.cpp index be104f2b6..3b9f9fe40 100644 --- a/examples/OpenGLWindow/opengl_fontstashcallbacks.cpp +++ b/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 { diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 0e75054cc..7350f7677 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b44b9a452..b97a02166 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/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); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 8ba6cfca9..9abacbfc7 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/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); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index ede214bc6..c9fad2ab1 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -615,8 +615,17 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Contact Point Information Request failed"); break; } - case CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED: - { + + case CMD_SAVE_WORLD_COMPLETED: + break; + + case CMD_SAVE_WORLD_FAILED: + { + b3Warning("Saving world failed"); + break; + } + case CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED: + { break; } case CMD_CALCULATE_INVERSE_KINEMATICS_FAILED: @@ -624,6 +633,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Calculate Inverse Kinematics Request failed"); break; } + default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 330d38bc7..ee2ff385a 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 1490fae68..bd6e7b459 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/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 }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 6e010141d..f3ef08ed4 100644 --- a/examples/pybullet/pybullet.c +++ b/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."}, diff --git a/examples/pybullet/saveWorld.py b/examples/pybullet/saveWorld.py new file mode 100644 index 000000000..f6f6278ed --- /dev/null +++ b/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() |