Add @rotation_format and @Degrees to //inertial/pose

Addisu Z. Taddese · Addisu Z. Taddese · commit fcac14d3c935 · 2021-09-20T14:39:39.000-05:00
Signed-off-by: Addisu Z. Taddese &lt;addisu@openrobotics.org&gt;
diff --git a/sdf/1.9/inertial.sdf b/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 represent 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">
diff --git a/test/integration/pose_1_9_sdf.cc b/test/integration/pose_1_9_sdf.cc
@@ -23,8 +23,10 @@
 #include <ignition/math/Angle.hh>
 #include <ignition/math/Pose3.hh>
 #include <ignition/math/Vector3.hh>
+#include "sdf/Actor.hh"
 #include "sdf/Element.hh"
 #include "sdf/Error.hh"
+#include "sdf/Link.hh"
 #include "sdf/Model.hh"
 #include "sdf/Root.hh"
 #include "sdf/World.hh"
@@ -34,7 +36,7 @@
 #include "test_utils.hh"
 
 //////////////////////////////////////////////////
-TEST(Pose1_9, ModelPose)
+TEST(Pose1_9, PoseExpressionFormats)
 {
   using Pose = ignition::math::Pose3d;
 
@@ -136,6 +138,25 @@ TEST(Pose1_9, ModelPose)
   ASSERT_NE(nullptr, model);
   ASSERT_EQ("model_quat_xyzw_degrees_false", model->Name());
   EXPECT_EQ(Pose(1, 2, 3, 0.7071068, 0.7071068, 0, 0), model->RawPose());
+
+  // //inertial/pose
+  model = world->ModelByIndex(17);
+  ASSERT_NE(nullptr, model);
+  ASSERT_EQ("model_with_inertia_pose", model->Name());
+  {
+    const auto link = model->LinkByIndex(0);
+    ASSERT_NE(nullptr, link);
+    ASSERT_EQ("link_euler_rpy_degrees_true", link->Name());
+    EXPECT_EQ(Pose(1, 2, 3, IGN_DTOR(90), IGN_DTOR(180), IGN_DTOR(270)),
+              link->Inertial().Pose());
+  }
+  {
+    const auto link = model->LinkByIndex(1);
+    ASSERT_NE(nullptr, link);
+    ASSERT_EQ("link_quat_xyzw", link->Name());
+    EXPECT_EQ(Pose(1, 2, 3, 0.7071068, 0.7071068, 0, 0),
+              link->Inertial().Pose());
+  }
 }
 
 //////////////////////////////////////////////////
diff --git a/test/sdf/pose_1_9.sdf b/test/sdf/pose_1_9.sdf
@@ -101,5 +101,21 @@
       <link name="link"/>
     </model>
 
+    <model name="model_with_inertia_pose">
+      <link name="link_euler_rpy_degrees_true">
+        <inertial>
+          <pose rotation_format="euler_rpy" degrees="true">1 2 3   90 180 270</pose>
+          <mass>10</mass>
+        </inertial>
+      </link>
+      <link name="link_quat_xyzw">
+        <inertial>
+          <pose rotation_format="quat_xyzw" degrees="false">
+            1 2 3   0.7071068 0 0 0.7071068
+          </pose>
+          <mass>10</mass>
+        </inertial>
+      </link>
+    </model>
   </world>
 </sdf>
