Support quaternions representation for poses (#690)

Support quaternions in the order of (x, y, z, w) for pose elements, using an new attribute '//pose[@rotation_format]', which is 'euler_rpy' by default, but can be set to 'quat_xyzw'.

Signed-off-by: Aaron Chong <[email protected]>
Signed-off-by: Addisu Z. Taddese <[email protected]>

include/sdf/Param.hh

@@ -204,7 +204,9 @@ namespace sdf
     /// \brief Set the parent Element of this Param.
     /// \param[in] _parentElement Pointer to new parent Element. A nullptr can
     /// provided to remove the current parent Element.
-    public: void SetParentElement(ElementPtr _parentElement);
+    /// \return True if the parent Element was set and the value was reparsed
+    /// successfully.
+    public: bool SetParentElement(ElementPtr _parentElement);
 
     /// \brief Reset the parameter to the default value.
     public: void Reset();
@@ -371,7 +373,10 @@ namespace sdf
     public: bool ignoreParentAttributes;
 
     /// \brief This parameter's value that was provided as a string
-    public: std::optional<std::string> strValue;
+    public: std::string strValue;
+
+    /// \brief This parameter's default value that was provided as a string
+    public: std::string defaultStrValue;
 
     /// \brief This parameter's default value
     public: ParamVariant defaultValue;

sdf/1.9/inertial.sdf

@@ -7,7 +7,23 @@
   </element>
 
   <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
-    <description>This is the pose of the inertial reference frame. The origin of the inertial reference frame needs to be at the center of gravity. The axes of the inertial reference frame do not need to be aligned with the principal axes of the inertia.</description>
+    <description>
+      A pose (translation, rotation) expressed in the frame of the link. The first three components (x, y, z) represent the position of the element's origin (in the @relative_to frame). The rotation component represents the orientation of the element as either a sequence of Euler rotations (r, p, y), see http://sdformat.org/tutorials?tut=specify_pose, or as a quaternion (x, y, z, w), where w is the real component.
+    </description>
+
+    <attribute name="rotation_format" type="string" default="euler_rpy" required="0">
+      <description>'euler_rpy' by default. Supported rotation formats are
+        'euler_rpy', Euler angles representation in roll, pitch, yaw. The pose is expected to have 6 values.
+        'quat_xyzw', Quaternion representation in x, y, z, w. The pose is expected to have 7 values.
+      </description>
+    </attribute>
+
+    <attribute name="degrees" type="bool" default="false" required="0">
+      <description>
+        Whether or not the euler angles are in degrees, otherwise they will be interpreted as radians by default.
+      </description>
+    </attribute>
+
   </element>
 
   <element name="inertia" required="0">

sdf/1.9/pose.sdf

@@ -1,7 +1,7 @@
 <!-- Pose -->
-<element name="pose" type="pose" default="0 0 0   0 0 0" required="0">
+<element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
   <description>
-    A pose (x, y, z, r, p, y) expressed in the frame named by @relative_to. The first three components (x, y, z) represent the position of the element's origin (in the @relative_to frame) and the last three components (roll, pitch, yaw) represent the orientation of the element as a sequence of Euler rotations. See http://sdformat.org/tutorials?tut=specify_pose.
+    A pose (translation, rotation) expressed in the frame named by @relative_to. The first three components (x, y, z) represent the position of the element's origin (in the @relative_to frame). The rotation component represents the orientation of the element as either a sequence of Euler rotations (r, p, y), see http://sdformat.org/tutorials?tut=specify_pose, or as a quaternion (x, y, z, w), where w is the real component.
   </description>
 
   <attribute name="relative_to" type="string" default="" required="0">
@@ -10,6 +10,13 @@
     </description>
   </attribute>
 
+  <attribute name="rotation_format" type="string" default="euler_rpy" required="0">
+    <description>'euler_rpy' by default. Supported rotation formats are
+      'euler_rpy', Euler angles representation in roll, pitch, yaw. The pose is expected to have 6 values.
+      'quat_xyzw', Quaternion representation in x, y, z, w. The pose is expected to have 7 values.
+    </description>
+  </attribute>
+
   <attribute name="degrees" type="bool" default="false" required="0">
     <description>
       Whether or not the euler angles are in degrees, otherwise they will be interpreted as radians by default.

src/Element.cc

@@ -152,7 +152,8 @@ void Element::AddValue(const std::string &_type,
   this->dataPtr->value =
       std::make_shared<Param>(this->dataPtr->name, _type, _defaultValue,
                               _required, _minValue, _maxValue, _description);
-  this->dataPtr->value->SetParentElement(shared_from_this());
+  SDF_ASSERT(this->dataPtr->value->SetParentElement(shared_from_this()),
+      "Cannot set parent Element of value to itself.");
 }
 
 /////////////////////////////////////////////////
@@ -164,7 +165,8 @@ ParamPtr Element::CreateParam(const std::string &_key,
 {
   ParamPtr param = std::make_shared<Param>(
       _key, _type, _defaultValue, _required, _description);
-  param->SetParentElement(shared_from_this());
+  SDF_ASSERT(param->SetParentElement(shared_from_this()),
+      "Cannot set parent Element of created Param to itself.");
   return param;
 }
 
@@ -199,7 +201,9 @@ ElementPtr Element::Clone() const
        aiter != this->dataPtr->attributes.end(); ++aiter)
   {
     auto clonedAttribute = (*aiter)->Clone();
-    clonedAttribute->SetParentElement(clone);
+    SDF_ASSERT(clonedAttribute->SetParentElement(clone),
+        "Cannot set parent Element of cloned attribute Param to cloned "
+        "Element.");
     clone->dataPtr->attributes.push_back(clonedAttribute);
   }
 
@@ -220,7 +224,8 @@ ElementPtr Element::Clone() const
   if (this->dataPtr->value)
   {
     clone->dataPtr->value = this->dataPtr->value->Clone();
-    clone->dataPtr->value->SetParentElement(clone);
+    SDF_ASSERT(clone->dataPtr->value->SetParentElement(clone),
+        "Cannot set parent Element of cloned value Param to cloned Element.");
   }
 
   if (this->dataPtr->includeElement)
@@ -254,7 +259,8 @@ void Element::Copy(const ElementPtr _elem)
     }
     ParamPtr param = this->GetAttribute((*iter)->GetKey());
     (*param) = (**iter);
-    param->SetParentElement(shared_from_this());
+    SDF_ASSERT(param->SetParentElement(shared_from_this()),
+        "Cannot set parent Element of copied attribute Param to itself.");
   }
 
   if (_elem->GetValue())
@@ -267,7 +273,8 @@ void Element::Copy(const ElementPtr _elem)
     {
       *(this->dataPtr->value) = *(_elem->GetValue());
     }
-    this->dataPtr->value->SetParentElement(shared_from_this());
+    SDF_ASSERT(this->dataPtr->value->SetParentElement(shared_from_this()),
+        "Cannot set parent Element of copied value Param to itself.");
   }
 
   this->dataPtr->elementDescriptions.clear();