Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

fix issue with btMultiBody friction in combination with soft contacts (friction should not re-use normal contact cfm/erp)

Browse Source 
implement friction anchors, position friction correction, disabled by default. Use colObj->setCollisionFlag(flag | CF_HAS_FRICTION_ANCHOR); See test/RobotClientAPI/SlopeFrictionMain.cpp. In URDF or SDF, add <friction_anchor/> in <contact> section of <link> to enable.
PhysicsServer: properly restore old activation state after releasing picked object
btMultiBodyConstraintSolver: disable flip/flop of contact/friction constraint solving by default (it breaks some internal flaky unit tests)
This commit is contained in:
Erwin Coumans
2017-03-20 10:58:07 -07:00
parent 865d37fcb5
commit 0b017b0f53
18 changed files with 299 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View File

@@ -43,10 +43,13 @@ struct btContactSolverInfoData
btScalar m_restitution;
int m_numIterations;
btScalar m_maxErrorReduction;
btScalar m_sor;
btScalar m_erp;//used as Baumgarte factor
btScalar m_erp2;//used in Split Impulse
btScalar m_globalCfm;//constraint force mixing
btScalar m_sor;//successive over-relaxation term
btScalar m_erp;//error reduction for non-contact constraints
btScalar m_erp2;//error reduction for contact constraints
btScalar m_globalCfm;//constraint force mixing for contacts and non-contacts
btScalar m_frictionERP;//error reduction for friction constraints
btScalar m_frictionCFM;//constraint force mixing for friction constraints
int m_splitImpulse;
btScalar m_splitImpulsePenetrationThreshold;
btScalar m_splitImpulseTurnErp;
@@ -79,6 +82,8 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_erp = btScalar(0.2);
m_erp2 = btScalar(0.2);
m_globalCfm = btScalar(0.);
m_frictionERP = btScalar(0.2);//positional friction 'anchors' are disabled by default
m_frictionCFM = btScalar(0.);
m_sor = btScalar(1.);
m_splitImpulse = true;
m_splitImpulsePenetrationThreshold = -.04f;

34
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -534,7 +534,7 @@ void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionOb
void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
{
@@ -612,7 +612,17 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
btScalar velocityError = desiredVelocity - rel_vel;
btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = velocityImpulse;
btScalar penetrationImpulse = btScalar(0);
if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
{
btScalar distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(normalAxis);
btScalar positionalError = -distance * infoGlobal.m_frictionERP/infoGlobal.m_timeStep;
penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
}
solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
@@ -621,12 +631,12 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
}
}
btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
{
btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
solverConstraint.m_frictionIndex = frictionIndex;
setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
return solverConstraint;
}
@@ -1168,6 +1178,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
///this will give a conveyor belt effect
///
if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
{
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
@@ -1177,7 +1188,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,infoGlobal);
if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
@@ -1185,7 +1196,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
cp.m_lateralFrictionDir2.normalize();//??
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
}
} else
@@ -1194,13 +1205,13 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
}
@@ -1212,10 +1223,10 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
} else
{
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_frictionCFM);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_frictionCFM);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
}
setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
@@ -1526,7 +1537,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
btScalar fsum = btFabs(sum);
btAssert(fsum > SIMD_EPSILON);
solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
btScalar sorRelaxation = 1.f;//todo: get from globalInfo?
solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?sorRelaxation/sum : 0.f;
}

3
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -62,6 +62,7 @@ protected:
void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
void setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
@@ -69,7 +70,7 @@ protected:
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar torsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);