From da2ee638ef74a46859908c48a95daed96cd1a605 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 5 Oct 2018 21:47:13 -0700 Subject: [PATCH 1/4] remove obsolete BT_PYBULLET_GRPC and PYBULLET_EGL --- examples/pybullet/pybullet.c | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 86c870a6a..3e651ba42 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -9662,30 +9662,14 @@ PyMODINIT_FUNC #if PY_MAJOR_VERSION >= 3 PyInit_pybullet(void) #else -#ifdef BT_USE_EGL2 -initpybullet_egl(void) -#else -#ifdef BT_PYBULLET_GRPC -initpybullet_grpc(void) -#else initpybullet(void) -#endif //BT_USE_EGL2 -#endif //BT_PYBULLET_GRPC #endif { PyObject* m; #if PY_MAJOR_VERSION >= 3 m = PyModule_Create(&moduledef); -#else -#ifdef BT_USE_EGL2 - m = Py_InitModule3("pybullet_egl", SpamMethods, "Python bindings for Bullet"); -#else -#ifdef BT_PYBULLET_GRPC - m = Py_InitModule3("pybullet_grpc", SpamMethods, "Python bindings for Bullet"); #else m = Py_InitModule3("pybullet", SpamMethods, "Python bindings for Bullet"); -#endif //BT_USE_EGL2 -#endif //BT_PYBULLET_GRPC #endif #if PY_MAJOR_VERSION >= 3 From 71d4a90c9820d9b0f3d9f82bbe6988baf5951afb Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 6 Oct 2018 15:25:22 -0700 Subject: [PATCH 2/4] btMultiBody: fix indexing issue causing wrong friction to be reported (has no effect, since we don't use warmstarting for multibody) --- .../Featherstone/btMultiBodyConstraintSolver.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 60e2e5de3..d273fb4d3 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -1232,7 +1232,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* /////setup the friction constraints #define ENABLE_FRICTION #ifdef ENABLE_FRICTION - solverConstraint.m_frictionIndex = frictionIndex; + solverConstraint.m_frictionIndex = m_multiBodyFrictionContactConstraints.size(); ///Bullet has several options to set the friction directions ///By default, each contact has only a single friction direction that is recomputed automatically every frame @@ -1501,7 +1501,12 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1].m_appliedImpulse; + } else + { + pt->m_appliedImpulseLateral2 = 0; } + } + //do a callback here? } } From c2486bb1a69f0c5183003b62f8f9061f94724c32 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 6 Oct 2018 15:27:56 -0700 Subject: [PATCH 3/4] add contactFriction.py example --- examples/pybullet/examples/contactFriction.py | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 examples/pybullet/examples/contactFriction.py diff --git a/examples/pybullet/examples/contactFriction.py b/examples/pybullet/examples/contactFriction.py new file mode 100644 index 000000000..44a90ecfb --- /dev/null +++ b/examples/pybullet/examples/contactFriction.py @@ -0,0 +1,34 @@ +import pybullet as p +p.connect(p.GUI) +useMaximalCoordinates = False +p.loadURDF("plane.urdf", useMaximalCoordinates=useMaximalCoordinates ) +#p.loadURDF("sphere2.urdf",[0,0,1]) +p.loadURDF("cube.urdf",[0,0,1], useMaximalCoordinates=useMaximalCoordinates ) +p.setGravity(0,3,-10) +while(1): + p.stepSimulation() + pts = p.getContactPoints() + + print("num pts=",len(pts)) + totalNormalForce = 0 + totalFrictionForce = [0,0,0] + totalLateralFrictionForce=[0,0,0] + for pt in pts: + #print("pt.normal=",pt[7]) + #print("pt.normalForce=",pt[9]) + totalNormalForce += pt[9] + #print("pt.lateralFrictionA=",pt[10]) + #print("pt.lateralFrictionADir=",pt[11]) + #print("pt.lateralFrictionB=",pt[12]) + #print("pt.lateralFrictionBDir=",pt[13]) + totalLateralFrictionForce[0]+=pt[11][0]*pt[10]+pt[13][0]*pt[12] + totalLateralFrictionForce[1]+=pt[11][1]*pt[10]+pt[13][1]*pt[12] + totalLateralFrictionForce[2]+=pt[11][2]*pt[10]+pt[13][2]*pt[12] + + + print("totalNormalForce=",totalNormalForce) + print("totalLateralFrictionForce=",totalLateralFrictionForce) + + + + \ No newline at end of file From 332cbfa6096e69db8d9d2acac8010c3e74f05743 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 6 Oct 2018 18:37:51 -0700 Subject: [PATCH 4/4] fix typo in previous commit --- src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index d273fb4d3..e97bd71cc 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -1508,7 +1508,6 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO } //do a callback here? - } } #if 0 //multibody joint feedback