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

apply newForceTorque.diff patch: it will allow to report

Browse Source 
joint reaction force/torque, while using impulse-based response
for btMultiBody
This commit is contained in:
=
2015-07-06 16:39:41 -07:00
parent 4630d0abb1
commit 33b0d429ba
6 changed files with 251 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -357,7 +357,8 @@ void btMultiBody::setupPlanar(int i,
void btMultiBody::finalizeMultiDof()
{
btAssert(m_isMultiDof);
m_deltaV.resize(0);
m_deltaV.resize(6 + m_dofCount);
m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices)
@@ -543,7 +544,17 @@ btVector3 btMultiBody::getAngularMomentum() const
return result;
}
void btMultiBody::clearConstraintForces()
{
m_baseConstraintForce.setValue(0, 0, 0);
m_baseConstraintTorque.setValue(0, 0, 0);
for (int i = 0; i < getNumLinks(); ++i) {
m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
}
}
void btMultiBody::clearForcesAndTorques()
{
m_baseForce.setValue(0, 0, 0);
@@ -574,6 +585,18 @@ void btMultiBody::addLinkTorque(int i, const btVector3 &t)
m_links[i].m_appliedTorque += t;
}
void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f)
{
m_links[i].m_appliedConstraintForce += f;
}
void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t)
{
m_links[i].m_appliedConstraintTorque += t;
}
void btMultiBody::addJointTorque(int i, btScalar Q)
{
m_links[i].m_jointTorque[0] += Q;
@@ -637,7 +660,8 @@ inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1) //r
void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m)
btAlignedObjectArray<btMatrix3x3> &scratch_m,
bool isConstraintPass)
{
// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
// and the base linear & angular accelerations.
@@ -734,8 +758,10 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
}
else
{
btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
//external forces
zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * m_baseTorque), -(rot_from_parent[0] * m_baseForce));
zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
//adding damping terms (only)
btScalar linDampMult = 1., angDampMult = 1.;
@@ -804,7 +830,10 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
// calculate zhat_i^A
//
//external forces
zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * m_links[i].m_appliedTorque), -(rot_from_world[i+1] * m_links[i].m_appliedForce));
btVector3 linkAppliedForce = isConstraintPass? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce ));
if (0)
{
@@ -1043,12 +1072,28 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
if (gJointFeedbackInWorldSpace)
{
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
if (isConstraintPass)
{
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
} else
{
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
}
} else
{
if (isConstraintPass)
{
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
}
else
{
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
}
}
}
@@ -1084,9 +1129,13 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
/////////////////
// Final step: add the accelerations (times dt) to the velocities.
if (!isConstraintPass)
{
if(dt > 0.)
applyDeltaVeeMultiDof(output, dt);
}
/////
//btScalar angularThres = 1;
//btScalar maxAngVel = 0.;

42
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -273,6 +273,8 @@ public:
//
void clearForcesAndTorques();
void clearConstraintForces();
void clearVelocities();
void addBaseForce(const btVector3 &f)
@@ -282,7 +284,17 @@ public:
void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
void addLinkForce(int i, const btVector3 &f);
void addLinkTorque(int i, const btVector3 &t);
void addJointTorque(int i, btScalar Q);
void addBaseConstraintForce(const btVector3 &f)
{
m_baseConstraintForce += f;
}
void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; }
void addLinkConstraintForce(int i, const btVector3 &f);
void addLinkConstraintTorque(int i, const btVector3 &t);
void addJointTorque(int i, btScalar Q);
void addJointTorqueMultiDof(int i, int dof, btScalar Q);
void addJointTorqueMultiDof(int i, const btScalar *Q);
@@ -318,7 +330,9 @@ public:
void stepVelocitiesMultiDof(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m);
btAlignedObjectArray<btMatrix3x3> &scratch_m,
bool isConstraintPass=false
);
// calcAccelerationDeltas
// input: force vector (in same format as jacobian, i.e.:
@@ -374,6 +388,23 @@ public:
}
}
void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier)
{
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{
m_deltaV[dof] += delta_vee[dof] * multiplier;
}
}
void processDeltaVeeMultiDof2()
{
applyDeltaVeeMultiDof(&m_deltaV[0],1);
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{
m_deltaV[dof] = 0.f;
}
}
void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier)
{
//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
@@ -591,7 +622,10 @@ private:
btVector3 m_baseForce; // external force applied to base. World frame.
btVector3 m_baseTorque; // external torque applied to base. World frame.
btVector3 m_baseConstraintForce; // external force applied to base. World frame.
btVector3 m_baseConstraintTorque; // external torque applied to base. World frame.
btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1.
btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
@@ -611,7 +645,7 @@ private:
// offset size array
// 0 num_links+1 rot_from_parent
//
btAlignedObjectArray<btScalar> m_deltaV;
btAlignedObjectArray<btScalar> m_realBuf;
btAlignedObjectArray<btVector3> m_vectorBuf;
btAlignedObjectArray<btMatrix3x3> m_matrixBuf;

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

@@ -170,7 +170,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
//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);
c.m_multiBodyA->applyDeltaVeeMultiDof2(&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
@@ -186,7 +186,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
//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);
c.m_multiBodyB->applyDeltaVeeMultiDof2(&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
@@ -887,7 +887,7 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS
#include "Bullet3Common/b3Logging.h"
void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
{
#if 1
#if 1
//bod->addBaseForce(m_gravity * bod->getBaseMass());
//bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
@@ -898,78 +898,49 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
if(c.m_multiBodyA->isMultiDof())
{
if (c.m_useJointForce)
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->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
//c.m_multiBodyA->addJointTorqueMultiDof(c.m_linkA,0,c.m_appliedImpulse/deltaTime);
c.m_multiBodyA->addBaseConstraintForce(force);
c.m_multiBodyA->addBaseConstraintTorque(torque);
} 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);
c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force);
//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque);
}
}
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);
c.m_multiBodyB->addBaseConstraintForce(force);
c.m_multiBodyB->addBaseConstraintTorque(torque);
} else
{
if (!c.m_useJointForce)
{
c.m_multiBodyB->addLinkForce(c.m_linkB,force);
c.m_multiBodyB->addLinkConstraintForce(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->addLinkConstraintTorque(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
#endif
#ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
if (c.m_multiBodyA)
{
@@ -1006,7 +977,6 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
#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
@@ -1029,7 +999,6 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
}
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)

133
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -441,7 +441,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
{
BT_PROFILE("btMultiBody addForce");
for (int i=0;i<this->m_multiBodies.size();i++)
@@ -476,7 +476,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
}//if (!isSleeping)
}
}
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
{
@@ -671,16 +671,61 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
}
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
bod->clearForcesAndTorques();
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
}//if (!isSleeping)
}
}
clearMultiBodyConstraintForces();
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
{
BT_PROFILE("btMultiBody stepVelocities");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
scratch_v.resize(bod->getNumLinks()+1);
scratch_m.resize(bod->getNumLinks()+1);
if(bod->isMultiDof())
{
if(!bod->isUsingRK4Integration())
{
bool isConstraintPass = true;
bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass);
}
}
}
}
}
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bod->processDeltaVeeMultiDof2();
}
}
@@ -882,3 +927,87 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
btDiscreteDynamicsWorld::debugDrawWorld();
}
void btMultiBodyDynamicsWorld::applyGravity()
{
btDiscreteDynamicsWorld::applyGravity();
#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
BT_PROFILE("btMultiBody addGravity");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
bod->addBaseForce(m_gravity * bod->getBaseMass());
for (int j = 0; j < bod->getNumLinks(); ++j)
{
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
}//if (!isSleeping)
}
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
}
void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces()
{
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bod->clearConstraintForces();
}
}
void btMultiBodyDynamicsWorld::clearMultiBodyForces()
{
{
BT_PROFILE("clearMultiBodyForces");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
btMultiBody* bod = m_multiBodies[i];
bod->clearForcesAndTorques();
}
}
}
}
void btMultiBodyDynamicsWorld::clearForces()
{
btDiscreteDynamicsWorld::clearForces();
#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
clearMultiBodyForces();
#endif
}

5
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
View File

@@ -18,6 +18,7 @@ subject to the following restrictions:
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
#define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
class btMultiBody;
class btMultiBodyConstraint;
@@ -85,6 +86,10 @@ public:
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
void forwardKinematics();
virtual void clearForces();
virtual void clearMultiBodyConstraintForces();
virtual void clearMultiBodyForces();
virtual void applyGravity();
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

3
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@@ -106,6 +106,9 @@ struct btMultibodyLink
btVector3 m_appliedForce; // In WORLD frame
btVector3 m_appliedTorque; // In WORLD frame
btVector3 m_appliedConstraintForce; // In WORLD frame
btVector3 m_appliedConstraintTorque; // In WORLD frame
btScalar m_jointPos[7];
//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.