New pose proposal from #252

Signed-off-by: Aaron Chong <[email protected]>
gazebosim · Jun 2, 2021 · 228351d · 228351d
1 parent b588a5f
commit 228351d
Show file tree

Hide file tree

Showing 5 changed files with 297 additions and 11 deletions.
diff --git a/sdf/1.9/pose.sdf b/sdf/1.9/pose.sdf
@@ -1,11 +1,37 @@
 <!-- Pose -->
 <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
-  <description>A position(x,y,z) and orientation(roll, pitch yaw) with respect to the frame named in the relative_to attribute.</description>
+  <description>A position(x,y,z) and orientation(roll, pitch, yaw) with respect to the frame named in the relative_to attribute.</description>
 
   <attribute name="relative_to" type="string" default="" required="*">
     <description>
       Name of frame relative to which the pose is applied.
     </description>
   </attribute>
 
+  <element name="translation" type="vector3" default="0 0 0" required="1">
+    <description>The position(x,y,z) with respect to the frame named in the relative_to attribute.</description>
+  </element>
+
+  <element name="rotation" type="vector3" default="0 0 0" required="1">
+    <description>The rotation with respect to the frame named in the relative_to attribute.</description>
+    <attribute name="type" type="string" default="__default__" required="1">
+      <description>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).
+      </description>
+    </attribute>
+  </element>
+
+  <element name="rotation" type="quaternion" default="1 0 0 0" required="1">
+    <description>The rotation with respect to the frame named in the relative_to attribute.</description>
+    <attribute name="type" type="string" default="__default__" required="1">
+      <description>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).
+      </description>
+    </attribute>
+  </element>
+
 </element> <!-- End Pose -->
diff --git 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<std::string, bool> framePair =
       sdf->Get<std::string>("relative_to", "");
 
-  // Read the pose value.
-  std::pair<ignition::math::Pose3d, bool> posePair =
-    sdf->Get<ignition::math::Pose3d>("", 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<ignition::math::Pose3d, bool> posePair =
+        sdf->Get<ignition::math::Pose3d>("", 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<ignition::math::Vector3d, bool> translationPair =
+        translationPtr->Get<ignition::math::Vector3d>(
+            "", 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<std::string, bool> typePair =
+        rotationPtr->Get<std::string>("type", "");
+    if (!typePair.second)
+      return false;
+
+    if (typePair.first == "rpy_degrees")
+    {
+      std::cout << "found rpy deg" << std::endl;
+      std::pair<ignition::math::Vector3d, bool> rpyDegPair =
+          rotationPtr->Get<ignition::math::Vector3d>(
+              "", 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<ignition::math::Vector3d, bool> rpyRadPair =
+          rotationPtr->Get<ignition::math::Vector3d>(
+              "", 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<ignition::math::Quaterniond, bool> quatPair =
+          rotationPtr->Get<ignition::math::Quaterniond>(
+              "", 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
@@ -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
@@ -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 <string>
+#include <gtest/gtest.h>
+
+#include <ignition/math/Angle.hh>
+#include <ignition/math/Pose3.hh>
+#include <ignition/math/Vector3.hh>
+#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
@@ -0,0 +1,81 @@
+<?xml version="1.0" ?>
+<sdf version="1.9">
+  <world name="default">
+
+    <model name="model_with_empty_pose">
+      <pose></pose>
+      <link name="link"/>
+    </model>
+
+    <model name="model_with_pose_value">
+      <pose>1 2 3 1 2 3</pose>
+      <link name="link"/>
+    </model>
+
+    <model name="model_with_empty_translation">
+      <pose>
+        <translation></translation>
+      </pose>
+      <link name="link"/>
+    </model>
+
+    <model name="model_with_translation_value">
+      <pose>
+        <translation>2 3 4</translation>
+      </pose>
+      <link name="link"/>
+    </model>
+
+    <model name="model_with_empty_rpy_deg">
+      <pose>
+        <rotation type="rpy_degrees"></rotation>
+      </pose>
+      <link name="link"/>
+    </model>
+
+    <model name="model_with_rpy_deg_value">
+      <pose>
+        <rotation type="rpy_degrees">0 90 180</rotation>
+      </pose>
+      <link name="link"/>
+    </model>
+
+    <model name="model_with_empty_rpy_rad">
+      <pose>
+        <rotation type="rpy_radians"></rotation>
+      </pose>
+      <link name="link"/>
+    </model>
+
+    <model name="model_with_rpy_rad_value">
+      <pose>
+        <rotation type="rpy_radians">1 2 3</rotation>
+      </pose>
+      <link name="link"/>
+    </model>
+
+    <model name="model_with_empty_quat">
+      <pose>
+        <rotation type="q_wxyz"></rotation>
+      </pose>
+      <link name="link"/>
+    </model>
+
+    <model name="model_with_quat_value">
+      <pose>
+        <rotation type="q_wxyz">0.7071068 0.7071068 0 0</rotation>
+      </pose>
+      <link name="link"/>
+    </model>
+
+    <model name="model_with_both_rotations">
+      <pose>
+        <rotation type="rpy_radians"></rotation>
+        <rotation type="q_wxyz"></rotation>
+      </pose>
+      <link name="link"/>
+    </model>
+
+  </world>
+</sdf>
+