From a428e2ad440a06f1b87ab9677562344ca1d3ed14 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Mon, 23 May 2022 11:47:32 -0700
Subject: [PATCH] Add Link::ResolveInertial API (#1012)

The Link::Inertial() method returns an Inertial object with a Pose
specified relative to the link frame. This adds a new method for
resolving the Inertial's Pose to a specified frame. The method
is demonstrated in the `ign sdf --inertial-states` command code.

Fixed #935.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 include/sdf/Link.hh | 10 ++++++++++
 src/Link.cc         | 15 +++++++++++++++
 src/ign.cc          |  8 ++------
 3 files changed, 27 insertions(+), 6 deletions(-)

diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh
index cb198416f..fc54da7b6 100644
--- a/include/sdf/Link.hh
+++ b/include/sdf/Link.hh
@@ -192,6 +192,16 @@ namespace sdf
     /// \sa const ignition::math::Inertiald &Inertial() const
     public: bool SetInertial(const ignition::math::Inertiald &_inertial);
 
+    /// \brief Resolve the Inertial to a specified frame.
+    /// If there are any errors resolving the Inertial, the output will not
+    /// be modified.
+    /// \param[out] _inertial The resolved Inertial.
+    /// \param[in] _resolveTo The Inertial will be resolved with respect to this
+    /// frame. If unset or empty, the default resolve-to frame will be used.
+    /// \return Errors in resolving pose.
+    public: Errors ResolveInertial(ignition::math::Inertiald &_inertial,
+                                   const std::string &_resolveTo = "") const;
+
     /// \brief Get the pose of the link. This is the pose of the link
     /// as specified in SDF (<link> <pose> ... </pose></link>).
     /// \return The pose of the link.
diff --git a/src/Link.cc b/src/Link.cc
index 0a4f7fdd5..1a2d965c1 100644
--- a/src/Link.cc
+++ b/src/Link.cc
@@ -349,6 +349,21 @@ bool Link::SetInertial(const ignition::math::Inertiald &_inertial)
   return _inertial.MassMatrix().IsValid();
 }
 
+/////////////////////////////////////////////////
+Errors Link::ResolveInertial(
+  ignition::math::Inertiald &_inertial,
+  const std::string &_resolveTo) const
+{
+  ignition::math::Pose3d linkPose;
+  auto errors = this->SemanticPose().Resolve(linkPose, _resolveTo);
+  if (errors.empty())
+  {
+    _inertial = this->dataPtr->inertial;
+    _inertial.SetPose(linkPose * _inertial.Pose());
+  }
+  return errors;
+}
+
 /////////////////////////////////////////////////
 const ignition::math::Pose3d &Link::Pose() const
 {
diff --git a/src/ign.cc b/src/ign.cc
index 3c490682d..06418efff 100644
--- a/src/ign.cc
+++ b/src/ign.cc
@@ -204,13 +204,9 @@ extern "C" SDFORMAT_VISIBLE int cmdInertialStats(
 
   for (uint64_t i = 0; i < model->LinkCount(); i++)
   {
-    ignition::math::Pose3d linkPoseRelativeToModel;
-    errors = model->LinkByIndex(i)->SemanticPose().
-      Resolve(linkPoseRelativeToModel, "__model__");
+    ignition::math::Inertiald currentLinkInertial;
+    model->LinkByIndex(i)->ResolveInertial(currentLinkInertial, "__model__");
 
-    auto currentLinkInertial = model->LinkByIndex(i)->Inertial();
-    currentLinkInertial.SetPose(linkPoseRelativeToModel *
-      currentLinkInertial.Pose());
     totalInertial += currentLinkInertial;
   }