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

Implemented tangential friction. Yay!

master
Martin Felis 2020-11-24 22:34:28 +01:00
parent ca4f4048df
commit d01417e8c9
3 changed files with 132 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
include/rbdlsim.h
View File

@ -55,26 +55,24 @@ struct CollisionInfo {
double biasVelocityB = 0.;
double accumImpulse = 0.;
double deltaImpulse = 0.;
Vector3d dir = Vector3d::Zero();
Vector3d dir = Vector3d(0., 1., 0.);
VectorNd jacA = VectorNd::Zero(1);
VectorNd jacB = VectorNd::Zero(1);
VectorNd MInvJacTA = VectorNd::Zero(1);
VectorNd MInvJacTB = VectorNd::Zero(1);
double GMInvGTA = 0.;
double GMInvGTB = 0.;
double tangentAccumImpulse0 = 0.;
double tangentAccumImpulse1 = 0.;
double tangentDeltaImpulse0 = 0.;
double tangentDeltaImpulse1 = 0.;
Vector3d tangent0 = Vector3d::Zero();
Vector3d tangent1 = Vector3d::Zero();
VectorNd tangentJacA = MatrixNd::Zero(2,2);
VectorNd tangentJacB = MatrixNd::Zero(2,2);
MatrixNd tangentMInvA = MatrixNd::Zero(2, 2);
MatrixNd tangentMInvB = MatrixNd::Zero(2, 2);
MatrixNd tangentGMInvGTA = MatrixNd::Zero(2,2);
MatrixNd tangentGMInvGTB = MatrixNd::Zero(2,2);
double accumFrictionImpulse[2] = {0.,0.};
double deltaFrictionImpulse[2] = {0.,0.};
Vector3d tangents[2] = {Vector3d::Zero(), Vector3d::Zero()};
VectorNd tangentJacA[2] = {VectorNd::Zero(1), VectorNd::Zero(1)};
VectorNd tangentJacB[2] = {VectorNd::Zero(1), VectorNd::Zero(1)};
VectorNd tangentMInvJacTA[2] = {VectorNd::Zero(1), VectorNd::Zero(1)};
VectorNd tangentMInvJacTB[2] = {VectorNd::Zero(1), VectorNd::Zero(1)};
double tangentGMInvGTA[2] = {0., 0.};
double tangentGMInvGTB[2] = {0., 0.};
double effectiveRestitution = 1.0;
double effectiveFriction = 0.2;
double depth = 0.;
};
@ -141,11 +139,21 @@ void CalcCollisions(
SimBody& body_a,
SimBody& body_b,
std::vector<CollisionInfo>& collisions);
void CalcFrictionImpulse(
SimBody* body_a,
SimBody* body_b,
CollisionInfo& cinfo);
void ApplyFrictionImpulse(
SimBody* body_a,
SimBody* body_b,
CollisionInfo& cinfo);
void CalcConstraintImpulse(
SimBody* body_a,
SimBody* body_b,
CollisionInfo& cinfo,
const double dt);
CollisionInfo& cinfo);
void ApplyConstraintImpulse(
SimBody* body_a,
SimBody* body_b,

132
src/rbdlsim.cc
View File

@ -110,6 +110,9 @@ static void sCalcTangentVectors(const Vector3d &normal, Vector3d* tangent0, Vect
*tangent0 = normal.cross(Vector3d(1., 0., 0.));
*tangent1 = tangent0->cross(normal);
}
assert (tangent0->squaredNorm() > cCollisionEps);
assert (tangent1->squaredNorm() > cCollisionEps);
}
bool CheckPenetration(
@ -121,29 +124,29 @@ bool CheckPenetration(
bool result = false;
if (shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Plane) {
result = CheckPenetrationSphereVsPlane(shape_a, shape_b, cinfo);
sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1);
sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]);
return result;
} else if (
shape_b.mType == SimShape::Sphere && shape_a.mType == SimShape::Plane) {
result = CheckPenetrationSphereVsPlane(shape_b, shape_a, cinfo);
sSwapCollisionInfoShapeOrder(cinfo);
sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1);
sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]);
return result;
} else if (
shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Sphere) {
result = CheckPenetrationSphereVsSphere(shape_a, shape_b, cinfo);
sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1);
sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]);
return result;
} else if (
shape_a.mType == SimShape::Box && shape_b.mType == SimShape::Plane) {
result = CheckPenetrationBoxVsPlane(shape_a, shape_b, cinfo);
sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1);
sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]);
return result;
} else if (
shape_a.mType == SimShape::Plane && shape_b.mType == SimShape::Box) {
bool result = CheckPenetrationBoxVsPlane(shape_b, shape_a, cinfo);
sSwapCollisionInfoShapeOrder(cinfo);
sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1);
sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]);
return result;
}
@ -166,6 +169,8 @@ bool CheckPenetration(
cinfo.depth = depth;
}
sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]);
return !intersect;
}
@ -511,12 +516,14 @@ void CalcImpulseVariables(
unsigned int body_index,
const Vector3d& pos,
const Vector3d& dir,
const Vector3d* tangents,
const double depth,
VectorNd* MInvJacT,
VectorNd* jac,
double* G_MInv_GT,
MatrixNd* tangentJac,
MatrixNd* tangentGMInvGT,
VectorNd* tangentJac,
VectorNd* tangentMInvJacT,
double* tangentGMInvGT,
double* bias_vel,
double restitution) {
if (body == nullptr || body->mIsStatic) {
@ -546,18 +553,24 @@ void CalcImpulseVariables(
CalcPointJacobian(*model, q, body_index, point_local_b, G_constr, false);
(*jac) = dir.transpose() * G_constr;
(*MInvJacT) = M.llt().solve(jac->transpose());
SimpleMath::LLT<MatrixNd> M_llt = M.llt();
(*MInvJacT) = M_llt.solve(jac->transpose());
*G_MInv_GT = (*jac) * (*MInvJacT);
assert(!isnan(*G_MInv_GT));
double beta = 0.01;
double delta_slop = cCollisionEps;
*bias_vel = (*jac) * qdot * restitution - beta / dt * std::max (0., -depth - 0.05);
double delta_slop = 0.05;
*bias_vel = (*jac) * qdot * restitution - beta / dt * std::max (0., -depth - delta_slop);
(*tangentJac).resize(2,ndof);
(*tangentJac).block(0,0,1,ndof) = tangent0.transpose() * G_constr;
(*tangentJac).block(1,0,1,ndof) = tangent0.transpose() * G_constr;
(*tangentGMInvGT) = (*tangentJac) * (*MInv) * (*tangentJac).transpose();
tangentJac[0] = tangents[0].transpose() * G_constr;
tangentJac[1] = tangents[1].transpose() * G_constr;
tangentMInvJacT[0] = M_llt.solve(tangentJac[0].transpose());
tangentMInvJacT[1] = M_llt.solve(tangentJac[1].transpose());
tangentGMInvGT[0] = tangentJac[0] * tangentMInvJacT[0];
tangentGMInvGT[1] = tangentJac[1] * tangentMInvJacT[1];
assert (tangentGMInvGT[0] > 0.);
assert (tangentGMInvGT[1] > 0.);
}
void PrepareConstraintImpulse(
@ -571,12 +584,14 @@ void PrepareConstraintImpulse(
cinfo.mBodyAIndex,
cinfo.posA,
cinfo.dir,
cinfo.tangents,
cinfo.depth,
&cinfo.MInvJacTA,
&cinfo.jacA,
&cinfo.GMInvGTA,
&cinfo.tangentJacA,
&cinfo.tangentGMInvGTA,
cinfo.tangentJacA,
cinfo.tangentMInvJacTA,
cinfo.tangentGMInvGTA,
&cinfo.biasVelocityA,
cinfo.effectiveRestitution);
@ -586,43 +601,93 @@ void PrepareConstraintImpulse(
cinfo.mBodyBIndex,
cinfo.posB,
cinfo.dir,
cinfo.tangents,
-cinfo.depth,
&cinfo.MInvJacTB,
&cinfo.jacB,
&cinfo.GMInvGTB,
&cinfo.tangentJacB,
&cinfo.tangentGMInvGTB,
cinfo.tangentJacB,
cinfo.tangentMInvJacTB,
cinfo.tangentGMInvGTB,
&cinfo.biasVelocityB,
cinfo.effectiveRestitution);
}
/// Calculates the impulse that we apply on body_b to resolve the contact.
void CalcFrictionImpulse(
SimBody* body_a,
SimBody* body_b,
CollisionInfo& cinfo) {
// Todo: add nonlinear effects * dt
double rhs_tangent[2] = {0., 0.};
if (body_a && !body_a->mIsStatic) {
rhs_tangent[0] += cinfo.tangentJacA[0] * body_a->qdot;
rhs_tangent[1] += cinfo.tangentJacA[1] * body_a->qdot;
}
if (body_b && !body_b->mIsStatic) {
rhs_tangent[0] -= cinfo.tangentJacB[0] * body_b->qdot;
rhs_tangent[1] -= cinfo.tangentJacB[1] * body_b->qdot;
}
for (int i = 0; i < 2; i++) {
double denom = cinfo.tangentGMInvGTA[i] + cinfo.tangentGMInvGTB[i];
assert (denom > cCollisionEps);
double old_impulse = cinfo.accumFrictionImpulse[i];
cinfo.deltaFrictionImpulse[i] = rhs_tangent[i] / denom;
cinfo.accumFrictionImpulse[i] = cinfo.accumFrictionImpulse[i] + cinfo.deltaFrictionImpulse[i];
if (cinfo.accumFrictionImpulse[i] >= cinfo.effectiveFriction * cinfo.accumImpulse) {
cinfo.accumFrictionImpulse[i] = cinfo.effectiveFriction * cinfo.accumImpulse;
}
if (cinfo.accumFrictionImpulse[i] < -cinfo.effectiveFriction * cinfo.accumImpulse) {
cinfo.accumFrictionImpulse[i] = -cinfo.effectiveFriction * cinfo.accumImpulse;
}
cinfo.deltaFrictionImpulse[i] = cinfo.accumFrictionImpulse[i] - old_impulse;
assert (!isnan(cinfo.deltaFrictionImpulse[i]));
}
}
void ApplyFrictionImpulse(
SimBody* body_a,
SimBody* body_b,
CollisionInfo& cinfo) {
if (body_a && !body_a->mIsStatic) {
body_a->qdot +=
cinfo.tangentMInvJacTA[0] * (-cinfo.deltaFrictionImpulse[0]);
body_a->qdot +=
cinfo.tangentMInvJacTA[1] * (-cinfo.deltaFrictionImpulse[1]);
assert(!isnan(body_a->qdot.squaredNorm()));
}
if (body_b && !body_b->mIsStatic) {
body_b->qdot +=
-cinfo.tangentMInvJacTB[0] * (-cinfo.deltaFrictionImpulse[0]);
body_b->qdot +=
-cinfo.tangentMInvJacTB[1] * (-cinfo.deltaFrictionImpulse[1]);
assert(!isnan(body_b->qdot.squaredNorm()));
}
}
/// Calculates the impulse that we apply on body_b to resolve the contact.
void CalcConstraintImpulse(
SimBody* body_a,
SimBody* body_b,
CollisionInfo& cinfo,
const double dt) {
CollisionInfo& cinfo) {
// 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.;
if (body_a && !body_a->mIsStatic) {
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) {
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;
assert(denom > cCollisionEps);
@ -723,7 +788,10 @@ void World::resolveCollisions(double dt) {
int num_iter = 20;
for (int i = 0; i < num_iter; i++) {
for (CollisionInfo& cinfo : mContactPoints) {
CalcConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo, dt);
CalcFrictionImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
ApplyFrictionImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
CalcConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
ApplyConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
}
}

18
tests/CollisionTests.cc
View File

@ -275,7 +275,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
PrepareConstraintImpulse(0.001, &ground_body, &sphere_a_body, cinfo);
SECTION("EnsureImpulseDirection") {
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
REQUIRE(fabs(cinfo.deltaImpulse) > 1.0e-3);
REQUIRE(cinfo.deltaImpulse > -1.0e-12);
}
@ -289,7 +289,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
}
SECTION("CalculateImpulse") {
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
double reference_impulse = -sphere_a_mass * sphere_a_body.qdot[1];
REQUIRE(fabs(cinfo.accumImpulse - reference_impulse) < 1.0e-12);
ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
@ -298,7 +298,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
SECTION("ImpulseMustNotPull") {
sphere_a_body.qdot[1] = 1.23;
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
REQUIRE(fabs(cinfo.accumImpulse) < 1.0e-12);
}
@ -306,7 +306,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
cinfo.effectiveRestitution = 1.0;
PrepareConstraintImpulse(0.001, &ground_body, &sphere_a_body, cinfo);
VectorNd old_vel = sphere_a_body.qdot;
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
REQUIRE(fabs(sphere_a_body.qdot[1] + old_vel[1]) < 1.0e-12);
}
@ -332,7 +332,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
PrepareConstraintImpulse(0.001, &sphere_a_body, &ground_body, cinfo);
SECTION("EnsureImpulseDirection") {
CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
REQUIRE(fabs(cinfo.deltaImpulse) > 1.0e-3);
REQUIRE(cinfo.deltaImpulse > -1.0e-12);
}
@ -346,7 +346,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
}
SECTION("CalculateImpulse") {
CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
double reference_impulse = -sphere_a_mass * sphere_a_body.qdot[1];
REQUIRE(fabs(cinfo.accumImpulse - reference_impulse) < 1.0e-12);
ApplyConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
@ -390,7 +390,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
}
SECTION("CalculateImpulse") {
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
REQUIRE(sphere_a_body.qdot[1] > -0.1);
REQUIRE(sphere_b_body.qdot[1] < 0.1);
@ -401,7 +401,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
PrepareConstraintImpulse(0.001, &sphere_a_body, &sphere_b_body, cinfo);
VectorNd old_vel_a = sphere_a_body.qdot;
VectorNd old_vel_b = sphere_b_body.qdot;
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
REQUIRE(fabs(sphere_a_body.qdot[1] + old_vel_a[1]) < 1.0e-12);
REQUIRE(fabs(sphere_b_body.qdot[1] + old_vel_b[1]) < 1.0e-12);
@ -444,7 +444,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
}
SECTION("CalculateImpulse") {
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
REQUIRE(sphere_a_body.qdot[1] < 0.1);
REQUIRE(sphere_b_body.qdot[1] > -0.1);