BallJoint: apply axis properties to ball joints

dartsim/src/SDFFeatures.cc

@@ -18,6 +18,7 @@
 #include "SDFFeatures.hh"
 
 #include <cmath>
+#include <cstddef>
 #include <limits>
 #include <memory>
 #include <string>
@@ -245,6 +246,36 @@ static dart::dynamics::UniversalJoint *ConstructUniversalJoint(
   return _child->moveTo<dart::dynamics::UniversalJoint>(_parent, properties);
 }
 
+/////////////////////////////////////////////////
+template <typename JointType>
+static JointType *ConstructBallJoint(
+    const ModelInfo &/*_modelInfo*/,
+    const ::sdf::Joint &_sdfJoint,
+    dart::dynamics::BodyNode * const _parent,
+    dart::dynamics::BodyNode * const _child,
+    const Eigen::Isometry3d &/*_T_joint*/)
+{
+  // SDF does not support any of the properties for ball joint, besides the
+  // name and relative transforms to its parent and child.
+  //
+  // To set other properties like joint limits, stiffness, etc,
+  // apply values in <axis> to all 3 DoF.
+  typename JointType::Properties properties;
+
+  const ::sdf::JointAxis * const sdfAxis = _sdfJoint.Axis(0);
+
+  // use default properties if sdfAxis is not set, otherwise apply to all DoF
+  if (sdfAxis)
+  {
+    for (const std::size_t index : {0u, 1u, 2u})
+    {
+      CopyStandardJointAxisProperties(index, properties, sdfAxis);
+    }
+  }
+
+  return _child->moveTo<JointType>(_parent, properties);
+}
+
 /////////////////////////////////////////////////
 struct ShapeAndTransform
 {
@@ -1138,11 +1169,8 @@ Identity SDFFeatures::ConstructSdfJoint(
 
   if (::sdf::JointType::BALL == type)
   {
-    // SDF does not support any of the properties for ball joint, besides the
-    // name and relative transforms to its parent and child, which will be taken
-    // care of below. All other properties like joint limits, stiffness, etc,
-    // will be the default values of +/- infinity or 0.0.
-    joint = _child->moveTo<dart::dynamics::BallJoint>(_parent);
+    joint = ConstructBallJoint<dart::dynamics::BallJoint>(
+          _modelInfo, _sdfJoint, _parent, _child, T_joint);
   }
   // TODO(MXG): Consider adding dartsim support for a CONTINUOUS joint type.
   // Alternatively, support the CONTINUOUS joint type by wrapping the

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(8u, dartWorld->getNumSkeletons());
+  ASSERT_EQ(9u, 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"/>
@@ -453,7 +464,7 @@
             <ambient>1 1 0 1</ambient>
           </material>
         </visual>
-      
+
         <visual name="vis_cylinder">
           <pose>0 0 0.5 0 0 0</pose>
           <geometry>
@@ -482,7 +493,7 @@
             </friction>
           </surface>
         </collision>
-    
+
         <collision name="col_cylinder">
           <pose>0 0 0.5 0 0 0</pose>
           <geometry>
@@ -493,7 +504,7 @@
           </geometry>
         </collision>
       </link>
-      
+
       <!-- pin joint for upper link, at origin of upper link -->
       <joint name="upper_joint" type="revolute">
         <parent>base</parent>
@@ -502,7 +513,7 @@
           <xyz>1.0 0 0</xyz>
         </axis>
       </joint>
-     
+
     </model>
   </world>
 </sdf>

tpe/plugin/src/SDFFeatures_TEST.cc

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