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

First step in btMultiBody joint force/torque feedback. There is still some work to be done for 'mobilizer limit/motor'.

Browse Source 
See examples/MultiBody/TestJointTorqueSetup and examples/Constraints/TestHingeTorque for joint feedback.
This commit is contained in:
erwincoumans
2015-06-19 09:18:27 -07:00
parent 41aa58560b
commit 89edc40d61
16 changed files with 689 additions and 768 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -13,6 +13,7 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btMultiBodyConstraintSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "btMultiBodyLinkCollider.h"
@@ -165,12 +166,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
if (c.m_multiBodyA)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
if(c.m_multiBodyA->isMultiDof())
c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
else
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
} else if(c.m_solverBodyIdA >= 0)
{
bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
@@ -179,12 +182,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
if (c.m_multiBodyB)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
if(c.m_multiBodyB->isMultiDof())
c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
else
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
} else if(c.m_solverBodyIdB >= 0)
{
bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
@@ -279,8 +284,18 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
relaxation = 1.f;
if (multiBodyA)
{
if (solverConstraint.m_linkA<0)
{
rel_pos1 = pos1 - multiBodyA->getBasePos();
} else
{
rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_worldPosition;
}
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
@@ -310,16 +325,30 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
else
multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = contactNormal;
} else
{
btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = contactNormal;
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
}
if (multiBodyB)
{
if (solverConstraint.m_linkB<0)
{
rel_pos2 = pos2 - multiBodyB->getBasePos();
} else
{
rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_worldPosition;
}
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
@@ -344,12 +373,18 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
else
multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -contactNormal;
} else
{
btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -contactNormal;
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
}
{
@@ -577,6 +612,7 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo
{
BT_PROFILE("addMultiBodyFrictionConstraint");
btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
solverConstraint.m_useJointForce = false;
solverConstraint.m_frictionIndex = frictionIndex;
bool isFriction = true;
@@ -644,6 +680,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
int frictionIndex = m_multiBodyNormalContactConstraints.size();
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
solverConstraint.m_useJointForce = false;
// btRigidBody* rb0 = btRigidBody::upcast(colObj0);
// btRigidBody* rb1 = btRigidBody::upcast(colObj1);
@@ -824,32 +861,267 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
{
return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
}
#if 0
static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
{
if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
{
//todo: get rid of those temporary memory allocations for the joint feedback
btAlignedObjectArray<btScalar> forceVector;
int numDofsPlusBase = 6+mb->getNumDofs();
forceVector.resize(numDofsPlusBase);
for (int i=0;i<numDofsPlusBase;i++)
{
forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
}
btAlignedObjectArray<btScalar> output;
output.resize(numDofsPlusBase);
bool applyJointFeedback = true;
mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
}
}
#endif
#include "Bullet3Common/b3Logging.h"
void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
{
#if 1
//bod->addBaseForce(m_gravity * bod->getBaseMass());
//bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
if (c.m_multiBodyA)
{
btScalar ai = c.m_appliedImpulse;
if(c.m_multiBodyA->isMultiDof())
{
if (c.m_useJointForce)
{
c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
//c.m_multiBodyA->addJointTorqueMultiDof(c.m_linkA,0,c.m_appliedImpulse/deltaTime);
} else
//if (c.m_multiBodyA->getCompanionId()>=0)
{
c.m_multiBodyA->setCompanionId(-1);
btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
if (c.m_linkA<0)
{
c.m_multiBodyA->addBaseForce(force);
c.m_multiBodyA->addBaseTorque(torque);
} else
{
if (c.m_useJointForce)
{
c.m_multiBodyA->addJointTorqueMultiDof(c.m_linkA,0,c.m_appliedImpulse/deltaTime);
} else
{
c.m_multiBodyA->addLinkForce(c.m_linkA,force);
//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
c.m_multiBodyA->addLinkTorque(c.m_linkA,torque);
}
}
//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())
{
if (c.m_useJointForce)
{
c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
} else
//if (c.m_multiBodyB->getCompanionId()>=0)
{
c.m_multiBodyB->setCompanionId(-1);
btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
if (c.m_linkB<0)
{
c.m_multiBodyB->addBaseForce(force);
c.m_multiBodyB->addBaseTorque(torque);
} else
{
if (!c.m_useJointForce)
{
c.m_multiBodyB->addLinkForce(c.m_linkB,force);
b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
c.m_multiBodyB->addLinkTorque(c.m_linkB,torque);
}
}
//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);
}
}
#else
if (c.m_multiBodyA)
{
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);
}
else
{
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
}
}
#endif
}
btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
{
BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
int j;
#ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
//or as applied force, so we can measure the joint reaction forces easier
for (int i=0;i<numPoolConstraints;i++)
{
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i];
writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep);
}
}
for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
{
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
}
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
for (j=0;j<numPoolConstraints;j++)
BT_PROFILE("warm starting write back");
for (int j=0;j<numPoolConstraints;j++)
{
const btMultiBodySolverConstraint& solveManifold = m_multiBodyNormalContactConstraints[j];
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
btAssert(pt);
pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex].m_appliedImpulse;
pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex+1].m_appliedImpulse;
pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
}
//do a callback here?
}
}
#if 0
//multibody joint feedback
{
BT_PROFILE("multi body joint feedback");
for (int j=0;j<numPoolConstraints;j++)
{
const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
//apply the joint feedback into all links of the btMultiBody
//todo: double-check the signs of the applied impulse
if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
{
applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
}
if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
{
applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
}
#if 0
if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
{
applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
}
if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
{
applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
}
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
{
applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
}
if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
{
applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
}
}
#endif
}
for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
{
const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
{
applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
}
if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
{
applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
}
}
}
numPoolConstraints = m_multiBodyNonContactConstraints.size();
@@ -878,7 +1150,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
}
#endif
#endif
return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
}