From a9ff5246c9885927256e73ad9982169f66b973f3 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 25 May 2018 08:18:12 +1000 Subject: [PATCH 1/2] make gjk a bit more robust, try different initial guess vector if it fails to find a solution (happens for queries with large differences in shape size) --- .../btGjkEpaPenetrationDepthSolver.cpp | 68 ++++++++++++------- .../btGjkPairDetector.cpp | 2 +- 2 files changed, 44 insertions(+), 26 deletions(-) diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index 572ec36f5..2fca19292 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -21,46 +21,64 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" -bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver, - const btConvexShape* pConvexA, const btConvexShape* pConvexB, - const btTransform& transformA, const btTransform& transformB, - btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, - class btIDebugDraw* debugDraw) +bool btGjkEpaPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver, + const btConvexShape* pConvexA, const btConvexShape* pConvexB, + const btTransform& transformA, const btTransform& transformB, + btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, + class btIDebugDraw* debugDraw) { (void)debugDraw; (void)v; (void)simplexSolver; -// const btScalar radialmargin(btScalar(0.)); - - btVector3 guessVector(transformB.getOrigin()-transformA.getOrigin()); - btGjkEpaSolver2::sResults results; - + btVector3 guessVectors[] = { + btVector3(transformB.getOrigin() - transformA.getOrigin()), + btVector3(transformA.getOrigin() - transformB.getOrigin()), + btVector3(0, 0, 1), + btVector3(0, 1, 0), + btVector3(1, 0, 0), + btVector3(1, 1, 0), + btVector3(1, 1, 1), + btVector3(0, 1, 1), + btVector3(1, 0, 1), + }; - if(btGjkEpaSolver2::Penetration(pConvexA,transformA, - pConvexB,transformB, - guessVector,results)) - - { - // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); - //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); - wWitnessOnA = results.witnesses[0]; - wWitnessOnB = results.witnesses[1]; - v = results.normal; - return true; - } else + int numVectors = sizeof(guessVectors) / sizeof(btVector3); + + for (int i = 0; i < numVectors; i++) { - if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results)) + simplexSolver.reset(); + btVector3 guessVector = guessVectors[i]; + + btGjkEpaSolver2::sResults results; + + if (btGjkEpaSolver2::Penetration(pConvexA, transformA, + pConvexB, transformB, + guessVector, results)) + { wWitnessOnA = results.witnesses[0]; wWitnessOnB = results.witnesses[1]; v = results.normal; - return false; + return true; + } + else + { + if (btGjkEpaSolver2::Distance(pConvexA, transformA, pConvexB, transformB, guessVector, results)) + { + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + v = results.normal; + return false; + } } } + //failed to find a distance/penetration + wWitnessOnA.setValue(0, 0, 0); + wWitnessOnB.setValue(0, 0, 0); + v.setValue(0, 0, 0); return false; } - diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 257b026d9..123421307 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -35,7 +35,7 @@ subject to the following restrictions: btScalar gGjkEpaPenetrationTolerance = 1e-7; #else #define REL_ERROR2 btScalar(1.0e-6) - btScalar gGjkEpaPenetrationTolerance = 0.001; + btScalar gGjkEpaPenetrationTolerance = BT_LARGE_FLOAT; #endif //temp globals, to improve GJK/EPA/penetration calculations From da73556ca7d11448eb5bf94d9ca4d40f0c85d9e6 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 25 May 2018 08:26:09 +1000 Subject: [PATCH 2/2] bump up pybullet version to 2.0.0 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index f4f143edf..cb1fa79c2 100644 --- a/setup.py +++ b/setup.py @@ -450,7 +450,7 @@ print("-----") setup( name = 'pybullet', - version='1.9.9', + version='2.0.0', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3',