From 1f47f13159f11ce391ee16637cd7aeb8b77adba1 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Thu, 10 Jun 2021 12:55:30 +0530 Subject: [PATCH 01/20] Add base inertia impl Signed-off-by: Atharva Pusalkar --- include/ignition/rendering/COMVisual.hh | 63 ++++++++ .../ignition/rendering/base/BaseCOMVisual.hh | 146 ++++++++++++++++++ 2 files changed, 209 insertions(+) create mode 100644 include/ignition/rendering/COMVisual.hh create mode 100644 include/ignition/rendering/base/BaseCOMVisual.hh diff --git a/include/ignition/rendering/COMVisual.hh b/include/ignition/rendering/COMVisual.hh new file mode 100644 index 000000000..d09d9280a --- /dev/null +++ b/include/ignition/rendering/COMVisual.hh @@ -0,0 +1,63 @@ +/* + * 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 +#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 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 + public: virtual void Load(const ignition::math::Pose3d &_pose, + const ignition::math::Vector3d &_scale) = 0; + + /// \brief Get inertia pose. + /// \return Inertia pose in link frame. + public: ignition::math::Pose3d InertiaPose() const; + + /// \brief Get the sphere visual + /// \return Pointer to the sphere visual + public: virtual VisualPtr SphereVisual() const = 0; + }; + } + } +} +#endif diff --git a/include/ignition/rendering/base/BaseCOMVisual.hh b/include/ignition/rendering/base/BaseCOMVisual.hh new file mode 100644 index 000000000..a555291fe --- /dev/null +++ b/include/ignition/rendering/base/BaseCOMVisual.hh @@ -0,0 +1,146 @@ +/* + * 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 "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 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 Load(const ignition::math::Pose3d &, + const ignition::math::Vector3d &) override; + + // Documentation inherited + public: ignition::math::Pose3d InertiaPose() const; + + // Documentation inherited + public: virtual VisualPtr SphereVisual() const override; + + /// \brief Inertia pose in link frame. + private: ignition::math::Pose3d inertiaPose = + ignition::math::Pose3d::Zero; + }; + + ////////////////////////////////////////////////// + template + BaseCOMVisual::BaseCOMVisual() + { + } + + ////////////////////////////////////////////////// + template + BaseCOMVisual::~BaseCOMVisual() + { + } + + ///////////////////////////////////////////////// + template + void BaseCOMVisual::PreRender() + { + T::PreRender(); + } + + ////////////////////////////////////////////////// + template + void BaseCOMVisual::Init() + { + T::Init(); + } + + ////////////////////////////////////////////////// + template + void BaseCOMVisual::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); + } + } + + ////////////////////////////////////////////////// + template + void BaseCOMVisual::Load(const ignition::math::Pose3d &, + const ignition::math::Vector3d &) + { + // no op + } + + ////////////////////////////////////////////////// + template + VisualPtr BaseCOMVisual::InertiaPose() const + { + return this->inertiaPose; + } + + ////////////////////////////////////////////////// + template + VisualPtr BaseCOMVisual::BoxVisual() const + { + return nullptr; + } + } + } +} +#endif From 3107995ec8dc2d4b4712e6049660b64d3204b437 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Mon, 14 Jun 2021 15:09:08 +0530 Subject: [PATCH 02/20] Add Ogre1 implementation Signed-off-by: Atharva Pusalkar --- include/ignition/rendering/COMVisual.hh | 25 ++- include/ignition/rendering/RenderTypes.hh | 5 + include/ignition/rendering/Scene.hh | 29 +++ .../ignition/rendering/base/BaseCOMVisual.hh | 82 ++++--- include/ignition/rendering/base/BaseScene.hh | 21 ++ .../ignition/rendering/ogre/OgreCOMVisual.hh | 85 ++++++++ .../rendering/ogre/OgreRenderTypes.hh | 2 + .../ignition/rendering/ogre/OgreScene.hh | 4 + ogre/src/OgreCOMVisual.cc | 201 ++++++++++++++++++ ogre/src/OgreScene.cc | 10 + ogre/src/media/materials/CMakeLists.txt | 1 + .../media/materials/textures/CMakeLists.txt | 4 + ogre/src/media/materials/textures/com.png | Bin 0 -> 444 bytes .../rendering/ogre2/Ogre2COMVisual.hh | 16 ++ .../rendering/ogre2/Ogre2RenderTypes.hh | 2 + .../ignition/rendering/ogre2/Ogre2Scene.hh | 4 + ogre2/src/Ogre2Scene.cc | 11 + src/base/BaseScene.cc | 46 ++++ 18 files changed, 514 insertions(+), 34 deletions(-) create mode 100644 ogre/include/ignition/rendering/ogre/OgreCOMVisual.hh create mode 100644 ogre/src/OgreCOMVisual.cc create mode 100644 ogre/src/media/materials/textures/CMakeLists.txt create mode 100644 ogre/src/media/materials/textures/com.png create mode 100644 ogre2/include/ignition/rendering/ogre2/Ogre2COMVisual.hh diff --git a/include/ignition/rendering/COMVisual.hh b/include/ignition/rendering/COMVisual.hh index d09d9280a..90ca44ee2 100644 --- a/include/ignition/rendering/COMVisual.hh +++ b/include/ignition/rendering/COMVisual.hh @@ -17,6 +17,7 @@ #ifndef IGNITION_RENDERING_COMVISUAL_HH_ #define IGNITION_RENDERING_COMVISUAL_HH_ +#include #include #include "ignition/rendering/config.hh" #include "ignition/rendering/Object.hh" @@ -43,15 +44,27 @@ namespace ignition 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 - public: virtual void Load(const ignition::math::Pose3d &_pose, - const ignition::math::Vector3d &_scale) = 0; + /// \brief Set the mass of the parent link + /// \param[in] _mass Parent link mass + public: virtual void SetMass( + const double &_mass) = 0; + + /// \brief Set the parent link of the visual + /// \param[in] _parentLink Parent link of the visual + public: virtual void SetParentLink( + const std::string &_parentLink) = 0; + + /// \brief Get the parent link name of the visual + /// \return Link name of the visual + public: virtual std::string ParentLink() const = 0; + + /// \brief Get the mass of the link + /// \return Link mass + public: virtual double Mass() const = 0; /// \brief Get inertia pose. /// \return Inertia pose in link frame. - public: ignition::math::Pose3d InertiaPose() const; + public: virtual ignition::math::Pose3d InertiaPose() const = 0; /// \brief Get the sphere visual /// \return Pointer to the sphere visual diff --git a/include/ignition/rendering/RenderTypes.hh b/include/ignition/rendering/RenderTypes.hh index 23ff5e202..8d43f702c 100644 --- a/include/ignition/rendering/RenderTypes.hh +++ b/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; @@ -153,6 +154,10 @@ namespace ignition /// \brief Shared pointer to Light typedef shared_ptr LightPtr; + /// \def COMVisualPtr + /// \brief Shared pointer to COMVisual + typedef shared_ptr COMVisualPtr; + /// \def LightVisualPtr /// \brief Shared pointer to Light typedef shared_ptr LightVisualPtr; diff --git a/include/ignition/rendering/Scene.hh b/include/ignition/rendering/Scene.hh index b61404580..8df6273d7 100644 --- a/include/ignition/rendering/Scene.hh +++ b/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 light visual. A unique ID and name will /// automatically be assigned to the light visual. /// \return The created light visual diff --git a/include/ignition/rendering/base/BaseCOMVisual.hh b/include/ignition/rendering/base/BaseCOMVisual.hh index a555291fe..49190bd89 100644 --- a/include/ignition/rendering/base/BaseCOMVisual.hh +++ b/include/ignition/rendering/base/BaseCOMVisual.hh @@ -17,6 +17,8 @@ #ifndef IGNITION_RENDERING_BASE_BASECOMVISUAL_HH_ #define IGNITION_RENDERING_BASE_BASECOMVISUAL_HH_ +#include + #include "ignition/common/Console.hh" #include "ignition/rendering/base/BaseObject.hh" @@ -53,18 +55,37 @@ namespace ignition const ignition::math::Inertiald &_inertial) override; // Documentation inherited. - public: virtual void Load(const ignition::math::Pose3d &, - const ignition::math::Vector3d &) override; + public: virtual void SetMass( + const double &_mass) override; + + // Documentation inherited. + public: virtual void SetParentLink( + const std::string &_parentLink) override; + + // Documentation inherited + public: virtual std::string ParentLink() const override; + + // Documentation inherited + public: virtual double Mass() const override; // Documentation inherited - public: ignition::math::Pose3d InertiaPose() const; + public: virtual ignition::math::Pose3d InertiaPose() const override; // Documentation inherited public: virtual VisualPtr SphereVisual() const override; + /// \brief Parent link name. + protected: std::string linkName = ""; + + /// \brief Link mass. + protected: double mass = 1.0; + /// \brief Inertia pose in link frame. - private: ignition::math::Pose3d inertiaPose = + protected: ignition::math::Pose3d inertiaPose = ignition::math::Pose3d::Zero; + + /// \brief Flag to indicate link properties have changed + protected: bool dirtyCOMVisual = false; }; ////////////////////////////////////////////////// @@ -98,45 +119,50 @@ namespace ignition void BaseCOMVisual::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); - } + this->inertiaPose = _inertial.Pose(); + this->dirtyCOMVisual = true; + } + + template + void BaseCOMVisual::SetMass( + const double &_mass) + { + this->mass = _mass; + this->dirtyCOMVisual = true; + } + + template + void BaseCOMVisual::SetParentLink( + const std::string &_parentLink) + { + this->linkName = _parentLink; + this->dirtyCOMVisual = true; + } + + ////////////////////////////////////////////////// + template + std::string BaseCOMVisual::ParentLink() const + { + return this->linkName; } ////////////////////////////////////////////////// template - void BaseCOMVisual::Load(const ignition::math::Pose3d &, - const ignition::math::Vector3d &) + double BaseCOMVisual::Mass() const { - // no op + return this->mass; } ////////////////////////////////////////////////// template - VisualPtr BaseCOMVisual::InertiaPose() const + ignition::math::Pose3d BaseCOMVisual::InertiaPose() const { return this->inertiaPose; } ////////////////////////////////////////////////// template - VisualPtr BaseCOMVisual::BoxVisual() const + VisualPtr BaseCOMVisual::SphereVisual() const { return nullptr; } diff --git a/include/ignition/rendering/base/BaseScene.hh b/include/ignition/rendering/base/BaseScene.hh index 31670265b..719c3f801 100644 --- a/include/ignition/rendering/base/BaseScene.hh +++ b/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 Light visual. /// \param[in] _id Unique id /// \param[in] _name Name of light visual @@ -360,6 +366,21 @@ namespace ignition public: virtual AxisVisualPtr CreateAxisVisual(unsigned int _id, 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; + // Documentation inherited public: virtual LightVisualPtr CreateLightVisual() override; diff --git a/ogre/include/ignition/rendering/ogre/OgreCOMVisual.hh b/ogre/include/ignition/rendering/ogre/OgreCOMVisual.hh new file mode 100644 index 000000000..3d37d60e9 --- /dev/null +++ b/ogre/include/ignition/rendering/ogre/OgreCOMVisual.hh @@ -0,0 +1,85 @@ +/* + * 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_OGRE_OGRECOMVISUAL_HH_ +#define IGNITION_RENDERING_OGRE_OGRECOMVISUAL_HH_ + +#include + +#include "ignition/rendering/base/BaseCOMVisual.hh" +#include "ignition/rendering/ogre/OgreIncludes.hh" +#include "ignition/rendering/ogre/OgreMaterial.hh" +#include "ignition/rendering/ogre/OgreVisual.hh" + +namespace Ogre +{ + class MovableObject; +} + +namespace ignition +{ + namespace rendering + { + inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { + + // Forward declaration + class OgreCOMVisualPrivate; + + class IGNITION_RENDERING_OGRE_VISIBLE OgreCOMVisual : + public BaseCOMVisual + { + /// \brief Constructor + protected: OgreCOMVisual(); + + /// \brief Destructor + public: virtual ~OgreCOMVisual(); + + // Documentation inherited. + public: virtual void Init() override; + + // Documentation inherited. + public: virtual void PreRender() override; + + // Documentation inherited. + public: Ogre::MovableObject *OgreObject() const; + + /// \brief Create the Light Visual in Ogre + public: void CreateVisual(); + + // Documentation inherited + public: virtual VisualPtr SphereVisual() const override; + + // Documentation inherited. + public: virtual MaterialPtr Material() const; + + // Documentation inherited. + public: virtual void SetMaterial( + MaterialPtr _material, bool _unique) override; + + /// \brief Set material to grid geometry. + /// \param[in] _material Ogre material. + protected: virtual void SetMaterialImpl(OgreMaterialPtr _material); + + private: friend class OgreScene; + + /// \brief Private data class + private: std::unique_ptr dataPtr; + }; + } + } +} +#endif diff --git a/ogre/include/ignition/rendering/ogre/OgreRenderTypes.hh b/ogre/include/ignition/rendering/ogre/OgreRenderTypes.hh index 3743b77de..bb8be4727 100644 --- a/ogre/include/ignition/rendering/ogre/OgreRenderTypes.hh +++ b/ogre/include/ignition/rendering/ogre/OgreRenderTypes.hh @@ -30,6 +30,7 @@ namespace ignition class OgreAxisVisual; class OgreCamera; class OgreCapsule; + class OgreCOMVisual; class OgreDepthCamera; class OgreDirectionalLight; class OgreGeometry; @@ -78,6 +79,7 @@ namespace ignition typedef shared_ptr OgreAxisVisualPtr; typedef shared_ptr OgreCameraPtr; typedef shared_ptr OgreCapsulePtr; + typedef shared_ptr OgreCOMVisualPtr; typedef shared_ptr OgreDepthCameraPtr; typedef shared_ptr OgreDirectionalLightPtr; typedef shared_ptr OgreGeometryPtr; diff --git a/ogre/include/ignition/rendering/ogre/OgreScene.hh b/ogre/include/ignition/rendering/ogre/OgreScene.hh index fca6a5c2d..193735a72 100644 --- a/ogre/include/ignition/rendering/ogre/OgreScene.hh +++ b/ogre/include/ignition/rendering/ogre/OgreScene.hh @@ -78,6 +78,10 @@ namespace ignition protected: virtual DirectionalLightPtr CreateDirectionalLightImpl( unsigned int _id, const std::string &_name) override; + // Documentation inherited + protected: virtual COMVisualPtr CreateCOMVisualImpl(unsigned int _id, + const std::string &_name) override; + // Documentation inherited protected: virtual LightVisualPtr CreateLightVisualImpl(unsigned int _id, const std::string &_name) override; diff --git a/ogre/src/OgreCOMVisual.cc b/ogre/src/OgreCOMVisual.cc new file mode 100644 index 000000000..5ba5155a3 --- /dev/null +++ b/ogre/src/OgreCOMVisual.cc @@ -0,0 +1,201 @@ +/* + * 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. + * + */ + +#include "ignition/rendering/ogre/OgreCOMVisual.hh" +#include "ignition/rendering/ogre/OgreDynamicLines.hh" + +#include "ignition/rendering/ogre/OgreScene.hh" + +class ignition::rendering::OgreCOMVisualPrivate +{ + /// \brief Grid materal + public: OgreMaterialPtr material = nullptr; + + /// \brief Lines that make the cross marking the center of mass. + public: std::shared_ptr crossLines = nullptr; + + /// \brief Sphere visual marking the center of mass + public: VisualPtr sphereVis = nullptr; +}; + +using namespace ignition; +using namespace rendering; + +////////////////////////////////////////////////// +OgreCOMVisual::OgreCOMVisual() + : dataPtr(new OgreCOMVisualPrivate) +{ +} + +////////////////////////////////////////////////// +OgreCOMVisual::~OgreCOMVisual() +{ +} + +////////////////////////////////////////////////// +void OgreCOMVisual::PreRender() +{ + if (this->dirtyCOMVisual) + { + this->CreateVisual(); + this->dirtyCOMVisual = false; + } +} + +////////////////////////////////////////////////// +void OgreCOMVisual::Init() +{ + BaseCOMVisual::Init(); + this->CreateVisual(); +} + +////////////////////////////////////////////////// +Ogre::MovableObject *OgreCOMVisual::OgreObject() const +{ + std::shared_ptr mv = + std::dynamic_pointer_cast(this->dataPtr->crossLines); + return mv.get(); +} + +////////////////////////////////////////////////// +void OgreCOMVisual::CreateVisual() +{ + if (!this->dataPtr->crossLines) + { + this->dataPtr->crossLines = std::shared_ptr( + new OgreDynamicLines(MT_LINE_LIST)); + this->ogreNode->attachObject(this->OgreObject()); + MaterialPtr COMVisualMaterial = + this->Scene()->Material("Default/TransGreen"); + this->SetMaterial(COMVisualMaterial, false); + } + + if (!this->dataPtr->sphereVis) + { + this->dataPtr->sphereVis = this->Scene()->CreateVisual(); + this->dataPtr->sphereVis->AddGeometry(this->Scene()->CreateSphere()); + this->dataPtr->sphereVis->SetMaterial("Default/CoM"); + this->dataPtr->sphereVis->SetInheritScale(false); + this->AddChild(this->dataPtr->sphereVis); + } + + // Compute radius of sphere with density of lead and equivalent mass. + double sphereRadius; + double dLead = 11340; + sphereRadius = cbrt((0.75 * this->Mass()) / (M_PI * dLead)); + + this->dataPtr->sphereVis->SetLocalScale(ignition::math::Vector3d( + sphereRadius*2, sphereRadius*2, sphereRadius*2)); + this->dataPtr->sphereVis->SetLocalPosition(this->InertiaPose().Pos()); + this->dataPtr->sphereVis->SetLocalRotation(this->InertiaPose().Rot()); + + // Get the link's bounding box + if (this->linkName.empty() || !this->Scene()->HasVisualName(this->linkName)) + return; + + VisualPtr vis = this->Scene()->VisualByName(this->linkName); + + ignition::math::AxisAlignedBox box; + if (vis) + box = vis->LocalBoundingBox(); + + // Clear any previous data from the grid and update + this->dataPtr->crossLines->Clear(); + this->dataPtr->crossLines->Update(); + + // CoM position indicator + ignition::math::Vector3d p1(0, 0, + box.Min().Z() - this->InertiaPose().Pos().Z()); + ignition::math::Vector3d p2(0, 0, + box.Max().Z() - this->InertiaPose().Pos().Z()); + + ignition::math::Vector3d p3(0, + box.Min().Y() - this->InertiaPose().Pos().Y(), 0); + ignition::math::Vector3d p4(0, + box.Max().Y() - this->InertiaPose().Pos().Y(), 0); + + ignition::math::Vector3d p5( + box.Min().X() - this->InertiaPose().Pos().X(), 0, 0); + ignition::math::Vector3d p6( + box.Max().X() - this->InertiaPose().Pos().X(), 0, 0); + + p1 += this->InertiaPose().Pos(); + p2 += this->InertiaPose().Pos(); + p3 += this->InertiaPose().Pos(); + p4 += this->InertiaPose().Pos(); + p5 += this->InertiaPose().Pos(); + p6 += this->InertiaPose().Pos(); + + this->dataPtr->crossLines->AddPoint(p1); + this->dataPtr->crossLines->AddPoint(p2); + this->dataPtr->crossLines->AddPoint(p3); + this->dataPtr->crossLines->AddPoint(p4); + this->dataPtr->crossLines->AddPoint(p5); + this->dataPtr->crossLines->AddPoint(p6); + + this->dataPtr->crossLines->Update(); + this->ogreNode->setVisible(true); +} + +////////////////////////////////////////////////// +void OgreCOMVisual::SetMaterial(MaterialPtr _material, bool _unique) +{ + _material = (_unique) ? _material->Clone() : _material; + + OgreMaterialPtr derived = + std::dynamic_pointer_cast(_material); + + if (!derived) + { + ignerr << "Cannot assign material created by another render-engine" + << std::endl; + + return; + } + + this->SetMaterialImpl(derived); +} + +////////////////////////////////////////////////// +void OgreCOMVisual::SetMaterialImpl(OgreMaterialPtr _material) +{ + std::string materialName = _material->Name(); + Ogre::MaterialPtr ogreMaterial = _material->Material(); + +// OGRE 1.10.7 +#if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7)) + this->dataPtr->crossLines->setMaterial(materialName); +#else + this->dataPtr->crossLines->setMaterial(ogreMaterial); +#endif + this->dataPtr->material = _material; + + this->dataPtr->material->SetReceiveShadows(false); + this->dataPtr->material->SetLightingEnabled(false); +} + +////////////////////////////////////////////////// +MaterialPtr OgreCOMVisual::Material() const +{ + return this->dataPtr->material; +} + +////////////////////////////////////////////////// +VisualPtr OgreCOMVisual::SphereVisual() const +{ + return this->dataPtr->sphereVis; +} diff --git a/ogre/src/OgreScene.cc b/ogre/src/OgreScene.cc index ea13d4ae8..c2e72321f 100644 --- a/ogre/src/OgreScene.cc +++ b/ogre/src/OgreScene.cc @@ -21,6 +21,7 @@ #include "ignition/rendering/ogre/OgreAxisVisual.hh" #include "ignition/rendering/ogre/OgreCamera.hh" #include "ignition/rendering/ogre/OgreCapsule.hh" +#include "ignition/rendering/ogre/OgreCOMVisual.hh" #include "ignition/rendering/ogre/OgreConversions.hh" #include "ignition/rendering/ogre/OgreDepthCamera.hh" #include "ignition/rendering/ogre/OgreGeometry.hh" @@ -375,6 +376,15 @@ PointLightPtr OgreScene::CreatePointLightImpl(unsigned int _id, return (result) ? light : nullptr; } +////////////////////////////////////////////////// +COMVisualPtr OgreScene::CreateCOMVisualImpl(unsigned int _id, + const std::string &_name) +{ + OgreCOMVisualPtr visual(new OgreCOMVisual); + bool result = this->InitObject(visual, _id, _name); + return (result) ? visual : nullptr; +} + ////////////////////////////////////////////////// LightVisualPtr OgreScene::CreateLightVisualImpl(unsigned int _id, const std::string &_name) diff --git a/ogre/src/media/materials/CMakeLists.txt b/ogre/src/media/materials/CMakeLists.txt index 0c8754d07..fdb4fb8cd 100644 --- a/ogre/src/media/materials/CMakeLists.txt +++ b/ogre/src/media/materials/CMakeLists.txt @@ -1,2 +1,3 @@ add_subdirectory(programs) add_subdirectory(scripts) +add_subdirectory(textures) diff --git a/ogre/src/media/materials/textures/CMakeLists.txt b/ogre/src/media/materials/textures/CMakeLists.txt new file mode 100644 index 000000000..e8dab05f2 --- /dev/null +++ b/ogre/src/media/materials/textures/CMakeLists.txt @@ -0,0 +1,4 @@ +file(GLOB files "*.png" "*.jpg") + +install(FILES ${files} DESTINATION ${IGN_RENDERING_RESOURCE_PATH}/ogre/media/materials/textures) + diff --git a/ogre/src/media/materials/textures/com.png b/ogre/src/media/materials/textures/com.png new file mode 100644 index 0000000000000000000000000000000000000000..3ed9e8879c4d9f8a4f528437d521cfae6002e49e GIT binary patch literal 444 zcmeAS@N?(olHy`uVBq!ia0vp^CxAGGgAGU?ZmZ`8QY^(zo*^7SP{WbZ0pxQQctjQh z)n5l;MkkHg6+l7B64!{5;QX|b^2DN4hVt@qz0ADq;^f4FRK5J7^x5xhq!<_&Ej?Wv zLn`LHy?U^d)sTnPG5hoR$k~qb+5Q-Y3fd@3oqzYP^1zup^FDOCpZj>m_Fo= Ogre2AxisVisualPtr; typedef shared_ptr Ogre2CameraPtr; typedef shared_ptr Ogre2CapsulePtr; + typedef shared_ptr Ogre2COMVisualPtr; typedef shared_ptr Ogre2DepthCameraPtr; typedef shared_ptr Ogre2DirectionalLightPtr; typedef shared_ptr Ogre2GeometryPtr; diff --git a/ogre2/include/ignition/rendering/ogre2/Ogre2Scene.hh b/ogre2/include/ignition/rendering/ogre2/Ogre2Scene.hh index e4d05a2f9..9d500cb54 100644 --- a/ogre2/include/ignition/rendering/ogre2/Ogre2Scene.hh +++ b/ogre2/include/ignition/rendering/ogre2/Ogre2Scene.hh @@ -119,6 +119,10 @@ namespace ignition // Documentation inherited protected: virtual bool InitImpl() override; + // Documentation inherited + protected: virtual COMVisualPtr CreateCOMVisualImpl(unsigned int _id, + const std::string &_name) override; + // Documentation inherited protected: virtual LightVisualPtr CreateLightVisualImpl(unsigned int _id, const std::string &_name) override; diff --git a/ogre2/src/Ogre2Scene.cc b/ogre2/src/Ogre2Scene.cc index 59f53be0e..60e831727 100644 --- a/ogre2/src/Ogre2Scene.cc +++ b/ogre2/src/Ogre2Scene.cc @@ -22,6 +22,7 @@ #include "ignition/rendering/ogre2/Ogre2AxisVisual.hh" #include "ignition/rendering/ogre2/Ogre2Camera.hh" #include "ignition/rendering/ogre2/Ogre2Capsule.hh" +#include "ignition/rendering/ogre2/Ogre2COMVisual.hh" #include "ignition/rendering/ogre2/Ogre2Conversions.hh" #include "ignition/rendering/ogre2/Ogre2DepthCamera.hh" #include "ignition/rendering/ogre2/Ogre2GizmoVisual.hh" @@ -739,6 +740,16 @@ AxisVisualPtr Ogre2Scene::CreateAxisVisualImpl(unsigned int _id, return (result) ? visual : nullptr; } +////////////////////////////////////////////////// +COMVisualPtr Ogre2Scene::CreateCOMVisualImpl(unsigned int _id, + const std::string &_name) +{ + // Ogre2COMVisualPtr visual(new Ogre2COMVisual); + // bool result = this->InitObject(visual, _id, _name); + // (result) ? visual : nullptr; + return nullptr; +} + ////////////////////////////////////////////////// LightVisualPtr Ogre2Scene::CreateLightVisualImpl(unsigned int _id, const std::string &_name) diff --git a/src/base/BaseScene.cc b/src/base/BaseScene.cc index 76094afb8..fa8871483 100644 --- a/src/base/BaseScene.cc +++ b/src/base/BaseScene.cc @@ -27,6 +27,7 @@ #include "ignition/rendering/ArrowVisual.hh" #include "ignition/rendering/AxisVisual.hh" +#include "ignition/rendering/COMVisual.hh" #include "ignition/rendering/LidarVisual.hh" #include "ignition/rendering/LightVisual.hh" #include "ignition/rendering/Camera.hh" @@ -918,6 +919,36 @@ AxisVisualPtr BaseScene::CreateAxisVisual(unsigned int _id, return (result) ? visual : nullptr; } +////////////////////////////////////////////////// +COMVisualPtr BaseScene::CreateCOMVisual() +{ + unsigned int objId = this->CreateObjectId(); + return this->CreateCOMVisual(objId); +} + +////////////////////////////////////////////////// +COMVisualPtr BaseScene::CreateCOMVisual(unsigned int _id) +{ + std::string objName = this->CreateObjectName(_id, "COMVisual"); + return this->CreateCOMVisual(_id, objName); +} + +////////////////////////////////////////////////// +COMVisualPtr BaseScene::CreateCOMVisual(const std::string &_name) +{ + unsigned int objId = this->CreateObjectId(); + return this->CreateCOMVisual(objId, _name); +} + +////////////////////////////////////////////////// +COMVisualPtr BaseScene::CreateCOMVisual(unsigned int _id, + const std::string &_name) +{ + COMVisualPtr visual = this->CreateCOMVisualImpl(_id, _name); + bool result = this->RegisterVisual(visual); + return (result) ? visual : nullptr; +} + ////////////////////////////////////////////////// LightVisualPtr BaseScene::CreateLightVisual() { @@ -1358,4 +1389,19 @@ void BaseScene::CreateMaterials() material->SetCastShadows(true); material->SetReceiveShadows(true); material->SetLightingEnabled(true); + + const char *env = std::getenv("IGN_RENDERING_RESOURCE_PATH"); + std::string resourcePath = (env) ? std::string(env) : + IGN_RENDERING_RESOURCE_PATH; + + // path to look for CoM material texture + std::string com_material_texture_path = common::joinPaths( + resourcePath, "ogre", "media", "materials", "textures", + "com.png"); + material = this->CreateMaterial("Default/CoM"); + material->SetTexture(com_material_texture_path); + material->SetTransparency(0); + material->SetCastShadows(false); + material->SetReceiveShadows(true); + material->SetLightingEnabled(true); } From c1891e7a910d8ae35fd27172cc98c97b2af976b0 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Tue, 15 Jun 2021 23:03:16 +0530 Subject: [PATCH 03/20] Add Ogre2 implementation Signed-off-by: Atharva Pusalkar --- .../rendering/ogre2/Ogre2COMVisual.hh | 67 ++++++ ogre2/src/Ogre2COMVisual.cc | 202 ++++++++++++++++++ ogre2/src/Ogre2Scene.cc | 7 +- 3 files changed, 272 insertions(+), 4 deletions(-) create mode 100644 ogre2/src/Ogre2COMVisual.cc diff --git a/ogre2/include/ignition/rendering/ogre2/Ogre2COMVisual.hh b/ogre2/include/ignition/rendering/ogre2/Ogre2COMVisual.hh index 7e6b9d4f7..f3706db36 100644 --- a/ogre2/include/ignition/rendering/ogre2/Ogre2COMVisual.hh +++ b/ogre2/include/ignition/rendering/ogre2/Ogre2COMVisual.hh @@ -14,3 +14,70 @@ * limitations under the License. * */ + +#ifndef IGNITION_RENDERING_OGRE_OGRE2COMVISUAL_HH_ +#define IGNITION_RENDERING_OGRE_OGRE2COMVISUAL_HH_ + +#include + +#include "ignition/rendering/base/BaseCOMVisual.hh" +#include "ignition/rendering/ogre2/Ogre2Visual.hh" + +namespace Ogre +{ + class MovableObject; +} + +namespace ignition +{ + namespace rendering + { + inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { + + // Forward declaration + class Ogre2COMVisualPrivate; + + class IGNITION_RENDERING_OGRE2_VISIBLE Ogre2COMVisual : + public BaseCOMVisual + { + /// \brief Constructor + protected: Ogre2COMVisual(); + + /// \brief Destructor + public: virtual ~Ogre2COMVisual(); + + // Documentation inherited. + public: virtual void Init() override; + + // Documentation inherited. + public: virtual void PreRender() override; + + // Documentation inherited. + public: Ogre::MovableObject *OgreObject() const; + + /// \brief Create the Light Visual in Ogre + public: void CreateVisual(); + + // Documentation inherited + public: virtual VisualPtr SphereVisual() const override; + + // Documentation inherited. + public: virtual MaterialPtr Material() const; + + // Documentation inherited. + public: virtual void SetMaterial( + MaterialPtr _material, bool _unique) override; + + /// \brief Set material to grid geometry. + /// \param[in] _material Ogre material. + protected: virtual void SetMaterialImpl(Ogre2MaterialPtr _material); + + private: friend class Ogre2Scene; + + /// \brief Private data class + private: std::unique_ptr dataPtr; + }; + } + } +} +#endif diff --git a/ogre2/src/Ogre2COMVisual.cc b/ogre2/src/Ogre2COMVisual.cc new file mode 100644 index 000000000..007b3a285 --- /dev/null +++ b/ogre2/src/Ogre2COMVisual.cc @@ -0,0 +1,202 @@ +/* + * 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. + * + */ + +#include "ignition/rendering/ogre2/Ogre2COMVisual.hh" +#include "ignition/rendering/ogre2/Ogre2Material.hh" +#include "ignition/rendering/ogre2/Ogre2DynamicRenderable.hh" + +#ifdef _MSC_VER + #pragma warning(push, 0) +#endif +#include +#ifdef _MSC_VER + #pragma warning(pop) +#endif + +class ignition::rendering::Ogre2COMVisualPrivate +{ + /// \brief Grid materal + public: Ogre2MaterialPtr material = nullptr; + + /// \brief Lines that make the cross marking the center of mass. + public: std::shared_ptr crossLines = nullptr; + + /// \brief Sphere visual marking the center of mass + public: VisualPtr sphereVis = nullptr; +}; + +using namespace ignition; +using namespace rendering; + +////////////////////////////////////////////////// +Ogre2COMVisual::Ogre2COMVisual() + : dataPtr(new Ogre2COMVisualPrivate) +{ +} + +////////////////////////////////////////////////// +Ogre2COMVisual::~Ogre2COMVisual() +{ +} + +////////////////////////////////////////////////// +void Ogre2COMVisual::PreRender() +{ + if (this->dirtyCOMVisual) + { + this->CreateVisual(); + this->dirtyCOMVisual = false; + } +} + +////////////////////////////////////////////////// +void Ogre2COMVisual::Init() +{ + BaseCOMVisual::Init(); + this->CreateVisual(); +} + +////////////////////////////////////////////////// +Ogre::MovableObject *Ogre2COMVisual::OgreObject() const +{ + return this->dataPtr->crossLines->OgreObject(); +} + +////////////////////////////////////////////////// +void Ogre2COMVisual::CreateVisual() +{ + if (!this->dataPtr->crossLines) + { + this->dataPtr->crossLines.reset( + new Ogre2DynamicRenderable(this->Scene())); + this->ogreNode->attachObject(this->OgreObject()); + } + + if (!this->dataPtr->sphereVis) + { + this->dataPtr->sphereVis = this->Scene()->CreateVisual(); + this->dataPtr->sphereVis->AddGeometry(this->Scene()->CreateSphere()); + this->dataPtr->sphereVis->SetMaterial("Default/CoM"); + this->dataPtr->sphereVis->SetInheritScale(false); + this->AddChild(this->dataPtr->sphereVis); + } + + // Compute radius of sphere with density of lead and equivalent mass. + double sphereRadius; + double dLead = 11340; + sphereRadius = cbrt((0.75 * this->Mass()) / (M_PI * dLead)); + + this->dataPtr->sphereVis->SetLocalScale(ignition::math::Vector3d( + sphereRadius*2, sphereRadius*2, sphereRadius*2)); + this->dataPtr->sphereVis->SetLocalPosition(this->InertiaPose().Pos()); + this->dataPtr->sphereVis->SetLocalRotation(this->InertiaPose().Rot()); + + // Get the link's bounding box + if (this->linkName.empty() || !this->Scene()->HasVisualName(this->linkName)) + return; + + VisualPtr vis = this->Scene()->VisualByName(this->linkName); + + ignition::math::AxisAlignedBox box; + if (vis) + box = vis->LocalBoundingBox(); + + // Clear any previous data from the grid and update + this->dataPtr->crossLines->Clear(); + this->dataPtr->crossLines->Update(); + + this->dataPtr->crossLines->SetOperationType(MarkerType::MT_LINE_LIST); + if (!this->dataPtr->material) + { + MaterialPtr COMVisualMaterial = + this->Scene()->Material("Default/TransGreen"); + this->SetMaterial(COMVisualMaterial, false); + } + + // CoM position indicator + ignition::math::Vector3d p1(0, 0, + box.Min().Z() - this->InertiaPose().Pos().Z()); + ignition::math::Vector3d p2(0, 0, + box.Max().Z() - this->InertiaPose().Pos().Z()); + + ignition::math::Vector3d p3(0, + box.Min().Y() - this->InertiaPose().Pos().Y(), 0); + ignition::math::Vector3d p4(0, + box.Max().Y() - this->InertiaPose().Pos().Y(), 0); + + ignition::math::Vector3d p5( + box.Min().X() - this->InertiaPose().Pos().X(), 0, 0); + ignition::math::Vector3d p6( + box.Max().X() - this->InertiaPose().Pos().X(), 0, 0); + + p1 += this->InertiaPose().Pos(); + p2 += this->InertiaPose().Pos(); + p3 += this->InertiaPose().Pos(); + p4 += this->InertiaPose().Pos(); + p5 += this->InertiaPose().Pos(); + p6 += this->InertiaPose().Pos(); + + this->dataPtr->crossLines->AddPoint(p1); + this->dataPtr->crossLines->AddPoint(p2); + this->dataPtr->crossLines->AddPoint(p3); + this->dataPtr->crossLines->AddPoint(p4); + this->dataPtr->crossLines->AddPoint(p5); + this->dataPtr->crossLines->AddPoint(p6); + + this->dataPtr->crossLines->Update(); + this->ogreNode->setVisible(true); +} + +////////////////////////////////////////////////// +void Ogre2COMVisual::SetMaterial(MaterialPtr _material, bool _unique) +{ + _material = (_unique) ? _material->Clone() : _material; + + Ogre2MaterialPtr derived = + std::dynamic_pointer_cast(_material); + + if (!derived) + { + ignerr << "Cannot assign material created by another render-engine" + << std::endl; + + return; + } + + // Set material for the underlying dynamic renderable + this->dataPtr->crossLines->SetMaterial(_material, false); + this->SetMaterialImpl(derived); +} + +////////////////////////////////////////////////// +void Ogre2COMVisual::SetMaterialImpl(Ogre2MaterialPtr _material) +{ + Ogre::MaterialPtr ogreMaterial = _material->Material(); + this->dataPtr->material = _material; +} + +////////////////////////////////////////////////// +MaterialPtr Ogre2COMVisual::Material() const +{ + return this->dataPtr->material; +} + +////////////////////////////////////////////////// +VisualPtr Ogre2COMVisual::SphereVisual() const +{ + return this->dataPtr->sphereVis; +} diff --git a/ogre2/src/Ogre2Scene.cc b/ogre2/src/Ogre2Scene.cc index 60e831727..5f6059830 100644 --- a/ogre2/src/Ogre2Scene.cc +++ b/ogre2/src/Ogre2Scene.cc @@ -744,10 +744,9 @@ AxisVisualPtr Ogre2Scene::CreateAxisVisualImpl(unsigned int _id, COMVisualPtr Ogre2Scene::CreateCOMVisualImpl(unsigned int _id, const std::string &_name) { - // Ogre2COMVisualPtr visual(new Ogre2COMVisual); - // bool result = this->InitObject(visual, _id, _name); - // (result) ? visual : nullptr; - return nullptr; + Ogre2COMVisualPtr visual(new Ogre2COMVisual); + bool result = this->InitObject(visual, _id, _name); + return (result) ? visual : nullptr; } ////////////////////////////////////////////////// From 154e1ddfa61e23f9e8e83b2db7a0b4d929091978 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Thu, 17 Jun 2021 12:29:33 +0530 Subject: [PATCH 04/20] Add invalid mass logic Signed-off-by: Atharva Pusalkar --- .../ignition/rendering/base/BaseCOMVisual.hh | 38 +++++++++++++++++++ ogre/src/OgreCOMVisual.cc | 2 +- 2 files changed, 39 insertions(+), 1 deletion(-) diff --git a/include/ignition/rendering/base/BaseCOMVisual.hh b/include/ignition/rendering/base/BaseCOMVisual.hh index 49190bd89..ac2be7324 100644 --- a/include/ignition/rendering/base/BaseCOMVisual.hh +++ b/include/ignition/rendering/base/BaseCOMVisual.hh @@ -120,6 +120,26 @@ namespace ignition const ignition::math::Inertiald &_inertial) { this->inertiaPose = _inertial.Pose(); + + if (_inertial.MassMatrix().Mass() <= 0) + { + // Unrealistic mass, load with default mass + if (_inertial.MassMatrix().Mass() < 0) + { + ignlog << "The link " << this->ParentLink() + << " has unrealistic mass, " + << "unable to visualize sphere of equivalent mass.\n"; + } + else + { + ignlog << "The link " << this->ParentLink() + << " is static or has mass of 0, " + << "so a sphere of equivalent mass will not be shown.\n"; + } + return; + } + + this->mass = _inertial.MassMatrix().Mass(); this->dirtyCOMVisual = true; } @@ -127,6 +147,24 @@ namespace ignition void BaseCOMVisual::SetMass( const double &_mass) { + if (_mass <= 0) + { + // Unrealistic mass, load with default mass + if (_mass < 0) + { + ignlog << "The link " << this->ParentLink() + << " has unrealistic mass, " + << "unable to visualize sphere of equivalent mass.\n"; + } + else + { + ignlog << "The link " << this->ParentLink() + << " 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; } diff --git a/ogre/src/OgreCOMVisual.cc b/ogre/src/OgreCOMVisual.cc index 5ba5155a3..967babd90 100644 --- a/ogre/src/OgreCOMVisual.cc +++ b/ogre/src/OgreCOMVisual.cc @@ -107,7 +107,7 @@ void OgreCOMVisual::CreateVisual() if (this->linkName.empty() || !this->Scene()->HasVisualName(this->linkName)) return; - VisualPtr vis = this->Scene()->VisualByName(this->linkName); + VisualPtr vis = this->Scene()->VisualByName(this->ParentLink()); ignition::math::AxisAlignedBox box; if (vis) From 96fcf4ff31d1b8970a725b55f260788436e8b845 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Thu, 17 Jun 2021 15:07:19 +0530 Subject: [PATCH 05/20] Fix visual creation Signed-off-by: Atharva Pusalkar --- ogre/src/OgreCOMVisual.cc | 4 ++-- ogre2/src/Ogre2COMVisual.cc | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ogre/src/OgreCOMVisual.cc b/ogre/src/OgreCOMVisual.cc index 967babd90..6212160e1 100644 --- a/ogre/src/OgreCOMVisual.cc +++ b/ogre/src/OgreCOMVisual.cc @@ -60,7 +60,6 @@ void OgreCOMVisual::PreRender() void OgreCOMVisual::Init() { BaseCOMVisual::Init(); - this->CreateVisual(); } ////////////////////////////////////////////////// @@ -104,7 +103,8 @@ void OgreCOMVisual::CreateVisual() this->dataPtr->sphereVis->SetLocalRotation(this->InertiaPose().Rot()); // Get the link's bounding box - if (this->linkName.empty() || !this->Scene()->HasVisualName(this->linkName)) + if (this->ParentLink().empty() || + !this->Scene()->HasVisualName(this->ParentLink())) return; VisualPtr vis = this->Scene()->VisualByName(this->ParentLink()); diff --git a/ogre2/src/Ogre2COMVisual.cc b/ogre2/src/Ogre2COMVisual.cc index 007b3a285..8a4ff9f0c 100644 --- a/ogre2/src/Ogre2COMVisual.cc +++ b/ogre2/src/Ogre2COMVisual.cc @@ -67,7 +67,6 @@ void Ogre2COMVisual::PreRender() void Ogre2COMVisual::Init() { BaseCOMVisual::Init(); - this->CreateVisual(); } ////////////////////////////////////////////////// @@ -106,7 +105,8 @@ void Ogre2COMVisual::CreateVisual() this->dataPtr->sphereVis->SetLocalRotation(this->InertiaPose().Rot()); // Get the link's bounding box - if (this->linkName.empty() || !this->Scene()->HasVisualName(this->linkName)) + if (this->ParentLink().empty() || + !this->Scene()->HasVisualName(this->ParentLink())) return; VisualPtr vis = this->Scene()->VisualByName(this->linkName); From f45fcc7057986f23894ae2f9d263589c10ddb1e4 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Thu, 17 Jun 2021 15:07:33 +0530 Subject: [PATCH 06/20] Add test Signed-off-by: Atharva Pusalkar --- src/COMVisual_TEST.cc | 116 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 116 insertions(+) create mode 100644 src/COMVisual_TEST.cc diff --git a/src/COMVisual_TEST.cc b/src/COMVisual_TEST.cc new file mode 100644 index 000000000..e0c656c38 --- /dev/null +++ b/src/COMVisual_TEST.cc @@ -0,0 +1,116 @@ +/* + * 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. + * +*/ + +#include +#include + +#include + +#include "test_config.h" // NOLINT(build/include) + +#include "ignition/rendering/COMVisual.hh" +#include "ignition/rendering/RenderEngine.hh" +#include "ignition/rendering/RenderingIface.hh" +#include "ignition/rendering/Scene.hh" + +using namespace ignition; +using namespace rendering; + +class COMVisualTest : public testing::Test, + public testing::WithParamInterface +{ + /// \brief Test basic API + public: void COMVisual(const std::string &_renderEngine); +}; + +///////////////////////////////////////////////// +void COMVisualTest::COMVisual(const std::string &_renderEngine) +{ + RenderEngine *engine = rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + ScenePtr scene = engine->CreateScene("scene"); + + // create visual + COMVisualPtr comVisual = scene->CreateCOMVisual(); + ASSERT_NE(nullptr, comVisual); + + ignition::math::MassMatrix3d massMatrix( + 0.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::Inertiald inertial; + inertial.SetMassMatrix(massMatrix); + inertial.SetPose(p); + + std::string parentLink = ""; + + // check initial values + EXPECT_EQ(nullptr, comVisual->SphereVisual()); + EXPECT_EQ(ignition::math::Pose3d::Zero, comVisual->InertiaPose()); + EXPECT_DOUBLE_EQ(1.0, comVisual->Mass()); + EXPECT_EQ(parentLink, comVisual->ParentLink()); + + // set invalid mass + comVisual->SetMass(-1.0); + comVisual->PreRender(); + EXPECT_EQ(nullptr, comVisual->SphereVisual()); + EXPECT_DOUBLE_EQ(1.0, comVisual->Mass()); + EXPECT_EQ(ignition::math::Pose3d::Zero, comVisual->InertiaPose()); + + // set invalid inertial + comVisual->SetInertial(inertial); + comVisual->PreRender(); + EXPECT_EQ(nullptr, comVisual->SphereVisual()); + EXPECT_DOUBLE_EQ(1.0, comVisual->Mass()); + EXPECT_EQ(inertial.Pose(), comVisual->InertiaPose()); + + // set valid mass + comVisual->SetMass(2.0); + comVisual->PreRender(); + EXPECT_NE(nullptr, comVisual->SphereVisual()); + EXPECT_DOUBLE_EQ(2.0, comVisual->Mass()); + + // set parent link + parentLink = "parent_link"; + comVisual->SetParentLink(parentLink); + EXPECT_EQ(parentLink, comVisual->ParentLink()); + + // Clean up + engine->DestroyScene(scene); + rendering::unloadEngine(engine->Name()); +} + +///////////////////////////////////////////////// +TEST_P(COMVisualTest, COMVisual) +{ + COMVisual(GetParam()); +} + +INSTANTIATE_TEST_CASE_P(Visual, COMVisualTest, + RENDER_ENGINE_VALUES, + ignition::rendering::PrintToStringParam()); + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 1a35c5d16d61817c3a78218839f586d0dc21ecbd Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Thu, 17 Jun 2021 16:41:47 +0530 Subject: [PATCH 07/20] Change texter directory Signed-off-by: Atharva Pusalkar --- ogre/src/media/materials/CMakeLists.txt | 1 - src/base/BaseScene.cc | 3 +-- src/base/CMakeLists.txt | 2 ++ src/base/media/CMakeLists.txt | 1 + src/base/media/materials/CMakeLists.txt | 1 + .../base}/media/materials/textures/CMakeLists.txt | 2 +- .../base}/media/materials/textures/com.png | Bin 7 files changed, 6 insertions(+), 4 deletions(-) create mode 100644 src/base/media/CMakeLists.txt create mode 100644 src/base/media/materials/CMakeLists.txt rename {ogre/src => src/base}/media/materials/textures/CMakeLists.txt (75%) rename {ogre/src => src/base}/media/materials/textures/com.png (100%) diff --git a/ogre/src/media/materials/CMakeLists.txt b/ogre/src/media/materials/CMakeLists.txt index fdb4fb8cd..0c8754d07 100644 --- a/ogre/src/media/materials/CMakeLists.txt +++ b/ogre/src/media/materials/CMakeLists.txt @@ -1,3 +1,2 @@ add_subdirectory(programs) add_subdirectory(scripts) -add_subdirectory(textures) diff --git a/src/base/BaseScene.cc b/src/base/BaseScene.cc index b99c6a18e..aaab8ef7e 100644 --- a/src/base/BaseScene.cc +++ b/src/base/BaseScene.cc @@ -1436,8 +1436,7 @@ void BaseScene::CreateMaterials() // path to look for CoM material texture std::string com_material_texture_path = common::joinPaths( - resourcePath, "ogre", "media", "materials", "textures", - "com.png"); + resourcePath, "media", "materials", "textures", "com.png"); material = this->CreateMaterial("Default/CoM"); material->SetTexture(com_material_texture_path); material->SetTransparency(0); diff --git a/src/base/CMakeLists.txt b/src/base/CMakeLists.txt index ff086f850..e41f951f5 100644 --- a/src/base/CMakeLists.txt +++ b/src/base/CMakeLists.txt @@ -14,3 +14,5 @@ set (sources ${sources} PARENT_SCOPE ) +add_subdirectory(media) + diff --git a/src/base/media/CMakeLists.txt b/src/base/media/CMakeLists.txt new file mode 100644 index 000000000..a28de6e0a --- /dev/null +++ b/src/base/media/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(materials) diff --git a/src/base/media/materials/CMakeLists.txt b/src/base/media/materials/CMakeLists.txt new file mode 100644 index 000000000..240c30121 --- /dev/null +++ b/src/base/media/materials/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(textures) diff --git a/ogre/src/media/materials/textures/CMakeLists.txt b/src/base/media/materials/textures/CMakeLists.txt similarity index 75% rename from ogre/src/media/materials/textures/CMakeLists.txt rename to src/base/media/materials/textures/CMakeLists.txt index e8dab05f2..4d981f2c2 100644 --- a/ogre/src/media/materials/textures/CMakeLists.txt +++ b/src/base/media/materials/textures/CMakeLists.txt @@ -1,4 +1,4 @@ file(GLOB files "*.png" "*.jpg") -install(FILES ${files} DESTINATION ${IGN_RENDERING_RESOURCE_PATH}/ogre/media/materials/textures) +install(FILES ${files} DESTINATION ${IGN_RENDERING_RESOURCE_PATH}/media/materials/textures) diff --git a/ogre/src/media/materials/textures/com.png b/src/base/media/materials/textures/com.png similarity index 100% rename from ogre/src/media/materials/textures/com.png rename to src/base/media/materials/textures/com.png From d9503624c8134b445e4e2e7f60daf922e55dd1b0 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Thu, 17 Jun 2021 16:42:03 +0530 Subject: [PATCH 08/20] Add example Signed-off-by: Atharva Pusalkar --- examples/visualization_demo/Main.cc | 36 ++++++++++++++++++++++++++--- 1 file changed, 33 insertions(+), 3 deletions(-) diff --git a/examples/visualization_demo/Main.cc b/examples/visualization_demo/Main.cc index 68540c77f..6ffa2eca1 100644 --- a/examples/visualization_demo/Main.cc +++ b/examples/visualization_demo/Main.cc @@ -101,13 +101,32 @@ void buildScene(ScenePtr _scene) VisualPtr sphere = _scene->CreateVisual(); sphere->AddGeometry(_scene->CreateSphere()); sphere->SetOrigin(0.0, -0.5, 0.0); - sphere->SetLocalPosition(3, -1, 0); + sphere->SetLocalPosition(3.0, -1.0, 0); sphere->SetLocalRotation(0, 0, 0); sphere->SetLocalScale(1, 1, 1); sphere->SetMaterial(red); 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); @@ -120,7 +139,7 @@ void buildScene(ScenePtr _scene) VisualPtr plane = _scene->CreateVisual(); plane->AddGeometry(_scene->CreatePlane()); plane->SetLocalScale(5, 8, 1); - plane->SetLocalPosition(3, 0, -0.5); + plane->SetLocalPosition(3, 0.0, -0.5); plane->SetMaterial(white); root->AddChild(plane); @@ -130,9 +149,20 @@ 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.0, 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); + comVisual->SetParentLink("parent_box"); + box->AddChild(comVisual); + // create camera CameraPtr camera = _scene->CreateCamera("camera"); camera->SetLocalPosition(0.0, 0.0, 0.0); From 5023fdf5b84dd694fa6ef7ff5f110337fec240b1 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Thu, 17 Jun 2021 16:42:53 +0530 Subject: [PATCH 09/20] Refactor SetInertial Signed-off-by: Atharva Pusalkar --- .../ignition/rendering/base/BaseCOMVisual.hh | 21 +------------------ 1 file changed, 1 insertion(+), 20 deletions(-) diff --git a/include/ignition/rendering/base/BaseCOMVisual.hh b/include/ignition/rendering/base/BaseCOMVisual.hh index ac2be7324..e4457a5e1 100644 --- a/include/ignition/rendering/base/BaseCOMVisual.hh +++ b/include/ignition/rendering/base/BaseCOMVisual.hh @@ -121,26 +121,7 @@ namespace ignition { this->inertiaPose = _inertial.Pose(); - if (_inertial.MassMatrix().Mass() <= 0) - { - // Unrealistic mass, load with default mass - if (_inertial.MassMatrix().Mass() < 0) - { - ignlog << "The link " << this->ParentLink() - << " has unrealistic mass, " - << "unable to visualize sphere of equivalent mass.\n"; - } - else - { - ignlog << "The link " << this->ParentLink() - << " is static or has mass of 0, " - << "so a sphere of equivalent mass will not be shown.\n"; - } - return; - } - - this->mass = _inertial.MassMatrix().Mass(); - this->dirtyCOMVisual = true; + this->SetMass(_inertial.MassMatrix().Mass()); } template From 17d2bd357b2821c8fe75709a0035db37a8b38bb6 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Thu, 17 Jun 2021 17:23:33 +0530 Subject: [PATCH 10/20] Add destroy method Signed-off-by: Atharva Pusalkar --- .../rendering/ogre2/Ogre2COMVisual.hh | 2 +- ogre2/src/Ogre2COMVisual.cc | 24 +++++++++++++++---- 2 files changed, 21 insertions(+), 5 deletions(-) diff --git a/ogre2/include/ignition/rendering/ogre2/Ogre2COMVisual.hh b/ogre2/include/ignition/rendering/ogre2/Ogre2COMVisual.hh index f3706db36..54c8dc92a 100644 --- a/ogre2/include/ignition/rendering/ogre2/Ogre2COMVisual.hh +++ b/ogre2/include/ignition/rendering/ogre2/Ogre2COMVisual.hh @@ -53,7 +53,7 @@ namespace ignition public: virtual void PreRender() override; // Documentation inherited. - public: Ogre::MovableObject *OgreObject() const; + protected: virtual void Destroy() override; /// \brief Create the Light Visual in Ogre public: void CreateVisual(); diff --git a/ogre2/src/Ogre2COMVisual.cc b/ogre2/src/Ogre2COMVisual.cc index 8a4ff9f0c..fefa81987 100644 --- a/ogre2/src/Ogre2COMVisual.cc +++ b/ogre2/src/Ogre2COMVisual.cc @@ -70,9 +70,25 @@ void Ogre2COMVisual::Init() } ////////////////////////////////////////////////// -Ogre::MovableObject *Ogre2COMVisual::OgreObject() const +void Ogre2COMVisual::Destroy() { - return this->dataPtr->crossLines->OgreObject(); + if (this->dataPtr->sphereVis != nullptr) + { + this->dataPtr->sphereVis->Destroy(); + this->dataPtr->sphereVis.reset(); + } + + if (this->dataPtr->crossLines) + { + this->dataPtr->crossLines->Destroy(); + this->dataPtr->crossLines.reset(); + } + + if (this->dataPtr->material && this->Scene()) + { + this->Scene()->DestroyMaterial(this->dataPtr->material); + this->dataPtr->material.reset(); + } } ////////////////////////////////////////////////// @@ -82,7 +98,7 @@ void Ogre2COMVisual::CreateVisual() { this->dataPtr->crossLines.reset( new Ogre2DynamicRenderable(this->Scene())); - this->ogreNode->attachObject(this->OgreObject()); + this->ogreNode->attachObject(this->dataPtr->crossLines->OgreObject()); } if (!this->dataPtr->sphereVis) @@ -123,7 +139,7 @@ void Ogre2COMVisual::CreateVisual() if (!this->dataPtr->material) { MaterialPtr COMVisualMaterial = - this->Scene()->Material("Default/TransGreen"); + this->Scene()->Material("Default/TransGreen")->Clone(); this->SetMaterial(COMVisualMaterial, false); } From 6f60f7ccc153965aaefd7267e3922842441c2e81 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Thu, 17 Jun 2021 17:30:44 +0530 Subject: [PATCH 11/20] Fix style Signed-off-by: Atharva Pusalkar --- examples/visualization_demo/Main.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/visualization_demo/Main.cc b/examples/visualization_demo/Main.cc index 6ffa2eca1..53126068f 100644 --- a/examples/visualization_demo/Main.cc +++ b/examples/visualization_demo/Main.cc @@ -101,7 +101,7 @@ void buildScene(ScenePtr _scene) VisualPtr sphere = _scene->CreateVisual(); sphere->AddGeometry(_scene->CreateSphere()); sphere->SetOrigin(0.0, -0.5, 0.0); - sphere->SetLocalPosition(3.0, -1.0, 0); + sphere->SetLocalPosition(3, -1, 0); sphere->SetLocalRotation(0, 0, 0); sphere->SetLocalScale(1, 1, 1); sphere->SetMaterial(red); @@ -139,7 +139,7 @@ void buildScene(ScenePtr _scene) VisualPtr plane = _scene->CreateVisual(); plane->AddGeometry(_scene->CreatePlane()); plane->SetLocalScale(5, 8, 1); - plane->SetLocalPosition(3, 0.0, -0.5); + plane->SetLocalPosition(3, 0, -0.5); plane->SetMaterial(white); root->AddChild(plane); @@ -149,7 +149,7 @@ 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(1.0, 0.0, 0.0); + inertiaVisual->SetLocalPosition(1, 0, 0); root->AddChild(inertiaVisual); // create CoM visual From 4ac06997889bb8be11ee2c7c019e49623394a6cf Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Mon, 21 Jun 2021 11:13:17 +0530 Subject: [PATCH 12/20] Use IGN_PI Signed-off-by: Atharva Pusalkar --- ogre/src/OgreCOMVisual.cc | 2 +- ogre2/src/Ogre2COMVisual.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ogre/src/OgreCOMVisual.cc b/ogre/src/OgreCOMVisual.cc index 6212160e1..7af0d8cd8 100644 --- a/ogre/src/OgreCOMVisual.cc +++ b/ogre/src/OgreCOMVisual.cc @@ -95,7 +95,7 @@ void OgreCOMVisual::CreateVisual() // Compute radius of sphere with density of lead and equivalent mass. double sphereRadius; double dLead = 11340; - sphereRadius = cbrt((0.75 * this->Mass()) / (M_PI * dLead)); + sphereRadius = cbrt((0.75 * this->Mass()) / (IGN_PI * dLead)); this->dataPtr->sphereVis->SetLocalScale(ignition::math::Vector3d( sphereRadius*2, sphereRadius*2, sphereRadius*2)); diff --git a/ogre2/src/Ogre2COMVisual.cc b/ogre2/src/Ogre2COMVisual.cc index fefa81987..6a88aecfc 100644 --- a/ogre2/src/Ogre2COMVisual.cc +++ b/ogre2/src/Ogre2COMVisual.cc @@ -113,7 +113,7 @@ void Ogre2COMVisual::CreateVisual() // Compute radius of sphere with density of lead and equivalent mass. double sphereRadius; double dLead = 11340; - sphereRadius = cbrt((0.75 * this->Mass()) / (M_PI * dLead)); + sphereRadius = cbrt((0.75 * this->Mass()) / (IGN_PI * dLead)); this->dataPtr->sphereVis->SetLocalScale(ignition::math::Vector3d( sphereRadius*2, sphereRadius*2, sphereRadius*2)); From 4319626315164ba87af3aed2d9f9d40856b19419 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Mon, 21 Jun 2021 20:03:39 +0530 Subject: [PATCH 13/20] Remove .jpg Signed-off-by: Atharva Pusalkar --- src/base/media/materials/textures/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/base/media/materials/textures/CMakeLists.txt b/src/base/media/materials/textures/CMakeLists.txt index 4d981f2c2..aec4532d5 100644 --- a/src/base/media/materials/textures/CMakeLists.txt +++ b/src/base/media/materials/textures/CMakeLists.txt @@ -1,4 +1,4 @@ -file(GLOB files "*.png" "*.jpg") +file(GLOB files "*.png") install(FILES ${files} DESTINATION ${IGN_RENDERING_RESOURCE_PATH}/media/materials/textures) From 27067620e2bd2a7bf19778b3893d673c3ab226da Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Mon, 21 Jun 2021 20:04:34 +0530 Subject: [PATCH 14/20] Reorder Ogre2 includes Signed-off-by: Atharva Pusalkar --- ogre2/src/Ogre2COMVisual.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ogre2/src/Ogre2COMVisual.cc b/ogre2/src/Ogre2COMVisual.cc index 6a88aecfc..0a9fa524b 100644 --- a/ogre2/src/Ogre2COMVisual.cc +++ b/ogre2/src/Ogre2COMVisual.cc @@ -16,8 +16,8 @@ */ #include "ignition/rendering/ogre2/Ogre2COMVisual.hh" -#include "ignition/rendering/ogre2/Ogre2Material.hh" #include "ignition/rendering/ogre2/Ogre2DynamicRenderable.hh" +#include "ignition/rendering/ogre2/Ogre2Material.hh" #ifdef _MSC_VER #pragma warning(push, 0) From 5d795faeebf641429ec3f7b49ee524361fc0ed51 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Thu, 24 Jun 2021 12:05:27 +0530 Subject: [PATCH 15/20] Remove const ref Signed-off-by: Atharva Pusalkar --- include/ignition/rendering/COMVisual.hh | 3 +-- include/ignition/rendering/base/BaseCOMVisual.hh | 6 ++---- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/include/ignition/rendering/COMVisual.hh b/include/ignition/rendering/COMVisual.hh index 90ca44ee2..ab7a5eb88 100644 --- a/include/ignition/rendering/COMVisual.hh +++ b/include/ignition/rendering/COMVisual.hh @@ -46,8 +46,7 @@ namespace ignition /// \brief Set the mass of the parent link /// \param[in] _mass Parent link mass - public: virtual void SetMass( - const double &_mass) = 0; + public: virtual void SetMass(double _mass) = 0; /// \brief Set the parent link of the visual /// \param[in] _parentLink Parent link of the visual diff --git a/include/ignition/rendering/base/BaseCOMVisual.hh b/include/ignition/rendering/base/BaseCOMVisual.hh index e4457a5e1..58e035d6f 100644 --- a/include/ignition/rendering/base/BaseCOMVisual.hh +++ b/include/ignition/rendering/base/BaseCOMVisual.hh @@ -55,8 +55,7 @@ namespace ignition const ignition::math::Inertiald &_inertial) override; // Documentation inherited. - public: virtual void SetMass( - const double &_mass) override; + public: virtual void SetMass(double _mass) override; // Documentation inherited. public: virtual void SetParentLink( @@ -125,8 +124,7 @@ namespace ignition } template - void BaseCOMVisual::SetMass( - const double &_mass) + void BaseCOMVisual::SetMass(double _mass) { if (_mass <= 0) { From f428ba1a17054dee412110e293f2868f74c68f96 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Thu, 24 Jun 2021 12:50:30 +0530 Subject: [PATCH 16/20] Remove SetParentLink Signed-off-by: Atharva Pusalkar --- examples/visualization_demo/Main.cc | 1 - include/ignition/rendering/COMVisual.hh | 23 ++++++------- .../ignition/rendering/base/BaseCOMVisual.hh | 32 ++++++------------- ogre/src/OgreCOMVisual.cc | 16 +++++----- ogre2/src/Ogre2COMVisual.cc | 16 +++++----- src/COMVisual_TEST.cc | 18 ++++++----- 6 files changed, 45 insertions(+), 61 deletions(-) diff --git a/examples/visualization_demo/Main.cc b/examples/visualization_demo/Main.cc index 53126068f..e36d550ec 100644 --- a/examples/visualization_demo/Main.cc +++ b/examples/visualization_demo/Main.cc @@ -160,7 +160,6 @@ void buildScene(ScenePtr _scene) 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); ignition::math::Inertiald comVisualInertial{comMassMatrix, comPose}; comVisual->SetInertial(comVisualInertial); - comVisual->SetParentLink("parent_box"); box->AddChild(comVisual); // create camera diff --git a/include/ignition/rendering/COMVisual.hh b/include/ignition/rendering/COMVisual.hh index ab7a5eb88..33ebc61d2 100644 --- a/include/ignition/rendering/COMVisual.hh +++ b/include/ignition/rendering/COMVisual.hh @@ -44,25 +44,20 @@ namespace ignition public: virtual void SetInertial( const ignition::math::Inertiald &_inertial) = 0; - /// \brief Set the mass of the parent link - /// \param[in] _mass Parent link mass + /// \brief Set the mass of the parent + /// \param[in] _mass Parent mass public: virtual void SetMass(double _mass) = 0; - /// \brief Set the parent link of the visual - /// \param[in] _parentLink Parent link of the visual - public: virtual void SetParentLink( - const std::string &_parentLink) = 0; + /// \brief Get the parent name of the visual + /// \return Name of the parent visual + public: virtual std::string ParentName() const = 0; - /// \brief Get the parent link name of the visual - /// \return Link name of the visual - public: virtual std::string ParentLink() const = 0; - - /// \brief Get the mass of the link - /// \return Link mass + /// \brief Get the mass of the parent + /// \return Parent mass public: virtual double Mass() const = 0; - /// \brief Get inertia pose. - /// \return Inertia pose in link frame. + /// \brief Get the inertia pose + /// \return Inertia pose in parent frame. public: virtual ignition::math::Pose3d InertiaPose() const = 0; /// \brief Get the sphere visual diff --git a/include/ignition/rendering/base/BaseCOMVisual.hh b/include/ignition/rendering/base/BaseCOMVisual.hh index 58e035d6f..f9df4de2f 100644 --- a/include/ignition/rendering/base/BaseCOMVisual.hh +++ b/include/ignition/rendering/base/BaseCOMVisual.hh @@ -57,12 +57,8 @@ namespace ignition // Documentation inherited. public: virtual void SetMass(double _mass) override; - // Documentation inherited. - public: virtual void SetParentLink( - const std::string &_parentLink) override; - // Documentation inherited - public: virtual std::string ParentLink() const override; + public: virtual std::string ParentName() const override; // Documentation inherited public: virtual double Mass() const override; @@ -73,17 +69,17 @@ namespace ignition // Documentation inherited public: virtual VisualPtr SphereVisual() const override; - /// \brief Parent link name. - protected: std::string linkName = ""; + /// \brief Parent visual name. + protected: std::string parentName = ""; - /// \brief Link mass. + /// \brief Parent mass. protected: double mass = 1.0; - /// \brief Inertia pose in link frame. + /// \brief Inertia pose in parent frame. protected: ignition::math::Pose3d inertiaPose = ignition::math::Pose3d::Zero; - /// \brief Flag to indicate link properties have changed + /// \brief Flag to indicate parent properties have changed. protected: bool dirtyCOMVisual = false; }; @@ -131,13 +127,13 @@ namespace ignition // Unrealistic mass, load with default mass if (_mass < 0) { - ignlog << "The link " << this->ParentLink() + ignlog << "The parent " << this->ParentName() << " has unrealistic mass, " << "unable to visualize sphere of equivalent mass.\n"; } else { - ignlog << "The link " << this->ParentLink() + ignlog << "The parent " << this->ParentName() << " is static or has mass of 0, " << "so a sphere of equivalent mass will not be shown.\n"; } @@ -148,19 +144,11 @@ namespace ignition this->dirtyCOMVisual = true; } - template - void BaseCOMVisual::SetParentLink( - const std::string &_parentLink) - { - this->linkName = _parentLink; - this->dirtyCOMVisual = true; - } - ////////////////////////////////////////////////// template - std::string BaseCOMVisual::ParentLink() const + std::string BaseCOMVisual::ParentName() const { - return this->linkName; + return this->parentName; } ////////////////////////////////////////////////// diff --git a/ogre/src/OgreCOMVisual.cc b/ogre/src/OgreCOMVisual.cc index 7af0d8cd8..a298e075d 100644 --- a/ogre/src/OgreCOMVisual.cc +++ b/ogre/src/OgreCOMVisual.cc @@ -49,8 +49,13 @@ OgreCOMVisual::~OgreCOMVisual() ////////////////////////////////////////////////// void OgreCOMVisual::PreRender() { - if (this->dirtyCOMVisual) + if (this->HasParent() && this->ParentName().empty()) + this->parentName = this->Parent()->Name(); + + if (this->dirtyCOMVisual && + !this->ParentName().empty()) { + this->parentName = this->Parent()->Name(); this->CreateVisual(); this->dirtyCOMVisual = false; } @@ -102,13 +107,8 @@ void OgreCOMVisual::CreateVisual() this->dataPtr->sphereVis->SetLocalPosition(this->InertiaPose().Pos()); this->dataPtr->sphereVis->SetLocalRotation(this->InertiaPose().Rot()); - // Get the link's bounding box - if (this->ParentLink().empty() || - !this->Scene()->HasVisualName(this->ParentLink())) - return; - - VisualPtr vis = this->Scene()->VisualByName(this->ParentLink()); - + // Get the bounding box of the parent visual + VisualPtr vis = this->Scene()->VisualByName(this->ParentName()); ignition::math::AxisAlignedBox box; if (vis) box = vis->LocalBoundingBox(); diff --git a/ogre2/src/Ogre2COMVisual.cc b/ogre2/src/Ogre2COMVisual.cc index 0a9fa524b..9c212e1af 100644 --- a/ogre2/src/Ogre2COMVisual.cc +++ b/ogre2/src/Ogre2COMVisual.cc @@ -56,8 +56,13 @@ Ogre2COMVisual::~Ogre2COMVisual() ////////////////////////////////////////////////// void Ogre2COMVisual::PreRender() { - if (this->dirtyCOMVisual) + if (this->HasParent() && this->ParentName().empty()) + this->parentName = this->Parent()->Name(); + + if (this->dirtyCOMVisual && + !this->ParentName().empty()) { + this->parentName = this->Parent()->Name(); this->CreateVisual(); this->dirtyCOMVisual = false; } @@ -120,13 +125,8 @@ void Ogre2COMVisual::CreateVisual() this->dataPtr->sphereVis->SetLocalPosition(this->InertiaPose().Pos()); this->dataPtr->sphereVis->SetLocalRotation(this->InertiaPose().Rot()); - // Get the link's bounding box - if (this->ParentLink().empty() || - !this->Scene()->HasVisualName(this->ParentLink())) - return; - - VisualPtr vis = this->Scene()->VisualByName(this->linkName); - + // Get the bounding box of the parent visual + VisualPtr vis = this->Scene()->VisualByName(this->ParentName()); ignition::math::AxisAlignedBox box; if (vis) box = vis->LocalBoundingBox(); diff --git a/src/COMVisual_TEST.cc b/src/COMVisual_TEST.cc index e0c656c38..cf099b160 100644 --- a/src/COMVisual_TEST.cc +++ b/src/COMVisual_TEST.cc @@ -61,13 +61,13 @@ void COMVisualTest::COMVisual(const std::string &_renderEngine) inertial.SetMassMatrix(massMatrix); inertial.SetPose(p); - std::string parentLink = ""; + std::string parentName = ""; // check initial values EXPECT_EQ(nullptr, comVisual->SphereVisual()); EXPECT_EQ(ignition::math::Pose3d::Zero, comVisual->InertiaPose()); EXPECT_DOUBLE_EQ(1.0, comVisual->Mass()); - EXPECT_EQ(parentLink, comVisual->ParentLink()); + EXPECT_EQ(parentName, comVisual->ParentName()); // set invalid mass comVisual->SetMass(-1.0); @@ -85,14 +85,16 @@ void COMVisualTest::COMVisual(const std::string &_renderEngine) // set valid mass comVisual->SetMass(2.0); - comVisual->PreRender(); - EXPECT_NE(nullptr, comVisual->SphereVisual()); + EXPECT_EQ(nullptr, comVisual->SphereVisual()); EXPECT_DOUBLE_EQ(2.0, comVisual->Mass()); - // set parent link - parentLink = "parent_link"; - comVisual->SetParentLink(parentLink); - EXPECT_EQ(parentLink, comVisual->ParentLink()); + // set parent + parentName = "parent_visual"; + VisualPtr parentVisual = scene->CreateVisual(parentName); + parentVisual->AddChild(comVisual); + comVisual->PreRender(); + EXPECT_EQ(parentName, comVisual->ParentName()); + EXPECT_NE(nullptr, comVisual->SphereVisual()); // Clean up engine->DestroyScene(scene); From 3300dc22a3ec2fd4b7d19f4ae6f69ead7a089f47 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Fri, 25 Jun 2021 15:15:16 +0530 Subject: [PATCH 17/20] Remove ParentName() Signed-off-by: Atharva Pusalkar --- include/ignition/rendering/COMVisual.hh | 4 ---- include/ignition/rendering/base/BaseCOMVisual.hh | 14 ++------------ ogre/src/OgreCOMVisual.cc | 6 +++--- ogre2/src/Ogre2COMVisual.cc | 6 +++--- src/COMVisual_TEST.cc | 7 +------ 5 files changed, 9 insertions(+), 28 deletions(-) diff --git a/include/ignition/rendering/COMVisual.hh b/include/ignition/rendering/COMVisual.hh index 33ebc61d2..a0f811cba 100644 --- a/include/ignition/rendering/COMVisual.hh +++ b/include/ignition/rendering/COMVisual.hh @@ -48,10 +48,6 @@ namespace ignition /// \param[in] _mass Parent mass public: virtual void SetMass(double _mass) = 0; - /// \brief Get the parent name of the visual - /// \return Name of the parent visual - public: virtual std::string ParentName() const = 0; - /// \brief Get the mass of the parent /// \return Parent mass public: virtual double Mass() const = 0; diff --git a/include/ignition/rendering/base/BaseCOMVisual.hh b/include/ignition/rendering/base/BaseCOMVisual.hh index f9df4de2f..168d70c8e 100644 --- a/include/ignition/rendering/base/BaseCOMVisual.hh +++ b/include/ignition/rendering/base/BaseCOMVisual.hh @@ -57,9 +57,6 @@ namespace ignition // Documentation inherited. public: virtual void SetMass(double _mass) override; - // Documentation inherited - public: virtual std::string ParentName() const override; - // Documentation inherited public: virtual double Mass() const override; @@ -127,13 +124,13 @@ namespace ignition // Unrealistic mass, load with default mass if (_mass < 0) { - ignlog << "The parent " << this->ParentName() + ignlog << "The parent " << this->parentName << " has unrealistic mass, " << "unable to visualize sphere of equivalent mass.\n"; } else { - ignlog << "The parent " << this->ParentName() + ignlog << "The parent " << this->parentName << " is static or has mass of 0, " << "so a sphere of equivalent mass will not be shown.\n"; } @@ -144,13 +141,6 @@ namespace ignition this->dirtyCOMVisual = true; } - ////////////////////////////////////////////////// - template - std::string BaseCOMVisual::ParentName() const - { - return this->parentName; - } - ////////////////////////////////////////////////// template double BaseCOMVisual::Mass() const diff --git a/ogre/src/OgreCOMVisual.cc b/ogre/src/OgreCOMVisual.cc index a298e075d..30c4a5913 100644 --- a/ogre/src/OgreCOMVisual.cc +++ b/ogre/src/OgreCOMVisual.cc @@ -49,11 +49,11 @@ OgreCOMVisual::~OgreCOMVisual() ////////////////////////////////////////////////// void OgreCOMVisual::PreRender() { - if (this->HasParent() && this->ParentName().empty()) + if (this->HasParent() && this->parentName.empty()) this->parentName = this->Parent()->Name(); if (this->dirtyCOMVisual && - !this->ParentName().empty()) + !this->parentName.empty()) { this->parentName = this->Parent()->Name(); this->CreateVisual(); @@ -108,7 +108,7 @@ void OgreCOMVisual::CreateVisual() this->dataPtr->sphereVis->SetLocalRotation(this->InertiaPose().Rot()); // Get the bounding box of the parent visual - VisualPtr vis = this->Scene()->VisualByName(this->ParentName()); + VisualPtr vis = this->Scene()->VisualByName(this->parentName); ignition::math::AxisAlignedBox box; if (vis) box = vis->LocalBoundingBox(); diff --git a/ogre2/src/Ogre2COMVisual.cc b/ogre2/src/Ogre2COMVisual.cc index 9c212e1af..10f556b2a 100644 --- a/ogre2/src/Ogre2COMVisual.cc +++ b/ogre2/src/Ogre2COMVisual.cc @@ -56,11 +56,11 @@ Ogre2COMVisual::~Ogre2COMVisual() ////////////////////////////////////////////////// void Ogre2COMVisual::PreRender() { - if (this->HasParent() && this->ParentName().empty()) + if (this->HasParent() && this->parentName.empty()) this->parentName = this->Parent()->Name(); if (this->dirtyCOMVisual && - !this->ParentName().empty()) + !this->parentName.empty()) { this->parentName = this->Parent()->Name(); this->CreateVisual(); @@ -126,7 +126,7 @@ void Ogre2COMVisual::CreateVisual() this->dataPtr->sphereVis->SetLocalRotation(this->InertiaPose().Rot()); // Get the bounding box of the parent visual - VisualPtr vis = this->Scene()->VisualByName(this->ParentName()); + VisualPtr vis = this->Scene()->VisualByName(this->parentName); ignition::math::AxisAlignedBox box; if (vis) box = vis->LocalBoundingBox(); diff --git a/src/COMVisual_TEST.cc b/src/COMVisual_TEST.cc index cf099b160..9038114e8 100644 --- a/src/COMVisual_TEST.cc +++ b/src/COMVisual_TEST.cc @@ -61,13 +61,10 @@ void COMVisualTest::COMVisual(const std::string &_renderEngine) inertial.SetMassMatrix(massMatrix); inertial.SetPose(p); - std::string parentName = ""; - // check initial values EXPECT_EQ(nullptr, comVisual->SphereVisual()); EXPECT_EQ(ignition::math::Pose3d::Zero, comVisual->InertiaPose()); EXPECT_DOUBLE_EQ(1.0, comVisual->Mass()); - EXPECT_EQ(parentName, comVisual->ParentName()); // set invalid mass comVisual->SetMass(-1.0); @@ -89,11 +86,9 @@ void COMVisualTest::COMVisual(const std::string &_renderEngine) EXPECT_DOUBLE_EQ(2.0, comVisual->Mass()); // set parent - parentName = "parent_visual"; - VisualPtr parentVisual = scene->CreateVisual(parentName); + VisualPtr parentVisual = scene->CreateVisual("parent_visual"); parentVisual->AddChild(comVisual); comVisual->PreRender(); - EXPECT_EQ(parentName, comVisual->ParentName()); EXPECT_NE(nullptr, comVisual->SphereVisual()); // Clean up From 360086d2f169ba55bb62c56ace60cdbf978ebcbb Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Fri, 25 Jun 2021 15:25:55 +0530 Subject: [PATCH 18/20] Add SphereRadius method Signed-off-by: Atharva Pusalkar --- include/ignition/rendering/base/BaseCOMVisual.hh | 15 +++++++++++++++ ogre/src/OgreCOMVisual.cc | 6 +----- ogre2/src/Ogre2COMVisual.cc | 6 +----- 3 files changed, 17 insertions(+), 10 deletions(-) diff --git a/include/ignition/rendering/base/BaseCOMVisual.hh b/include/ignition/rendering/base/BaseCOMVisual.hh index 168d70c8e..2c0d56f6e 100644 --- a/include/ignition/rendering/base/BaseCOMVisual.hh +++ b/include/ignition/rendering/base/BaseCOMVisual.hh @@ -66,6 +66,9 @@ namespace ignition // Documentation inherited public: virtual VisualPtr SphereVisual() const override; + // Get the radius of the CoM sphere + protected: double SphereRadius() const; + /// \brief Parent visual name. protected: std::string parentName = ""; @@ -161,6 +164,18 @@ namespace ignition { return nullptr; } + + ////////////////////////////////////////////////// + template + double BaseCOMVisual::SphereRadius() const + { + // Compute radius of sphere with density of lead and equivalent mass. + double sphereRadius; + double dLead = 11340; + sphereRadius = cbrt((0.75 * this->Mass()) / (IGN_PI * dLead)); + + return sphereRadius; + } } } } diff --git a/ogre/src/OgreCOMVisual.cc b/ogre/src/OgreCOMVisual.cc index 30c4a5913..6b4845596 100644 --- a/ogre/src/OgreCOMVisual.cc +++ b/ogre/src/OgreCOMVisual.cc @@ -97,11 +97,7 @@ void OgreCOMVisual::CreateVisual() this->AddChild(this->dataPtr->sphereVis); } - // Compute radius of sphere with density of lead and equivalent mass. - double sphereRadius; - double dLead = 11340; - sphereRadius = cbrt((0.75 * this->Mass()) / (IGN_PI * dLead)); - + double sphereRadius = this->SphereRadius(); this->dataPtr->sphereVis->SetLocalScale(ignition::math::Vector3d( sphereRadius*2, sphereRadius*2, sphereRadius*2)); this->dataPtr->sphereVis->SetLocalPosition(this->InertiaPose().Pos()); diff --git a/ogre2/src/Ogre2COMVisual.cc b/ogre2/src/Ogre2COMVisual.cc index 10f556b2a..6d9052b4b 100644 --- a/ogre2/src/Ogre2COMVisual.cc +++ b/ogre2/src/Ogre2COMVisual.cc @@ -115,11 +115,7 @@ void Ogre2COMVisual::CreateVisual() this->AddChild(this->dataPtr->sphereVis); } - // Compute radius of sphere with density of lead and equivalent mass. - double sphereRadius; - double dLead = 11340; - sphereRadius = cbrt((0.75 * this->Mass()) / (IGN_PI * dLead)); - + double sphereRadius = this->SphereRadius(); this->dataPtr->sphereVis->SetLocalScale(ignition::math::Vector3d( sphereRadius*2, sphereRadius*2, sphereRadius*2)); this->dataPtr->sphereVis->SetLocalPosition(this->InertiaPose().Pos()); From 59afa123e396c3235a41d3ff197a7cea01bfb5b8 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Fri, 25 Jun 2021 16:54:30 +0530 Subject: [PATCH 19/20] Add function doc Signed-off-by: Atharva Pusalkar --- include/ignition/rendering/base/BaseCOMVisual.hh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/ignition/rendering/base/BaseCOMVisual.hh b/include/ignition/rendering/base/BaseCOMVisual.hh index 2c0d56f6e..c785d8503 100644 --- a/include/ignition/rendering/base/BaseCOMVisual.hh +++ b/include/ignition/rendering/base/BaseCOMVisual.hh @@ -66,7 +66,8 @@ namespace ignition // Documentation inherited public: virtual VisualPtr SphereVisual() const override; - // Get the radius of the CoM sphere + /// \brief Get the radius of the CoM sphere + /// \return Radius of the CoM sphere protected: double SphereRadius() const; /// \brief Parent visual name. From aee5617f0745a06486c3c2bd95dc7f8a9a71763e Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Fri, 25 Jun 2021 22:42:17 +0530 Subject: [PATCH 20/20] Rename to densityLead Signed-off-by: Atharva Pusalkar --- include/ignition/rendering/base/BaseCOMVisual.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ignition/rendering/base/BaseCOMVisual.hh b/include/ignition/rendering/base/BaseCOMVisual.hh index c785d8503..29059091d 100644 --- a/include/ignition/rendering/base/BaseCOMVisual.hh +++ b/include/ignition/rendering/base/BaseCOMVisual.hh @@ -172,8 +172,8 @@ namespace ignition { // Compute radius of sphere with density of lead and equivalent mass. double sphereRadius; - double dLead = 11340; - sphereRadius = cbrt((0.75 * this->Mass()) / (IGN_PI * dLead)); + double densityLead = 11340; + sphereRadius = cbrt((0.75 * this->Mass()) / (IGN_PI * densityLead)); return sphereRadius; }