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 <[email protected]>

Author: scpeters

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.

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
 {

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;
   }