diff --git a/vrx_ign/CMakeLists.txt b/vrx_ign/CMakeLists.txt index b6bd800e7..b5ac77a79 100644 --- a/vrx_ign/CMakeLists.txt +++ b/vrx_ign/CMakeLists.txt @@ -20,6 +20,8 @@ find_package(ignition-plugin1 REQUIRED COMPONENTS loader register) set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) find_package(ignition-rendering6 REQUIRED) set(IGN_RENDERING_VER ${ignition-rendering6_VERSION_MAJOR}) +find_package(ignition-utils1 REQUIRED) +set(IGN_UTILS_VER ${ignition-utils1_VERSION_MAJOR}) find_package(sdformat12 REQUIRED) find_package(std_msgs REQUIRED) @@ -44,6 +46,22 @@ install( TARGETS Waves DESTINATION lib) +# Buoyancy +add_library(PolyhedraBuoyancyDrag SHARED + src/PolyhedraBuoyancyDrag.cc + src/PolyhedronVolume.cc + src/ShapeVolume.cc +) +target_link_libraries(PolyhedraBuoyancyDrag PUBLIC + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-gazebo${IGN_GAZEBO_VER}::core + ignition-math${IGN_MATH_VER} + Waves +) +install( + TARGETS PolyhedraBuoyancyDrag + DESTINATION lib) + # Plugins list(APPEND VRX_IGN_PLUGINS SimpleHydrodynamics @@ -57,8 +75,9 @@ foreach(PLUGIN ${VRX_IGN_PLUGINS}) ignition-gazebo${IGN_GAZEBO_VER}::core ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} + ignition-utils${IGN_UTILS_VER}::ignition-utils${IGN_UTILS_VER} Waves - ) +) endforeach() install( @@ -78,5 +97,4 @@ install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) - ament_package() diff --git a/vrx_ign/models/wam-v/model.sdf.erb b/vrx_ign/models/wam-v/model.sdf.erb index 492bcb6c4..09b675add 100644 --- a/vrx_ign/models/wam-v/model.sdf.erb +++ b/vrx_ign/models/wam-v/model.sdf.erb @@ -69,16 +69,48 @@ end <%= $model_name%>/right/thruster/joint/cmd_pos + base_link - 4.9 - 2.4 + 4.9 0.213 0 + + 0.6 1.03 0 + -1.4 1.03 0 + + + <%= $wavefield_size%> <%= $wavefield_size%> + <%= $wavefield_cell_count%> <%=$wavefield_cell_count%> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + - + + + base_link + 4.9 + 0.213 + 0 + + 0.6 -1.03 0 + -1.4 -1.03 0 + <%= $wavefield_size%> <%= $wavefield_size%> <%= $wavefield_cell_count%> <%=$wavefield_cell_count%> @@ -87,7 +119,7 @@ end 5.0 3 1.1 - 0.5 + 0.3 1 0 0.4 2.0 diff --git a/vrx_ign/src/PolyhedraBuoyancyDrag.cc b/vrx_ign/src/PolyhedraBuoyancyDrag.cc new file mode 100644 index 000000000..1781f45d5 --- /dev/null +++ b/vrx_ign/src/PolyhedraBuoyancyDrag.cc @@ -0,0 +1,357 @@ +/* + * Copyright (C) 2015 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "PolyhedraBuoyancyDrag.hh" +#include "ShapeVolume.hh" +#include "Wavefield.hh" + +using namespace ignition; +using namespace vrx; + +/// \brief A class for storing buoyancy object properties. +/// +/// ## Required parameters +/// +/// * `` is the name of the link used to apply forces. +/// * `` is the geometry element modeling the buoyancy of the object. +/// +/// ## Optional parameters +/// +/// * `` is the offset from `link_name` where forces will be applied. +class BuoyancyObject +{ + /// \brief Load a buoyancy object from SDF. + /// \param[in] _entity The model entity contining the buoyancy object. + /// \param[in] _sdf The SDF Element tree containing the object parameters. + /// \param[in] _ecm Gazebo's ECM. + public: void Load(const gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + gazebo::EntityComponentManager &_ecm); + + /// \brief Stream extraction operator. + /// \param[in] _out output stream. + /// \param[in] _obj BuoyancyObject to output. + /// \return The stream. + public: friend std::ostream + &operator<<(std::ostream &_out, const BuoyancyObject &_obj) + { + _out << "Buoyancy object:\n" + << "\tLink: " << _obj.linkName << "[" << _obj.linkId << "]\n" + << "\tPose: " << _obj.pose << std::endl + << "\tGeometry: " << _obj.shape->Display() << std::endl + << "\tMass: " << _obj.mass << std::endl + << "\tWater height: " << _obj.height << std::endl + << "\tWater velocity: " << _obj.waterSpeed << std::endl; + + return _out; + } + + /// \brief The link entity. + public: gazebo::Link link; + + /// \brief Associated link ID. + public: gazebo::Entity linkId = -1; + + /// \brief Associated link name. + public: std::string linkName = ""; + + /// \brief Pose of buoyancy relative to link origin. + public: math::Pose3d pose = math::Pose3d::Zero; + + /// \brief Object mass (from inertial elem). + public: double mass = 0; + + /// \brief Buoyancy object's shape properties. + public: ShapeVolumePtr shape = nullptr; + + /// \brief Water height at pose. + public: double height = 0; + + /// \brief Water speed at pose. + public: double waterSpeed = 0; +}; + +/////////////////////////////////////////////////// +void BuoyancyObject::Load(const gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + gazebo::EntityComponentManager &_ecm) +{ + // Parse required . + if (!_sdf->HasElement("link_name")) + { + ignerr << "No specified" << std::endl; + return; + } + + this->linkName = _sdf->Get("link_name"); + gazebo::Model model(_entity); + this->link = gazebo::Link(model.LinkByName(_ecm, this->linkName)); + if (!this->link.Valid(_ecm)) + { + ignerr << "Could not find link named [" << this->linkName + << "] in model" << std::endl; + return; + } + this->link.EnableVelocityChecks(_ecm, true); + this->linkId = this->link.Entity(); + + // Get mass. + auto inertial = _ecm.Component(this->linkId); + if (!inertial) + { + ignerr << "Could not inertial element for [" << this->linkName + << "] in model" << std::endl; + return; + } + this->mass = inertial->Data().MassMatrix().Mass(); + + // Parse required . + if (!_sdf->HasElement("geometry")) + { + ignerr << "No specified" << std::endl; + return; + } + + auto ptr = const_cast(_sdf.get()); + sdf::ElementPtr geometry = ptr->GetElement("geometry"); + this->shape = std::move(ShapeVolume::makeShape(geometry)); + + // Parse optional . + if (_sdf->HasElement("pose")) + { + this->pose = _sdf->Get("pose"); + } +} + +/// \brief Private PolyhedraBuoyancyDrag data class. +class PolyhedraBuoyancyDrag::Implementation +{ + /// \brief The wavefield. + public: Wavefield wavefield; + + /// \brief The density of the fluid in which the object is submerged in kg/m^3 + public: double fluidDensity = 997; + + /// \brief The height of the fluid/air interface [m]. Defaults to 0. + public: double fluidLevel = 0; + + /// \brief Linear drag coefficient. Defaults to 0. + public: double linearDrag = 0; + + /// \brief Angular drag coefficient. Defaults to 0. + public: double angularDrag = 0; + + /// \brief List of buoyancy objects for model. + public: std::vector buoyancyObjects; + + /// \brief Previous update time. + public: double lastSimTime{0}; + + /// \brief The world's gravity [m/s^2]. + public: math::Vector3d gravity; +}; + +////////////////////////////////////////////////// +PolyhedraBuoyancyDrag::PolyhedraBuoyancyDrag() + : System(), dataPtr(utils::MakeUniqueImpl()) +{ +} + +////////////////////////////////////////////////// +void PolyhedraBuoyancyDrag::Configure(const gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + gazebo::EntityComponentManager &_ecm, + gazebo::EventManager &/*_eventMgr*/) +{ + // Parse optional . + this->dataPtr->wavefield.Load(_sdf); + + // Parse optional . + if (_sdf->HasElement("fluid_density")) + this->dataPtr->fluidDensity = _sdf->Get("fluid_density"); + + // Parse optional . + if (_sdf->HasElement("fluid_level")) + this->dataPtr->fluidLevel = _sdf->Get("fluid_level"); + + // Parse optional . + if (_sdf->HasElement("linear_drag")) + this->dataPtr->linearDrag = _sdf->Get("linear_drag"); + + // Parse optional . + if (_sdf->HasElement("angular_drag")) + this->dataPtr->angularDrag = _sdf->Get("angular_drag"); + + // Parse optional . + if (_sdf->HasElement("buoyancy")) + { + gazebo::Model model(_entity); + igndbg << "Found buoyancy element(s), looking at each element..." + << std::endl; + auto ptr = const_cast(_sdf.get()); + for (sdf::ElementPtr buoyancyElem = ptr->GetElement("buoyancy"); + buoyancyElem; + buoyancyElem = buoyancyElem->GetNextElement("buoyancy")) + { + BuoyancyObject buoyObj; + buoyObj.Load(_entity, buoyancyElem, _ecm); + buoyObj.height = this->dataPtr->fluidLevel; + + // Add buoyancy object to list and display stats. + igndbg << buoyObj << std::endl; + this->dataPtr->buoyancyObjects.push_back(std::move(buoyObj)); + } + } + + // Get the gravity from the world. + auto worldEntity = gazebo::worldEntity(_ecm); + auto world = gazebo::World(worldEntity); + auto gravityOpt = world.Gravity(_ecm); + if (!gravityOpt) + { + ignerr << "Unable to get the gravity from the world" << std::endl; + return; + } + this->dataPtr->gravity = *gravityOpt; +} + +////////////////////////////////////////////////// +void PolyhedraBuoyancyDrag::PreUpdate(const gazebo::UpdateInfo &_info, + gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("PolyhedraBuoyancyDrag::PreUpdate"); + + // Elapsed time since the last update. + double dt; + double simTime; + if (this->dataPtr->wavefield.Active()) + { + simTime = std::chrono::duration(_info.simTime).count(); + dt = simTime - this->dataPtr->lastSimTime; + this->dataPtr->lastSimTime = simTime; + } + + for (auto &buoyancyObj : this->dataPtr->buoyancyObjects) + { + // Link pose in world coordinates. + auto worldPoseComp = buoyancyObj.link.WorldPose(_ecm); + if (!worldPoseComp) + { + ignerr << "No worldpose component found" << std::endl; + continue; + } + math::Pose3d linkFrame = *worldPoseComp; + + // Apply the offset. + linkFrame = linkFrame * buoyancyObj.pose; + + // std::cerr << simTime << " " << linkFrame.Pos() << std::endl; + + if (this->dataPtr->wavefield.Active()) + { + // Compute the wave displacement at the offset point, + // relative to the mean water level. + double waveHeight = this->dataPtr->wavefield.ComputeDepthSimply( + linkFrame.Pos(), simTime); + buoyancyObj.waterSpeed = (waveHeight - buoyancyObj.height) / dt; + buoyancyObj.height = waveHeight; + } + + auto submergedVolume = buoyancyObj.shape->CalculateVolume(linkFrame, + buoyancyObj.height + this->dataPtr->fluidLevel); + + // Calculate buoyancy and drag forces. + if (submergedVolume.volume > 1e-6) + { + // By Archimedes' principle, + math::Vector3d buoyancy = -this->dataPtr->fluidDensity * + submergedVolume.volume * this->dataPtr->gravity; + + float partialMass = buoyancyObj.mass * submergedVolume.volume + / buoyancyObj.shape->volume; + + auto linVel = buoyancyObj.link.WorldLinearVelocity(_ecm); + if (!linVel) + { + ignerr << "No linear velocity component found" << std::endl; + continue; + } + + // Linear drag. + math::Vector3d relVel = + math::Vector3d(0, 0, buoyancyObj.waterSpeed) - *linVel; + + math::Vector3d dragForce = + this->dataPtr->linearDrag * partialMass * relVel; + + buoyancy += dragForce; + + if (buoyancy.Z() < 0.0) + buoyancy.Z() = 0.0; + + // We apply a force with an offset. The offset should be in the center of + // mass (COM) link's frame. + auto comPose = buoyancyObj.link.WorldInertialPose(_ecm); + auto centroidLocal = comPose->Rot().Inverse() * + (submergedVolume.centroid - comPose->Pos()); + + // Apply force. + buoyancyObj.link.AddWorldForce(_ecm, buoyancy, centroidLocal); + + auto worldAngularVel = buoyancyObj.link.WorldAngularVelocity(_ecm); + if (!worldAngularVel) + { + ignerr << "No angular velocity component found" <<"\n"; + continue; + } + + auto localAngularVel = comPose->Rot().Inverse() * (*worldAngularVel); + + // Drag torque. + double averageLength2 = ::pow(buoyancyObj.shape->averageLength, 2); + math::Vector3d dragTorque = (-partialMass * this->dataPtr->angularDrag * + averageLength2) * localAngularVel; + + math::Vector3d torqueWorld = (*comPose).Rot().RotateVector(dragTorque); + + // Apply torque. + buoyancyObj.link.AddWorldWrench(_ecm, {0, 0, 0}, torqueWorld); + } + } +} + +IGNITION_ADD_PLUGIN(PolyhedraBuoyancyDrag, + gazebo::System, + PolyhedraBuoyancyDrag::ISystemConfigure, + PolyhedraBuoyancyDrag::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(vrx::PolyhedraBuoyancyDrag, + "vrx::PolyhedraBuoyancyDrag") diff --git a/vrx_ign/src/PolyhedraBuoyancyDrag.hh b/vrx_ign/src/PolyhedraBuoyancyDrag.hh new file mode 100644 index 000000000..bf5a36060 --- /dev/null +++ b/vrx_ign/src/PolyhedraBuoyancyDrag.hh @@ -0,0 +1,135 @@ +/* + * Copyright (C) 2019 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 VRX_POLYHEDRABUOYANCYDRAG_HH_ +#define VRX_POLYHEDRABUOYANCYDRAG_HH_ + +#include +#include +#include +#include + +namespace vrx +{ + /// \brief This plugin simulates buoyancy of an object in fluid. + /// The code computes buoyancy and drag forces on rigid bodies. + /// The algorithm determines an exact buoyancy force for a polyhedron in a + /// water volume. + /// \ref Erin Catto. “Exact Buoyancy for Polyhedra”. + /// In Game Programming Gems 6, Charles River Media, 2006, pp. 175–187. + /// + /// ## Optional system parameters + /// : The wavefield parameters. See `Wavefield.hh`. + /// + /// : Sets the density of the fluid that surrounds the + /// buoyant object [kg/m^3]. Default value is 1000 kg/m^3). + /// + /// : The height of the fluid/air interface [m]. + /// The default value is 0 m. + /// + /// : Linear drag coefficent [N/(m/s)]. + /// Translational drag implement as linear function + /// of velocity. The default value is 0. + /// + /// : Angular drag coefficent [(Nm)/(rad/s)]. + /// Rotational drag implemented as linear function + /// of velocity. The default value is 0. + /// + /// : Describes the volume properties. It might contain the + /// following elements: + /// + /// : Required element containing the name of + /// the link used to apply forces. + /// : Required element modeling the buoyancy + /// of the object. + /// : Optional element containing the offset + /// from `link_name` where forces will be applied + /// + /// For example: + /// + /// + /// link + /// + /// + /// 5 + /// 0.17 + /// + /// + /// + /// + /// ## Example + /// + /// 1000 + /// 0.0 + /// 25.0 + /// 2.0 + /// + /// link + /// 0 0 0 0 0 0 + /// + /// + /// 4 + /// 0.2 + /// + /// + /// + /// + /// 1000 1000 + /// 50 50 + /// + /// PMS + /// 5.0 + /// 3 + /// 1.1 + /// 0.5 + /// 1 0 + /// 0.4 + /// 2.0 + /// 0.0 + /// 0.0 + /// + /// + /// + class PolyhedraBuoyancyDrag + : public ignition::gazebo::System, + public ignition::gazebo::ISystemConfigure, + public ignition::gazebo::ISystemPreUpdate + { + /// \brief Constructor. + public: PolyhedraBuoyancyDrag(); + + /// \brief Destructor. + public: ~PolyhedraBuoyancyDrag() override = default; + + // Documentation inherited. + public: void Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) override; + + // Documentation inherited. + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer. + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) + }; +} + +#endif diff --git a/vrx_ign/src/PolyhedronVolume.cc b/vrx_ign/src/PolyhedronVolume.cc new file mode 100644 index 000000000..73f45b34a --- /dev/null +++ b/vrx_ign/src/PolyhedronVolume.cc @@ -0,0 +1,297 @@ +/* + * Copyright (C) 2019 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 "PolyhedronVolume.hh" + +using namespace vrx; +using Face = Polyhedron::Face; + +////////////////////////////////////////////////////// +Plane::Plane() + : normal(0., 0., 1.), offset(0.) +{ +} + +////////////////////////////////////////////////////// +Polyhedron::Face::Face(int _i1, int _i2, int _i3) + : i1(_i1), i2(_i2), i3(_i3) +{ +} + +////////////////////////////////////////////////////// +Volume::Volume() + : volume(0.0), centroid(ignition::math::Vector3d({0, 0, 0})) +{ +} + +////////////////////////////////////////////////////// +Volume &Volume::operator+=(const Volume &_rhs) +{ + this->volume += _rhs.volume; + this->centroid += _rhs.centroid; + return *this; +} + +////////////////////////////////////////////////////// +Polyhedron Polyhedron::makeCube(double _x, double _y, double _z) +{ + Polyhedron cube; + // generate vertices + for (int i = 0; i < 2; ++i) + { + for (int j = 0; j < 2; ++j) + { + for (int k = 0; k < 2; ++k) + { + cube.vertices.emplace_back( + ignition::math::Vector3d(i * _x - _x / 2.0, + j * _y - _y / 2.0, k * _z - _z / 2.0)); + } + } + } + // generate faces + cube.faces.emplace_back(Face(0, 6, 4)); + cube.faces.emplace_back(Face(0, 2, 6)); + cube.faces.emplace_back(Face(0, 3, 2)); + cube.faces.emplace_back(Face(0, 1, 3)); + cube.faces.emplace_back(Face(2, 7, 6)); + cube.faces.emplace_back(Face(2, 3, 7)); + cube.faces.emplace_back(Face(4, 6, 7)); + cube.faces.emplace_back(Face(4, 7, 5)); + cube.faces.emplace_back(Face(0, 4, 5)); + cube.faces.emplace_back(Face(0, 5, 1)); + cube.faces.emplace_back(Face(1, 5, 7)); + cube.faces.emplace_back(Face(1, 7, 3)); + + return cube; +} + +////////////////////////////////////////////////////// +Polyhedron Polyhedron::makeCylinder(double _r, double _l, int _n) +{ + assert(_n > 4); + + Polyhedron cylinder; + + // Generate all vertices. + double angle_step = 2.0 * M_PI / _n; + double l_2 = _l / 2.0; + cylinder.vertices.resize(2 * _n + 2); + cylinder.vertices[0] = ignition::math::Vector3d{0, 0, -l_2}; + for (int i = 1; i <= _n; ++i) + { + double x = _r * ::sin(angle_step * (i - 1)); + double y = _r * ::cos(angle_step * (i - 1)); + // bottom plate + cylinder.vertices[i] = ignition::math::Vector3d{x, y, -l_2}; + // top plate + cylinder.vertices[i + _n] = ignition::math::Vector3d{x, y, l_2}; + } + cylinder.vertices[2 * _n + 1] = ignition::math::Vector3d{0, 0, l_2}; + + // generate all faces. + for (int i = 1; i <= _n; ++i) + { + // bottom plate. + cylinder.faces.emplace_back(Face(0, i, (i % _n) + 1)); + } + for (int i = 1; i <= _n; ++i) + { + // walls + cylinder.faces.emplace_back(Face(i + 1, i, _n + i)); + cylinder.faces.emplace_back(Face( + (i % _n) + _n, (i % _n) + _n + 1, (i % _n) + 1)); + } + for (int i = 1; i <= _n; ++i) + { + // top plate + cylinder.faces.emplace_back(Face(i + _n, 2 * _n + 1, (i % _n) + _n + 1)); + } + + assert(cylinder.vertices.size() == 2 * _n + 2); + assert(cylinder.faces.size() == 4 * _n); + + return cylinder; +} + +////////////////////////////////////////////////////// +Volume Polyhedron::ComputeFullVolume() const +{ + Volume output; + // Compute the contribution of each triangle face + for (const auto& face : faces) + { + ignition::math::Vector3d v1 = vertices[face.i1]; + ignition::math::Vector3d v2 = vertices[face.i2]; + ignition::math::Vector3d v3 = vertices[face.i3]; + output += tetrahedronVolume(v1, v2, v3); + } + return output; +} + +////////////////////////////////////////////////////// +Volume Polyhedron::SubmergedVolume(const ignition::math::Vector3d &_x, + const ignition::math::Quaterniond &_q, const Plane &_plane) const +{ + // Transform the plane into the polyhedron frame. + auto qt = _q.Inverse(); + auto normal = qt.RotateVector(_plane.normal); + double offset = _plane.offset - _plane.normal.Dot(_x); + + // Compute vertex heights relative to surface. + std::vector ds(vertices.size()); + int numSubmerged = 0; + int sampleVert = 0; + for (size_t i = 0; i < vertices.size(); ++i) + { + ds[i] = normal.Dot(vertices[i]) - offset; + if (ds[i] < -EPSILON) + { + ++numSubmerged; + sampleVert = i; + } + } + + // If no submerged vertices return. + if (numSubmerged == 0) + { + return Volume{}; + } + + // Find a point on the water surface. Project a submerged point to + // get improved accuracy. This point serves as the point of origin for + // computing all the tetrahedron volumes. Since this point is on the + // surface, all of the surface faces get zero volume tetrahedrons. + // This way the surface polygon does not need to be considered. + ignition::math::Vector3d p = vertices[sampleVert] - ds[sampleVert] * normal; + + // Compute the contribution of each triangle. + Volume output; + for (const auto &face : faces) + { + ignition::math::Vector3d v1 = vertices[face.i1]; + ignition::math::Vector3d v2 = vertices[face.i2]; + ignition::math::Vector3d v3 = vertices[face.i3]; + double d1 = ds[face.i1]; + double d2 = ds[face.i2]; + double d3 = ds[face.i3]; + + if (d1 * d2 < 0) + { + // v1-v2 crosses the plane + output += clipTriangle(v1, v2, v3, d1, d2, d3, p); + } + else if (d1 * d3 < 0) + { + // v1-v3 crosses the plane + output += clipTriangle(v3, v1, v2, d3, d1, d2, p); + } + else if (d2 * d3 < 0) + { + // v2-v3 crosses the plane + output += clipTriangle(v2, v3, v1, d2, d3, d1, p); + } + else if (d1 < 0 || d2 < 0 || d3 < 0) + { + // fully submerged + output += tetrahedronVolume(v1, v2, v3, p); + } + } + + // Small submerged slivers may have rounding error leading to + // a zero or negative volume. If so, then return a result of zero. + if (output.volume <= EPSILON) + { + return Volume{}; + } + + // Normalize the centroid by the total volume. + output.centroid *= 1.0 / output.volume; + // Transform the centroid into world coordinates. + output.centroid = _x + _q.RotateVector(output.centroid); + // If centroid is very small make it zero. + output.centroid.X() = ::fabs(output.centroid[0]) < EPSILON ? + 0 : output.centroid.X(); + output.centroid.Y() = ::fabs(output.centroid[1]) < EPSILON ? + 0 : output.centroid.Y(); + output.centroid.Z() = ::fabs(output.centroid[2]) < EPSILON ? + 0 : output.centroid.Z(); + return output; +} + +////////////////////////////////////////////////////// +Volume Polyhedron::tetrahedronVolume(const ignition::math::Vector3d &_v1, + const ignition::math::Vector3d &_v2, const ignition::math::Vector3d &_v3, + const ignition::math::Vector3d &_p) +{ + ignition::math::Vector3d a = _v2 - _v1; + ignition::math::Vector3d b = _v3 - _v1; + ignition::math::Vector3d r = _p - _v1; + + Volume output; + output.volume = (1 / 6.) * (b.Cross(a)).Dot(r); + output.centroid = 0.25 * output.volume * (_v1 + _v2 + _v3 + _p); + return output; +} + +////////////////////////////////////////////////////// +Volume Polyhedron::clipTriangle(const ignition::math::Vector3d &_v1, + const ignition::math::Vector3d &_v2, const ignition::math::Vector3d &_v3, + double _d1, double _d2, double _d3, const ignition::math::Vector3d &_p) +{ + assert(_d1 * _d2 < 0); + Volume output; + + // Calculate the intersection point from a to b. + ignition::math::Vector3d ab = _v1 + (_d1 / (_d1 - _d2)) * (_v2 - _v1); + if (_d1 < 0) + { + // b to c crosses the clipping plane. + if (_d3 < 0) + { + // Case B - a quadrilateral or two triangles + // Calculate intersection point from b to c. + ignition::math::Vector3d bc = _v2 + (_d2 / (_d2 - _d3)) * (_v3 - _v2); + output += tetrahedronVolume(ab, bc, _v1, _p); + output += tetrahedronVolume(bc, _v3, _v1, _p); + } + else + { + // Case A - a single triangle. + ignition::math::Vector3d ac = _v1 + (_d1 / (_d1 - _d3)) * (_v3 - _v1); + output += tetrahedronVolume(ab, ac, _v1, _p); + } + } + else + { + if (_d3 < 0) + { + // Case B. + ignition::math::Vector3d ac = _v1 + (_d1 / (_d1 - _d3)) * (_v3 - _v1); + output += tetrahedronVolume(ab, _v2, _v3, _p); + output += tetrahedronVolume(ab, _v3, ac, _p); + } + else + { + // Case A. + ignition::math::Vector3d bc = _v2 + (_d2 / (_d2 - _d3)) * (_v3 - _v2); + output += tetrahedronVolume(ab, _v2, bc, _p); + } + } + return output; +} diff --git a/vrx_ign/src/PolyhedronVolume.hh b/vrx_ign/src/PolyhedronVolume.hh new file mode 100644 index 000000000..d83a28473 --- /dev/null +++ b/vrx_ign/src/PolyhedronVolume.hh @@ -0,0 +1,158 @@ +/* + * Copyright (C) 2019 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 VRX_POLYHEDRONVOLUME_HH_ +#define VRX_POLYHEDRONVOLUME_HH_ + +#include +#include +#include + +namespace vrx +{ + /// \brief Represents a plane as a normal and offset. + struct Plane + { + /// \brief Initializes plane at z=0. + Plane(); + + /// \brief Vector3 normal to plane. + ignition::math::Vector3d normal; + + /// \brief Offset w.r.t. normal. + float offset; + }; + + /// \brief Represents output volume with centroid. + struct Volume + { + Volume(); + + /// \brief Overloads += for volume object. + Volume &operator+=(const Volume &_rhs); + + /// \brief Submerged volume of shape. + double volume; + + /// \brief Vector3 representing volume centroid. + ignition::math::Vector3d centroid; + }; + + /// \brief Submerged volume calculation using polyhedron + /// based on: Exact Buoyancy for Polyhedra by Eric Catto. + /// \ref Erin Catto. “Exact Buoyancy for Polyhedra”. + /// In Game Programming Gems 6, Charles River Media, 2006, pp. 175–187. + class Polyhedron + { + /// \brief Store vertex index for a triangular face. + public: struct Face + { + /// \brief Default constructor. + Face() = default; + + /// \brief Constructor. + /// \param[in] _i1 Index 1. + /// \param[in] _i2 Index 2. + /// \param[in] _i3 Index 3. + Face(int _i1, + int _i2, + int _i3); + + /// \brief Index of vertex. + public: int i1; + + /// \brief Index of vertex. + public: int i2; + + /// \brief Index of vertex. + public: int i3; + }; + + /// \brief Generate a cube polyhedron centered at origin. + /// \param[in] _x Length of cube. + /// \param[in] _y Width of cube. + /// \param[in] _z Height of cube. + /// \return Polyhedron object. + public: static Polyhedron makeCube(double _x, + double _y, + double _z); + + /// \brief Generate a cylinder polyhedron centered at origin. + /// \param[in] _r Radius of cylinder. + /// \param[in] _l Length of cylinder. + /// \param[in] _n Number of segments. + /// \return Polyhedron object. + public: static Polyhedron makeCylinder(double _r, + double _l, + int _n); + + /// \brief Compute full volume and center of buoyancy of the polyhedron. + /// \return Volume object with volume and centroid. + public: Volume ComputeFullVolume() const; + + /// \brief Compute submerged volume and center of buoyancy of a polyhedron. + /// \param[in] _x Our position. + /// \param[in] _q Our orientation (quaternions). + /// \param[in] _plane Water surface defined as a plane. + /// \return Volume object with volume and centroid (relative to world). + public: Volume SubmergedVolume(const ignition::math::Vector3d &_x, + const ignition::math::Quaterniond &_q, + const Plane &_plane) const; + + /// \brief Computes volume and centroid of tetrahedron. + /// tetrahedron formed by triangle + arbitrary point. + /// \param[in] _v1 Point on triangle. + /// \param[in] _v2 Point on triangle. + /// \param[in] _v3 Point on triangle. + /// \param[in] _p Arbitrary point. + /// \return Volume object with volume and centroid. + private: static Volume tetrahedronVolume( + const ignition::math::Vector3d &_v1, + const ignition::math::Vector3d &_v2, + const ignition::math::Vector3d &_v3, + const ignition::math::Vector3d &_p = + ignition::math::Vector3d::Zero); + + /// \brief Clips a partially submerged triangle. + /// \param[in] _v1 Point on triangle. + /// \param[in] _v2 Point on triangle. + /// \param[in] _v3 Point on triangle. + /// \param[in] _d1 Distance of point v1 to the splitting plane. + /// \param[in] _d2 Distance of point v2 to the splitting plane. + /// \param[in] _d3 Distance of point v3 to the splitting plane. + /// \return Volume object for clipped tetrahedron. + private: static Volume clipTriangle(const ignition::math::Vector3d &_v1, + const ignition::math::Vector3d &_v2, + const ignition::math::Vector3d &_v3, + double _d1, + double _d2, + double _d3, + const ignition::math::Vector3d &_p = + ignition::math::Vector3d::Zero); + + /// \brief Object vertices. + private: std::vector vertices; + + /// \brief Object faces. + private: std::vector faces; + + /// \brief Values below this are zeroed out. + private: const double EPSILON = 1e-6; + }; // class Polyhedron +} + +#endif diff --git a/vrx_ign/src/ShapeVolume.cc b/vrx_ign/src/ShapeVolume.cc new file mode 100644 index 000000000..b705d4253 --- /dev/null +++ b/vrx_ign/src/ShapeVolume.cc @@ -0,0 +1,232 @@ +/* + * Copyright (C) 2019 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 "ShapeVolume.hh" + +using namespace ignition; +using namespace vrx; + +///////////////////////////////////////////////// +ShapeVolumePtr ShapeVolume::makeShape(const sdf::ElementPtr _sdf) +{ + double epsilon = 1e-20; + + ShapeVolume* shape = nullptr; + + if (_sdf->HasElement("box")) + { + auto boxElem = _sdf->GetElement("box"); + if (boxElem->HasElement("size")) + { + math::Vector3d dim = boxElem->GetElement("size")->Get(); + if (dim[0] > epsilon && dim[1] > epsilon && dim[2] > epsilon) + { + shape = dynamic_cast( + new BoxVolume(dim[0], dim[1], dim[2])); + } + else + { + throw ParseException("box", "incorrect dimensions"); + } + } + else + { + throw ParseException("box", "missing element"); + } + } + else if (_sdf->HasElement("sphere")) + { + auto sphereElem = _sdf->GetElement("sphere"); + if (sphereElem->HasElement("radius")) + { + auto r = sphereElem->GetElement("radius")->Get(); + if (r > epsilon) + { + shape = dynamic_cast(new SphereVolume(r)); + } + else + { + throw ParseException("sphere", "incorrect dimensions"); + } + } + else + { + throw ParseException("sphere", "missing element"); + } + } + else if (_sdf->HasElement("cylinder")) + { + auto cylinderElem = _sdf->GetElement("cylinder"); + if (cylinderElem->HasElement("radius") && + cylinderElem->HasElement("length")) + { + auto r = cylinderElem->GetElement("radius")->Get(); + auto l = cylinderElem->GetElement("length")->Get(); + if (r > epsilon || l > epsilon) + { + shape = dynamic_cast(new CylinderVolume(r, l)); + } + else + { + throw ParseException("cylinder", "incorrect dimensions"); + } + } + else + { + throw ParseException("cylinder", "missing or element"); + } + } + else + { + throw ParseException( + "geometry", "missing , or element"); + } + + return std::unique_ptr(shape); +} + +///////////////////////////////////////////////// +std::string ShapeVolume::Display() const +{ + switch (this->type) + { + case ShapeType::None: + return "None"; + case ShapeType::Box: + return "Box"; + case ShapeType::Cylinder: + return "Cylinder"; + case ShapeType::Sphere: + return "Sphere"; + } + return "None"; +} + +////////////////////////////////////////////////// +BoxVolume::BoxVolume(double _x, double _y, double _z) + : x(_x), + y(_y), + z(_z), + polyhedron(Polyhedron::makeCube(_x, _y, _z)) +{ + this->type = ShapeType::Box; + this->volume = _x * _y * _z; + this->averageLength = (_x + _y + _z) / 3.0; +} + +////////////////////////////////////////////////// +std::string BoxVolume::Display() const +{ + std::stringstream ss; + ss << ShapeVolume::Display() << ":" + << this->x << "," << this->y << "," << this->z; + return ss.str(); +} + +////////////////////////////////////////////////// +Volume BoxVolume::CalculateVolume(const math::Pose3d &_pose, + double _fluidLevel) const +{ + Plane waterSurface; + waterSurface.offset = _fluidLevel; + return this->polyhedron.SubmergedVolume( + _pose.Pos(), _pose.Rot(), waterSurface); +} + +///////////////////////////////////////////////// +CylinderVolume::CylinderVolume(double _r, double _h) + : r(_r), + h(_h), + polyhedron(Polyhedron::makeCylinder(_r, _h, 20)) +{ + this->type = ShapeType::Cylinder; + this->volume = M_PI * _r * _r * _h; + this->averageLength = (2 * _r + _h) / 2.0; +} + +///////////////////////////////////////////////// +std::string CylinderVolume::Display() const +{ + std::stringstream ss; + ss << ShapeVolume::Display() << ":" << this->r << "," << this->h; + return ss.str(); +} + +///////////////////////////////////////////////// +Volume CylinderVolume::CalculateVolume(const math::Pose3d &_pose, + double _fluidLevel) const +{ + Plane waterSurface; + waterSurface.offset = _fluidLevel; + return this->polyhedron.SubmergedVolume( + _pose.Pos(), _pose.Rot(), waterSurface); +} + +////////////////////////////////////////////////// +SphereVolume::SphereVolume(double _r) + : r(_r) +{ + this->type = ShapeType::Sphere; + this->volume = 4./3. * M_PI * _r * _r * _r; + this->averageLength = 2 * _r; +} + +////////////////////////////////////////////////// +std::string SphereVolume::Display() const +{ + std::stringstream ss; + ss << ShapeVolume::Display() << ":" << this->r; + return ss.str(); +} + +////////////////////////////////////////////////// +Volume SphereVolume::CalculateVolume(const math::Pose3d &_pose, + double _fluidLevel) const +{ + Volume output{}; + // Location of bottom of object relative to the fluid surface - assumes + // origin is at cog of the object. + double h = _fluidLevel - (_pose.Pos().Z() - r); + + if (h <= 0) + return output; + + // limits of integration + double intLimitLower = -r; + double intLimitUpper = (-r + h) > r ? r : (-r + h); + + // volume = integral of (R^2 - z^2) dz * pi + output.volume = (pow(r, 2) * (intLimitUpper - intLimitLower) + - (pow(intLimitUpper, 3) / 3.0 - pow(intLimitLower, 3) / 3.0)) * M_PI; + output.centroid.X() = _pose.Pos().X(); + output.centroid.Y() = _pose.Pos().Y(); + + if (output.volume > 0) + { + // centroid is always centered to object in X and Y plane + output.centroid.X() = _pose.Pos().X(); + output.centroid.Y() = _pose.Pos().Y(); + // z_bar = (integral of (z(R)^2 - z^3) dz) * pi / volume + output.centroid.Z() = (pow(r, 2) / 2.0 * + (pow(intLimitUpper, 2) - pow(intLimitLower, 2)) - + (pow(intLimitUpper, 4) - pow(intLimitLower, 4))/ 4.0) + * M_PI / output.volume; + // convert centroid.z to global frame + output.centroid.Z() = _pose.Pos().Z() + output.centroid.Z(); + } + return output; +} diff --git a/vrx_ign/src/ShapeVolume.hh b/vrx_ign/src/ShapeVolume.hh new file mode 100644 index 000000000..5d6be2a69 --- /dev/null +++ b/vrx_ign/src/ShapeVolume.hh @@ -0,0 +1,173 @@ +/* + * Copyright (C) 2019 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 VRX_SHAPEVOLUME_HH_ +#define VRX_SHAPEVOLUME_HH_ + +#include +#include +#include +#include +#include +#include + +#include "PolyhedronVolume.hh" + +namespace vrx +{ + /// \brief Type of geometry shape. + enum class ShapeType + { + None, + Box, + Sphere, + Cylinder + }; + + /// \brief Parent shape object for volume objects. + struct ShapeVolume + { + /// \brief Default destructor. + virtual ~ShapeVolume() = default; + + /// \brief Factory method for shape. Parses a shape object from sdf data + /// \param[in] _sdf geometry SDF element + /// \return A pointer to a new ShapeVolume. + static std::unique_ptr makeShape(const sdf::ElementPtr _sdf); + + /// \brief Display string for shape object. + /// \return A string representation of the ShapeVolume. + virtual std::string Display() const; + + /// \brief Calculates volume + centroid of submerged shape. + /// If the shape is out of water returns Volume{}. + /// \param[in] _pose World pose of volume. + /// \param[in] _fluidLevel Height of fluid. Default is 0. + /// \return Volume object with volume + centroid (relative to world). + virtual Volume CalculateVolume(const ignition::math::Pose3d &_pose, + double _fluidLevel) const = 0; + + /// \brief Type of shape. + ShapeType type; + + /// \brief Full volume of object. + double volume; + + /// \brief Average length of object. + /// Estimate used for drag torque calculation. + double averageLength; + }; + + typedef std::unique_ptr ShapeVolumePtr; + + /// \brief Box shape volume. + struct BoxVolume : public ShapeVolume + { + /// \brief Default constructor. + /// \param[in] _x Length. + /// \param[in] _y Width. + /// \param[in] _z Height. + explicit BoxVolume(double _x, + double _y, + double _z); + + // Documentation inherited. + std::string Display() const override; + + // Documentation inherited. + Volume CalculateVolume(const ignition::math::Pose3d &_pose, + double _fluidLevel) const override; + + /// \brief Length. + double x; + + /// \brief Width. + double y; + + /// \brief Height. + double z; + + /// \brief Polyhedron defining a box. + private: Polyhedron polyhedron; + }; + + /// \brief Cylinder shape volume. + struct CylinderVolume : public ShapeVolume + { + /// \brief Default constructor + /// \param[in] _r Radius. + /// \param[in] _l Length. + explicit CylinderVolume(double _r, + double _l); + + // Documentation inherited. + std::string Display() const override; + + // Documentation inherited. + Volume CalculateVolume(const ignition::math::Pose3d &_pose, + double _fluidLevel) const override; + + /// \brief Radius of cylinder. + double r; + + /// \brief Height of cylinder. + double h; + + /// \brief Polyhedron defining a cylinder. + private: Polyhedron polyhedron; + }; + + /// \brief Sphere shape volume. + struct SphereVolume : public ShapeVolume + { + /// \brief Default constructor. + /// \param[in] _r Radius. + explicit SphereVolume(double _r); + + // Documentation inherited. + std::string Display() const override; + + // Documentation inherited. + Volume CalculateVolume(const ignition::math::Pose3d &_pose, + double _fluidLevel) const override; + + /// \brief Radius of sphere. + double r; + }; + + /// \brief Custom exception for parsing errors. + struct ParseException : public std::exception + { + ParseException(const char* shape, const char* message) + : output_("") + { + std::stringstream ss; + ss << "Parse error for <" << shape << ">: " << message; + // cppcheck-suppress useInitializationList + this->output_ = ss.str(); + } + + const char* what() const throw() + { + return this->output_.c_str(); + } + + private: std::string output_; + }; +} + +#endif diff --git a/vrx_ign/src/Surface.cc b/vrx_ign/src/Surface.cc index f1b035f7e..f06d54615 100644 --- a/vrx_ign/src/Surface.cc +++ b/vrx_ign/src/Surface.cc @@ -17,9 +17,9 @@ #include #include #include +#include #include #include -#include #include #include #include @@ -35,38 +35,24 @@ #include "Wavefield.hh" using namespace ignition; -using namespace gazebo; -using namespace systems; using namespace vrx; -class vrx::SurfacePrivate +/// \brief Private Surface data class. +class vrx::Surface::Implementation { - /// \brief Convenience function for calculating the area of circle segment. - /// \param[in] _r Radius of circle. - /// \param[in] _h Height of the chord line. - /// \return The area. - /// \ref https://www.mathopenref.com/segmentareaht.html - public: double CircleSegment(double _r, - double _h) const; + /// \brief Parse the points via SDF. + /// \param[in] _sdf Pointer to the SDF. + public: void ParsePoints(const std::shared_ptr &_sdf); - /// \brief The link entity - public: ignition::gazebo::Link link; - - /// \brief Model interface - public: Model model{kNullEntity}; + /// \brief The link entity. + public: gazebo::Link link; /// \brief Vessel length [m]. - public: double vehicleLength = 4.9; - - /// \brief Vessel width [m]. - public: double vehicleWidth = 2.4; + public: double hullLength = 4.9; /// \brief Demi-hull radius [m]. public: double hullRadius = 0.213; - /// \brief Length discretization, i.e., "N" - public: int numSamples = 2; - /// \brief Fluid height [m]. public: double fluidLevel = 0; @@ -74,34 +60,101 @@ class vrx::SurfacePrivate public: double fluidDensity = 997.7735; /// \brief The world's gravity [m/s^2]. - public: ignition::math::Vector3d gravity; + public: math::Vector3d gravity; + + /// \brief The points where the plugin applies forces. These points are + /// relative to the link paramter's origin. Note that we don't check that the + /// points are contained within the hull. You should pass reasonable points. + public: std::vector points; /// \brief The wavefield. public: Wavefield wavefield; }; ////////////////////////////////////////////////// -double SurfacePrivate::CircleSegment(double _r, double _h) const +void Surface::Implementation::ParsePoints( + const std::shared_ptr &_sdf) +{ + if (!_sdf->HasElement("points")) + return; + + auto ptr = const_cast(_sdf.get()); + auto sdfPoints = ptr->GetElement("points"); + + // We need at least one point. + if (!sdfPoints->HasElement("point")) + ignerr << "Unable to find element in SDF." << std::endl; + + auto pointElem = sdfPoints->GetElement("point"); + + // Parse a new point. + while (pointElem) + { + ignition::math::Vector3d point; + pointElem->GetValue()->Get(point); + this->points.push_back(point); + + // Parse the next point. + pointElem = pointElem->GetNextElement("point"); + } + + + + + + // if (!_sdf->HasElement("hull")) + // return; + + // auto ptr = const_cast(_sdf.get()); + + + // auto sdfHull = ptr->GetElement("hull"); + // while (sdfHull) + // { + // // We need the hull's name. + // if (!sdfHull->HasElement("name")) + // ignerr << "Unable to find element in SDF." << std::endl; + + // std::string name = sdfHull->Get("name"); + + // // We need at least one point. + // if (!sdfHull->HasElement("point")) + // ignerr << "Unable to find element in SDF." << std::endl; + + // auto pointElem = sdfHull->GetElement("point"); + // while (pointElem) + // { + // ignition::math::Vector3d point; + // pointElem->GetValue()->Get(point); + // this->hulls[name].push_back(point); + + // // Parse the next point. + // pointElem = pointElem->GetNextElement("point"); + // } + + // sdfHull = sdfHull->GetNextElement("hull"); + // } +} + +////////////////////////////////////////////////// +double Surface::CircleSegment(double _r, double _h) const { return _r * _r * acos((_r -_h) / _r ) - (_r - _h) * sqrt(2 * _r * _h - _h * _h); } - ////////////////////////////////////////////////// Surface::Surface() - : dataPtr(std::make_unique()) + : System(), dataPtr(utils::MakeUniqueImpl()) { } ////////////////////////////////////////////////// -void Surface::Configure(const Entity &_entity, +void Surface::Configure(const gazebo::Entity &_entity, const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &/*_eventMgr*/) + gazebo::EntityComponentManager &_ecm, + gazebo::EventManager &/*_eventMgr*/) { - this->dataPtr->model = Model(_entity); - // Parse required elements. if (!_sdf->HasElement("link_name")) { @@ -109,8 +162,9 @@ void Surface::Configure(const Entity &_entity, return; } + gazebo::Model model(_entity); std::string linkName = _sdf->Get("link_name"); - this->dataPtr->link = Link(this->dataPtr->model.LinkByName(_ecm, linkName)); + this->dataPtr->link = gazebo::Link(model.LinkByName(_ecm, linkName)); if (!this->dataPtr->link.Valid(_ecm)) { ignerr << "Could not find link named [" << linkName @@ -118,32 +172,18 @@ void Surface::Configure(const Entity &_entity, return; } - // Required parameters. - if (!_sdf->HasElement("vehicle_length")) - { - ignerr << "No specified" << std::endl; - return; - } - this->dataPtr->vehicleLength = _sdf->Get("vehicle_length"); - - if (!_sdf->HasElement("vehicle_width")) - { - ignerr << "No specified" << std::endl; - return; - } - this->dataPtr->vehicleWidth = _sdf->Get("vehicle_width"); - - if (!_sdf->HasElement("hull_radius")) + // Optional parameters. + // Although some of these parameters are required in this plugin, a potential + // derived plugin might not need them. Make sure that the default values are + // reasonable. + if (_sdf->HasElement("hull_length")) { - ignerr << "No specified" << std::endl; - return; + this->dataPtr->hullLength = _sdf->Get("hull_length"); } - this->dataPtr->hullRadius = _sdf->Get("hull_radius"); - // Optional parameters. - if (_sdf->HasElement("num_samples")) + if (_sdf->HasElement("hull_radius")) { - this->dataPtr->numSamples = _sdf->Get("num_samples"); + this->dataPtr->hullRadius = _sdf->Get("hull_radius"); } if (_sdf->HasElement("fluid_level")) @@ -156,6 +196,9 @@ void Surface::Configure(const Entity &_entity, this->dataPtr->fluidDensity = _sdf->Get("fluid_density"); } + // Parse the optional element. + this->dataPtr->ParsePoints(_sdf); + // Get the gravity from the world. auto worldEntity = gazebo::worldEntity(_ecm); auto world = gazebo::World(worldEntity); @@ -170,123 +213,130 @@ void Surface::Configure(const Entity &_entity, // Wavefield this->dataPtr->wavefield.Load(_sdf); - // Create necessary components if not present. - enableComponent(_ecm, this->dataPtr->link.Entity()); - enableComponent(_ecm, this->dataPtr->link.Entity()); - igndbg << "Surface plugin successfully configured with the following " << "parameters:" << std::endl; igndbg << " : " << linkName << std::endl; - igndbg << " : " << this->dataPtr->vehicleLength << std::endl; - igndbg << " : " << this->dataPtr->vehicleWidth << std::endl; + igndbg << " : " << this->dataPtr->hullLength << std::endl; igndbg << " : " << this->dataPtr->hullRadius << std::endl; - igndbg << " : " << this->dataPtr->numSamples << std::endl; igndbg << " : " << this->dataPtr->fluidLevel << std::endl; igndbg << " : " << this->dataPtr->fluidDensity << std::endl; + igndbg << " :" << std::endl; + for (const auto &p : this->dataPtr->points) + igndbg << " [" << p << "]" << std::endl; } ////////////////////////////////////////////////// -void Surface::PreUpdate(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) +void Surface::PreUpdate(const gazebo::UpdateInfo &_info, + gazebo::EntityComponentManager &_ecm) { IGN_PROFILE("Surface::PreUpdate"); if (_info.paused) return; - // Vehicle frame transform + // Vehicle frame transform. const auto kPose = this->dataPtr->link.WorldPose(_ecm); - // std::optional kPose; - // *kPose = {-532, 162, -0.008054, 0, 0, 1}; if (!kPose) { ignerr << "Unable to get world pose from link [" << this->dataPtr->link.Entity() << "]" << std::endl; return; } - const ignition::math::Vector3d kEuler = (*kPose).Rot().Euler(); - ignition::math::Quaternion vq(kEuler.X(), kEuler.Y(), kEuler.Z()); - - // Loop over boat grid points - // Grid point location in boat frame - might be able to precalculate these? - ignition::math::Vector3d bpnt(0, 0, 0); - // Grid point location in world frame - ignition::math::Vector3d bpntW(0, 0, 0); - // For each hull - // Debug output: - // igndbg << "===" << std::endl; - for (int i = 0; i < 2; ++i) + const math::Vector3d kEuler = (*kPose).Rot().Euler(); + math::Quaternion vq(kEuler.X(), kEuler.Y(), kEuler.Z()); + + for (auto const &bpnt : this->dataPtr->points) { - // Grid point in boat frame - bpnt.Set(bpnt.X(), (i * 2.0 - 1.0) * this->dataPtr->vehicleWidth / 2.0, - bpnt.Z()); - - // For each length segment - for (int j = 1; j <= this->dataPtr->numSamples; ++j) - { - bpnt.Set(((j - 0.5) / (static_cast(this->dataPtr->numSamples)) - - 0.5) * this->dataPtr->vehicleLength, bpnt.Y(), bpnt.Z()); - - // Transform from vessel to fluid/world frame. - bpntW = vq * bpnt; - - // Vertical location of boat grid point in world frame. - const float kDdz = (*kPose).Pos().Z() + bpntW.Z(); - - // Find vertical displacement of wave field - // World location of grid point - ignition::math::Vector3d point; - point.X() = (*kPose).Pos().X() + bpntW.X(); - point.Y() = (*kPose).Pos().Y() + bpntW.Y(); - - // Compute the depth at the grid point. - double simTime = std::chrono::duration(_info.simTime).count(); - double depth = - this->dataPtr->wavefield.ComputeDepthSimply(point, simTime); - - // Vertical wave displacement. - double dz = depth + point.Z(); - - // Total z location of boat grid point relative to fluid surface - double deltaZ = (this->dataPtr->fluidLevel + dz) - kDdz; - // enforce only upward buoy force - deltaZ = std::max(deltaZ, 0.0); - deltaZ = std::min(deltaZ, this->dataPtr->hullRadius); - - // Buoyancy force at grid point - const float kBuoyForce = - this->dataPtr->CircleSegment(this->dataPtr->hullRadius, deltaZ) * - this->dataPtr->vehicleLength / - (static_cast(this->dataPtr->numSamples)) * + // Transform from vessel to fluid/world frame. + const math::Vector3d kBpntW = vq * bpnt; + + // Vertical location of boat grid point in world frame. + const float kDdz = (*kPose).Pos().Z() + kBpntW.Z(); + + // World location of grid point. + math::Vector3d point; + point.X() = (*kPose).Pos().X() + kBpntW.X(); + point.Y() = (*kPose).Pos().Y() + kBpntW.Y(); + + // Compute the depth at the grid point. + double simTime = std::chrono::duration(_info.simTime).count(); + double depth = this->dataPtr->wavefield.ComputeDepthSimply(point, simTime); + + // Vertical wave displacement. + double dz = depth + point.Z(); + + // Total z location of boat grid point relative to fluid surface. + double deltaZ = (this->dataPtr->fluidLevel + dz) - kDdz; + // Enforce only upward buoy force + deltaZ = std::max(deltaZ, 0.0); + deltaZ = std::min(deltaZ, this->dataPtr->hullRadius); + + const float kBuoyForce = + this->CircleSegment(this->dataPtr->hullRadius, deltaZ) * + this->dataPtr->hullLength / this->dataPtr->points.size() * -this->dataPtr->gravity.Z() * this->dataPtr->fluidDensity; - // Apply force at grid point - // Position is in the link frame and force is in world frame. - this->dataPtr->link.AddWorldForce(_ecm, - ignition::math::Vector3d(0, 0, kBuoyForce), - bpnt); - - // Debug output: - // igndbg << bpnt.X() << "," << bpnt.Y() << "," << bpnt.Z() << std::endl; - // igndbg << "-" << std::endl; - // igndbg << bpntW.X() << "," << bpntW.Y() << "," << bpntW.Z() << std::endl; - // igndbg << "X: " << X << std::endl; - // igndbg << "depth: " << depth << std::endl; - // igndbg << "dz: " << dz << std::endl; - // igndbg << "kDdz: " << kDdz << std::endl; - // igndbg << "deltaZ: " << deltaZ << std::endl; - // igndbg << "hull radius: " << this->dataPtr->hullRadius << std::endl; - // igndbg << "vehicle length: " << this->dataPtr->vehicleLength << std::endl; - // igndbg << "num samples: " << this->dataPtr->numSamples << std::endl; - // igndbg << "gravity z: " << -this->dataPtr->gravity.Z() << std::endl; - // igndbg << "fluid density: " << this->dataPtr->fluidDensity << std::endl; - // igndbg << "Force: " << kBuoyForce << std::endl << std::endl; - } + // Apply force at the point. + // Position is in the link frame and force is in world frame. + this->dataPtr->link.AddWorldForce(_ecm, + math::Vector3d(0, 0, kBuoyForce), + bpnt); + + // Debug output: + // igndbg << bpnt.X() << "," << bpnt.Y() << "," << bpnt.Z() << std::endl; + // igndbg << "-" << std::endl; + // igndbg << bpntW.X() << "," << bpntW.Y() << "," << bpntW.Z() << std::endl; + // igndbg << "X: " << X << std::endl; + // igndbg << "depth: " << depth << std::endl; + // igndbg << "dz: " << dz << std::endl; + // igndbg << "kDdz: " << kDdz << std::endl; + // igndbg << "deltaZ: " << deltaZ << std::endl; + // igndbg << "hull radius: " << this->dataPtr->hullRadius << std::endl; + // igndbg << "vehicle length: " << this->dataPtr->hullLength << std::endl; + // igndbg << "gravity z: " << -this->dataPtr->gravity.Z() << std::endl; + // igndbg << "fluid density: " << this->dataPtr->fluidDensity << std::endl; + // igndbg << "Force: " << kBuoyForce << std::endl << std::endl; } } -IGNITION_ADD_PLUGIN(vrx::Surface, - ignition::gazebo::System, +////////////////////////////////////////////////// +math::Vector3d Surface::Gravity() const +{ + return this->dataPtr->gravity; +} + +////////////////////////////////////////////////// +double Surface::HullLength() const +{ + return this->dataPtr->hullLength; +} + +////////////////////////////////////////////////// +double Surface::HullRadius() const +{ + return this->dataPtr->hullRadius; +} + +////////////////////////////////////////////////// +double Surface::FluidDensity() const +{ + return this->dataPtr->fluidDensity; +} + +////////////////////////////////////////////////// +// double Surface::BuoyancyAtPoint(const gazebo::UpdateInfo &/*_info*/, +// const math::Vector3d &/*_point*/, const uint16_t _pointsPerHull, +// double _deltaZ, +// gazebo::EntityComponentManager &/*_ecm*/) +// { +// // Buoyancy force at this point. +// return this->CircleSegment(this->dataPtr->hullRadius, _deltaZ) * +// this->dataPtr->hullLength / _pointsPerHull * +// -this->dataPtr->gravity.Z() * this->dataPtr->fluidDensity; +// } + +IGNITION_ADD_PLUGIN(Surface, + gazebo::System, Surface::ISystemConfigure, Surface::ISystemPreUpdate) diff --git a/vrx_ign/src/Surface.hh b/vrx_ign/src/Surface.hh index 1f4163794..13f450f92 100644 --- a/vrx_ign/src/Surface.hh +++ b/vrx_ign/src/Surface.hh @@ -14,34 +14,45 @@ * limitations under the License. * */ + #ifndef VRX_SURFACE_HH_ #define VRX_SURFACE_HH_ -#include #include +#include +#include #include namespace vrx { - // Forward declaration - class SurfacePrivate; - /// \brief A system that simulates the buoyancy of an object at the surface of /// a fluid. This system must be attached to a model and the system will apply - /// buoyancy to a few sample points around a given link. + /// buoyancy to a collection of points around a given link. + /// + /// This system models the vehicle's buoyancy assuming a single hull with a + /// cylindrical shape. It's possible to derive from this plugin and provide + /// your own buoyancy function at each point. For this purpose you + /// should override `BuoyancyAtPoint()` in the derived plugin. + /// + /// This plugin also supports waves. If you provide a wavefield via SDF, the + /// plugin will account for the delta Z that the waves generate at each point. /// /// ## Required system parameters /// /// * `` is the name of the link used to apply forces. - /// * `` is the length of the vessel [m]. - /// * `` is the width of the vessel [m]. - /// * `` is the radius of the vessel's hull [m]. /// /// ## Optional system parameters /// - /// * `` is the number of samples where forces will be applied. + /// * `` is the length of the vessel [m]. + /// * `` is the radius of the vessel's hull [m]. /// * `` is the depth at which the fluid should be in the vehicle /// * `` is the density of the fluid. + /// * `` contains a collection of points where the forces generated + /// by this plugin will be applied. See the format of each point + /// next: + /// * `` Relative position of the point relative to + /// `link_name`. + /// * : The wavefield parameters. See `Wavefield.hh`. /// /// ## Example /// 4.9 /// 2.4 /// 0.213 + /// 0 + + /// + /// + /// + /// 1.225 1.2 0 + /// + /// + /// 1.225 -1.2 0 + /// + /// + /// -1.225 1.2 0 + /// + /// + /// -1.225 -1.2 0 + /// + /// + + /// + /// + /// <%= $wavefield_size%> <%= $wavefield_size%> + /// <%= $wavefield_cell_count%> <%=$wavefield_cell_count%> + /// + /// PMS + /// 5.0 + /// 3 + /// 1.1 + /// 0.5 + /// 1 0 + /// 0.4 + /// 2.0 + /// 0.0 + /// 0.0 + /// + /// /// class Surface : public ignition::gazebo::System, @@ -74,8 +120,45 @@ namespace vrx const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) override; + /// \brief Get the gravity component. + /// \return Gravity vector. + public: ignition::math::Vector3d Gravity() const; + + /// \brief Get the vehicle length. + /// \return Vechicle length in m. + public: double HullLength() const; + + /// \brief Get the hull radius. + /// \return The hull radius in m. + public: double HullRadius() const; + + /// \brief Get the fluid density. + /// \return The fluid density in kg/m^3. + public: double FluidDensity() const; + + /// \brief Compute the buoyancy generated at a point in the vehicle. + /// \param[in] _info Simulator information about the current timestep. + /// \param[in] _point The point. + /// \param[in] _deltaZ Total Z location of the point relative to fluid + /// surface. + /// \param[in] _ecm Ignition's ECM. + // public: virtual double BuoyancyAtPoint( + // const ignition::gazebo::UpdateInfo &_info, + // const ignition::math::Vector3d &_point, + // const uint16_t _pointsPerHull, + // double _deltaZ, + // ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Convenience function for calculating the area of circle segment. + /// \param[in] _r Radius of circle. + /// \param[in] _h Height of the chord line. + /// \return The area. + /// \ref https://www.mathopenref.com/segmentareaht.html + public: double CircleSegment(double _r, + double _h) const; + /// \brief Private data pointer. - private: std::unique_ptr dataPtr; + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; } diff --git a/vrx_ign/src/Wavefield.cc b/vrx_ign/src/Wavefield.cc index 5b67adf30..4356a792b 100644 --- a/vrx_ign/src/Wavefield.cc +++ b/vrx_ign/src/Wavefield.cc @@ -130,6 +130,10 @@ class vrx::WavefieldPrivate /// \brief The component wave dirctions (derived). public: std::vector directions; + /// \brief True when waves are present. + public: bool active = false; + + /// \brief Recalculate for constant wavelength-amplitude ratio public: void RecalculateCwr() { @@ -337,6 +341,14 @@ void Wavefield::Load(const std::shared_ptr &_sdf) this->data->Recalculate(); } this->DebugPrint(); + + this->data->active = true; +} + +/////////////////////////////////////////////////////////////////////////////// +bool Wavefield::Active() const +{ + return this->data->active; } /////////////////////////////////////////////////////////////////////////////// diff --git a/vrx_ign/src/Wavefield.hh b/vrx_ign/src/Wavefield.hh index 9e7e41330..420c99512 100644 --- a/vrx_ign/src/Wavefield.hh +++ b/vrx_ign/src/Wavefield.hh @@ -111,6 +111,10 @@ namespace vrx /// \param[in] _sdf The SDF Element tree containing the wavefield parameters public: void Load(const std::shared_ptr &_sdf); + /// \brief Is the wavefield loaded. + /// \return True when the wavefield has been loaded or false otherwise. + public: bool Active() const; + /// \brief The number of wave components (3 max if visualisation required). public: size_t Number() const; diff --git a/vrx_ign/worlds/sydney_regatta.sdf b/vrx_ign/worlds/sydney_regatta.sdf index 66fcf1b62..4f83787e6 100644 --- a/vrx_ign/worlds/sydney_regatta.sdf +++ b/vrx_ign/worlds/sydney_regatta.sdf @@ -349,6 +349,46 @@ coast_waves + + mb_marker_buoy_red + -528 190 0 0 1.57 0 + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/mb_marker_buoy_red + + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 -0.3 0 0 0 + + + 0.325 + 0.1 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + post_0