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]>
gazebosim · May 23, 2022 · a428e2a · a428e2a
1 parent a978ade
commit a428e2a
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 6 deletions.
diff --git 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
@@ -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
@@ -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;
  }