BuoyantObject.cc
Go to the documentation of this file.
1 // Copyright (c) 2016 The UUV Simulator Authors.
2 // All rights reserved.
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 #include <cmath>
17 
18 #include <gazebo/gazebo.hh>
20 
21 namespace gazebo {
22 
24 BuoyantObject::BuoyantObject(physics::LinkPtr _link)
25 {
26  GZ_ASSERT(_link != NULL, "Invalid link pointer");
27 
28  // Initial volume
29  this->volume = 0.0;
30  // Fluid density for sea water at 0 degrees Celsius
31  this->fluidDensity = 1028.0;
32  this->g = 9.81;
33  this->centerOfBuoyancy.Set(0, 0, 0);
34  this->debugFlag = false;
35  this->isSubmerged = true;
36  this->metacentricWidth = 0.0;
37  this->metacentricLength = 0.0;
38  this->waterLevelPlaneArea = 0.0;
39  this->submergedHeight = 0.0;
40  this->isSurfaceVessel = false;
41  this->scalingVolume = 1.0;
42  this->offsetVolume = 0.0;
43  this->isSurfaceVesselFloating = false;
44 
45  this->link = _link;
46  // Retrieve the bounding box
47  // FIXME(mam0box) Gazebo's bounding box method is NOT working
48 
49  // TODO(mam0box) Change the way the bounding box is retrieved,
50  // it should come from the physics engine but it is still not resolved
51 #if GAZEBO_MAJOR_VERSION >= 8
52  this->boundingBox = link->BoundingBox();
53 #else
54  math::Box bBox = link->GetBoundingBox();
55  this->boundingBox = ignition::math::Box(bBox.min.x, bBox.min.y, bBox.min.z,
56  bBox.max.x, bBox.max.y, bBox.max.z);
57 #endif
58  // Set neutrally buoyant flag to false
59  this->neutrallyBuoyant = false;
60 }
61 
64 
67 {
68  this->neutrallyBuoyant = true;
69  // Calculate the equivalent volume for the submerged body
70  // so that it will be neutrally buoyant
71  double mass;
72 #if GAZEBO_MAJOR_VERSION >= 8
73  mass = this->link->GetInertial()->Mass();
74 #else
75  mass = this->link->GetInertial()->GetMass();
76 #endif
77  this->volume = mass / this->fluidDensity;
78  gzmsg << this->link->GetName() << " is neutrally buoyant" << std::endl;
79 }
80 
82 void BuoyantObject::GetBuoyancyForce(const ignition::math::Pose3d &_pose,
83  ignition::math::Vector3d &buoyancyForce,
84  ignition::math::Vector3d &buoyancyTorque)
85 {
86  double height = this->boundingBox.ZLength();
87  double z = _pose.Pos().Z();
88  double volume = 0.0;
89 
90  buoyancyForce = ignition::math::Vector3d(0, 0, 0);
91  buoyancyTorque = ignition::math::Vector3d(0, 0, 0);
92 
93  double mass;
94 #if GAZEBO_MAJOR_VERSION >= 8
95  mass = this->link->GetInertial()->Mass();
96 #else
97  mass = this->link->GetInertial()->GetMass();
98 #endif
99 
100  if (!this->isSurfaceVessel)
101  {
102  if (z + height / 2 > 0 && z < 0)
103  {
104  this->isSubmerged = false;
105  volume = this->GetVolume() * (std::fabs(z) + height / 2) / height;
106  }
107  else if (z + height / 2 < 0)
108  {
109  this->isSubmerged = true;
110  volume = this->GetVolume();
111  }
112 
113  if (!this->neutrallyBuoyant || volume != this->volume)
114  buoyancyForce = ignition::math::Vector3d(0, 0,
115  volume * this->fluidDensity * this->g);
116  else if (this->neutrallyBuoyant)
117  buoyancyForce = ignition::math::Vector3d(
118  0, 0, mass * this->g);
119  }
120  else
121  {
122  // Implementation of the linear (small angle) theory for boxed-shaped
123  // vessels. Further details can be seen at
124  // T. I. Fossen, "Handbook of Marine Craft Hydrodynamics and Motion Control," Apr. 2011.
125  // Page 65
126  if (this->waterLevelPlaneArea <= 0)
127  {
128  this->waterLevelPlaneArea = this->boundingBox.XLength() *
129  this->boundingBox.YLength();
130  gzmsg << this->link->GetName() << "::" << "waterLevelPlaneArea = " <<
131  this->waterLevelPlaneArea << std::endl;
132  }
133 
134  this->waterLevelPlaneArea = mass / (this->fluidDensity * this->submergedHeight);
135  double curSubmergedHeight;
136  GZ_ASSERT(this->waterLevelPlaneArea > 0.0,
137  "Water level plane area must be greater than zero");
138 
139  if (z > height / 2.0) {
140  // Vessel is completely out of the water
141  buoyancyForce = ignition::math::Vector3d(0, 0, 0);
142  buoyancyTorque = ignition::math::Vector3d(0, 0, 0);
143  return;
144  } else if (z < -height / 2.0) {
145  curSubmergedHeight = this->boundingBox.ZLength();
146  } else {
147  curSubmergedHeight = height / 2.0 - z;
148  }//else
149 
150  volume = curSubmergedHeight * this->waterLevelPlaneArea;
151  buoyancyForce = ignition::math::Vector3d(0, 0, volume * this->fluidDensity * this->g);
152  buoyancyTorque = ignition::math::Vector3d(
153  -1 * this->metacentricWidth * sin(_pose.Rot().Roll()) * buoyancyForce.Z(),
154  -1 * this->metacentricLength * sin(_pose.Rot().Pitch()) * buoyancyForce.Z(),
155  0);
156 
157  // Store the restoring force vector, if needed
158  this->StoreVector(RESTORING_FORCE, buoyancyForce);
159  }
160 
161  // Store the restoring force vector, if needed
162  this->StoreVector(RESTORING_FORCE, buoyancyForce);
163 }
164 
167 {
168  // Link's pose
169  ignition::math::Pose3d pose;
170 #if GAZEBO_MAJOR_VERSION >= 8
171  pose = this->link->WorldPose();
172 #else
173  pose = this->link->GetWorldPose().Ign();
174 #endif
175  // Get the buoyancy force in world coordinates
176  ignition::math::Vector3d buoyancyForce, buoyancyTorque;
177 
178  this->GetBuoyancyForce(pose, buoyancyForce, buoyancyTorque);
179 
180  GZ_ASSERT(!std::isnan(buoyancyForce.Length()),
181  "Buoyancy force is invalid");
182  GZ_ASSERT(!std::isnan(buoyancyTorque.Length()),
183  "Buoyancy torque is invalid");
184  if (!this->isSurfaceVessel)
185 #if GAZEBO_MAJOR_VERSION >= 8
186  this->link->AddForceAtRelativePosition(buoyancyForce, this->GetCoB());
187 #else
188  this->link->AddForceAtRelativePosition(
189  math::Vector3(buoyancyForce.X(), buoyancyForce.Y(), buoyancyForce.Z()),
190  math::Vector3(this->GetCoB().X(), this->GetCoB().Y(), this->GetCoB().Z()));
191 #endif
192  else
193  {
194 #if GAZEBO_MAJOR_VERSION >= 8
195  this->link->AddForce(buoyancyForce);
196  this->link->AddRelativeTorque(buoyancyTorque);
197 #else
198  this->link->AddForce(
199  math::Vector3(buoyancyForce.X(), buoyancyForce.Y(), buoyancyForce.Z()));
200  this->link->AddRelativeTorque(
201  math::Vector3(buoyancyTorque.X(), buoyancyTorque.Y(), buoyancyTorque.Z()));
202 #endif
203  }
204 
205 }
206 
208 void BuoyantObject::SetBoundingBox(const ignition::math::Box &_bBox)
209 {
210  this->boundingBox = ignition::math::Box(_bBox);
211 
212  gzmsg << "New bounding box for " << this->link->GetName() << "::"
213  << this->boundingBox << std::endl;
214 }
215 
217 void BuoyantObject::SetVolume(double _volume)
218 {
219  GZ_ASSERT(_volume > 0, "Invalid input volume");
220  this->volume = _volume;
221 }
222 
225 {
226  return std::max(0.0, this->scalingVolume * (this->volume + this->offsetVolume));
227 }
228 
230 void BuoyantObject::SetFluidDensity(double _fluidDensity)
231 {
232  GZ_ASSERT(_fluidDensity > 0, "Fluid density must be a positive value");
233  this->fluidDensity = _fluidDensity;
234 }
235 
238 
240 void BuoyantObject::SetCoB(const ignition::math::Vector3d &_centerOfBuoyancy)
241 {
242  this->centerOfBuoyancy = ignition::math::Vector3d(
243  _centerOfBuoyancy.X(), _centerOfBuoyancy.Y(), _centerOfBuoyancy.Z());
244 }
245 
247 ignition::math::Vector3d BuoyantObject::GetCoB() { return this->centerOfBuoyancy; }
248 
251 {
252  GZ_ASSERT(_g > 0, "Acceleration of gravity must be positive");
253  this->g = _g;
254 }
255 
257 double BuoyantObject::GetGravity() { return this->g; }
258 
260 void BuoyantObject::SetDebugFlag(bool _debugOn) { this->debugFlag = _debugOn; }
261 
263 bool BuoyantObject::GetDebugFlag() { return this->debugFlag; }
264 
266 void BuoyantObject::SetStoreVector(std::string _tag)
267 {
268  if (!this->debugFlag)
269  return;
270  // Test if field exists
271  if (!this->hydroWrench.count(_tag))
272  this->hydroWrench[_tag] = ignition::math::Vector3d(0, 0, 0);
273 }
274 
276 ignition::math::Vector3d BuoyantObject::GetStoredVector(std::string _tag)
277 {
278  if (!this->debugFlag)
279  return ignition::math::Vector3d(0, 0, 0);
280  if (this->hydroWrench.count(_tag))
281  return this->hydroWrench[_tag];
282  else
283  return ignition::math::Vector3d(0, 0, 0);
284 }
285 
287 void BuoyantObject::StoreVector(std::string _tag,
288  ignition::math::Vector3d _vec)
289 {
290  if (!this->debugFlag)
291  return;
292  // Test if field exists
293  if (this->hydroWrench.count(_tag))
294  this->hydroWrench[_tag] = _vec;
295 }
296 
299 {
300  return this->isSubmerged;
301 }
302 
305 {
306  return this->neutrallyBuoyant;
307 }
308 }
double volume
Volume of fluid displaced by the submerged object.
double submergedHeight
Height of the robot that is submerged (only for surface vessels)
void StoreVector(std::string _tag, ignition::math::Vector3d _vec)
Store vector in the hydroWrench map if the field has been created.
bool IsNeutrallyBuoyant()
Returns true if the link was set to be neutrally buoyant.
Description of a buoyant object.
void SetDebugFlag(bool _debugOn=true)
Set debug flag to store intermediate forces and torques.
bool debugFlag
Debug flag, storing all intermediate forces and torques.
ignition::math::Box boundingBox
TEMP for calculation of the buoyancy force close to the surface.
void SetNeutrallyBuoyant()
Sets this link as neutrally buoyant.
std::map< std::string, ignition::math::Vector3d > hydroWrench
Storage for hydrodynamic and hydrostatic forces and torques for debugging purposes.
ignition::math::Vector3d GetStoredVector(std::string _tag)
Get vector from the hydroWrench map.
void SetCoB(const ignition::math::Vector3d &_centerOfBuoyancy)
Sets the position of the center of buoyancy on the body frame.
double GetGravity()
Get stored acceleration of gravity.
bool neutrallyBuoyant
If true, the restoring force will be equal to the gravitational.
double offsetVolume
Offset for the volume.
#define RESTORING_FORCE
double g
Acceleration of gravity.
double waterLevelPlaneArea
If the cross section area around water level of the surface vessel is not given, it will be computed ...
bool GetDebugFlag()
Returns the debug flag.
void SetBoundingBox(const ignition::math::Box &_bBox)
Sets bounding box.
bool isSubmerged
Is submerged flag.
void SetFluidDensity(double _fluidDensity)
Sets the fluid density in kg/m^3.
void ApplyBuoyancyForce()
Applies buoyancy force on link.
double GetVolume()
Returns the stored link submerged volume.
~BuoyantObject()
Destructor.
bool isSurfaceVessel
Flag set to true if the information about the metacentric width and height is available.
void SetVolume(double _volume=-1)
Sets the link&#39;s submerged volume.
void SetStoreVector(std::string _tag)
Adds a field in the hydroWrench map.
BuoyantObject(physics::LinkPtr _link)
Constructor.
void SetGravity(double _g)
Set acceleration of gravity.
double fluidDensity
Fluid density.
ignition::math::Vector3d centerOfBuoyancy
Center of buoyancy in the body frame.
ignition::math::Vector3d GetCoB()
Returns the stored center of buoyancy.
void GetBuoyancyForce(const ignition::math::Pose3d &_pose, ignition::math::Vector3d &buoyancyForce, ignition::math::Vector3d &buoyancyTorque)
Returns the buoyancy force vector in the world frame.
double metacentricLength
Metacentric length of the robot, used only for surface vessels and floating objects.
double GetFluidDensity()
Returns the stored fluid density.
physics::LinkPtr link
Pointer to the correspondent robot link.
double scalingVolume
Scaling factor for the volume.
bool isSurfaceVesselFloating
Flag set to true if the vessel has reached its submerged height.
bool IsSubmerged()
Returns true if the robot is completely submerged.


uuv_gazebo_plugins
Author(s): Musa Morena Marcusso Manhaes , Sebastian Scherer , Luiz Ricardo Douat
autogenerated on Thu Jun 18 2020 03:28:24