BaseCOMVisual.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2021 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16  */
17 #ifndef GZ_RENDERING_BASE_BASECOMVISUAL_HH_
18 #define GZ_RENDERING_BASE_BASECOMVISUAL_HH_
19 
20 #include <string>
21 
22 #include "gz/common/Console.hh"
23 
27 #include "gz/rendering/Scene.hh"
28 
29 namespace gz
30 {
31  namespace rendering
32  {
33  inline namespace GZ_RENDERING_VERSION_NAMESPACE {
34  //
36  template <class T>
37  class BaseCOMVisual :
38  public virtual COMVisual,
39  public virtual T
40  {
42  protected: BaseCOMVisual();
43 
45  public: virtual ~BaseCOMVisual();
46 
47  // Documentation inherited.
48  protected: virtual void Init() override;
49 
50  // Documentation inherited.
51  protected: virtual void PreRender() override;
52 
53  // Documentation inherited.
54  public: virtual void SetInertial(
55  const gz::math::Inertiald &_inertial) override;
56 
57  // Documentation inherited.
58  public: virtual void SetMass(double _mass) override;
59 
60  // Documentation inherited
61  public: virtual double Mass() const override;
62 
63  // Documentation inherited
64  public: virtual gz::math::Pose3d InertiaPose() const override;
65 
66  // Documentation inherited
67  public: virtual VisualPtr SphereVisual() const override;
68 
71  protected: double SphereRadius() const;
72 
74  protected: std::string parentName = "";
75 
77  protected: double mass = 1.0;
78 
80  protected: gz::math::Pose3d inertiaPose =
82 
84  protected: bool dirtyCOMVisual = false;
85  };
86 
88  template <class T>
90  {
91  }
92 
94  template <class T>
96  {
97  }
98 
100  template <class T>
102  {
103  T::PreRender();
104  }
105 
107  template <class T>
109  {
110  T::Init();
111  }
112 
114  template <class T>
116  const gz::math::Inertiald &_inertial)
117  {
118  this->inertiaPose = _inertial.Pose();
119 
120  this->SetMass(_inertial.MassMatrix().Mass());
121  }
122 
123  template <class T>
124  void BaseCOMVisual<T>::SetMass(double _mass)
125  {
126  if (_mass <= 0)
127  {
128  // Unrealistic mass, load with default mass
129  if (_mass < 0)
130  {
131  gzlog << "The parent " << this->parentName
132  << " has unrealistic mass, "
133  << "unable to visualize sphere of equivalent mass.\n";
134  }
135  else
136  {
137  gzlog << "The parent " << this->parentName
138  << " is static or has mass of 0, "
139  << "so a sphere of equivalent mass will not be shown.\n";
140  }
141  return;
142  }
143 
144  this->mass = _mass;
145  this->dirtyCOMVisual = true;
146  }
147 
149  template <class T>
150  double BaseCOMVisual<T>::Mass() const
151  {
152  return this->mass;
153  }
154 
156  template <class T>
158  {
159  return this->inertiaPose;
160  }
161 
163  template <class T>
165  {
166  return nullptr;
167  }
168 
170  template <class T>
172  {
173  // Compute radius of sphere with density of lead and equivalent mass.
174  double sphereRadius;
175  double densityLead = 11340;
176  sphereRadius = cbrt((0.75 * this->Mass()) / (GZ_PI * densityLead));
177 
178  return sphereRadius;
179  }
180  }
181  }
182 }
183 #endif