[WIP] Add extra contact data test

gazebosim · Jun 13, 2020 · 03ef01a · 03ef01a
1 parent 47dd41b
commit 03ef01a
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 0 deletions.
diff --git a/dartsim/src/SimulationFeatures_TEST.cc b/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();
@@ -282,6 +290,23 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts)
 
       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);
+      // mass = ?
+      // gravity = ?
+      // EXPECT_NEAR(extraContactData->force[2], mass * gravity, 1e-3);
     }
   }
 }

diff --git a/dartsim/worlds/contact.sdf b/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>