Add extra contact data test

Co-authored-by: Addisu Taddese <addisu@gmail.com>
Signed-off-by: Diego Ferigo <diego.ferigo@iit.it>

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/detail/GetContacts.hh

@@ -63,7 +63,6 @@ auto GetContactsFromLastStepFeature::World<
       contactOutput.template Get<ExtraContactData>() =
           std::move(*extraContactData);
     }
-
   }
   return output;
 }