BallJoint: add test

Signed-off-by: Ian Chen <ichen@openrobotics.org>

Author: iche033

dartsim/src/SDFFeatures_TEST.cc

@@ -17,6 +17,7 @@
 
 #include <type_traits>
 
+#include <dart/dynamics/BallJoint.hpp>
 #include <dart/dynamics/BodyNode.hpp>
 #include <dart/dynamics/DegreeOfFreedom.hpp>
 #include <dart/dynamics/FreeJoint.hpp>
@@ -321,7 +322,7 @@ TEST_P(SDFFeatures_TEST, CheckDartsimData)
   dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld();
   ASSERT_NE(nullptr, dartWorld);
 
-  ASSERT_EQ(7u, dartWorld->getNumSkeletons());
+  ASSERT_EQ(8u, dartWorld->getNumSkeletons());
 
   const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton(1);
   ASSERT_NE(nullptr, skeleton);
@@ -417,6 +418,14 @@ TEST_P(SDFFeatures_TEST, CheckDartsimData)
       screwJointTest->getJoint(1));
   ASSERT_NE(nullptr, screwJoint);
   EXPECT_DOUBLE_EQ(-GZ_PI, screwJoint->getPitch());
+
+  const dart::dynamics::SkeletonPtr ballJointTest =
+      dartWorld->getSkeleton("ball_joint_test");
+  ASSERT_NE(nullptr, ballJointTest);
+  ASSERT_EQ(2u, ballJointTest->getNumBodyNodes());
+  const auto *ballJoint = dynamic_cast<const dart::dynamics::BallJoint*>(
+      ballJointTest->getJoint(1));
+  ASSERT_NE(nullptr, ballJoint);
 }
 
 /////////////////////////////////////////////////

test/common_test/worlds/test.world

@@ -358,6 +358,17 @@
         <thread_pitch>2</thread_pitch>
       </joint>
     </model>
+    <model name="ball_joint_test">
+      <link name="link0"/>
+      <link name="link1"/>
+      <joint name="j0" type="ball">
+        <parent>link0</parent>
+        <child>link1</child>
+        <axis>
+          <damping>0.1</damping>
+        </axis>
+      </joint>
+    </model>
     <!-- Remove joint types as they get supported -->
     <model name="unsupported_joint_test">
       <link name="link0"/>

tpe/plugin/src/SDFFeatures_TEST.cc

@@ -95,7 +95,7 @@ TEST(SDFFeatures_TEST, CheckTpeData)
   auto tpeWorld = world.GetTpeLibWorld();
   ASSERT_NE(nullptr, tpeWorld);
 
-  ASSERT_EQ(7u, tpeWorld->GetChildCount());
+  ASSERT_EQ(8u, tpeWorld->GetChildCount());
 
   // check model 01
   {