Center of mass visual

examples/visualization_demo/Main.cc

@@ -108,6 +108,25 @@ void buildScene(ScenePtr _scene)
   sphere->SetWireframe(true);
   root->AddChild(sphere);
 
+  // create blue material
+  MaterialPtr blue = _scene->CreateMaterial();
+  blue->SetAmbient(0.0, 0.0, 0.5);
+  blue->SetDiffuse(0.0, 0.0, 1.0);
+  blue->SetSpecular(0.5, 0.5, 0.5);
+  blue->SetShininess(50);
+  blue->SetReflectivity(0);
+  blue->SetTransparency(0.5);
+  blue->SetDepthWriteEnabled(false);
+
+  // create box visual
+  VisualPtr box = _scene->CreateVisual("parent_box");
+  box->AddGeometry(_scene->CreateBox());
+  box->SetOrigin(0.0, 0.0, 0.0);
+  box->SetLocalPosition(4.5, 0.0, 0.0);
+  box->SetLocalRotation(0, 0, 0);
+  box->SetMaterial(blue);
+  root->AddChild(box);
+
   // create white material
   MaterialPtr white = _scene->CreateMaterial();
   white->SetAmbient(0.5, 0.5, 0.5);
@@ -130,9 +149,19 @@ void buildScene(ScenePtr _scene)
   ignition::math::Pose3d p(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
   ignition::math::Inertiald inertial{massMatrix, p};
   inertiaVisual->SetInertial(inertial);
-  inertiaVisual->SetLocalPosition(3.2, 1.5, 0);
+  inertiaVisual->SetLocalPosition(1, 0, 0);
   root->AddChild(inertiaVisual);
 
+  // create CoM visual
+  COMVisualPtr comVisual = _scene->CreateCOMVisual();
+  ignition::math::MassMatrix3d comMassMatrix(
+      5.0, {0.1, 0.1, 0.1}, {0.0, 0.0, 0.0});
+  ignition::math::Pose3d comPose(
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+  ignition::math::Inertiald comVisualInertial{comMassMatrix, comPose};
+  comVisual->SetInertial(comVisualInertial);
+  box->AddChild(comVisual);
+
   // create camera
   CameraPtr camera = _scene->CreateCamera("camera");
   camera->SetLocalPosition(0.0, 0.0, 0.0);

include/ignition/rendering/COMVisual.hh

@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#ifndef IGNITION_RENDERING_COMVISUAL_HH_
+#define IGNITION_RENDERING_COMVISUAL_HH_
+
+#include <string>
+#include <ignition/math/Inertial.hh>
+#include "ignition/rendering/config.hh"
+#include "ignition/rendering/Object.hh"
+#include "ignition/rendering/RenderTypes.hh"
+#include "ignition/rendering/Visual.hh"
+
+namespace ignition
+{
+  namespace rendering
+  {
+    inline namespace IGNITION_RENDERING_VERSION_NAMESPACE {
+
+    /// \class COMVisual COMVisual.hh
+    /// ignition/rendering/COMVisual.hh
+    /// \brief Represents a center of mass visual
+    class IGNITION_RENDERING_VISIBLE COMVisual :
+      public virtual Visual
+    {
+      /// \brief Destructor
+      public: virtual ~COMVisual() {}
+
+      /// \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 Set the mass of the parent
+      /// \param[in] _mass Parent mass
+      public: virtual void SetMass(double _mass) = 0;
+
+      /// \brief Get the mass of the parent
+      /// \return Parent mass
+      public: virtual double Mass() const = 0;
+
+      /// \brief Get the inertia pose
+      /// \return Inertia pose in parent frame.
+      public: virtual ignition::math::Pose3d InertiaPose() const = 0;
+
+      /// \brief Get the sphere visual
+      /// \return Pointer to the sphere visual
+      public: virtual VisualPtr SphereVisual() const = 0;
+    };
+    }
+  }
+}
+#endif

include/ignition/rendering/RenderTypes.hh

@@ -50,6 +50,7 @@ namespace ignition
     class AxisVisual;
     class Camera;
     class Capsule;
+    class COMVisual;
     class DepthCamera;
     class DirectionalLight;
     class GaussianNoisePass;
@@ -158,6 +159,10 @@ namespace ignition
     /// \brief Shared pointer to Light
     typedef shared_ptr<Light> LightPtr;
 
+    /// \def COMVisualPtr
+    /// \brief Shared pointer to COMVisual
+    typedef shared_ptr<COMVisual> COMVisualPtr;
+
     /// \def LightVisualPtr
     /// \brief Shared pointer to Light
     typedef shared_ptr<LightVisual> LightVisualPtr;

include/ignition/rendering/Scene.hh

@@ -823,6 +823,35 @@ namespace ignition
       public: virtual GizmoVisualPtr CreateGizmoVisual(
                   unsigned int _id, const std::string &_name) = 0;
 
+      /// \brief Create new CoM visual. A unique ID and name will
+      /// automatically be assigned to the CoM visual.
+      /// \return The created CoM visual
+      public: virtual COMVisualPtr CreateCOMVisual() = 0;
+
+      /// \brief Create new CoM visual with the given ID. A unique name
+      /// will automatically be assigned to the visual. If the given ID is
+      /// already in use, NULL will be returned.
+      /// \param[in] _id ID of the new CoM visual
+      /// \return The created CoM visual
+      public: virtual COMVisualPtr CreateCOMVisual(
+                  unsigned int _id) = 0;
+
+      /// \brief Create new CoM visual with the given name. A unique ID
+      /// will automatically be assigned to the visual. If the given name is
+      /// already in use, NULL will be returned.
+      /// \param[in] _name Name of the new CoM visual
+      /// \return The created CoM visual
+      public: virtual COMVisualPtr CreateCOMVisual(
+                  const std::string &_name) = 0;
+
+      /// \brief Create new CoM visual with the given name. If either the
+      /// given ID or name is already in use, NULL will be returned.
+      /// \param[in] _id ID of the new CoM visual
+      /// \param[in] _name Name of the new CoM visual
+      /// \return The created CoM visual
+      public: virtual COMVisualPtr CreateCOMVisual(
+                  unsigned int _id, const std::string &_name) = 0;
+
       /// \brief Create new inertia visual. A unique ID and name will
       /// automatically be assigned to the inertia visual.
       /// \return The created inertia visual

include/ignition/rendering/base/BaseCOMVisual.hh

@@ -0,0 +1,183 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#ifndef IGNITION_RENDERING_BASE_BASECOMVISUAL_HH_
+#define IGNITION_RENDERING_BASE_BASECOMVISUAL_HH_
+
+#include <string>
+
+#include "ignition/common/Console.hh"
+
+#include "ignition/rendering/base/BaseObject.hh"
+#include "ignition/rendering/base/BaseRenderTypes.hh"
+#include "ignition/rendering/COMVisual.hh"
+#include "ignition/rendering/Scene.hh"
+
+namespace ignition
+{
+  namespace rendering
+  {
+    inline namespace IGNITION_RENDERING_VERSION_NAMESPACE {
+    //
+    /// \brief Base implementation of an center of mass visual
+    template <class T>
+    class BaseCOMVisual :
+      public virtual COMVisual,
+      public virtual T
+    {
+      /// \brief Constructor
+      protected: BaseCOMVisual();
+
+      /// \brief Destructor
+      public: virtual ~BaseCOMVisual();
+
+      // Documentation inherited.
+      protected: virtual void Init() override;
+
+      // Documentation inherited.
+      protected: virtual void PreRender() override;
+
+      // Documentation inherited.
+      public: virtual void SetInertial(
+                  const ignition::math::Inertiald &_inertial) override;
+
+      // Documentation inherited.
+      public: virtual void SetMass(double _mass) override;
+
+      // Documentation inherited
+      public: virtual double Mass() const override;
+
+      // Documentation inherited
+      public: virtual ignition::math::Pose3d InertiaPose() const override;
+
+      // Documentation inherited
+      public: virtual VisualPtr SphereVisual() const override;
+
+      /// \brief Get the radius of the CoM sphere
+      /// \return Radius of the CoM sphere
+      protected: double SphereRadius() const;
+
+      /// \brief Parent visual name.
+      protected: std::string parentName = "";
+
+      /// \brief Parent mass.
+      protected: double mass = 1.0;
+
+      /// \brief Inertia pose in parent frame.
+      protected: ignition::math::Pose3d inertiaPose =
+          ignition::math::Pose3d::Zero;
+
+      /// \brief Flag to indicate parent properties have changed.
+      protected: bool dirtyCOMVisual = false;
+    };
+
+    //////////////////////////////////////////////////
+    template <class T>
+    BaseCOMVisual<T>::BaseCOMVisual()
+    {
+    }
+
+    //////////////////////////////////////////////////
+    template <class T>
+    BaseCOMVisual<T>::~BaseCOMVisual()
+    {
+    }
+
+    /////////////////////////////////////////////////
+    template <class T>
+    void BaseCOMVisual<T>::PreRender()
+    {
+      T::PreRender();
+    }
+
+    //////////////////////////////////////////////////
+    template <class T>
+    void BaseCOMVisual<T>::Init()
+    {
+      T::Init();
+    }
+
+    //////////////////////////////////////////////////
+    template <class T>
+    void BaseCOMVisual<T>::SetInertial(
+          const ignition::math::Inertiald &_inertial)
+    {
+      this->inertiaPose = _inertial.Pose();
+
+      this->SetMass(_inertial.MassMatrix().Mass());
+    }
+
+    template <class T>
+    void BaseCOMVisual<T>::SetMass(double _mass)
+    {
+      if (_mass <= 0)
+      {
+        // Unrealistic mass, load with default mass
+        if (_mass < 0)
+        {
+          ignlog << "The parent " << this->parentName
+              << " has unrealistic mass, "
+              << "unable to visualize sphere of equivalent mass.\n";
+        }
+        else
+        {
+          ignlog << "The parent " << this->parentName
+              << " is static or has mass of 0, "
+              << "so a sphere of equivalent mass will not be shown.\n";
+        }
+        return;
+      }
+
+      this->mass = _mass;
+      this->dirtyCOMVisual = true;
+    }
+
+    //////////////////////////////////////////////////
+    template <class T>
+    double BaseCOMVisual<T>::Mass() const
+    {
+      return this->mass;
+    }
+
+    //////////////////////////////////////////////////
+    template <class T>
+    ignition::math::Pose3d BaseCOMVisual<T>::InertiaPose() const
+    {
+      return this->inertiaPose;
+    }
+
+    //////////////////////////////////////////////////
+    template <class T>
+    VisualPtr BaseCOMVisual<T>::SphereVisual() const
+    {
+      return nullptr;
+    }
+
+    //////////////////////////////////////////////////
+    template <class T>
+    double BaseCOMVisual<T>::SphereRadius() const
+    {
+      // Compute radius of sphere with density of lead and equivalent mass.
+      double sphereRadius;
+      double densityLead = 11340;
+      sphereRadius = cbrt((0.75 * this->Mass()) / (IGN_PI * densityLead));
+
+      return sphereRadius;
+    }
+    }
+  }
+}
+#endif

include/ignition/rendering/base/BaseScene.hh

@@ -265,6 +265,12 @@ namespace ignition
       public: virtual PointLightPtr CreatePointLight(unsigned int _id,
                   const std::string &_name) override;
 
+      /// \brief Implementation for creating CoM visual.
+      /// \param[in] _id Unique id
+      /// \param[in] _name Name of CoM visual
+      protected: virtual COMVisualPtr CreateCOMVisualImpl(unsigned int _id,
+                     const std::string &_name) = 0;
+
       /// \brief Implementation for creating Inertia visual.
       /// \param[in] _id Unique id
       /// \param[in] _name Name of inertia visual
@@ -367,6 +373,20 @@ namespace ignition
                   const std::string &_name) override;
 
       // Documentation inherited
+      public: virtual COMVisualPtr CreateCOMVisual() override;
+
+      // Documentation inherited
+      public: virtual COMVisualPtr CreateCOMVisual(unsigned int _id)
+                      override;
+
+      // Documentation inherited
+      public: virtual COMVisualPtr CreateCOMVisual(const std::string &_name)
+                      override;
+
+      // Documentation inherited
+      public: virtual COMVisualPtr CreateCOMVisual(unsigned int _id,
+                  const std::string &_name) override;
+
       public: virtual InertiaVisualPtr CreateInertiaVisual() override;
 
       // Documentation inherited