Merge pull request #13 from ros/quarkytale/joint_support

Support for universal and ball joint in sdf
ros · Jun 20, 2022 · 8b7909b · 8b7909b
2 parents d3d8eb4 + b69f03c
commit 8b7909b
Show file tree

Hide file tree

Showing 7 changed files with 166 additions and 15 deletions.
diff --git a/sdformat_test_files/CMakeLists.txt b/sdformat_test_files/CMakeLists.txt
@@ -34,6 +34,7 @@ set(model_names
   "joint_fixed"
   "joint_gearbox"
   "joint_prismatic"
+  "joint_prismatic_no_axis"
   "joint_revolute"
   "joint_revolute2"
   "joint_revolute_axis"

diff --git a/sdformat_test_files/models/joint_prismatic_no_axis/joint_prismatic_no_axis.sdf b/sdformat_test_files/models/joint_prismatic_no_axis/joint_prismatic_no_axis.sdf
@@ -0,0 +1,65 @@
+<?xml version='1.0' encoding='utf-8'?>
+<sdf version="1.7">
+  <model name="joint_prismatic_no_axis">
+    <link name="link_1">
+      <visual name="link_1_visual">
+        <geometry>
+          <box>
+            <size>0.1 0.2 0.4</size>
+          </box>
+        </geometry>
+      </visual>
+      <collision name="link_1_collision">
+        <geometry>
+          <box>
+            <size>0.1 0.2 0.4</size>
+          </box>
+        </geometry>
+      </collision>
+      <inertial>
+        <mass>12.3</mass>
+        <inertia>
+          <ixx>0.205</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.17425</iyy>
+          <iyz>0</iyz>
+          <izz>0.05125</izz>
+        </inertia>
+      </inertial>
+    </link>
+    <link name="link_2">
+      <pose>0.1 0 0.1 0 0 0</pose>
+      <visual name="link_2_visual">
+        <geometry>
+          <box>
+            <size>0.1 0.2 0.3</size>
+          </box>
+        </geometry>
+      </visual>
+      <collision name="link_2_collision">
+        <geometry>
+          <box>
+            <size>0.1 0.2 0.3</size>
+          </box>
+        </geometry>
+      </collision>
+      <inertial>
+        <mass>1.23</mass>
+        <inertia>
+          <ixx>0.013325</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.01025</iyy>
+          <iyz>0</iyz>
+          <izz>0.005125</izz>
+        </inertia>
+      </inertial>
+    </link>
+
+    <joint name="joint_prismatic" type="prismatic">
+      <parent>link_1</parent>
+      <child>link_2</child>
+    </joint>
+  </model>
+</sdf>
diff --git a/sdformat_test_files/models/joint_prismatic_no_axis/model.config b/sdformat_test_files/models/joint_prismatic_no_axis/model.config
@@ -0,0 +1,19 @@
+<?xml version='1.0' encoding='utf-8'?>
+<model>
+  <name>joint_prismatic_no_axis</name>
+  <version>1.0</version>
+  <sdf version="1.7">joint_prismatic_no_axis.sdf</sdf>
+
+  <author>
+    <name>Shane Loretz</name>
+    <email>[email protected]</email>
+  </author>
+  <author>
+    <name>Dharini Dutia</name>
+    <email>[email protected]</email>
+  </author>
+
+  <description>
+  A model with two links connected by a prismatic joint having no axis described.
+  </description>
+</model>
diff --git a/sdformat_urdf/CMakeLists.txt b/sdformat_urdf/CMakeLists.txt
@@ -101,6 +101,7 @@ if(BUILD_TESTING)
   sdformat_test_files_get_model_sdf("path_to_sdf_joint_fixed" "joint_fixed")
   sdformat_test_files_get_model_sdf("path_to_sdf_joint_gearbox" "joint_gearbox")
   sdformat_test_files_get_model_sdf("path_to_sdf_joint_prismatic" "joint_prismatic")
+  sdformat_test_files_get_model_sdf("path_to_sdf_joint_prismatic_no_axis" "joint_prismatic_no_axis")
   sdformat_test_files_get_model_sdf("path_to_sdf_joint_revolute" "joint_revolute")
   sdformat_test_files_get_model_sdf("path_to_sdf_joint_revolute2" "joint_revolute2")
   sdformat_test_files_get_model_sdf("path_to_sdf_joint_revolute_axis" "joint_revolute_axis")

diff --git a/sdformat_urdf/src/sdformat_urdf.cpp b/sdformat_urdf/src/sdformat_urdf.cpp
@@ -494,22 +494,30 @@ sdformat_urdf::convert_joint(const sdf::Joint & sdf_joint, sdf::Errors & errors)
     case sdf::JointType::PRISMATIC:
       urdf_joint->type = urdf::Joint::PRISMATIC;
       break;
-    case sdf::JointType::INVALID:     // Unsupported: fall through to default
-    case sdf::JointType::BALL:        //  |
+    case sdf::JointType::UNIVERSAL:   // Unsupported: fall through to floating
+    case sdf::JointType::BALL:        //  Will require custom TF publisher
     case sdf::JointType::GEARBOX:     //  |
     case sdf::JointType::REVOLUTE2:   //  |
-    case sdf::JointType::SCREW:       //  |
-    case sdf::JointType::UNIVERSAL:   //  V
+    case sdf::JointType::SCREW:       //  V
+      urdf_joint->type = urdf::Joint::FLOATING;
+      break;
+    case sdf::JointType::INVALID:
     default:
       errors.emplace_back(
         sdf::ErrorCode::STRING_READ,
         "Unsupported joint type on joint [" + sdf_joint.Name() + "]");
       return nullptr;
   }
 
-  if (urdf::Joint::FIXED != urdf_joint->type) {
-    // Add axis info for non-fixed joints
+  if ((urdf::Joint::FIXED != urdf_joint->type) && (urdf::Joint::FLOATING != urdf_joint->type)) {
+    // Add axis info for non-fixed and non-floating joints
     const sdf::JointAxis * sdf_axis = sdf_joint.Axis(0);
+    if (nullptr == sdf_axis) {
+      errors.emplace_back(
+        sdf::ErrorCode::STRING_READ,
+        "Axes missing for joint [" + sdf_joint.Name() + "]");
+      return nullptr;
+    }
 
     // URDF expects axis to be expressed in the joint frame
     ignition::math::Vector3d axis_xyz;

diff --git a/sdformat_urdf/test/joint_tests.cpp b/sdformat_urdf/test/joint_tests.cpp
@@ -12,7 +12,6 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-
 #include <gtest/gtest.h>
 #include <urdf_model/model.h>
 #include <urdf_model/types.h>
@@ -28,9 +27,21 @@ TEST(Joint, joint_ball)
   sdf::Errors errors;
   urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse(
     get_file(PATH_TO_SDF_JOINT_BALL), errors);
-  EXPECT_FALSE(errors.empty());
+  EXPECT_TRUE(errors.empty()) << errors;
   EXPECT_NO_ALGORITHM_ERRORS(errors);
-  ASSERT_FALSE(model);
+  ASSERT_TRUE(model);
+  ASSERT_EQ("joint_ball", model->getName());
+
+  urdf::JointConstSharedPtr joint = model->getJoint("joint_ball");
+  ASSERT_NE(nullptr, joint);
+
+  EXPECT_EQ("joint_ball", joint->name);
+  EXPECT_EQ(urdf::Joint::FLOATING, joint->type);
+  ASSERT_EQ(nullptr, joint->dynamics);
+  ASSERT_EQ(nullptr, joint->limits);
+  ASSERT_EQ(nullptr, joint->safety);
+  ASSERT_EQ(nullptr, joint->calibration);
+  ASSERT_EQ(nullptr, joint->mimic);
 }
 
 TEST(Joint, joint_continuous)
@@ -120,6 +131,15 @@ TEST(Joint, joint_prismatic)
   ASSERT_EQ(nullptr, joint->mimic);
 }
 
+TEST(Joint, joint_prismatic_no_axis)
+{
+  sdf::Errors errors;
+  urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse(
+    get_file(PATH_TO_SDF_JOINT_PRISMATIC_NO_AXIS), errors);
+  EXPECT_FALSE(errors.empty());
+  ASSERT_FALSE(model);
+}
+
 TEST(Joint, joint_revolute)
 {
   sdf::Errors errors;
@@ -152,9 +172,21 @@ TEST(Joint, joint_revolute2)
   sdf::Errors errors;
   urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse(
     get_file(PATH_TO_SDF_JOINT_REVOLUTE2), errors);
-  EXPECT_FALSE(errors.empty());
+  EXPECT_TRUE(errors.empty()) << errors;
   EXPECT_NO_ALGORITHM_ERRORS(errors);
-  ASSERT_FALSE(model);
+  ASSERT_TRUE(model);
+  ASSERT_EQ("joint_revolute2", model->getName());
+
+  urdf::JointConstSharedPtr joint = model->getJoint("joint_revolute2");
+  ASSERT_NE(nullptr, joint);
+
+  EXPECT_EQ("joint_revolute2", joint->name);
+  EXPECT_EQ(urdf::Joint::FLOATING, joint->type);
+  ASSERT_EQ(nullptr, joint->dynamics);
+  ASSERT_EQ(nullptr, joint->limits);
+  ASSERT_EQ(nullptr, joint->safety);
+  ASSERT_EQ(nullptr, joint->calibration);
+  ASSERT_EQ(nullptr, joint->mimic);
 }
 
 TEST(Joint, joint_revolute_axis)
@@ -244,17 +276,41 @@ TEST(Joint, joint_screw)
   sdf::Errors errors;
   urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse(
     get_file(PATH_TO_SDF_JOINT_SCREW), errors);
-  EXPECT_FALSE(errors.empty());
+  EXPECT_TRUE(errors.empty()) << errors;
   EXPECT_NO_ALGORITHM_ERRORS(errors);
-  ASSERT_FALSE(model);
+  ASSERT_TRUE(model);
+  ASSERT_EQ("joint_screw", model->getName());
+
+  urdf::JointConstSharedPtr joint = model->getJoint("joint_screw");
+  ASSERT_NE(nullptr, joint);
+
+  EXPECT_EQ("joint_screw", joint->name);
+  EXPECT_EQ(urdf::Joint::FLOATING, joint->type);
+  ASSERT_EQ(nullptr, joint->dynamics);
+  ASSERT_EQ(nullptr, joint->limits);
+  ASSERT_EQ(nullptr, joint->safety);
+  ASSERT_EQ(nullptr, joint->calibration);
+  ASSERT_EQ(nullptr, joint->mimic);
 }
 
 TEST(Joint, joint_universal)
 {
   sdf::Errors errors;
   urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse(
     get_file(PATH_TO_SDF_JOINT_UNIVERSAL), errors);
-  EXPECT_FALSE(errors.empty());
+  EXPECT_TRUE(errors.empty()) << errors;
   EXPECT_NO_ALGORITHM_ERRORS(errors);
-  ASSERT_FALSE(model);
+  ASSERT_TRUE(model);
+  ASSERT_EQ("joint_universal", model->getName());
+
+  urdf::JointConstSharedPtr joint = model->getJoint("joint_universal");
+  ASSERT_NE(nullptr, joint);
+
+  EXPECT_EQ("joint_universal", joint->name);
+  EXPECT_EQ(urdf::Joint::FLOATING, joint->type);
+  ASSERT_EQ(nullptr, joint->dynamics);
+  ASSERT_EQ(nullptr, joint->limits);
+  ASSERT_EQ(nullptr, joint->safety);
+  ASSERT_EQ(nullptr, joint->calibration);
+  ASSERT_EQ(nullptr, joint->mimic);
 }
diff --git a/sdformat_urdf/test/sdf_paths.hpp.in b/sdformat_urdf/test/sdf_paths.hpp.in
@@ -35,6 +35,7 @@
 #define PATH_TO_SDF_JOINT_FIXED "@path_to_sdf_joint_fixed@"
 #define PATH_TO_SDF_JOINT_GEARBOX "@path_to_sdf_joint_gearbox@"
 #define PATH_TO_SDF_JOINT_PRISMATIC "@path_to_sdf_joint_prismatic@"
+#define PATH_TO_SDF_JOINT_PRISMATIC_NO_AXIS "@path_to_sdf_joint_prismatic_no_axis@"
 #define PATH_TO_SDF_JOINT_REVOLUTE "@path_to_sdf_joint_revolute@"
 #define PATH_TO_SDF_JOINT_REVOLUTE2 "@path_to_sdf_joint_revolute2@"
 #define PATH_TO_SDF_JOINT_REVOLUTE_AXIS "@path_to_sdf_joint_revolute_axis@"