Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1492 from erwincoumans/master

Browse Source 
implement pybullet.saveState command, for in-memory storage of state.
This commit is contained in:
erwincoumans
2017-12-31 19:19:35 -08:00
committed by GitHub
parent 515aa9ee9b 35b44f8a85
commit a1492f50fc
9 changed files with 125 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp
View File

@@ -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++)
{

4
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

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

4
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -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");

58
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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;
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();

2
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -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);

3
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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

22
examples/pybullet/examples/saveRestoreState.py
View File

@@ -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)

9
examples/pybullet/pybullet.c
View File

@@ -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)

4
src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
View File

@@ -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);