Extend contact data with force, normal, and penetration depth

[diegoferigo]

This PR provides the support of exporting optional contact data that the physics engine might provide. In particular:

1. Contact force
2. Contact normal
3. Penetration depth

This PR also extends the DART implementation to fill the new data structure containing the additional contact data.

I'm not yet very familiar with all the templates of ign-physics. This is the first working implementation I managed to obtain, I'm not sure if there's a better way to handle the data exchanged between the public APIs and the implementation method.

In the current form, ABI is likely broken. Given the field CompositeData extraData, I'm not sure if there's any other way to get that data without the need to define a new public struct.

Closes #24.

[diegoferigo]

cc @azeey @scpeters

[diegoferigo]

Related issue for visualizing contacts gazebosim/gz-sim#112

[azeey]

Thanks for the contribution @diegoferigo. This looks good so far. Would you be able to add some tests for the extra contact data?

Also left some nitpick comments.

[azeey]

Friendly ping @diegoferigo. If it's okay with you, I can commit those suggestions and merge this PR.

[diegoferigo]

Hi @azeey, thanks for the review and sorry for the delay. The requested changes are trivial and I just updated this PR.

I was trying to find some time to address the tests request. As far as I saw, in the past a common pattern to test physics features was opening a corresponding PR in ign-gazebo and add a test there. In this case it would also require updating the Physics system to extract the new contact data. I haven't yet found enough time to start this process.

[azeey]

Oh my bad, I forgot I had asked for some tests. The tests don't have to be in ign-gazebo though. There is already a test related to contacts in https://github.com/ignitionrobotics/ign-physics/blob/master/dartsim/src/SimulationFeatures_TEST.cc#L175. Maybe you add to that?

[diegoferigo]

Thanks for the link, I'll have a look to the existing test and check how to update it with the extra contact data.

[diegoferigo]

@azeey in 03ef01a I extended the existing test and added a check of the contact normal and contact force.

I haven't yet figured out how to extract the gravity vector from the world object, and the mass from each sphere->GetLink(•). Do you have any suggestion? If it's easier editing directly the commit instead of writing how to do it, go ahead :)

If the mass is not specified explicitly in the SDF, 1kg is assumed. If that's ok for you, I could also hardcode 1.0 * 9.80 even if it would be less robust.

[azeey]

@diegoferigo, I'm okay with hardcoding the mass values since we don't have a convenient way to retrieve them right now. Looks like I don't have permission to push to your branch directly. Here's the patch I was going to push?

diff --git a/dartsim/src/SimulationFeatures_TEST.cc b/dartsim/src/SimulationFeatures_TEST.cc
index 478decb..265a7b7 100644
--- a/dartsim/src/SimulationFeatures_TEST.cc
+++ b/dartsim/src/SimulationFeatures_TEST.cc
@@ -264,6 +264,16 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
       {sphere->GetLink(3)->GetShape(0), {1.0, 1.0, 0.0}},
     };
 
+    const double gravity = 9.8;
+    std::map<TestShapePtr, double> forceExpectations
+    {
+      // Contact force expectations are: link mass * gravity.
+      {sphere->GetLink(0)->GetShape(0), 0.1 * gravity},
+      {sphere->GetLink(1)->GetShape(0), 1.0 * gravity},
+      {sphere->GetLink(2)->GetShape(0), 2.0 * gravity},
+      {sphere->GetLink(3)->GetShape(0), 3.0 * gravity},
+    };
+
     for (auto &contact : contacts)
     {
       const auto &contactPoint = contact.Get<ContactPoint>();
@@ -277,17 +287,17 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
       EXPECT_NE(contactPoint.collision1, contactPoint.collision2);
 
       Eigen::Vector3d expectedContactPos = Eigen::Vector3d::Zero();
-      // One of the two collisions is the ground plane and the other is the
-      // collision we're interested in.
-      try
-      {
-        expectedContactPos = expectations.at(contactPoint.collision1);
-      }
-      catch (...)
+
+      // The test expectations are all on the collision that is not the ground
+      // plane.
+      auto testCollision = contactPoint.collision1;
+      if (testCollision == groundPlaneCollision)
       {
-        expectedContactPos = expectations.at(contactPoint.collision2);
+        testCollision = contactPoint.collision2;
       }
 
+      expectedContactPos = expectations.at(testCollision);
+
       EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos,
                                                  contactPoint.point, 1e-6));
 
@@ -304,9 +314,8 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
       // the the weight of the sphere times the gravitational acceleration
       EXPECT_NEAR(extraContactData->force[0], 0.0, 1e-3);
       EXPECT_NEAR(extraContactData->force[1], 0.0, 1e-3);
-      // mass = ?
-      // gravity = ?
-      // EXPECT_NEAR(extraContactData->force[2], mass * gravity, 1e-3);
+      EXPECT_NEAR(extraContactData->force[2],
+                  forceExpectations.at(testCollision), 1e-3);
     }
   }
 }
diff --git a/include/ignition/physics/detail/GetContacts.hh b/include/ignition/physics/detail/GetContacts.hh
index 0de81b2..80e7584 100644
--- a/include/ignition/physics/detail/GetContacts.hh
+++ b/include/ignition/physics/detail/GetContacts.hh
@@ -63,7 +63,6 @@ auto GetContactsFromLastStepFeature::World<
       contactOutput.template Get<ExtraContactData>() =
           std::move(*extraContactData);
     }
-
   }
   return output;
 }

[diegoferigo]

Looks like I don't have permission to push to your branch directly.

Mmh strange.

I'm okay with hardcoding the mass values since we don't have a convenient way to retrieve them right now.

Awesome, thanks a lot for the patch @azeey! Branch updated.

[azeey]

Thanks for the contribution!

[azeey]

@osrf-jenkins run tests

[azeey]

Looks like a test is failing. I'm looking into it.

[diegoferigo]

CI went green, thanks a lot @azeey for the fix in #77 🚀