From 228351d4d15763c45f254086436dddd558bd8cc7 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 2 Jun 2021 21:41:02 +0800 Subject: [PATCH 1/4] New pose proposal from #252 Signed-off-by: Aaron Chong --- sdf/1.9/pose.sdf | 28 +++++++- src/Utils.cc | 86 +++++++++++++++++++++--- test/integration/CMakeLists.txt | 1 + test/integration/pose_1_9_sdf.cc | 112 +++++++++++++++++++++++++++++++ test/sdf/pose_1_9.sdf | 81 ++++++++++++++++++++++ 5 files changed, 297 insertions(+), 11 deletions(-) create mode 100644 test/integration/pose_1_9_sdf.cc create mode 100644 test/sdf/pose_1_9.sdf diff --git a/sdf/1.9/pose.sdf b/sdf/1.9/pose.sdf index 523f0894b..71dc8e604 100644 --- a/sdf/1.9/pose.sdf +++ b/sdf/1.9/pose.sdf @@ -1,6 +1,6 @@ - A position(x,y,z) and orientation(roll, pitch yaw) with respect to the frame named in the relative_to attribute. + A position(x,y,z) and orientation(roll, pitch, yaw) with respect to the frame named in the relative_to attribute. @@ -8,4 +8,30 @@ + + The position(x,y,z) with respect to the frame named in the relative_to attribute. + + + + The rotation with respect to the frame named in the relative_to attribute. + + The type of rotation convention, which must be one of the following: + (rpy_degrees) rotation in the form of (roll, pitch, yaw), in degrees. + (rpy_radians) rotation in the form of (roll, pitch, yaw), in radians. + (q_wxyz) rotation in the form of a quaternion (w, x, y, z). + + + + + + The rotation with respect to the frame named in the relative_to attribute. + + The type of rotation convention, which must be one of the following: + (rpy_degrees) rotation in the form of (roll, pitch, yaw), in degrees. + (rpy_radians) rotation in the form of (roll, pitch, yaw), in radians. + (q_wxyz) rotation in the form of a quaternion (w, x, y, z). + + + + diff --git a/src/Utils.cc b/src/Utils.cc index 3b2834706..cc20a924c 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -56,24 +56,90 @@ bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose, return false; } - // Read the frame. An empty frame implies the parent frame. + // Default zero pose. + ignition::math::Pose3d pose( + ignition::math::Vector3d::Zero, ignition::math::Quaterniond::Identity); + + // Read the frame. An empty frame implies the parent frame. This is optional. std::pair framePair = sdf->Get("relative_to", ""); - // Read the pose value. - std::pair posePair = - sdf->Get("", ignition::math::Pose3d::Zero); - - // Set output, but only if the return value is true. - if (posePair.second) + // Start checking for translation and rotation elements. + sdf::ElementPtr translationPtr = sdf->GetElement("translation"); + sdf::ElementPtr rotationPtr = sdf->GetElement("rotation"); + + // If both are not explicitly set, the pose is parsed using the value. + if (!translationPtr->GetExplicitlySetInFile() && + !rotationPtr->GetExplicitlySetInFile()) { + std::pair posePair = + sdf->Get("", ignition::math::Pose3d::Zero); _pose = posePair.first; _frame = framePair.first; + + // In this scenario, return true or false based on pose element value. + return posePair.second; + } + + // Read the translation values. + if (translationPtr->GetExplicitlySetInFile()) + { + std::pair translationPair = + translationPtr->Get( + "", ignition::math::Vector3d::Zero); + if (translationPair.second) + { + std::cout << "found translation" << std::endl; + pose.Set(translationPair.first, pose.Rot()); + } + } + + // Read the rotation values. + if (rotationPtr->GetExplicitlySetInFile()) + { + std::pair typePair = + rotationPtr->Get("type", ""); + if (!typePair.second) + return false; + + if (typePair.first == "rpy_degrees") + { + std::cout << "found rpy deg" << std::endl; + std::pair rpyDegPair = + rotationPtr->Get( + "", ignition::math::Vector3d::Zero); + if (rpyDegPair.second) + pose.Set(pose.Pos(), IGN_DTOR(rpyDegPair.first)); + } + else if (typePair.first == "rpy_radians") + { + std::cout << "found rpy rad" << std::endl; + std::pair rpyRadPair = + rotationPtr->Get( + "", ignition::math::Vector3d::Zero); + if (rpyRadPair.second) + pose.Set(pose.Pos(), rpyRadPair.first); + } + else if (typePair.first == "q_wxyz") + { + std::cout << "found quat" << std::endl; + std::pair quatPair = + rotationPtr->Get( + "", ignition::math::Quaterniond::Identity); + if (quatPair.second) + { + pose.Set(pose.Pos(), quatPair.first); + std::cout << quatPair.first << std::endl; + std::cout << "set quat" << std::endl; + } + } + else + return false; } - // The frame attribute is optional, so only return true or false based - // on the pose element value. - return posePair.second; + _pose = pose; + _frame = framePair.first; + return true; } ///////////////////////////////////////////////// diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 849e7b2de..5eda1c932 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -38,6 +38,7 @@ set(tests plugin_attribute.cc plugin_bool.cc plugin_include.cc + pose_1_9_sdf.cc provide_feedback.cc root_dom.cc scene_dom.cc diff --git a/test/integration/pose_1_9_sdf.cc b/test/integration/pose_1_9_sdf.cc new file mode 100644 index 000000000..0a2279225 --- /dev/null +++ b/test/integration/pose_1_9_sdf.cc @@ -0,0 +1,112 @@ +/* + * Copyright 2021 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. + * + */ + +#include +#include + +#include +#include +#include +#include "sdf/Element.hh" +#include "sdf/Error.hh" +#include "sdf/Model.hh" +#include "sdf/Root.hh" +#include "sdf/World.hh" +#include "sdf/Filesystem.hh" +#include "test_config.h" + +////////////////////////////////////////////////// +TEST(Pose1_9, ModelPoses) +{ + using Pose = ignition::math::Pose3d; + + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", "pose.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + for (const auto e : errors) + std::cout << e << std::endl; + // std::cout << errors << std::endl; + ASSERT_TRUE(errors.empty()); + EXPECT_EQ(SDF_PROTOCOL_VERSION, root.Version()); + + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + std::cout << "modelWithEmptyPose" << std::endl; + const sdf::Model *model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_empty_pose", model->Name()); + EXPECT_EQ(Pose::Zero, model->RawPose()); + + std::cout << "modelWithPoseValue" << std::endl; + model = world->ModelByIndex(1); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_pose_value", model->Name()); + EXPECT_EQ(Pose(1, 2, 3, 1, 2, 3), model->RawPose()); + + std::cout << "modelWithEmptyTranslation" << std::endl; + model = world->ModelByIndex(2); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_empty_translation", model->Name()); + EXPECT_EQ(Pose::Zero, model->RawPose()); + + std::cout << "modelWithTranslationValue" << std::endl; + model = world->ModelByIndex(3); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_translation_value", model->Name()); + EXPECT_EQ(Pose(2, 3, 4, 0, 0, 0), model->RawPose()); + + std::cout << "modelWithEmptyRPYDeg" << std::endl; + model = world->ModelByIndex(4); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_empty_rpy_deg", model->Name()); + EXPECT_EQ(Pose::Zero, model->RawPose()); + + std::cout << "modelWithRPYDegValue" << std::endl; + model = world->ModelByIndex(5); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_rpy_deg_value", model->Name()); + EXPECT_EQ(Pose(0, 0, 0, 0, IGN_DTOR(90), IGN_DTOR(180)), model->RawPose()); + + std::cout << "modelWithEmptyRPYRad" << std::endl; + model = world->ModelByIndex(6); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_empty_rpy_rad", model->Name()); + EXPECT_EQ(Pose::Zero, model->RawPose()); + + std::cout << "modelWithRPYRadValue" << std::endl; + model = world->ModelByIndex(7); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_rpy_rad_value", model->Name()); + EXPECT_EQ(Pose(0, 0, 0, 1, 2, 3), model->RawPose()); + + std::cout << "modelWithEmptyQuat" << std::endl; + model = world->ModelByIndex(8); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_empty_quat", model->Name()); + EXPECT_EQ(Pose::Zero, model->RawPose()); + + std::cout << "modelWithQuatValue" << std::endl; + model = world->ModelByIndex(9); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_quat_value", model->Name()); + EXPECT_EQ(Pose(0, 0, 0, 0.7071068, 0.7071068, 0, 0), model->RawPose()); +} + diff --git a/test/sdf/pose_1_9.sdf b/test/sdf/pose_1_9.sdf new file mode 100644 index 000000000..b084dc49d --- /dev/null +++ b/test/sdf/pose_1_9.sdf @@ -0,0 +1,81 @@ + + + + + + + + + + + 1 2 3 1 2 3 + + + + + + + + + + + + + 2 3 4 + + + + + + + + + + + + + + 0 90 180 + + + + + + + + + + + + + + 1 2 3 + + + + + + + + + + + + + + 0.7071068 0.7071068 0 0 + + + + + + + + + + + + + + + From af7703d5cdce7c7edf1e727d820e27a8c4bfb797 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 3 Jun 2021 20:40:50 +0800 Subject: [PATCH 2/4] Using correct test file Signed-off-by: Aaron Chong --- test/integration/pose_1_9_sdf.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/integration/pose_1_9_sdf.cc b/test/integration/pose_1_9_sdf.cc index 0a2279225..57d0e58f9 100644 --- a/test/integration/pose_1_9_sdf.cc +++ b/test/integration/pose_1_9_sdf.cc @@ -34,8 +34,8 @@ TEST(Pose1_9, ModelPoses) { using Pose = ignition::math::Pose3d; - const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", "pose.sdf"); + const std::string testFile = sdf::testing::TestFile( + "sdf", "pose_1_9.sdf"); // Load the SDF file sdf::Root root; From 118ff49dbe5623257d110658541e2139ed61755d Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 3 Jun 2021 22:38:52 +0800 Subject: [PATCH 3/4] Added new rotation element type, helper function to parse Signed-off-by: Aaron Chong --- sdf/1.9/pose.sdf | 13 +---- src/Param.cc | 83 ++++++++++++++++++++++++++++++++ src/Utils.cc | 12 +---- test/integration/pose_1_9_sdf.cc | 16 +++--- 4 files changed, 94 insertions(+), 30 deletions(-) diff --git a/sdf/1.9/pose.sdf b/sdf/1.9/pose.sdf index 71dc8e604..efb969b61 100644 --- a/sdf/1.9/pose.sdf +++ b/sdf/1.9/pose.sdf @@ -12,18 +12,7 @@ The position(x,y,z) with respect to the frame named in the relative_to attribute. - - The rotation with respect to the frame named in the relative_to attribute. - - The type of rotation convention, which must be one of the following: - (rpy_degrees) rotation in the form of (roll, pitch, yaw), in degrees. - (rpy_radians) rotation in the form of (roll, pitch, yaw), in radians. - (q_wxyz) rotation in the form of a quaternion (w, x, y, z). - - - - - + The rotation with respect to the frame named in the relative_to attribute. The type of rotation convention, which must be one of the following: diff --git a/src/Param.cc b/src/Param.cc index 4335fa890..fd75782cf 100644 --- a/src/Param.cc +++ b/src/Param.cc @@ -427,6 +427,83 @@ bool ParseColorUsingStringStream(const std::string &_input, return isValidColor; } +////////////////////////////////////////////////// +/// \brief Helper function for Param::ValueFromString for parsing rotations +/// This checks the rotation components specified in _input are rpy or wxyz +/// (expects 3 or 4 values) and the resulting quaternion is valid. +/// \param[in] _input Input string. +/// \param[in] _key Key of the parameter, used for error message. +/// \param[out] _value This will be set with the parsed value. +/// \return True if parsing colors succeeded. +bool ParseRotationUsingStringStream(const std::string &_input, + const std::string &_key, ParamPrivate::ParamVariant &_value) +{ + StringStreamClassicLocale ss(_input); + std::string token; + std::vector values; + double v; + bool isValidRotation = true; + while (ss >> token) + { + try + { + v = std::stod(token); + } + // Catch invalid argument exception from std::stod + catch(std::invalid_argument &) + { + sdferr << "Invalid argument. Unable to set value ["<< token + << "] for key [" << _key << "].\n"; + isValidRotation = false; + break; + } + // Catch out of range exception from std::stof + catch(std::out_of_range &) + { + sdferr << "Out of range. Unable to set value [" << token + << "] for key [" << _key << "].\n"; + isValidRotation = false; + break; + } + + if (values.size() > 4) + { + sdferr << "Rotation values cannot have more than 4 values.\n"; + isValidRotation = false; + break; + } + + if (!std::isfinite(v)) + { + sdferr << "Rotation values must be finite.\n"; + isValidRotation = false; + break; + } + + values.push_back(v); + } + + if (!isValidRotation) + return false; + + if (values.size() == 3u) + { + _value = ignition::math::Vector3d(values[0], values[1], values[2]); + } + else if (values.size() == 4u) + { + _value = ignition::math::Quaterniond( + values[0], values[1], values[2], values[3]); + } + else + { + sdferr << "Rotation values must have either 3 or 4 values.\n"; + return false; + } + + return true; +} + ////////////////////////////////////////////////// bool Param::ValueFromString(const std::string &_value) { @@ -553,6 +630,11 @@ bool Param::ValueFromString(const std::string &_value) return ParseUsingStringStream( tmp, this->dataPtr->key, this->dataPtr->value); } + else if (this->dataPtr->typeName == "rotation") + { + return ParseRotationUsingStringStream( + tmp, this->dataPtr->key, this->dataPtr->value); + } else { sdferr << "Unknown parameter type[" << this->dataPtr->typeName << "]\n"; @@ -698,3 +780,4 @@ bool Param::ValidateValue() const return true; }, this->dataPtr->value); } + diff --git a/src/Utils.cc b/src/Utils.cc index cc20a924c..90f9ea2d0 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -67,7 +67,7 @@ bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose, // Start checking for translation and rotation elements. sdf::ElementPtr translationPtr = sdf->GetElement("translation"); sdf::ElementPtr rotationPtr = sdf->GetElement("rotation"); - + // If both are not explicitly set, the pose is parsed using the value. if (!translationPtr->GetExplicitlySetInFile() && !rotationPtr->GetExplicitlySetInFile()) @@ -80,7 +80,7 @@ bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose, // In this scenario, return true or false based on pose element value. return posePair.second; } - + // Read the translation values. if (translationPtr->GetExplicitlySetInFile()) { @@ -88,10 +88,7 @@ bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose, translationPtr->Get( "", ignition::math::Vector3d::Zero); if (translationPair.second) - { - std::cout << "found translation" << std::endl; pose.Set(translationPair.first, pose.Rot()); - } } // Read the rotation values. @@ -104,7 +101,6 @@ bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose, if (typePair.first == "rpy_degrees") { - std::cout << "found rpy deg" << std::endl; std::pair rpyDegPair = rotationPtr->Get( "", ignition::math::Vector3d::Zero); @@ -113,7 +109,6 @@ bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose, } else if (typePair.first == "rpy_radians") { - std::cout << "found rpy rad" << std::endl; std::pair rpyRadPair = rotationPtr->Get( "", ignition::math::Vector3d::Zero); @@ -122,15 +117,12 @@ bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose, } else if (typePair.first == "q_wxyz") { - std::cout << "found quat" << std::endl; std::pair quatPair = rotationPtr->Get( "", ignition::math::Quaterniond::Identity); if (quatPair.second) { pose.Set(pose.Pos(), quatPair.first); - std::cout << quatPair.first << std::endl; - std::cout << "set quat" << std::endl; } } else diff --git a/test/integration/pose_1_9_sdf.cc b/test/integration/pose_1_9_sdf.cc index 57d0e58f9..fdb90a9c4 100644 --- a/test/integration/pose_1_9_sdf.cc +++ b/test/integration/pose_1_9_sdf.cc @@ -35,8 +35,8 @@ TEST(Pose1_9, ModelPoses) using Pose = ignition::math::Pose3d; const std::string testFile = sdf::testing::TestFile( - "sdf", "pose_1_9.sdf"); - + "sdf", "pose_1_9.sdf"); + // Load the SDF file sdf::Root root; auto errors = root.Load(testFile); @@ -45,17 +45,17 @@ TEST(Pose1_9, ModelPoses) // std::cout << errors << std::endl; ASSERT_TRUE(errors.empty()); EXPECT_EQ(SDF_PROTOCOL_VERSION, root.Version()); - + const sdf::World *world = root.WorldByIndex(0); ASSERT_NE(nullptr, world); - + std::cout << "modelWithEmptyPose" << std::endl; const sdf::Model *model = world->ModelByIndex(0); ASSERT_NE(nullptr, model); ASSERT_EQ("model_with_empty_pose", model->Name()); EXPECT_EQ(Pose::Zero, model->RawPose()); - - std::cout << "modelWithPoseValue" << std::endl; + + std::cout << "modelWithPoseValue" << std::endl; model = world->ModelByIndex(1); ASSERT_NE(nullptr, model); ASSERT_EQ("model_with_pose_value", model->Name()); @@ -77,13 +77,13 @@ TEST(Pose1_9, ModelPoses) model = world->ModelByIndex(4); ASSERT_NE(nullptr, model); ASSERT_EQ("model_with_empty_rpy_deg", model->Name()); - EXPECT_EQ(Pose::Zero, model->RawPose()); + EXPECT_EQ(Pose::Zero, model->RawPose()); std::cout << "modelWithRPYDegValue" << std::endl; model = world->ModelByIndex(5); ASSERT_NE(nullptr, model); ASSERT_EQ("model_with_rpy_deg_value", model->Name()); - EXPECT_EQ(Pose(0, 0, 0, 0, IGN_DTOR(90), IGN_DTOR(180)), model->RawPose()); + EXPECT_EQ(Pose(0, 0, 0, 0, IGN_DTOR(90), IGN_DTOR(180)), model->RawPose()); std::cout << "modelWithEmptyRPYRad" << std::endl; model = world->ModelByIndex(6); From 485ea7df134bf910e0760f0f8aeb83e0502b036e Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 3 Jun 2021 23:19:11 +0800 Subject: [PATCH 4/4] Adding more cases for tests Signed-off-by: Aaron Chong --- test/integration/pose_1_9_sdf.cc | 67 +++++++++++++++++++++++++ test/sdf/pose_1_9.sdf | 84 +++++++++++++++++++++++++++++++- 2 files changed, 150 insertions(+), 1 deletion(-) diff --git a/test/integration/pose_1_9_sdf.cc b/test/integration/pose_1_9_sdf.cc index fdb90a9c4..1a393bad5 100644 --- a/test/integration/pose_1_9_sdf.cc +++ b/test/integration/pose_1_9_sdf.cc @@ -108,5 +108,72 @@ TEST(Pose1_9, ModelPoses) ASSERT_NE(nullptr, model); ASSERT_EQ("model_with_quat_value", model->Name()); EXPECT_EQ(Pose(0, 0, 0, 0.7071068, 0.7071068, 0, 0), model->RawPose()); + + std::cout << "modelWithBothRotationsNoValue" << std::endl; + model = world->ModelByIndex(10); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_both_rotations_no_value", model->Name()); + EXPECT_EQ(Pose::Zero, model->RawPose()); + + std::cout << "modelWithBothRotationsSecondValue" << std::endl; + model = world->ModelByIndex(11); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_both_rotations_second_value", model->Name()); + EXPECT_EQ(Pose::Zero, model->RawPose()); + + std::cout << "modelWithBothRotationsFirstValue" << std::endl; + model = world->ModelByIndex(12); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_both_rotations_first_value", model->Name()); + EXPECT_EQ(Pose(0, 0, 0, 1, 2, 3), model->RawPose()); + + std::cout << "modelWithTranslationRotationNoValue" << std::endl; + model = world->ModelByIndex(13); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_translation_rotation_no_value", model->Name()); + EXPECT_EQ(Pose::Zero, model->RawPose()); + + std::cout << "modelWithTranslationRotationValue" << std::endl; + model = world->ModelByIndex(14); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_translation_rotation_value", model->Name()); + EXPECT_EQ(Pose(2, 3, 4, 0.7071068, 0.7071068, 0, 0), model->RawPose()); + + std::cout << "modelWithValueAndTranslationNoValue" << std::endl; + model = world->ModelByIndex(15); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_value_and_translation_no_value", model->Name()); + EXPECT_EQ(Pose::Zero, model->RawPose()); + + std::cout << "modelWithValueAndTranslationValue" << std::endl; + model = world->ModelByIndex(16); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_value_and_translation_value", model->Name()); + EXPECT_EQ(Pose(2, 3, 4, 0, 0, 0), model->RawPose()); + + std::cout << "modelWithValueAndRotationNoValue" << std::endl; + model = world->ModelByIndex(17); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_value_and_rotation_no_value", model->Name()); + EXPECT_EQ(Pose::Zero, model->RawPose()); + + std::cout << "modelWithValueAndRotationValue" << std::endl; + model = world->ModelByIndex(18); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_value_and_rotation_value", model->Name()); + EXPECT_EQ(Pose(0, 0, 0, IGN_DTOR(1), IGN_DTOR(2), IGN_DTOR(3)), + model->RawPose()); + + std::cout << "modelWithValueAndTranslationRotationNoValue" << std::endl; + model = world->ModelByIndex(19); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_value_and_translation_rotation_no_value", model->Name()); + EXPECT_EQ(Pose::Zero, model->RawPose()); + + std::cout << "modelWithValueAndTranslationRotationValue" << std::endl; + model = world->ModelByIndex(20); + ASSERT_NE(nullptr, model); + ASSERT_EQ("model_with_value_and_translation_rotation_value", model->Name()); + EXPECT_EQ(Pose(2, 3, 4, 0.7071068, 0.7071068, 0, 0), model->RawPose()); } diff --git a/test/sdf/pose_1_9.sdf b/test/sdf/pose_1_9.sdf index b084dc49d..0aefc03dc 100644 --- a/test/sdf/pose_1_9.sdf +++ b/test/sdf/pose_1_9.sdf @@ -68,7 +68,7 @@ - + @@ -76,6 +76,88 @@ + + + + 0.7071068 0.7071068 0 0 + + + + + + + 1 2 3 + + + + + + + + + + + + + + + + 2 3 4 + 0.7071068 0.7071068 0 0 + + + + + + + 1 2 3 1 2 3 + + + + + + + + 1 2 3 1 2 3 + 2 3 4 + + + + + + + 1 2 3 1 2 3 + + + + + + + + 1 2 3 1 2 3 + 1 2 3 + + + + + + + 1 2 3 1 2 3 + + + + + + + + + 1 2 3 1 2 3 + 2 3 4 + 0.7071068 0.7071068 0 0 + + + +