Added restitution to CreateSphereBody, debugging impulse computation.

src/main.cc

@@ -8,7 +8,7 @@ using namespace RBDLSim;
 void simplesim() {
   World world;
   SimBody sphere_body =
-      CreateSphereBody(10., 1., Vector3d(0., 5.405, 0.), Vector3d::Zero());
+      CreateSphereBody(10., 1., 0., Vector3d(0., 5.405, 0.), Vector3d::Zero());
   world.mBodies.push_back(sphere_body);
 
   SimShape ground_shape;

src/rbdlsim.cc

@@ -338,6 +338,9 @@ void PrepareConstraintImpulse(
       &cinfo.MInvB,
       &cinfo.jacB,
       &cinfo.GMInvGTB);
+  cout << "jacA = " << cinfo.jacA << endl;
+  cout << "jacB = " << cinfo.jacB << endl;
+
 }
 
 void CalcConstraintImpulse(
@@ -367,121 +370,28 @@ void CalcConstraintImpulse(
 
   if (body_b && !body_b->mIsStatic) {
     double old_impulse = cinfo.accumImpulseB;
-    cinfo.deltaImpulseB = std::max(0., rhs / denom);
+    cinfo.deltaImpulseB = -rhs / denom;
     cinfo.accumImpulseB += cinfo.deltaImpulseB;
     cinfo.accumImpulseB = std::max(0., cinfo.accumImpulseB);
     cinfo.deltaImpulseB = cinfo.accumImpulseB - old_impulse;
-    gLog ("deltaImpulse: %f", cinfo.deltaImpulseB);
   }
 }
 
-void ApplyConstraintImpulse(
-    SimBody* body,
-    const MatrixNd& MInv,
-    const VectorNd& jac,
-    double impulse) {}
-
 void ApplyConstraintImpulse(
     SimBody* body_a,
     SimBody* body_b,
     CollisionInfo& cinfo) {
   if (body_a && !body_a->mIsStatic) {
     body_a->qdot += cinfo.MInvA * cinfo.jacA.transpose()
-                    * (-cinfo.deltaImpulseA);
+                    * (cinfo.deltaImpulseA);
   }
 
   if (body_b && !body_b->mIsStatic) {
     body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose()
-                    * (-cinfo.deltaImpulseB);
+                    * (cinfo.deltaImpulseB);
   }
 }
 
-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].posA,
-        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)*1. / dt + G * (qdot + Minv * dt * (tau - N));
-
-  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::calcUnconstrainedVelUpdate(double dt) {
   for (SimBody& body : mBodies) {
     ForwardDynamics(body.mModel, body.q, body.qdot, body.tau, body.qddot);
@@ -560,6 +470,7 @@ bool World::integrateWorld(double dt) {
 SimBody CreateSphereBody(
     double mass,
     double radius,
+    double restitution,
     const Vector3d& pos,
     const Vector3d& vel) {
   SimBody result;
@@ -596,7 +507,7 @@ SimBody CreateSphereBody(
   shape.pos = pos;
   shape.orientation.set(0., 0., 0., 1.);
   shape.scale.set(radius, radius, radius);
-  shape.restitution = 0.3;
+  shape.restitution = restitution;
 
   result.mCollisionShapes.push_back(
       SimBody::BodyCollisionInfo(sphere_body, shape));

src/simulator.cc

@@ -36,11 +36,11 @@ void simulator_init() {
   sWorld.mStaticShapes.push_back(sGroundShape);
 
   sSphereBody =
-      CreateSphereBody(1., 1., Vector3d(0., 1.405, 0.), Vector3d::Zero());
+      CreateSphereBody(1., 1., 0., Vector3d(0., 1.405, 0.), Vector3d::Zero());
   sWorld.mBodies.push_back(sSphereBody);
 
   sSphereBody2 =
-      CreateSphereBody(1., 1., Vector3d(0.3, 2.405, 0.), Vector3d::Zero());
+      CreateSphereBody(1., 1., 0., Vector3d(0.3, 2.405, 0.), Vector3d::Zero());
 //  sWorld.mBodies.push_back(sSphereBody2);
 
   sWorld.mSimTime = 0.;

tests/CollisionTests.cc

@@ -160,9 +160,9 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
   double sphere_b_mass = 1.5;
 
   SimBody sphere_a_body = CreateSphereBody(
-      sphere_a_mass, 1.0, Vector3d (0., 0.5, 0.), Vector3d (0., -1., 0.));
+      sphere_a_mass, 1.0, 0., Vector3d (0., 0.5, 0.), Vector3d (0., -1., 0.));
   SimBody sphere_b_body = CreateSphereBody(
-      sphere_b_mass, 1.0, Vector3d (0., 0.5, 0.), Vector3d (0., -1., 0.));
+      sphere_b_mass, 1.0, 0., Vector3d (0., 0.5, 0.), Vector3d (0., -1., 0.));
 
   CollisionInfo cinfo;
 
@@ -182,9 +182,10 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
     REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
 
     PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
-    CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
+    CalcConstraintImpulse(&ground_body,&sphere_a_body, cinfo, 0);
-    REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 0.)).norm() < 1.0e-12);
+    double reference_impulseB = sphere_a_mass * sphere_a_body.qdot[1];
-    REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_a_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12);
+    REQUIRE(cinfo.accumImpulseA < 1.0e-12);
+    REQUIRE(cinfo.accumImpulseB + reference_impulseB < 1.0e-12);
 
     ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
     REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12);
@@ -192,7 +193,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
 
   SECTION ("SphereVsSphereCollision") {
     double sphere_b_mass = 1.5;
-    SimBody sphere_b_body = CreateSphereBody(sphere_b_mass, 1.0, Vector3d (0., -0.5, 0.), Vector3d (0., 1., 0.));
+    SimBody sphere_b_body = CreateSphereBody(sphere_b_mass, 1.0, 0., Vector3d (0., -0.5, 0.), Vector3d (0., 1., 0.));
 
     sphere_a_body.q[1] = 0.5;
     sphere_a_body.qdot[1] = -1.23;
@@ -223,56 +224,4 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
     REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12);
     REQUIRE(sphere_b_body.qdot.norm() < 1.0e-12);
   }
-
-  SECTION ("DoubleSpheresOnGround") {
-    sphere_b_body = CreateSphereBody(
-        sphere_b_mass, 5.0, Vector3d (0., 1.5, 0.), Vector3d (0., -1., 0.));
-
-    sphere_a_body.q[1] = 0.5;
-    sphere_a_body.qdot[1] = -1.8;
-    sphere_a_body.updateCollisionShapes();
-
-    sphere_b_body.q[1] = 1.5;
-    sphere_b_body.qdot[1] = -1.23;
-    sphere_b_body.updateCollisionShapes();
-
-    std::vector<CollisionInfo> collisions;
-    CalcCollisions(sphere_a_body, sphere_b_body, collisions);
-    REQUIRE(collisions.size() == 1);
-    CollisionInfo sph_v_sph_cinfo = collisions[0];
-//    REQUIRE((sph_v_sph_cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
-
-    CalcCollisions(ground_body, sphere_a_body, collisions);
-    REQUIRE(collisions.size() == 1);
-    CollisionInfo sph_v_ground_cinfo = collisions[0];
-    REQUIRE((sph_v_ground_cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);
-
-    PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, sph_v_sph_cinfo);
-    PrepareConstraintImpulse(&ground_body, &sphere_a_body, sph_v_ground_cinfo);
-
-    int num_iter = 20;
-    for (int i = 0; i < num_iter; i++) {
-      cout << "-- Iter " << i << endl;
-
-      //    REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 0.)).norm() < 1.0e-12);
-      //    REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_a_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12);
-
-      cout << "pre impulse:  " << sphere_a_body.qdot.transpose() << endl;
-      cout << "pre impulse2: " << sphere_b_body.qdot.transpose() << endl;
-      CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, sph_v_sph_cinfo, 0);
-      ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, sph_v_sph_cinfo);
-      cout << "Sph v Sph Impulse: " << sph_v_sph_cinfo.accumImpulseB.transpose() * sph_v_sph_cinfo.dir << endl;
-      cout << "pst sph_v_sph impulse: " << sphere_a_body.qdot.transpose()
-           << endl;
-      cout << "pst sph_v_sph impulse2: " << sphere_b_body.qdot.transpose()
-           << endl;
-
-      CalcConstraintImpulse(&ground_body, &sphere_a_body, sph_v_ground_cinfo, 0);
-      ApplyConstraintImpulse(&ground_body, &sphere_a_body, sph_v_ground_cinfo);
-      cout << "Gnd v Sph Impulse: " << sph_v_ground_cinfo.accumImpulseB.transpose() * sph_v_ground_cinfo.dir << endl;
-
-      cout << "pst gnd_v_sph impulse: " << sphere_a_body.qdot.transpose()
-           << endl;
-    }
-  }
 }