Normalize joint axis xyz vector when parsing from SDFormat

Changelog.md

@@ -4,6 +4,9 @@
 
 ### libsdformat 10.0.0 (202X-XX-XX)
 
+1. Normalize joint axis xyz vector when parsing from SDFormat
+    * [Pull request 312](https://github.com/osrf/sdformat/pull/312)
+
 1. Enforce minimum/maximum values specified in SDFormat description files.
     * [Pull request 303](https://github.com/osrf/sdformat/pull/303)
 

Migration.md

@@ -16,6 +16,10 @@ but with improved human-readability..
 
 ### Modifications
 
+1. Axis vectors specified in <joint><axis><xyz> are normalized if their norm is
+   greater than 0. A vector with 0 norm generates an error
+    * [Pull request 312](https://github.com/osrf/sdformat/pull/312)
+
 1. + Minimum/maximum values specified in SDFormat description files are now enforced
    + [Pull request 303](https://github.com/osrf/sdformat/pull/303)
 

src/JointAxis.cc

@@ -138,6 +138,15 @@ Errors JointAxis::Load(ElementPtr _sdf)
   {
     this->dataPtr->xyz = _sdf->Get<ignition::math::Vector3d>("xyz",
         ignition::math::Vector3d::UnitZ).first;
+    if (sdf::equal(this->dataPtr->xyz.Length(), 0.0))
+    {
+      errors.push_back({ErrorCode::ELEMENT_INVALID,
+                        "The norm of the xyz vector cannot be zero"});
+    }
+    else
+    {
+      this->dataPtr->xyz.Normalize();
+    }
     auto e = _sdf->GetElement("xyz");
     if (e->HasAttribute("expressed_in"))
     {

test/integration/joint_axis_dom.cc

@@ -219,3 +219,54 @@ TEST(DOMJointAxis, XyzExpressedIn)
   EXPECT_EQ(0u, model->FrameCount());
   EXPECT_EQ(nullptr, model->FrameByIndex(0));
 }
+
+//////////////////////////////////////////////////
+TEST(DOMJointAxis, XyzNormalization)
+{
+  const std::string testFile =
+    sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf",
+        "joint_axis_xyz_normalization.sdf");
+
+  // Load the SDF file
+  sdf::Root root;
+  sdf::Errors errors = root.Load(testFile);
+
+  ASSERT_EQ(1u, errors.size());
+  EXPECT_EQ("The norm of the xyz vector cannot be zero", errors[0].Message());
+
+  using ignition::math::Vector3d;
+
+  // Get the first model
+  const sdf::Model *model = root.ModelByIndex(0);
+  ASSERT_NE(nullptr, model);
+
+  {
+    auto joint1 = model->JointByName("joint1");
+    ASSERT_FALSE(nullptr == joint1);
+    ASSERT_FALSE(nullptr == joint1->Axis(0));
+    EXPECT_EQ(Vector3d::UnitZ, joint1->Axis(0)->Xyz());
+  }
+
+  {
+    auto joint2 = model->JointByName("joint2");
+    ASSERT_FALSE(nullptr == joint2);
+    ASSERT_FALSE(nullptr == joint2->Axis(0));
+    EXPECT_EQ(Vector3d::UnitX, joint2->Axis(0)->Xyz());
+  }
+
+  {
+    auto joint3 = model->JointByName("joint3");
+    ASSERT_FALSE(nullptr == joint3);
+    ASSERT_FALSE(nullptr == joint3->Axis(0));
+    EXPECT_EQ(-Vector3d::UnitX, joint3->Axis(0)->Xyz());
+    ASSERT_FALSE(nullptr == joint3->Axis(1));
+    EXPECT_EQ(Vector3d::UnitY, joint3->Axis(1)->Xyz());
+  }
+
+  {
+    auto joint4 = model->JointByName("joint4");
+    ASSERT_FALSE(nullptr == joint4);
+    ASSERT_FALSE(nullptr == joint4->Axis(0));
+    EXPECT_EQ(Vector3d::Zero, joint4->Axis(0)->Xyz());
+  }
+}

test/sdf/joint_axis_xyz_normalization.sdf

@@ -0,0 +1,43 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="test">
+    <link name="link1"/>
+    <link name="link2"/>
+    <link name="link3"/>
+    <link name="link4"/>
+    <link name="link5"/>
+
+    <joint name="joint1" type="revolute">
+      <child>link1</child>
+      <parent>link2</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+    <joint name="joint2" type="revolute">
+      <child>link3</child>
+      <parent>link4</parent>
+      <axis>
+        <xyz>10 0 0</xyz>
+      </axis>
+    </joint>
+    <joint name="joint3" type="universal">
+      <child>link4</child>
+      <parent>link5</parent>
+      <axis>
+        <xyz>-10 0 0</xyz>
+      </axis>
+      <axis2>
+        <xyz>0 10 0</xyz>
+      </axis2>
+    </joint>
+    <joint name="joint4" type="revolute">
+      <child>link5</child>
+      <parent>link6</parent>
+      <axis>
+        <xyz>0 0 0</xyz>
+      </axis>
+    </joint>
+  </model>
+</sdf>
+