Add SetInertial method

Signed-off-by: Atharva Pusalkar <atharvapusalkar18@gmail.com>

examples/visualization_demo/Main.cc

@@ -126,10 +126,11 @@ void buildScene(ScenePtr _scene)
 
   // create inertia visual
   InertiaVisualPtr inertiaVisual = _scene->CreateInertiaVisual();
+  ignition::math::MassMatrix3d massMatrix(1.0, {0.1, 0.1, 0.1}, {0.0, 0.0, 0.0});
   ignition::math::Pose3d p(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
-  ignition::math::Vector3d s(0.4, 0.4, 0.4);
-  inertiaVisual->Load(p, s);
-  inertiaVisual->SetLocalPosition(3.2, 0.5, 0);
+  ignition::math::Inertiald inertial{massMatrix, p};
+  inertiaVisual->SetInertial(inertial);
+  inertiaVisual->SetLocalPosition(3.2, 1.5, 0);
   root->AddChild(inertiaVisual);
 
   // create camera

include/ignition/rendering/InertiaVisual.hh

@@ -17,6 +17,7 @@
 #ifndef IGNITION_RENDERING_INERTIAVISUAL_HH_
 #define IGNITION_RENDERING_INERTIAVISUAL_HH_
 
+#include <ignition/math/Inertial.hh>
 #include "ignition/rendering/config.hh"
 #include "ignition/rendering/Object.hh"
 #include "ignition/rendering/RenderTypes.hh"
@@ -37,6 +38,11 @@ namespace ignition
       /// \brief Destructor
       public: virtual ~InertiaVisual() {}
 
+      /// \brief Set the inertial component of the visual
+      /// \param[in] _inertial Inertial component of the visual
+      public: virtual void SetInertial(
+                  const ignition::math::Inertiald &_inertial) = 0;
+
       /// \brief Load the Inertia visual from its pose and scale
       /// \param[in] _pose Pose of the Inertia visual
       /// \param[in] _scale Scale factor of the box visual

include/ignition/rendering/base/BaseInertiaVisual.hh

@@ -17,8 +17,6 @@
 #ifndef IGNITION_RENDERING_BASE_BASEINERTIAVISUAL_HH_
 #define IGNITION_RENDERING_BASE_BASEINERTIAVISUAL_HH_
 
-#include <vector>
-
 #include "ignition/rendering/base/BaseObject.hh"
 #include "ignition/rendering/base/BaseRenderTypes.hh"
 #include "ignition/rendering/InertiaVisual.hh"
@@ -48,6 +46,10 @@ namespace ignition
       // Documentation inherited.
       protected: virtual void PreRender() override;
 
+      // Documentation inherited.
+      public: virtual void SetInertial(
+                  const ignition::math::Inertiald &) override;
+
       // Documentation inherited.
       public: virtual void Load(const ignition::math::Pose3d &,
           const ignition::math::Vector3d &) override;
@@ -82,6 +84,13 @@ namespace ignition
       T::Init();
     }
 
+    //////////////////////////////////////////////////
+    template <class T>
+    void BaseInertiaVisual<T>::SetInertial(const ignition::math::Inertiald &)
+    {
+      // no op
+    }
+
     //////////////////////////////////////////////////
     template <class T>
     void BaseInertiaVisual<T>::Load(const ignition::math::Pose3d &,

ogre/include/ignition/rendering/ogre/OgreInertiaVisual.hh

@@ -57,6 +57,11 @@ namespace ignition
       // Documentation inherited.
       public: Ogre::MovableObject *OgreObject() const;
 
+      /// \brief Set the inertial component of the visual
+      /// \param[in] _inertial Inertial component of the visual
+      public: void SetInertial(
+              const ignition::math::Inertiald &_inertial) override;
+
       /// \brief Load the Inertia visual from its pose and scale
       /// \param[in] _pose Pose of the Inertia visual
       /// \param[in] _scale Scale factor of the box visual

ogre/src/OgreInertiaVisual.cc

@@ -65,6 +65,31 @@ Ogre::MovableObject *OgreInertiaVisual::OgreObject() const
   return mv.get();
 }
 
+//////////////////////////////////////////////////
+void OgreInertiaVisual::SetInertial(
+    const ignition::math::Inertiald &_inertial)
+{
+  auto xyz = _inertial.Pose().Pos();
+  auto q = _inertial.Pose().Rot();
+
+  // Use ignition::math::MassMatrix3 to compute
+  // equivalent box size and rotation
+  auto m = _inertial.MassMatrix();
+  ignition::math::Vector3d boxScale;
+  ignition::math::Quaterniond boxRot;
+  if (!m.EquivalentBox(boxScale, boxRot))
+  {
+    // Invalid inertia, load with default scale
+    ignlog << "The link is static or has unrealistic "
+        << "inertia, so the equivalent inertia box will not be shown.\n";
+  }
+  else
+  {
+    // Apply additional rotation by boxRot
+    this->Load(ignition::math::Pose3d(xyz, q * boxRot), boxScale);
+  }
+}
+
 //////////////////////////////////////////////////
 void OgreInertiaVisual::Load(const ignition::math::Pose3d &_pose,
                              const ignition::math::Vector3d &_scale)

ogre2/include/ignition/rendering/ogre2/Ogre2InertiaVisual.hh

@@ -52,6 +52,11 @@ namespace ignition
       // Documentation inherited.
       public: virtual void PreRender() override;
 
+      /// \brief Set the inertial component of the visual
+      /// \param[in] _inertial Inertial component of the visual
+      public: void SetInertial(
+              const ignition::math::Inertiald &_inertial) override;
+
       /// \brief Load the Inertia visual from its pose and scale
       /// \param[in] _pose Pose of the Inertia visual
       /// \param[in] _scale Scale factor of the box visual

ogre2/src/Ogre2InertiaVisual.cc

@@ -63,6 +63,31 @@ void Ogre2InertiaVisual::Init()
   BaseInertiaVisual::Init();
 }
 
+//////////////////////////////////////////////////
+void Ogre2InertiaVisual::SetInertial(
+    const ignition::math::Inertiald &_inertial)
+{
+  auto xyz = _inertial.Pose().Pos();
+  auto q = _inertial.Pose().Rot();
+
+  // Use ignition::math::MassMatrix3 to compute
+  // equivalent box size and rotation
+  auto m = _inertial.MassMatrix();
+  ignition::math::Vector3d boxScale;
+  ignition::math::Quaterniond boxRot;
+  if (!m.EquivalentBox(boxScale, boxRot))
+  {
+    // Invalid inertia, load with default scale
+    ignlog << "The link is static or has unrealistic "
+        << "inertia, so the equivalent inertia box will not be shown.\n";
+  }
+  else
+  {
+    // Apply additional rotation by boxRot
+    this->Load(ignition::math::Pose3d(xyz, q * boxRot), boxScale);
+  }
+}
+
 //////////////////////////////////////////////////
 void Ogre2InertiaVisual::Load(const ignition::math::Pose3d &_pose,
                               const ignition::math::Vector3d &_scale)

src/InertiaVisual_TEST.cc

@@ -57,10 +57,17 @@ void InertiaVisualTest::InertiaVisual(const std::string &_renderEngine)
   // check initial values
   EXPECT_EQ(nullptr, inertiaVisual->BoxVisual());
 
+  ignition::math::MassMatrix3d massMatrix(
+      2.0, {2.0, 1.5, 1.0}, {0.0, 0.0, 0.0});
   ignition::math::Pose3d p(0.0, 1.0, 2.5, 1.0, 0.4, 0.4);
-  ignition::math::Vector3d s(0.5, 0.5, 0.5);
-  inertiaVisual->Load(p, s);
+  ignition::math::Inertiald inertial;
 
+  inertiaVisual->SetInertial(inertial);
+  EXPECT_EQ(nullptr, inertiaVisual->BoxVisual());
+
+  inertial.SetMassMatrix(massMatrix);
+  inertial.SetPose(p);
+  inertiaVisual->SetInertial(inertial);
   EXPECT_NE(nullptr, inertiaVisual->BoxVisual());
 
   // Clean up