diff --git a/Changelog.md b/Changelog.md index dac0785e8..ec5e5c359 100644 --- a/Changelog.md +++ b/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. 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 index 983d47648..050be91c2 100644 --- a/Migration.md +++ b/Migration.md @@ -16,6 +16,10 @@ but with improved human-readability.. ### Modifications +1. Axis vectors specified in 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 index eae4810e0..bf9d001b5 100644 --- a/include/sdf/JointAxis.hh +++ b/include/sdf/JointAxis.hh @@ -21,6 +21,7 @@ #include #include #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 index bedc16b18..f9cd705f3 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -14,8 +14,13 @@ * limitations under the License. * */ +#include +#include + #include #include + +#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("xyz", - ignition::math::Vector3d::UnitZ).first; + using ignition::math::Vector3d; + auto errs = this->SetXyz(_sdf->Get("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 index a62b2928d..753aef9f2 100644 --- a/src/JointAxis_TEST.cc +++ b/src/JointAxis_TEST.cc @@ -40,8 +40,11 @@ TEST(DOMJointAxis, Construction) axis.SetInitialPosition(1.2); EXPECT_DOUBLE_EQ(1.2, axis.InitialPosition()); - 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()); @@ -85,7 +88,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()); @@ -95,7 +98,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; @@ -107,7 +110,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()); @@ -118,7 +121,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); @@ -130,11 +133,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 @@ -145,3 +148,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 index 1f1fc6100..043ff0828 100644 --- a/src/Joint_TEST.cc +++ b/src/Joint_TEST.cc @@ -84,10 +84,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)); @@ -106,10 +106,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)); @@ -127,10 +127,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); @@ -154,10 +154,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; @@ -176,10 +176,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; @@ -204,19 +204,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 index 24d6fbd10..f6af2ada4 100644 --- a/test/integration/joint_axis_dom.cc +++ b/test/integration/joint_axis_dom.cc @@ -219,3 +219,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 new file mode 100644 index 000000000..86b4ab34b --- /dev/null +++ b/test/sdf/joint_axis_xyz_normalization.sdf @@ -0,0 +1,43 @@ + + + + + + + + + + + link1 + link2 + + 0 0 1 + + + + link3 + link4 + + 10 0 0 + + + + link4 + link5 + + -10 0 0 + + + 0 10 0 + + + + link5 + link6 + + 0 0 0 + + + + +