From 4c6df650d49f69532cef97134da0f91079e6c386 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 31 Dec 2017 11:28:22 -0800 Subject: [PATCH 1/2] skip serialization of empty contact manifolds. --- src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index acc42cd1e..3de8d6995 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/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) - // continue; + if (manifold->getNumContacts() == 0) + continue; btChunk* chunk = serializer->allocate(manifold->calculateSerializeBufferSize(), 1); const char* structType = manifold->serialize(manifold, chunk->m_oldPtr, serializer); From 35b44f8a85838e7a9afae2e950cecf1881b17130 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 31 Dec 2017 15:37:16 -0800 Subject: [PATCH 2/2] implement pybullet.saveState command, for in-memory storage of state. bump up pybullet API version (SHARED_MEMORY_MAGIC_NUMBER) to 201801010 --- .../btMultiBodyWorldImporter.cpp | 40 ++++++++++++- .../PhysicsClientSharedMemory.cpp | 4 ++ examples/SharedMemory/PhysicsDirect.cpp | 4 ++ .../PhysicsServerCommandProcessor.cpp | 58 +++++++++++++++++-- .../PhysicsServerCommandProcessor.h | 2 +- examples/SharedMemory/SharedMemoryPublic.h | 3 +- .../pybullet/examples/saveRestoreState.py | 22 +++++-- examples/pybullet/pybullet.c | 9 +-- 8 files changed, 123 insertions(+), 19 deletions(-) diff --git a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp index ddfd70d0a..e07d44489 100644 --- a/Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp +++ b/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++) { diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 9ca17b8e8..9a5dea478 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/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"); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 9b6a2b4f3..5b8822737 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/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"); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b2f735154..0e36f4f01 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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 m_savedStates; + btAlignedObjectArray m_keyboardEvents; btAlignedObjectArray 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; - int len = 0; - ok = importer->loadFileFromMemory(memoryBuffer, len); + if (clientCmd.m_loadStateArguments.m_stateId < m_data->m_savedStates.size()) + { + bParse::btBulletFile* bulletFile = m_data->m_savedStates[clientCmd.m_loadStateArguments.m_stateId].m_bulletFile; + ok = importer->convertAllObjects(bulletFile); + } } else { @@ -8227,8 +8264,7 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar if (ok) { 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(); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index fb26b52fb..4961af147 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/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); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index b152a70c3..8bff46b3d 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/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 diff --git a/examples/pybullet/examples/saveRestoreState.py b/examples/pybullet/examples/saveRestoreState.py index 3f0dea84b..b0f53ac28 100644 --- a/examples/pybullet/examples/saveRestoreState.py +++ b/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"]): - time.sleep(1) +p.stopStateLogging(logId) + +#while (p.getConnectionInfo()["isConnected"]): +# time.sleep(1) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index ee031bf12..232913605 100644 --- a/examples/pybullet/pybullet.c +++ b/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; - pylist = PyTuple_New(1); - PyTuple_SetItem(pylist, 0, PyInt_FromLong(stateId)); - return pylist; - } + stateId = b3GetStatusGetStateId(statusHandle); + return PyInt_FromLong(stateId); } static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* keywds)