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

ping-pong back/forward in PGS solving iterations to reduce bias in constraint order

Browse Source 
add experimental rhs clamp in btMultiBodyJointMotor to control maximum error resolution.
This commit is contained in:
erwin coumans
2016-09-28 11:17:11 -07:00
parent cf046093ab
commit b81eb79ef5
8 changed files with 63 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -31,9 +31,12 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
//solve featherstone non-contact constraints
//printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
for (int j=0;j<m_multiBodyNonContactConstraints.size();j++)
{
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j];
int index = iteration&1? j : m_multiBodyNonContactConstraints.size()-1-j;
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
resolveSingleConstraintRowGeneric(constraint);
if(constraint.m_multiBodyA)

15
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
View File

@@ -27,7 +27,8 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal
m_desiredPosition(0),
m_kd(1.),
m_kp(0),
m_erp(1)
m_erp(1),
m_rhsClamp(SIMD_INFINITY)
{
m_maxAppliedImpulse = maxMotorImpulse;
@@ -59,7 +60,8 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int li
m_desiredPosition(0),
m_kd(1.),
m_kp(0),
m_erp(1)
m_erp(1),
m_rhsClamp(SIMD_INFINITY)
{
btAssert(linkDoF < body->getLink(link).m_dofCount);
@@ -126,8 +128,17 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;
if (rhs>m_rhsClamp)
{
rhs=m_rhsClamp;
}
if (rhs<-m_rhsClamp)
{
rhs=-m_rhsClamp;
}
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,rhs);

6
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
View File

@@ -30,6 +30,7 @@ protected:
btScalar m_kd;
btScalar m_kp;
btScalar m_erp;
btScalar m_rhsClamp;//maximum error
public:
@@ -66,7 +67,10 @@ public:
{
return m_erp;
}
virtual void setRhsClamp(btScalar rhsClamp)
{
m_rhsClamp = rhsClamp;
}
virtual void debugDraw(class btIDebugDraw* drawer)
{
//todo(erwincoumans)