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

Signed-off-by: Addisu Z. Taddese <[email protected]>
gazebosim · Sep 20, 2021 · fcac14d · scpeters · Sep 20, 2021 · jennuine
1 parent 7b09f70
commit fcac14d
Show file tree

Hide file tree

Showing 3 changed files with 55 additions and 2 deletions.
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>