sdf -> usd converter: add joints (#837)

Signed-off-by: Ashton Larkin <[email protected]>

Co-authored-by: ahcorde <[email protected]>

test/integration/joint_dom.cc

@@ -176,7 +176,7 @@ TEST(DOMJoint, LoadJointParentWorld)
   EXPECT_EQ(1u, model->LinkCount());
   EXPECT_NE(nullptr, model->LinkByIndex(0));
   EXPECT_EQ(nullptr, model->LinkByIndex(1));
-  EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), model->RawPose());
+  EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), model->RawPose());
   EXPECT_EQ("", model->PoseRelativeTo());
 
   ASSERT_TRUE(model->LinkNameExists("link"));

test/sdf/ball_prismatic_joint.sdf

@@ -0,0 +1,28 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="test">
+    <link name="link1">
+    </link>
+    <link name="link2">
+    </link>
+    <link name="link3">
+    </link>
+    <link name="link4">
+    </link>
+
+    <joint name="ball_joint" type="ball">
+      <pose>0 1 0 0 0 0</pose>
+      <child>link1</child>
+      <parent>link2</parent>
+    </joint>
+    <joint name="prismatic_joint" type="prismatic">
+      <pose>0 0 0 0 0 1</pose>
+      <child>link3</child>
+      <parent>link4</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+  </model>
+</sdf>

test/sdf/double_pendulum.sdf

@@ -197,6 +197,12 @@
       <child>upper_link</child>
       <axis>
         <xyz>1.0 0 0</xyz>
+        <dynamics>
+          <damping>35</damping>
+        </dynamics>
+        <limit>
+          <stiffness>350</stiffness>
+        </limit>
       </axis>
     </joint>
     <!-- pin joint for lower link, at origin of child link -->
@@ -205,6 +211,12 @@
       <child>lower_link</child>
       <axis>
         <xyz>1.0 0 0</xyz>
+        <dynamics>
+          <damping>35</damping>
+        </dynamics>
+        <limit>
+          <stiffness>350</stiffness>
+        </limit>
       </axis>
     </joint>
   </model>

test/sdf/joint_parent_world.sdf

@@ -1,6 +1,7 @@
 <?xml version="1.0" ?>
 <sdf version="1.6">
   <model name="joint_parent_world">
+    <pose>1 0 0 0 0 0</pose>
     <link name="link">
       <pose>0 0 1 0 0 0</pose>
     </link>

usd/include/sdf/usd/sdf_parser/Joint.hh

@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef SDF_USD_SDF_PARSER_JOINT_HH_
+#define SDF_USD_SDF_PARSER_JOINT_HH_
+
+#include <unordered_map>
+#include <string>
+
+// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings
+// are reported using #pragma message so normal diagnostic flags cannot remove
+// them. This workaround requires this block to be used whenever usd is
+// included.
+#pragma push_macro ("__DEPRECATED")
+#undef __DEPRECATED
+#include <pxr/usd/sdf/path.h>
+#include <pxr/usd/usd/stage.h>
+#pragma pop_macro ("__DEPRECATED")
+
+#include "sdf/Joint.hh"
+#include "sdf/Model.hh"
+#include "sdf/config.hh"
+#include "sdf/usd/Export.hh"
+#include "sdf/usd/UsdError.hh"
+
+namespace sdf
+{
+  // Inline bracke to help doxygen filtering.
+  inline namespace SDF_VERSION_NAMESPACE {
+  //
+  namespace usd
+  {
+    /// \brief Parse a SDF joint into a USD stage.
+    /// \param[in] _joint The SDF joint to parse.
+    /// \param[in] _stage The stage that should contain the USD representation
+    /// of _joint. This must be a valid, initialized stage.
+    /// \param[in] _path The USD path of the parsed joint in _stage, which must
+    /// be a valid USD path.
+    /// \param[in] _parentModel The model that is the parent of _joint
+    /// \param[in] _linkToUsdPath a map of a link's SDF name to the link's USD
+    /// path. This is used to determine which USD prims should be assigned as
+    /// the USD joint's relative links.
+    /// \param[in] _worldPath The USD path of the world prim. This is needed if
+    /// _joint's parent is the world.
+    /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError
+    /// includes an error code and message. An empty vector indicates no errors
+    /// occurred when parsing _joint to its USD representation.
+    UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfJoint(
+        const sdf::Joint &_joint,
+        pxr::UsdStageRefPtr &_stage, const std::string &_path,
+        const sdf::Model &_parentModel,
+        const std::unordered_map<std::string, pxr::SdfPath> &_linkToUsdPath,
+        const pxr::SdfPath &_worldPath);
+  }
+  }
+}
+
+#endif

usd/src/CMakeLists.txt

@@ -1,6 +1,7 @@
 set(sources
   UsdError.cc
   sdf_parser/Geometry.cc
+  sdf_parser/Joint.cc
   sdf_parser/Light.cc
   sdf_parser/Link.cc
   sdf_parser/Model.cc
@@ -25,6 +26,7 @@ target_link_libraries(${usd_target}
 set(gtest_sources
   sdf_parser/sdf2usd_TEST.cc
   sdf_parser/Geometry_Sdf2Usd_TEST.cc
+  sdf_parser/Joint_Sdf2Usd_TEST.cc
   sdf_parser/Light_Sdf2Usd_TEST.cc
   sdf_parser/Link_Sdf2Usd_TEST.cc
   # TODO(adlarkin) add a test for SDF -> USD models once model parsing