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

Merge pull request #1491 from erwincoumans/master

Browse Source 
some progress in saving and restoring the state
This commit is contained in:
erwincoumans
2017-12-31 13:39:27 -08:00
committed by GitHub
parent e3a85493e8 f104765c47
commit 515aa9ee9b
26 changed files with 2215 additions and 1222 deletions
Download Patch File Download Diff File Expand all files Collapse all files

102
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2172,9 +2172,12 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_pairCache = new btHashedOverlappingPairCache();
m_data->m_pairCache->setOverlapFilterCallback(m_data->m_broadphaseCollisionFilterCallback);
//int maxProxies = 32768;
//m_data->m_broadphase = new btSimpleBroadphase(maxProxies, m_data->m_pairCache);
m_data->m_broadphase = new btDbvtBroadphase(m_data->m_pairCache);
m_data->m_solver = new btMultiBodyConstraintSolver;
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
@@ -2183,7 +2186,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
#endif
//Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
//Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(32768);
m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
@@ -3773,29 +3776,78 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
char pathPrefix[1024];
pathPrefix[0] = 0;
if (visualShape.m_geometry.m_type == URDF_GEOM_MESH)
{
std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshFileName;
const std::string& error_message_prefix="";
std::string out_found_filename;
int out_type;
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type);
visualShape.m_geometry.m_meshFileType = out_type;
visualShape.m_geometry.m_meshFileName=fileName;
const b3CreateUserShapeData& visShape = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex];
visualShape.m_geometry.m_meshScale.setValue(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[0],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[1],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[2]);
}
visualShape.m_name = "bla";
switch (visualShape.m_geometry.m_type)
{
case URDF_GEOM_CYLINDER:
{
visualShape.m_geometry.m_capsuleHeight = visShape.m_capsuleHeight;
visualShape.m_geometry.m_capsuleRadius = visShape.m_capsuleRadius;
break;
}
case URDF_GEOM_BOX:
{
visualShape.m_geometry.m_boxSize.setValue(2.*visShape.m_boxHalfExtents[0],
2.*visShape.m_boxHalfExtents[1],
2.*visShape.m_boxHalfExtents[2]);
break;
}
case URDF_GEOM_SPHERE:
{
visualShape.m_geometry.m_sphereRadius = visShape.m_sphereRadius;
break;
}
case URDF_GEOM_CAPSULE:
{
visualShape.m_geometry.m_hasFromTo = visShape.m_hasFromTo;
if (visualShape.m_geometry.m_hasFromTo)
{
visualShape.m_geometry.m_capsuleFrom.setValue(visShape.m_capsuleFrom[0],
visShape.m_capsuleFrom[1],
visShape.m_capsuleFrom[2]);
visualShape.m_geometry.m_capsuleTo.setValue(visShape.m_capsuleTo[0],
visShape.m_capsuleTo[1],
visShape.m_capsuleTo[2]);
}
else
{
visualShape.m_geometry.m_capsuleHeight = visShape.m_capsuleHeight;
visualShape.m_geometry.m_capsuleRadius = visShape.m_capsuleRadius;
}
break;
}
case URDF_GEOM_MESH:
{
std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshFileName;
const std::string& error_message_prefix = "";
std::string out_found_filename;
int out_type;
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
visualShape.m_geometry.m_meshFileType = out_type;
visualShape.m_geometry.m_meshFileName = fileName;
visualShape.m_geometry.m_meshScale.setValue(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[0],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[1],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[2]);
break;
}
default:
{
}
};
visualShape.m_name = "in_memory";
visualShape.m_materialName="";
visualShape.m_sourceFileLocation="blaat_line_10";
visualShape.m_sourceFileLocation="in_memory_unknown_line";
visualShape.m_linkLocalFrame.setIdentity();
visualShape.m_geometry.m_hasLocalMaterial = false;
@@ -3845,7 +3897,6 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
}
u2b.convertURDFToVisualShapeInternal(&visualShape, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
if (vertices.size() && indices.size())
@@ -8305,6 +8356,9 @@ bool PhysicsServerCommandProcessor::processSaveBulletCommand(const struct Shared
if (f)
{
btDefaultSerializer* ser = new btDefaultSerializer();
int currentFlags = ser->getSerializationFlags();
ser->setSerializationFlags(currentFlags | BT_SERIALIZE_CONTACT_MANIFOLDS);
m_data->m_dynamicsWorld->serialize(ser);
fwrite(ser->getBufferPointer(), ser->getCurrentBufferSize(), 1, f);
fclose(f);

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

@@ -1,30 +1,118 @@
import pybullet as p
import math, time
import difflib,sys
numSteps = 500
numSteps2 = 30
p.connect(p.GUI, options="--width=1024 --height=768")
numObjects = 50
verbose = 0
p.loadURDF("plane.urdf")
for i in range (10):
cube = p.loadURDF("cube_small.urdf",0,i*0.01,i*0.5)
p.setGravity(0,0,-10)
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,i*0.02,(i+1)*0.2)
#p.changeDynamics(cube,-1,mass=100)
p.stepSimulation()
p.setGravity(0,0,-10)
#for i in range (500):
# p.stepSimulation()
saveState = 1
if saveState:
for i in range (500):
p.stepSimulation()
p.saveBullet("state.bullet")
else:
p.restoreState(fileName="state.bullet")
for i in range (10):
print("pos[",i,"]=",p.getBasePositionAndOrientation(i))
def dumpStateToFile(file):
for i in range (p.getNumBodies()):
pos,orn = p.getBasePositionAndOrientation(i)
linVel,angVel = p.getBaseVelocity(i)
txtPos = "pos="+str(pos)+"\n"
txtOrn = "orn="+str(orn)+"\n"
txtLinVel = "linVel"+str(linVel)+"\n"
txtAngVel = "angVel"+str(angVel)+"\n"
file.write(txtPos)
file.write(txtOrn)
file.write(txtLinVel)
file.write(txtAngVel)
def compareFiles(file1,file2):
diff = difflib.unified_diff(
file1.readlines(),
file2.readlines(),
fromfile='saveFile.txt',
tofile='restoreFile.txt',
)
for line in diff:
sys.stdout.write(line)
setupWorld()
for i in range (numSteps):
p.stepSimulation()
p.saveBullet("state.bullet")
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points=")
for q in p.getContactPoints():
print(q)
for i in range (numSteps2):
p.stepSimulation()
file = open("saveFile.txt","w")
dumpStateToFile(file)
file.close()
#################################
setupWorld()
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("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)
time.sleep(1)

4
examples/pybullet/pybullet.c
View File

@@ -7876,8 +7876,8 @@ static PyMethodDef SpamMethods[] = {
{ "restoreState", (PyCFunction)pybullet_restoreState, METH_VARARGS | METH_KEYWORDS,
"Restore the full state of an existing world." },
// { "saveState", (PyCFunction)pybullet_saveState, METH_VARARGS | METH_KEYWORDS,
// "Save the full state of the world to memory." },
{ "saveState", (PyCFunction)pybullet_saveState, METH_VARARGS | METH_KEYWORDS,
"Save the full state of the world to memory." },
{"loadMJCF", (PyCFunction)pybullet_loadMJCF, METH_VARARGS | METH_KEYWORDS,
"Load multibodies from an MJCF file."},