diff --git a/.travis.yml b/.travis.yml index 5723a41f8..c93c1d2ce 100644 --- a/.travis.yml +++ b/.travis.yml @@ -19,6 +19,8 @@ script: - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then sudo pip3 install setuptools; fi - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then sudo python3 setup.py install; fi - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then python3 examples/pybullet/unittests/unittests.py --verbose; fi + - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then python3 examples/pybullet/unittests/userDataTest.py --verbose; fi + - if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then python3 examples/pybullet/unittests/saveRestoreStateTest.py --verbose; fi - cmake . -DBUILD_PYBULLET=ON -G"Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror - make -j8 - ctest -j8 --output-on-failure diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index a92bb5240..b2ef1027a 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -820,8 +820,8 @@ static PyObject* pybullet_getUserData(PyObject* self, PyObject* args, PyObject* if (!b3GetUserData(sm, bodyUniqueId, linkIndex, userDataId, &value)) { - PyErr_SetString(SpamError, "Cannot get user data"); - return NULL; + Py_INCREF(Py_None); + return Py_None; } if (value.m_type != USER_DATA_VALUE_TYPE_STRING) { diff --git a/examples/pybullet/unittests/saveRestoreStateTest.py b/examples/pybullet/unittests/saveRestoreStateTest.py new file mode 100644 index 000000000..da3480904 --- /dev/null +++ b/examples/pybullet/unittests/saveRestoreStateTest.py @@ -0,0 +1,144 @@ +import unittest +import pybullet as p +import math, time +import difflib,sys + +from utils import allclose, dot + +class TestPybulletSaveRestoreMethods(unittest.TestCase): + + def setupWorld(self): + numObjects = 50 + + + maximalCoordinates = False + + p.resetSimulation() + p.setPhysicsEngineParameter(deterministicOverlappingPairs=1) + p.loadURDF("planeMesh.urdf",useMaximalCoordinates=maximalCoordinates) + kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10], useMaximalCoordinates=maximalCoordinates) + 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) + + + def dumpStateToFile(self, 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(self, file1,file2): + diff = difflib.unified_diff( + file1.readlines(), + file2.readlines(), + fromfile='saveFile.txt', + tofile='restoreFile.txt', + ) + numDifferences = 0 + for line in diff: + numDifferences = numDifferences+1 + sys.stdout.write(line) + self.assertEqual(numDifferences,0) + + + def testSaveRestoreState(self): + numSteps = 500 + numSteps2 = 30 + + + verbose = 0 + self.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") + self.dumpStateToFile(file) + file.close() + + ################################# + self.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() + 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") + self.dumpStateToFile(file) + file.close() + + p.restoreState(stateId) + 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") + self.dumpStateToFile(file) + file.close() + + file1 = open("saveFile.txt","r") + file2 = open("restoreFile.txt","r") + self.compareFiles(file1,file2) + file1.close() + file2.close() + + file1 = open("saveFile.txt","r") + file2 = open("restoreFile2.txt","r") + self.compareFiles(file1,file2) + file1.close() + file2.close() + + + +#while (p.getConnectionInfo()["isConnected"]): +# time.sleep(1) + +if __name__ == '__main__': + p.connect(p.DIRECT) + unittest.main() diff --git a/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/src/BulletCollision/BroadphaseCollision/btDispatcher.h index a0e4c1892..d7a97e8e0 100644 --- a/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -47,7 +47,7 @@ struct btDispatcherInfo m_allowedCcdPenetration(btScalar(0.04)), m_useConvexConservativeDistanceUtil(false), m_convexConservativeDistanceThreshold(0.0f), - m_deterministicOverlappingPairs(false) + m_deterministicOverlappingPairs(true) { } diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 5739a1ef0..af521ea9c 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -252,7 +252,15 @@ void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pa btCollisionPairCallback collisionCallback(dispatchInfo,this); - pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher); + if (dispatchInfo.m_deterministicOverlappingPairs) + { + BT_PROFILE("sortOverlappingPairs"); + pairCache->sortOverlappingPairs(this); + } + { + BT_PROFILE("processAllOverlappingPairs"); + pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher); + } //m_blockedForChanges = false;