Normalize joint axis xyz vector when parsing from SDFormat (#312)

* Normalize joint axis xyz vector when parsing from SDFormat Signed-off-by: Addisu Z. Taddese <[email protected]> Co-authored-by: Steve Peters <[email protected]>
gazebosim · Aug 18, 2020 · 1350f55 · 1350f55
1 parent 4582b72
commit 1350f55
Show file tree

Hide file tree

Showing 8 changed files with 159 additions and 26 deletions.
diff --git a/Changelog.md b/Changelog.md
@@ -16,6 +16,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. Migrate to using TinyXML2.
     * [Pull request 264](https://github.com/osrf/sdformat/pull/264)
     * [Pull request 321](https://github.com/osrf/sdformat/pull/321)

diff --git a/Migration.md b/Migration.md
@@ -24,6 +24,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. + Depend on tinyxml2 instead of tinyxml for XML parsing.
    + [Pull request 264](https://github.com/osrf/sdformat/pull/264)
 

diff --git a/include/sdf/JointAxis.hh b/include/sdf/JointAxis.hh
@@ -21,6 +21,7 @@
 #include <string>
 #include <ignition/math/Vector3.hh>
 #include "sdf/Element.hh"
+#include "sdf/Exception.hh"
 #include "sdf/Types.hh"
 #include "sdf/sdf_config.h"
 #include "sdf/system_util.hh"
@@ -94,7 +95,9 @@ namespace sdf
     /// \brief Set the x,y,z components of the axis unit vector.
     /// \param[in] _xyz The x,y,z components of the axis unit vector.
     /// \sa ignition::math::Vector3d Xyz() const
-    public: void SetXyz(const ignition::math::Vector3d &_xyz);
+    /// \return Errors will have an entry if the norm of the xyz vector is 0.
+    public: [[nodiscard]] sdf::Errors SetXyz(
+                const ignition::math::Vector3d &_xyz);
 
     /// \brief Get whether to interpret the axis xyz value in the parent model
     /// frame instead of joint frame. The default value is false.

diff --git a/src/JointAxis.cc b/src/JointAxis.cc
@@ -14,8 +14,13 @@
  * limitations under the License.
  *
  */
+#include <algorithm>
+#include <iterator>
+
 #include <ignition/math/Pose3.hh>
 #include <ignition/math/Vector3.hh>
+
+#include "sdf/Assert.hh"
 #include "sdf/Error.hh"
 #include "sdf/JointAxis.hh"
 #include "FrameSemantics.hh"
@@ -136,8 +141,9 @@ Errors JointAxis::Load(ElementPtr _sdf)
   // Read the xyz values.
   if (_sdf->HasElement("xyz"))
   {
-    this->dataPtr->xyz = _sdf->Get<ignition::math::Vector3d>("xyz",
-        ignition::math::Vector3d::UnitZ).first;
+    using ignition::math::Vector3d;
+    auto errs = this->SetXyz(_sdf->Get<Vector3d>("xyz", Vector3d::UnitZ).first);
+    std::copy(errs.begin(), errs.end(), std::back_inserter(errors));
     auto e = _sdf->GetElement("xyz");
     if (e->HasAttribute("expressed_in"))
     {
@@ -209,9 +215,16 @@ ignition::math::Vector3d JointAxis::Xyz() const
 }
 
 /////////////////////////////////////////////////
-void JointAxis::SetXyz(const ignition::math::Vector3d &_xyz)
+sdf::Errors JointAxis::SetXyz(const ignition::math::Vector3d &_xyz)
 {
+  if (sdf::equal(_xyz.Length(), 0.0))
+  {
+    return {Error(ErrorCode::ELEMENT_INVALID,
+                  "The norm of the xyz vector cannot be zero")};
+  }
   this->dataPtr->xyz = _xyz;
+  this->dataPtr->xyz.Normalize();
+  return sdf::Errors();
 }
 
 /////////////////////////////////////////////////

diff --git a/src/JointAxis_TEST.cc b/src/JointAxis_TEST.cc
@@ -44,8 +44,11 @@ TEST(DOMJointAxis, Construction)
   EXPECT_DOUBLE_EQ(1.2, axis.InitialPosition());
   SDF_SUPPRESS_DEPRECATED_END
 
-  axis.SetXyz(ignition::math::Vector3d(0, 1, 0));
-  EXPECT_EQ(ignition::math::Vector3d::UnitY, axis.Xyz());
+  {
+    sdf::Errors errors = axis.SetXyz(ignition::math::Vector3d(0, 1, 0));
+    EXPECT_TRUE(errors.empty());
+    EXPECT_EQ(ignition::math::Vector3d::UnitY, axis.Xyz());
+  }
 
   axis.SetXyzExpressedIn("__model__");
   EXPECT_EQ("__model__", axis.XyzExpressedIn());
@@ -89,7 +92,7 @@ TEST(DOMJointAxis, Construction)
 TEST(DOMJointAxis, CopyConstructor)
 {
   sdf::JointAxis jointAxis;
-  jointAxis.SetXyz(ignition::math::Vector3d(0, 1, 0));
+  EXPECT_TRUE(jointAxis.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty());
 
   sdf::JointAxis jointAxisCopy(jointAxis);
   EXPECT_EQ(jointAxis.Xyz(), jointAxisCopy.Xyz());
@@ -99,7 +102,7 @@ TEST(DOMJointAxis, CopyConstructor)
 TEST(DOMJointAxis, AssignmentOperator)
 {
   sdf::JointAxis jointAxis;
-  jointAxis.SetXyz(ignition::math::Vector3d(0, 1, 0));
+  EXPECT_TRUE(jointAxis.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty());
 
   sdf::JointAxis jointAxisCopy;
   jointAxisCopy = jointAxis;
@@ -111,7 +114,7 @@ TEST(DOMJointAxis, MoveConstructor)
 {
   ignition::math::Vector3d axis{0, 1, 0};
   sdf::JointAxis jointAxis;
-  jointAxis.SetXyz(axis);
+  EXPECT_TRUE(jointAxis.SetXyz(axis).empty());
 
   sdf::JointAxis jointAxisMoved(std::move(jointAxis));
   EXPECT_EQ(axis, jointAxisMoved.Xyz());
@@ -122,7 +125,7 @@ TEST(DOMJointAxis, MoveAssignmentOperator)
 {
   ignition::math::Vector3d axis{0, 1, 0};
   sdf::JointAxis jointAxis;
-  jointAxis.SetXyz(axis);
+  EXPECT_TRUE(jointAxis.SetXyz(axis).empty());
 
   sdf::JointAxis jointAxisMoved;
   jointAxisMoved = std::move(jointAxis);
@@ -134,11 +137,11 @@ TEST(DOMJointAxis, CopyAssignmentAfterMove)
 {
   ignition::math::Vector3d axis1{0, 1, 0};
   sdf::JointAxis jointAxis1;
-  jointAxis1.SetXyz(axis1);
+  EXPECT_TRUE(jointAxis1.SetXyz(axis1).empty());
 
   ignition::math::Vector3d axis2{1, 0, 0};
   sdf::JointAxis jointAxis2;
-  jointAxis2.SetXyz(axis2);
+  EXPECT_TRUE(jointAxis2.SetXyz(axis2).empty());
 
   // This is similar to what std::swap does except it uses std::move for each
   // assignment
@@ -149,3 +152,14 @@ TEST(DOMJointAxis, CopyAssignmentAfterMove)
   EXPECT_EQ(axis2, jointAxis1.Xyz());
   EXPECT_EQ(axis1, jointAxis2.Xyz());
 }
+
+/////////////////////////////////////////////////
+TEST(DOMJointAxis, ZeroNormVectorReturnsError)
+{
+  sdf::JointAxis axis;
+  EXPECT_TRUE(axis.SetXyz({1.0, 0, 0}).empty());
+
+  sdf::Errors errors = axis.SetXyz(ignition::math::Vector3d::Zero);
+  ASSERT_FALSE(errors.empty());
+  EXPECT_EQ(errors[0].Message(), "The norm of the xyz vector cannot be zero");
+}
diff --git a/src/Joint_TEST.cc b/src/Joint_TEST.cc
@@ -90,10 +90,10 @@ TEST(DOMJoint, Construction)
   EXPECT_EQ(nullptr, joint.Axis(0));
   EXPECT_EQ(nullptr, joint.Axis(1));
   sdf::JointAxis axis;
-  axis.SetXyz(ignition::math::Vector3d(1, 0, 0));
+  EXPECT_TRUE(axis.SetXyz(ignition::math::Vector3d(1, 0, 0)).empty());
   joint.SetAxis(0, axis);
   sdf::JointAxis axis1;
-  axis1.SetXyz(ignition::math::Vector3d(0, 1, 0));
+  EXPECT_TRUE(axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty());
   joint.SetAxis(1, axis1);
   ASSERT_TRUE(nullptr != joint.Axis(0));
   ASSERT_TRUE(nullptr != joint.Axis(1));
@@ -112,10 +112,10 @@ TEST(DOMJoint, MoveConstructor)
   sdf::Joint joint;
   joint.SetName("test_joint");
   sdf::JointAxis axis;
-  axis.SetXyz(ignition::math::Vector3d(1, 0, 0));
+  EXPECT_TRUE(axis.SetXyz(ignition::math::Vector3d(1, 0, 0)).empty());
   joint.SetAxis(0, axis);
   sdf::JointAxis axis1;
-  axis1.SetXyz(ignition::math::Vector3d(0, 1, 0));
+  EXPECT_TRUE(axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty());
   joint.SetAxis(1, axis1);
 
   sdf::Joint joint2(std::move(joint));
@@ -133,10 +133,10 @@ TEST(DOMJoint, CopyConstructor)
   sdf::Joint joint;
   joint.SetName("test_joint");
   sdf::JointAxis axis;
-  axis.SetXyz(ignition::math::Vector3d(1, 0, 0));
+  EXPECT_TRUE(axis.SetXyz(ignition::math::Vector3d(1, 0, 0)).empty());
   joint.SetAxis(0, axis);
   sdf::JointAxis axis1;
-  axis1.SetXyz(ignition::math::Vector3d(0, 1, 0));
+  EXPECT_TRUE(axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty());
   joint.SetAxis(1, axis1);
 
   sdf::Joint joint2(joint);
@@ -160,10 +160,10 @@ TEST(DOMJoint, MoveAssignment)
   sdf::Joint joint;
   joint.SetName("test_joint");
   sdf::JointAxis axis;
-  axis.SetXyz(ignition::math::Vector3d(1, 0, 0));
+  EXPECT_TRUE(axis.SetXyz(ignition::math::Vector3d(1, 0, 0)).empty());
   joint.SetAxis(0, axis);
   sdf::JointAxis axis1;
-  axis1.SetXyz(ignition::math::Vector3d(0, 1, 0));
+  EXPECT_TRUE(axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty());
   joint.SetAxis(1, axis1);
 
   sdf::Joint joint2;
@@ -182,10 +182,10 @@ TEST(DOMJoint, CopyAssignment)
   sdf::Joint joint;
   joint.SetName("test_joint");
   sdf::JointAxis axis;
-  axis.SetXyz(ignition::math::Vector3d(1, 0, 0));
+  EXPECT_TRUE(axis.SetXyz(ignition::math::Vector3d(1, 0, 0)).empty());
   joint.SetAxis(0, axis);
   sdf::JointAxis axis1;
-  axis1.SetXyz(ignition::math::Vector3d(0, 1, 0));
+  EXPECT_TRUE(axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty());
   joint.SetAxis(1, axis1);
 
   sdf::Joint joint2;
@@ -210,19 +210,19 @@ TEST(DOMJoint, CopyAssignmentAfterMove)
   sdf::Joint joint1;
   joint1.SetName("test_joint1");
   sdf::JointAxis joint1Axis;
-  joint1Axis.SetXyz(ignition::math::Vector3d(1, 0, 0));
+  EXPECT_TRUE(joint1Axis.SetXyz(ignition::math::Vector3d(1, 0, 0)).empty());
   joint1.SetAxis(0, joint1Axis);
   sdf::JointAxis joint1Axis1;
-  joint1Axis1.SetXyz(ignition::math::Vector3d(0, 1, 0));
+  EXPECT_TRUE(joint1Axis1.SetXyz(ignition::math::Vector3d(0, 1, 0)).empty());
   joint1.SetAxis(1, joint1Axis1);
 
   sdf::Joint joint2;
   joint2.SetName("test_joint2");
   sdf::JointAxis joint2Axis;
-  joint2Axis.SetXyz(ignition::math::Vector3d(0, 0, 1));
+  EXPECT_TRUE(joint2Axis.SetXyz(ignition::math::Vector3d(0, 0, 1)).empty());
   joint2.SetAxis(0, joint2Axis);
   sdf::JointAxis joint2Axis1;
-  joint2Axis1.SetXyz(ignition::math::Vector3d(-1, 0, 0));
+  EXPECT_TRUE(joint2Axis1.SetXyz(ignition::math::Vector3d(-1, 0, 0)).empty());
   joint2.SetAxis(1, joint2Axis1);
 
   // This is similar to what std::swap does except it uses std::move for each

diff --git a/test/integration/joint_axis_dom.cc b/test/integration/joint_axis_dom.cc
@@ -224,3 +224,56 @@ 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_TRUE(
+      errors[0].Message().find("The norm of the xyz vector cannot be zero") !=
+      std::string::npos);
+
+  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::UnitZ, joint4->Axis(0)->Xyz());
+  }
+}
diff --git a/test/sdf/joint_axis_xyz_normalization.sdf b/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>
+