martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Explicitly formulating bias term.

master
Martin Felis 2020-11-15 21:46:43 +01:00
parent 4389dd19fc
commit 4a8d691334
1 changed files with 28 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/rbdlsim.cc
View File

@ -293,10 +293,13 @@ void CalcImpulseVariables(
const Vector3d& dir, const Vector3d& dir,
MatrixNd* MInv, MatrixNd* MInv,
VectorNd* jac, VectorNd* jac,
double* G_MInv_GT) { double* G_MInv_GT,
double* bias_vel,
double restitution) {
if (body == nullptr || body->mIsStatic) { if (body == nullptr || body->mIsStatic) {
jac->setZero(); jac->setZero();
*G_MInv_GT = 0.; *G_MInv_GT = 0.;
*bias_vel = 0.;
return; return;
} }
@ -320,6 +323,8 @@ void CalcImpulseVariables(
(*jac) = dir.transpose() * G_constr; (*jac) = dir.transpose() * G_constr;
*G_MInv_GT = (*jac) * (*MInv) * (*jac).transpose(); *G_MInv_GT = (*jac) * (*MInv) * (*jac).transpose();
*bias_vel = (*jac) * qdot * restitution;
} }
void PrepareConstraintImpulse( void PrepareConstraintImpulse(
@ -333,7 +338,10 @@ void PrepareConstraintImpulse(
cinfo.dir, cinfo.dir,
&cinfo.MInvA, &cinfo.MInvA,
&cinfo.jacA, &cinfo.jacA,
&cinfo.GMInvGTA); &cinfo.GMInvGTA,
&cinfo.biasVelocityA,
cinfo.effectiveRestitution);
CalcImpulseVariables( CalcImpulseVariables(
body_b, body_b,
cinfo.mBodyBIndex, cinfo.mBodyBIndex,
@ -341,7 +349,9 @@ void PrepareConstraintImpulse(
cinfo.dir, cinfo.dir,
&cinfo.MInvB, &cinfo.MInvB,
&cinfo.jacB, &cinfo.jacB,
&cinfo.GMInvGTB); &cinfo.GMInvGTB,
&cinfo.biasVelocityB,
cinfo.effectiveRestitution);
} }
/// Calculates the impulse that we apply on body_b to resolve the contact. /// Calculates the impulse that we apply on body_b to resolve the contact.
@ -352,19 +362,30 @@ void CalcConstraintImpulse(
const double dt) { const double dt) {
// Todo: add nonlinear effects * dt // Todo: add nonlinear effects * dt
double ref_a = 0.;
double ref_b = 0.;
double vel_a = 0.;
double vel_b = 0.;
double ref = 0.;
double rhs = 0.; double rhs = 0.;
if (body_a && !body_a->mIsStatic) { if (body_a && !body_a->mIsStatic) {
rhs += cinfo.jacA * body_a->qdot * (1.0 + cinfo.effectiveRestitution); vel_a = cinfo.jacA * body_a->qdot;
ref_a += cinfo.jacA * body_a->qdot * (1.0 + cinfo.effectiveRestitution);
rhs += cinfo.jacA * body_a->qdot + cinfo.biasVelocityA;
} }
if (body_b && !body_b->mIsStatic) { if (body_b && !body_b->mIsStatic) {
rhs += -cinfo.jacB * (body_b->qdot) * (1.0 + cinfo.effectiveRestitution); vel_b = cinfo.jacB * body_b->qdot;
ref_b += -cinfo.jacB * (body_b->qdot) * (1.0 + cinfo.effectiveRestitution);
rhs += -cinfo.jacB * body_b->qdot - cinfo.biasVelocityB;
} }
ref = ref_a + ref_b;
double denom = cinfo.GMInvGTA + cinfo.GMInvGTB; double denom = cinfo.GMInvGTA + cinfo.GMInvGTB;
double old_impulse = cinfo.accumImpulse; double old_impulse = cinfo.accumImpulse;
// TODO: is this really needed here?? // TODO: is this really needed here??
cinfo.deltaImpulse = std::max(0., rhs / denom); cinfo.deltaImpulse = rhs / denom;
cinfo.accumImpulse = std::max(0., cinfo.accumImpulse + cinfo.deltaImpulse); cinfo.accumImpulse = std::max(0., cinfo.accumImpulse + cinfo.deltaImpulse);
cinfo.deltaImpulse = cinfo.accumImpulse - old_impulse; cinfo.deltaImpulse = cinfo.accumImpulse - old_impulse;
} }
@ -446,6 +467,7 @@ void World::resolveCollisions(double dt) {
for (CollisionInfo& cinfo : mContactPoints) { for (CollisionInfo& cinfo : mContactPoints) {
CalcConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo, dt); CalcConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo, dt);
ApplyConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo); ApplyConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
gLog ("Iter %d: Apply impulse: %f", i, cinfo.deltaImpulse);
} }
} }
} }