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

further work on saveRestoreState.py

Browse Source
This commit is contained in:
Erwin Coumans
2017-12-30 21:57:42 -08:00
parent 9c55b61315
commit 20e00d11d8
3 changed files with 71 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -80,9 +80,42 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
for (int i = 0; i < mbd->m_numLinks; i++)
{
switch (mbd->m_links[i].m_jointType)
{
case btMultibodyLink::eFixed:
{
break;
}
case btMultibodyLink::ePrismatic:
{
mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
break;
}
case btMultibodyLink::eRevolute:
{
mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
break;
}
case btMultibodyLink::eSpherical:
{
btScalar jointPos[3] = { mbd->m_links[i].m_jointPos[0], mbd->m_links[i].m_jointPos[1], mbd->m_links[i].m_jointPos[2] };
btScalar jointVel[3] = { mbd->m_links[i].m_jointVel[0], mbd->m_links[i].m_jointVel[1], mbd->m_links[i].m_jointVel[2] };
mb->setJointPosMultiDof(i, jointPos);
mb->setJointVelMultiDof(i, jointVel);
break;
}
case btMultibodyLink::ePlanar:
{
break;
}
default:
{
}
}
}
btVector3 linvel = mb->getBaseVel();
btVector3 angvel = mb->getBaseOmega();
mb->forwardKinematics(scratchQ, scratchM);
mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM);
}
@@ -145,7 +178,8 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
}
else
{
printf("Issue: cannot find maching contact manifold (%d, %d), may cause issues in determinism.\n", uid0, uid1);
existingManifold->setNumContacts(0);
//printf("Issue: cannot find maching contact manifold (%d, %d), may cause issues in determinism.\n", uid0, uid1);
}
manifoldArray.clear();

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

@@ -3,16 +3,20 @@ import math, time
import difflib,sys
numSteps = 500
numSteps2 = 3
numSteps2 = 30
p.connect(p.GUI, options="--width=1024 --height=768")
numObjects = 10
numObjects = 50
verbose = 0
def setupWorld():
p.resetSimulation()
p.loadURDF("planeMesh.urdf")
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10])
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,0,(i+1)*0.5)
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)
@@ -80,15 +84,35 @@ file = open("restoreFile.txt","w")
dumpStateToFile(file)
file.close()
p.restoreState(fileName="state.bullet")
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points restored=")
for q in p.getContactPoints():
print(q)
for i in range (numSteps2):
p.stepSimulation()
file = open("restoreFile2.txt","w")
dumpStateToFile(file)
file.close()
file1 = open("saveFile.txt","r")
file2 = open("restoreFile.txt","r")
compareFiles(file1,file2)
file1.close()
file2.close()
file1 = open("saveFile.txt","r")
file2 = open("restoreFile2.txt","r")
compareFiles(file1,file2)
file1.close()
file2.close()
while (p.getConnectionInfo()["isConnected"]):
time.sleep(1)
file1.close()
file2.close()

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