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

fix an issue with btMultiBodyConstraint, automatically 'finalizeMultiDof' to pre-allocate jacobian data

Browse Source 
enable joint limit for slider/prismatic joint in btMultiBody version of URDF loader
This commit is contained in:
Erwin Coumans
2015-06-05 11:46:53 -07:00
parent a94ac6300a
commit 1a4ce475f7
12 changed files with 307 additions and 249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
View File

@@ -3,7 +3,7 @@
#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
:m_bodyA(bodyA),
m_bodyB(bodyB),
@@ -13,14 +13,15 @@ btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bod
m_jacSizeA(0),
m_jacSizeBoth(0),
m_isUnilateral(isUnilateral),
m_maxAppliedImpulse(100)
m_maxAppliedImpulse(100),
m_numDofsFinalized(-1)
{
finalizeMultiDof();
}
void btMultiBodyConstraint::finalizeMultiDof()
void btMultiBodyConstraint::updateJacobianSizes()
{
if(m_bodyA)
if(m_bodyA)
{
if(m_bodyA->isMultiDof())
m_jacSizeA = (6 + m_bodyA->getNumDofs());
@@ -37,7 +38,12 @@ void btMultiBodyConstraint::finalizeMultiDof()
}
else
m_jacSizeBoth = m_jacSizeA;
}
void btMultiBodyConstraint::allocateJacobiansMultiDof()
{
updateJacobianSizes();
m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
m_data.resize((2 + m_jacSizeBoth) * m_numRows);
}
@@ -48,25 +54,27 @@ btMultiBodyConstraint::~btMultiBodyConstraint()
void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
{
for (int i = 0; i < ndof; ++i)
for (int i = 0; i < ndof; ++i)
data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
}
btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint,
btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
btScalar* jacOrgA, btScalar* jacOrgB,
const btVector3& contactNormalOnB,
const btVector3& posAworld, const btVector3& posBworld,
const btVector3& posAworld, const btVector3& posBworld,
btScalar posError,
const btContactSolverInfo& infoGlobal,
btScalar lowerLimit, btScalar upperLimit,
btScalar relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
solverConstraint.m_multiBodyA = m_bodyA;
solverConstraint.m_multiBodyB = m_bodyB;
solverConstraint.m_linkA = m_linkA;
solverConstraint.m_linkB = m_linkB;
solverConstraint.m_linkB = m_linkB;
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
@@ -181,13 +189,13 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
}
else if(rb1)
{
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -contactNormalOnB;
}
{
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
@@ -233,7 +241,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
}
//
btScalar d = denom0+denom1;
if (d>SIMD_EPSILON)
@@ -244,10 +252,10 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
{
//disable the constraint row to handle singularity/redundant constraint
solverConstraint.m_jacDiagABInv = 0.f;
}
}
}
//compute rhs and remaining solverConstraint fields
btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
@@ -260,7 +268,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
{
ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
}
else if(rb0)
@@ -271,7 +279,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
{
ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
}
@@ -321,11 +329,11 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
{
{
btScalar positionalError = 0.f;
btScalar velocityError = desiredVelocity - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))