From 39808ddfdf8da27aa27f68a1d7f8baeaa76b4632 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 29 May 2018 09:28:31 -0700 Subject: [PATCH 1/3] allow to run minitaur environment examples from pip version, like this: python -m pybullet_envs.minitaur.envs.minitaur_trotting_env_example python -m pybullet_envs.minitaur.envs.minitaur_reactive_env_example --- .../minitaur/envs/minitaur_reactive_env_example.py | 2 +- .../minitaur/envs/minitaur_trotting_env_example.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py index a2d3c75e4..5057d9cf4 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py @@ -9,7 +9,7 @@ import time import tensorflow as tf from pybullet_envs.minitaur.agents.scripts import utility import pybullet_data -import simple_ppo_agent +from pybullet_envs.minitaur.envs import simple_ppo_agent flags = tf.app.flags FLAGS = tf.app.flags.FLAGS diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_trotting_env_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_trotting_env_example.py index 88fef5e52..e158e1160 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_trotting_env_example.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_trotting_env_example.py @@ -9,7 +9,7 @@ import time import tensorflow as tf from pybullet_envs.minitaur.agents.scripts import utility import pybullet_data -import simple_ppo_agent +from pybullet_envs.minitaur.envs import simple_ppo_agent flags = tf.app.flags FLAGS = tf.app.flags.FLAGS From ec4b3505a4598b0446da98caea292cb3b74ca80f Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 29 May 2018 16:49:07 -0700 Subject: [PATCH 2/3] fix fixed constraint between btMultiBody and btRigidBody --- .../Featherstone/btMultiBodyConstraint.cpp | 10 +++++---- .../btMultiBodyConstraintSolver.cpp | 21 +++---------------- 2 files changed, 9 insertions(+), 22 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index d52852dd8..9f61874b8 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -253,7 +253,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr { vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); if (angConstraint) { - denom0 = rb0->getInvMass() + constraintNormalAng.dot(vec); + denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA); } else { denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec); @@ -277,7 +277,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr { vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); if (angConstraint) { - denom1 = rb1->getInvMass() + constraintNormalAng.dot(vec); + denom1 = constraintNormalAng.dot(-solverConstraint.m_angularComponentB); } else { denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec); @@ -315,7 +315,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr } else if(rb0) { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + rel_vel += rb0->getLinearVelocity().dot(solverConstraint.m_contactNormal1); + rel_vel += rb0->getAngularVelocity().dot(solverConstraint.m_relpos1CrossNormal); } if (multiBodyB) { @@ -327,7 +328,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr } else if(rb1) { - rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + rel_vel += rb1->getLinearVelocity().dot(solverConstraint.m_contactNormal2); + rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal); } solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 5bab043d6..8c6cb6aa8 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -246,7 +246,7 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt { c.m_appliedImpulse = sum; } - + if (c.m_multiBodyA) { applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA); @@ -1487,27 +1487,12 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv if (c.m_multiBodyA) { - - if(c.m_multiBodyA->isMultiDof()) - { - c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); - } - else - { - c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); - } + c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); } if (c.m_multiBodyB) { - if(c.m_multiBodyB->isMultiDof()) - { - c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); - } - else - { - c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); - } + c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); } #endif From 11d8f069f03be902a98dc3c0a5977f8d2eda3d45 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 29 May 2018 17:06:35 -0700 Subject: [PATCH 3/3] Disable overlapping pair debug variables behind the BT_DEBUG_COLLISION_PAIRS preprocessor definition. --- .../CollisionDispatch/btHashedSimplePairCache.cpp | 7 ++++++- .../CollisionDispatch/btHashedSimplePairCache.h | 6 ++++-- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp b/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp index 8c8a7c3c1..8271981b2 100644 --- a/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp +++ b/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp @@ -20,11 +20,12 @@ subject to the following restrictions: #include +#ifdef BT_DEBUG_COLLISION_PAIRS int gOverlappingSimplePairs = 0; int gRemoveSimplePairs =0; int gAddedSimplePairs =0; int gFindSimplePairs =0; - +#endif //BT_DEBUG_COLLISION_PAIRS @@ -61,7 +62,9 @@ void btHashedSimplePairCache::removeAllPairs() btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gFindSimplePairs++; +#endif /*if (indexA > indexB) @@ -172,7 +175,9 @@ btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB) void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gRemoveSimplePairs++; +#endif /*if (indexA > indexB) diff --git a/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h index 2aaf6201f..318981cda 100644 --- a/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h +++ b/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h @@ -43,12 +43,12 @@ struct btSimplePair typedef btAlignedObjectArray btSimplePairArray; - +#ifdef BT_DEBUG_COLLISION_PAIRS extern int gOverlappingSimplePairs; extern int gRemoveSimplePairs; extern int gAddedSimplePairs; extern int gFindSimplePairs; - +#endif //BT_DEBUG_COLLISION_PAIRS @@ -75,7 +75,9 @@ public: // no new pair is created and the old one is returned. virtual btSimplePair* addOverlappingPair(int indexA,int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gAddedSimplePairs++; +#endif return internalAddPair(indexA,indexB); }