Implemented tangential friction. Yay!

include/rbdlsim.h

@@ -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 accumFrictionImpulse[2] = {0.,0.};
-  double tangentAccumImpulse1 = 0.;
+  double deltaFrictionImpulse[2] = {0.,0.};
-  double tangentDeltaImpulse0 = 0.;
+  Vector3d tangents[2] = {Vector3d::Zero(), Vector3d::Zero()};
-  double tangentDeltaImpulse1 = 0.;
+  VectorNd tangentJacA[2] = {VectorNd::Zero(1), VectorNd::Zero(1)};
-  Vector3d tangent0 = Vector3d::Zero();
+  VectorNd tangentJacB[2] = {VectorNd::Zero(1), VectorNd::Zero(1)};
-  Vector3d tangent1 = Vector3d::Zero();
+  VectorNd tangentMInvJacTA[2] = {VectorNd::Zero(1), VectorNd::Zero(1)};
-  VectorNd tangentJacA = MatrixNd::Zero(2,2);
+  VectorNd tangentMInvJacTB[2] = {VectorNd::Zero(1), VectorNd::Zero(1)};
-  VectorNd tangentJacB = MatrixNd::Zero(2,2);
+  double tangentGMInvGTA[2] = {0., 0.};
-  MatrixNd tangentMInvA = MatrixNd::Zero(2, 2);
+  double tangentGMInvGTB[2] = {0., 0.};
-  MatrixNd tangentMInvB = MatrixNd::Zero(2, 2);
-  MatrixNd tangentGMInvGTA = MatrixNd::Zero(2,2);
-  MatrixNd tangentGMInvGTB = MatrixNd::Zero(2,2);
   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,
+    CollisionInfo& cinfo);
-    const double dt);
 void ApplyConstraintImpulse(
     SimBody* body_a,
     SimBody* body_b,

src/rbdlsim.cc

@@ -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,
+    VectorNd* tangentJac,
-    MatrixNd* tangentGMInvGT,
+    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;
+  double delta_slop = 0.05;
-  *bias_vel = (*jac) * qdot * restitution - beta / dt * std::max (0., -depth - 0.05);
+  *bias_vel = (*jac) * qdot * restitution - beta / dt * std::max (0., -depth - delta_slop);
 
-  (*tangentJac).resize(2,ndof);
+  tangentJac[0] = tangents[0].transpose() * G_constr;
-  (*tangentJac).block(0,0,1,ndof) = tangent0.transpose() * G_constr;
+  tangentJac[1] = tangents[1].transpose() * G_constr;
-  (*tangentJac).block(1,0,1,ndof) = tangent0.transpose() * G_constr;
+  tangentMInvJacT[0] = M_llt.solve(tangentJac[0].transpose());
-  (*tangentGMInvGT) = (*tangentJac) * (*MInv) * (*tangentJac).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.tangentJacA,
-      &cinfo.tangentGMInvGTA,
+      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.tangentJacB,
-      &cinfo.tangentGMInvGTB,
+      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,
+    CollisionInfo& cinfo) {
-    const double 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.;
   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);
     }
   }

tests/CollisionTests.cc

@@ -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);