19 #ifndef __UUV_GAZEBO_PLUGINS_BUOYANT_OBJECT_HH__ 20 #define __UUV_GAZEBO_PLUGINS_BUOYANT_OBJECT_HH__ 25 #include <gazebo/gazebo.hh> 26 #include <gazebo/physics/Link.hh> 27 #include <gazebo/physics/Collision.hh> 28 #include <gazebo/physics/Shape.hh> 30 #define RESTORING_FORCE "restoring_force" 46 ignition::math::Vector3d &buoyancyForce,
47 ignition::math::Vector3d &buoyancyTorque);
53 public:
void SetVolume(
double _volume = -1);
65 public:
void SetCoB(
const ignition::math::Vector3d &_centerOfBuoyancy);
68 public: ignition::math::Vector3d
GetCoB();
101 protected:
void StoreVector(std::string _tag, ignition::math::Vector3d _vec);
127 protected: std::map<std::string, ignition::math::Vector3d>
hydroWrench;
136 protected: physics::LinkPtr
link;
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.
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.
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'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.
Class describing the dynamics of a buoyant object, useful for simple representations of underwater st...
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.