New pose proposal (#252)

sdf/1.9/pose.sdf

@@ -1,11 +1,26 @@
 <!-- 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="rotation" 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 -->

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<double> 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<ignition::math::Quaterniond>(
           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);
 }
+

src/Utils.cc

@@ -56,24 +56,82 @@ 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);
+  // Start checking for translation and rotation elements.
+  sdf::ElementPtr translationPtr = sdf->GetElement("translation");
+  sdf::ElementPtr rotationPtr = sdf->GetElement("rotation");
 
-  // Set output, but only if the return value is true.
-  if (posePair.second)
+  // 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)
+      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::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::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::pair<ignition::math::Quaterniond, bool> quatPair =
+          rotationPtr->Get<ignition::math::Quaterniond>(
+              "", ignition::math::Quaterniond::Identity);
+      if (quatPair.second)
+      {
+        pose.Set(pose.Pos(), quatPair.first);
+      }
+    }
+    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;
 }
 
 /////////////////////////////////////////////////

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