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

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);