Basic but not yet working LCP time-stepping based collision response

include/rbdlsim.h

@@ -11,6 +11,10 @@ namespace RBDLSim {
 using namespace RigidBodyDynamics;
 using namespace RigidBodyDynamics::Math;
 
+struct SimShape;
+struct SimBody;
+struct CollisionInfo;
+
 struct SimShape {
   enum ShapeType {
     Box = 0,
@@ -30,6 +34,8 @@ struct SimBody {
   std::vector<BodyCollisionInfo> mCollisionShapes;
 
   void step(double ts);
+  void resolveCollisions (double dt, std::vector<CollisionInfo>& collisions);
+  void calcNextPositions (double dt, const VectorNd& in_qdot, VectorNd& out_q);
   void updateCollisionShapes();
 };
 
@@ -38,8 +44,11 @@ struct CollisionInfo {
   const SimBody* mBodyB;
   const SimShape* mShapeA;
   const SimShape* mShapeB;
+  int mBodyAIndex;
+  int mBodyBIndex;
   Vector3d pos;
   Vector3d dir;
+  double depth;
 };
 
 struct World {
@@ -50,7 +59,8 @@ struct World {
 
   void updateCollisionShapes();
   void detectCollisions();
-  bool step(double dt);
+  void resolveCollisions(double dt);
+  bool integrateWorld(double dt);
 };
 
 bool CheckPenetration(

src/main.cc

@@ -9,7 +9,6 @@ void simplesim() {
   World world;
   SimBody sphere_body =
       CreateSphereBody(10., 1., Vector3d(0., 5.405, 0.), Vector3d::Zero());
-  sphere_body.qdot[3] = 1 * M_PI;
   world.mBodies.push_back(sphere_body);
 
   SimShape ground_shape;
@@ -23,14 +22,13 @@ void simplesim() {
   world.mSimTime = 0.;
   cout << world.mBodies[0].q.transpose() << endl;
 
+  double dt = 1.0e-3;
   do {
     world.updateCollisionShapes();
     world.detectCollisions();
-    world.step(1.0e-3);
+    world.resolveCollisions(dt);
-  } while (world.mContactPoints.size() == 0);
+    world.integrateWorld(dt);
-
+  } while (world.mSimTime < 1.01);
-  cout << "Collision at t = " << world.mSimTime << endl
-       << "q = " << world.mBodies[0].q.transpose() << endl;
 }
 
 int main(int argc, char* argv[]) {

src/rbdlsim.cc

@@ -15,25 +15,28 @@ using namespace std;
 namespace RBDLSim {
 
 void SimBody::step(double dt) {
-  ForwardDynamics(mModel, q, qdot, tau, qddot);
+  // Prerequisite: qdot has already been updated by resolveCollisions();
-
+  calcNextPositions(dt, qdot, q);
-  // semi-implicit eulers
+}
-  qdot += dt * qddot;
 
+void SimBody::calcNextPositions(
+    double dt,
+    const VectorNd& in_qdot,
+    VectorNd& io_q) {
   for (int i = 1; i < mModel.mJoints.size(); ++i) {
     const Joint& joint = mModel.mJoints[i];
     if (joint.mJointType != JointTypeSpherical) {
-      q.block(joint.q_index, 0, joint.mDoFCount, 1) +=
+      io_q.block(joint.q_index, 0, joint.mDoFCount, 1) +=
-          dt * qdot.block(joint.q_index, 0, joint.mDoFCount, 1);
+          dt * in_qdot.block(joint.q_index, 0, joint.mDoFCount, 1);
       continue;
     }
 
     // Integrate the Quaternion
-    Quaternion q0 = mModel.GetQuaternion(i, q);
+    Quaternion q0 = mModel.GetQuaternion(i, io_q);
-    Vector3d omega(qdot.block(joint.q_index, 0, 3, 1));
+    Vector3d omega(in_qdot.block(joint.q_index, 0, 3, 1));
     Quaternion qd = q0.omegaToQDot(omega);
     Quaternion q1 = (q0 + qd * dt).normalize();
-    mModel.SetQuaternion(i, q1, q);
+    mModel.SetQuaternion(i, q1, io_q);
   }
 }
 
@@ -105,6 +108,7 @@ bool CheckPenetration(
   if (intersect == 0) {
     cinfo.pos.set(pos.v[0], pos.v[1], pos.v[2]);
     cinfo.dir.set(dir.v[0], dir.v[1], dir.v[2]);
+    cinfo.depth = depth;
   }
 
   return !intersect;
@@ -124,6 +128,139 @@ void SimBody::updateCollisionShapes() {
   }
 }
 
+bool SolveGaussSeidelProj(
+    MatrixNd& A,
+    VectorNd& b,
+    VectorNd& x,
+    VectorNd& xlo,
+    VectorNd& xhi,
+    double tol = 1.0e-6,
+    int maxiter = 100) {
+  int n = A.cols();
+  for (int iter = 0; iter < maxiter; iter++) {
+    double residual = 0.;
+    for (int i = 0; i < n; i++) {
+      double sigma = 0.;
+      for (int j = 0; j < i; j++) {
+        sigma += A(i, j) * x[j];
+      }
+      for (int j = i + 1; j < n; j++) {
+        sigma += A(i, j) * x[j];
+      }
+      double x_old = x[i];
+      x[i] = (b[i] - sigma) / A(i, i);
+
+      // Projection onto admissible values
+      if (x[i] < xlo[i]) {
+        x[i] = xlo[i];
+      }
+      if (x[i] > xhi[i]) {
+        x[i] = xhi[i];
+      }
+
+      double diff = x[i] - x_old;
+      residual += diff * diff;
+    }
+
+    if (residual < tol) {
+      cout << "Numiter: " << iter << endl;
+      return true;
+    }
+
+    if  (iter > maxiter) {
+      break;
+    }
+  }
+
+  return false;
+}
+
+void SimBody::resolveCollisions(
+    double dt,
+    std::vector<CollisionInfo>& collisions) {
+  if (collisions.size() == 0) {
+    // No contacts, calculate new qdot using simple forward
+    // dynamics
+    ForwardDynamics(mModel, q, qdot, tau, qddot);
+
+    // semi-implicit eulers
+    qdot += dt * qddot;
+
+    return;
+  }
+
+  int ndof = mModel.qdot_size;
+  int nconstraints = collisions.size();
+
+  // Allocate space for the constraint system
+  MatrixNd M(MatrixNd::Zero(ndof, ndof));
+  MatrixNd Minv(MatrixNd::Zero(ndof, ndof));
+
+  VectorNd N(VectorNd::Zero(ndof));
+  MatrixNd G(MatrixNd::Zero(nconstraints, ndof));
+  VectorNd gamma(VectorNd::Zero(nconstraints));
+
+  // Calculate local coordinates of the contact point
+  std::vector<Vector3d> pos_local;
+  VectorNd constr_value(VectorNd::Zero(nconstraints));
+  UpdateKinematicsCustom(mModel, &q, nullptr, nullptr);
+  for (int i = 0; i < nconstraints; i++) {
+    constr_value[i] = -collisions[i].depth;
+    pos_local.push_back(CalcBaseToBodyCoordinates(
+        mModel,
+        q,
+        collisions[i].mBodyAIndex,
+        collisions[i].pos,
+        false));
+  }
+
+  // Calculate predicted position state
+  VectorNd q_next_pred(q);
+  calcNextPositions(dt, qdot, q_next_pred);
+
+  // Compute vectors and matrices of the contact system
+  NonlinearEffects(mModel, q_next_pred, qdot, N);
+  CompositeRigidBodyAlgorithm(mModel, q_next_pred, M, false);
+  Minv = M.inverse();
+
+  // Calculate contact Jacobians
+  for (int i = 0; i < nconstraints; i++) {
+    MatrixNd G_constr(MatrixNd::Zero(3, ndof));
+    CalcPointJacobian(
+        mModel,
+        q_next_pred,
+        collisions[i].mBodyAIndex,
+        pos_local[i],
+        G_constr,
+        false);
+    G.block(i, 0, 1, ndof) = collisions[i].dir.transpose() * G_constr;
+  }
+
+  MatrixNd A(MatrixNd::Zero(nconstraints, nconstraints));
+  VectorNd b(VectorNd::Zero(nconstraints));
+
+  // Solve for the impules hlambda
+  A = G * Minv * G.transpose();
+  b = (constr_value + VectorNd::Constant(nconstraints, 0.0001) ) * 1. / dt + G * (qdot + Minv * dt * (tau - N)) * -1.0;
+
+  VectorNd hlambda (VectorNd::Zero(nconstraints));
+  VectorNd hlambda_lo (VectorNd::Constant(nconstraints, 0.));
+  VectorNd hlambda_hi (VectorNd::Constant(nconstraints, 1.0e9));
+
+  bool solve_result = false;
+  solve_result = SolveGaussSeidelProj(A, b, hlambda, hlambda_lo, hlambda_hi);
+
+//  VectorNd hlambda = A.colPivHouseholderQr().solve(b) * -1.;
+
+  if (!solve_result) {
+    cout << "Impulse Solve Failed!! " << endl;
+  }
+  cout << "Contact impulse: " << hlambda.transpose() << endl;
+
+  // solve for the new velocity
+  qdot = qdot + Minv * (dt * tau + dt * N + G.transpose() * hlambda);
+}
+
 void World::updateCollisionShapes() {
   for (SimBody& body : mBodies) {
     body.updateCollisionShapes();
@@ -146,7 +283,9 @@ void World::detectCollisions() {
 
         if (has_penetration) {
           cinfo.mBodyA = nullptr;
+          cinfo.mBodyAIndex = -1;
           cinfo.mBodyB = &body;
+          cinfo.mBodyBIndex = body_col_info.first;
           mContactPoints.push_back(cinfo);
         }
       }
@@ -154,7 +293,50 @@ void World::detectCollisions() {
   }
 }
 
-bool World::step(double dt) {
+void World::resolveCollisions(double dt) {
+  // so far only solve Body vs World collisions
+  for (SimBody& body : mBodies) {
+    // collect all collisions for current body
+    std::vector<CollisionInfo> body_collisions;
+
+    for (const CollisionInfo cinfo : mContactPoints) {
+      if (cinfo.mBodyA == &body) {
+        body_collisions.push_back(cinfo);
+      } else if (cinfo.mBodyB == &body) {
+        // Make sure the collision info is expressed in terms
+        // of mBodyA.
+        CollisionInfo rev_cinfo;
+
+        rev_cinfo.mBodyA = &body;
+        rev_cinfo.mShapeA = cinfo.mShapeB;
+        rev_cinfo.mBodyAIndex = cinfo.mBodyBIndex;
+
+        rev_cinfo.mBodyB = cinfo.mBodyA;
+        rev_cinfo.mShapeB = cinfo.mShapeA;
+        rev_cinfo.mBodyBIndex = cinfo.mBodyAIndex;
+
+        rev_cinfo.pos = cinfo.pos;
+        rev_cinfo.dir = -cinfo.dir;
+        rev_cinfo.depth = cinfo.depth;
+
+        body_collisions.push_back(rev_cinfo);
+      }
+    }
+
+    if (body_collisions.size() > 0) {
+      cout << "Collision at t = " << mSimTime
+           << ", pos = " << body_collisions[0].pos.transpose()
+           << ", depth = " << body_collisions[0].depth << endl
+           << " qdotpre  = " << mBodies[0].qdot.transpose() << endl;
+    }
+    body.resolveCollisions(dt, body_collisions);
+    if (body_collisions.size() > 0) {
+      cout << " qdotpost = " << mBodies[0].qdot.transpose() << endl;
+    }
+  }
+}
+
+bool World::integrateWorld(double dt) {
   for (SimBody& body : mBodies) {
     body.step(dt);
   }