Extend contact data with force, normal, and penetration depth

dartsim/src/SimulationFeatures.cc

@@ -21,6 +21,7 @@
 #include "SimulationFeatures.hh"
 
 #include "ignition/common/Profiler.hh"
+#include "ignition/physics/GetContacts.hh"
 
 namespace ignition {
 namespace physics {
@@ -82,8 +83,14 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const
       std::size_t shape2ID =
           this->shapes.IdentityOf(dtShapeFrame2->asShapeNode());
 
-      // TODO(addisu) Add normal, depth and wrench to extraData.
       CompositeData extraData;
+
+      // Add normal, depth and wrench to extraData.
+      auto &extraContactData = extraData.Get<ExtraContactData>();
+      extraContactData.force = dtContact.force;
+      extraContactData.normal = dtContact.normal;
+      extraContactData.depth = dtContact.penetrationDepth;
+
       outContacts.push_back(
           {this->GenerateIdentity(shape1ID, this->shapes.at(shape1ID)),
            this->GenerateIdentity(shape2ID, this->shapes.at(shape2ID)),

dartsim/src/SimulationFeatures_TEST.cc

@@ -54,6 +54,8 @@ struct TestFeatureList : ignition::physics::FeatureList<
 using TestWorldPtr = ignition::physics::World3dPtr<TestFeatureList>;
 using TestShapePtr = ignition::physics::Shape3dPtr<TestFeatureList>;
 using ContactPoint = ignition::physics::World3d<TestFeatureList>::ContactPoint;
+using ExtraContactData =
+    ignition::physics::World3d<TestFeatureList>::ExtraContactData;
 
 std::unordered_set<TestWorldPtr> LoadWorlds(
     const std::string &_library,
@@ -235,6 +237,12 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
     auto groundPlane = world->GetModel("ground_plane");
     auto groundPlaneCollision = groundPlane->GetLink(0)->GetShape(0);
 
+    // The first step already has contacts, but the contact force due to the
+    // impact does not match the steady-state force generated by the
+    // body's weight.
+    StepWorld(world);
+
+    // After a second step, the contact force reaches steady-state
     StepWorld(world);
 
     auto contacts = world->GetContactsFromLastStep();
@@ -256,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>();
@@ -269,19 +287,35 @@ 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));
+
+      // Check if the engine populated the extra contact data struct
+      const auto* extraContactData = contact.Query<ExtraContactData>();
+      ASSERT_NE(nullptr, extraContactData);
+
+      // The normal of the contact force is a vector pointing up (z positive)
+      EXPECT_NEAR(extraContactData->normal[0], 0.0, 1e-3);
+      EXPECT_NEAR(extraContactData->normal[1], 0.0, 1e-3);
+      EXPECT_NEAR(extraContactData->normal[2], 1.0, 1e-3);
+
+      // The contact force has only a z component and its value is
+      // 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);
+      EXPECT_NEAR(extraContactData->force[2],
+                  forceExpectations.at(testCollision), 1e-3);
     }
   }
 }

dartsim/worlds/contact.sdf

@@ -36,6 +36,9 @@
             <sphere><radius>1</radius></sphere>
           </geometry>
         </visual>
+        <inertial>
+          <mass>0.1</mass>
+        </inertial>
       </link>
       <link name="link1">
         <pose>0 1 0.5 0 0 0</pose>
@@ -49,6 +52,9 @@
             <sphere><radius>1</radius></sphere>
           </geometry>
         </visual>
+        <inertial>
+          <mass>1</mass>
+        </inertial>
       </link>
       <link name="link2">
         <pose>1 0 0.5 0 0 0</pose>
@@ -62,6 +68,9 @@
             <sphere><radius>1</radius></sphere>
           </geometry>
         </visual>
+        <inertial>
+          <mass>2</mass>
+        </inertial>
       </link>
       <link name="link3">
         <pose>1 1 0.5 0 0 0</pose>
@@ -75,6 +84,9 @@
             <sphere><radius>1</radius></sphere>
           </geometry>
         </visual>
+        <inertial>
+          <mass>3</mass>
+        </inertial>
       </link>
     </model>
   </world>

include/ignition/physics/GetContacts.hh

@@ -33,12 +33,30 @@ namespace physics
 class IGNITION_PHYSICS_VISIBLE GetContactsFromLastStepFeature
     : public virtual FeatureWithRequirements<ForwardStep>
 {
+  public: template <typename PolicyT>
+  struct ExtraContactDataT
+  {
+    using Scalar = typename PolicyT::Scalar;
+    using VectorType = typename FromPolicy<PolicyT>::template Use<Vector>;
+
+    /// \brief The contact force acting on the first body expressed
+    /// in the world frame
+    VectorType force;
+    /// \brief The normal of the force acting on the first body expressed
+    /// in the world frame
+    VectorType normal;
+    /// \brief The penetration depth
+    Scalar depth;
+  };
+
   public: template <typename PolicyT, typename FeaturesT>
   class World : public virtual Feature::World<PolicyT, FeaturesT>
   {
+    public: using Scalar = typename PolicyT::Scalar;
     public: using ShapePtrType = ShapePtr<PolicyT, FeaturesT>;
     public: using VectorType =
         typename FromPolicy<PolicyT>::template Use<Vector>;
+    public: using ExtraContactData = ExtraContactDataT<PolicyT>;
 
     public: struct ContactPoint
     {
@@ -50,7 +68,9 @@ class IGNITION_PHYSICS_VISIBLE GetContactsFromLastStepFeature
       VectorType point;
     };
 
-    public: using Contact = RequireData<ContactPoint>;
+    public: using Contact = SpecifyData<
+            RequireData<ContactPoint>,
+            ExpectData<ExtraContactData> >;
 
     /// \brief Get contacts generated in the previous simulation step
     public: std::vector<Contact> GetContactsFromLastStep() const;
@@ -59,8 +79,10 @@ class IGNITION_PHYSICS_VISIBLE GetContactsFromLastStepFeature
   public: template <typename PolicyT>
   class Implementation : public virtual Feature::Implementation<PolicyT>
   {
+    public: using Scalar = typename PolicyT::Scalar;
     public: using VectorType =
         typename FromPolicy<PolicyT>::template Use<Vector>;
+    public: using ExtraContactData = ExtraContactDataT<PolicyT>;
 
     public: struct ContactInternal
     {

include/ignition/physics/detail/GetContacts.hh

@@ -54,6 +54,15 @@ auto GetContactsFromLastStepFeature::World<
     //
     auto &contactOutput = output.emplace_back();
     contactOutput.template Get<ContactPoint>() = std::move(contactPoint);
+
+    auto *extraContactData =
+        contact.extraData.template Query<ExtraContactData>();
+
+    if (extraContactData)
+    {
+      contactOutput.template Get<ExtraContactData>() =
+          std::move(*extraContactData);
+    }
   }
   return output;
 }