Merge pull request #1716 from erwincoumans/master

fix fixed constraint between btMultiBody and btRigidBody, fix minitaur examples when ran from pip version

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

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

src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp

@@ -20,11 +20,12 @@ subject to the following restrictions:
 
 #include <stdio.h>
 
+#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) 

src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h

@@ -43,12 +43,12 @@ struct btSimplePair
 typedef btAlignedObjectArray<btSimplePair>	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);
 	}

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;

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)
 	{
-		
+		c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
-		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);
-		}
 	}
 	
 	if (c.m_multiBodyB)
 	{
-		if(c.m_multiBodyB->isMultiDof())
+		c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
-		{
-			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);
-		}
 	}
 #endif