diff --git a/src/systems/hydrodynamics/Hydrodynamics.cc b/src/systems/hydrodynamics/Hydrodynamics.cc index 61f7dc115a..767a7e036d 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.cc +++ b/src/systems/hydrodynamics/Hydrodynamics.cc @@ -20,6 +20,8 @@ #include +#include "ignition/msgs/vector3d.pb.h" + #include "ignition/gazebo/components/AngularVelocity.hh" #include "ignition/gazebo/components/LinearVelocity.hh" #include "ignition/gazebo/components/Pose.hh" @@ -29,6 +31,8 @@ #include "ignition/gazebo/System.hh" #include "ignition/gazebo/Util.hh" +#include "ignition/transport/Node.hh" + #include "Hydrodynamics.hh" using namespace ignition; @@ -96,17 +100,36 @@ class ignition::gazebo::systems::HydrodynamicsPrivateData /// \brief Water density [kg/m^3]. public: double waterDensity; + /// \brief The ignition transport node + public: transport::Node node; + + /// \brief Ocean current experienced by this body + public: math::Vector3d currentVector {0, 0, 0}; + /// \brief Added mass of vehicle; /// See: https://en.wikipedia.org/wiki/Added_mass - Eigen::MatrixXd Ma; + public: Eigen::MatrixXd Ma; /// \brief Previous state. public: Eigen::VectorXd prevState; - /// Link entity - public: ignition::gazebo::Entity linkEntity; + /// \brief Link entity + public: Entity linkEntity; + + /// \brief Ocean current callback + public: void UpdateCurrent(const msgs::Vector3d &_msg); + + /// \brief Mutex + public: std::mutex mtx; }; +///////////////////////////////////////////////// +void HydrodynamicsPrivateData::UpdateCurrent(const msgs::Vector3d &_msg) +{ + std::lock_guard lock(this->mtx); + this->currentVector = ignition::msgs::Convert(_msg); +} + ///////////////////////////////////////////////// void AddAngularVelocityComponent( const ignition::gazebo::Entity &_entity, @@ -180,7 +203,16 @@ void Hydrodynamics::Configure( ignition::gazebo::EventManager &/*_eventMgr*/ ) { - this->dataPtr->waterDensity = SdfParamDouble(_sdf, "waterDensity", 998); + if (_sdf->HasElement("waterDensity")) + { + ignwarn << + " parameter is deprecated and will be removed Ignition G.\n" + << "\tPlease update your SDF to use instead."; + } + + this->dataPtr->waterDensity = SdfParamDouble(_sdf, "waterDensity", + SdfParamDouble(_sdf, "water_density", 998) + ); this->dataPtr->paramXdotU = SdfParamDouble(_sdf, "xDotU" , 5); this->dataPtr->paramYdotV = SdfParamDouble(_sdf, "yDotV" , 5); this->dataPtr->paramZdotW = SdfParamDouble(_sdf, "zDotW" , 0.1); @@ -202,7 +234,22 @@ void Hydrodynamics::Configure( // Create model object, to access convenient functions auto model = ignition::gazebo::Model(_entity); - if(!_sdf->HasElement("link_name")) + + std::string ns {""}; + std::string currentTopic {"/ocean_current"}; + if (_sdf->HasElement("namespace")) + { + ns = _sdf->Get("namespace"); + currentTopic = ignition::transport::TopicUtils::AsValidTopic( + "/model/" + ns + "/ocean_current"); + } + + this->dataPtr->node.Subscribe( + currentTopic, + &HydrodynamicsPrivateData::UpdateCurrent, + this->dataPtr.get()); + + if (!_sdf->HasElement("link_name")) { ignerr << "You musk specify a for the hydrodynamic" << " plugin to act upon"; @@ -210,12 +257,17 @@ void Hydrodynamics::Configure( } auto linkName = _sdf->Get("link_name"); this->dataPtr->linkEntity = model.LinkByName(_ecm, linkName); - if(!_ecm.HasEntity(this->dataPtr->linkEntity)) + if (!_ecm.HasEntity(this->dataPtr->linkEntity)) { ignerr << "Link name" << linkName << "does not exist"; return; } + if(_sdf->HasElement("default_current")) + { + this->dataPtr->currentVector = _sdf->Get("default_current"); + } + this->dataPtr->prevState = Eigen::VectorXd::Zero(6); AddWorldPose(this->dataPtr->linkEntity, _ecm); @@ -266,11 +318,18 @@ void Hydrodynamics::PreUpdate( return; } + // Get current vector + math::Vector3d currentVector; + { + std::lock_guard lock(this->dataPtr->mtx); + currentVector = this->dataPtr->currentVector; + } // Transform state to local frame auto pose = baseLink.WorldPose(_ecm); // Since we are transforming angular and linear velocity we only care about // rotation - auto localLinearVelocity = pose->Rot().Inverse() * linearVelocity->Data(); + auto localLinearVelocity = pose->Rot().Inverse() * + (linearVelocity->Data() - currentVector); auto localRotationalVelocity = pose->Rot().Inverse() * *rotationalVelocity; state(0) = localLinearVelocity.X(); diff --git a/src/systems/hydrodynamics/Hydrodynamics.hh b/src/systems/hydrodynamics/Hydrodynamics.hh index 8a313282cc..a06b37be7c 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.hh +++ b/src/systems/hydrodynamics/Hydrodynamics.hh @@ -31,47 +31,58 @@ namespace systems class HydrodynamicsPrivateData; /// \brief This class provides hydrodynamic behaviour for underwater vehicles - /// It is shamelessly based off Brian Bingham's plugin for VRX + /// It is shamelessly based off Brian Bingham's + /// [plugin for VRX](https://github.com/osrf/vrx). /// which in turn is based of Fossen's equations described in "Guidance and - /// Control of Ocean Vehicles". The class should be used together with the + /// Control of Ocean Vehicles" [1]. The class should be used together with the /// buoyancy plugin to help simulate behaviour of maritime vehicles. /// Hydrodynamics refers to the behaviour of bodies in water. It includes /// forces like linear and quadratic drag, buoyancy (not provided by this /// plugin), etc. /// - /// ## System Parameters + /// # System Parameters /// The exact description of these parameters can be found on p. 37 of /// Fossen's book. They are used to calculate added mass, linear and quadratic /// drag and coriolis force. - /// - Added mass in x direction [kg] - /// - Added mass in y direction [kg] - /// - Added mass in z direction [kg] - /// - Added mass in roll direction [kgm^2] - /// - Added mass in pitch direction [kgm^2] - /// - Added mass in yaw direction [kgm^2] - /// - Stability derivative, 2nd order, x component [kg/m] - /// - Stability derivative, 1st order, x component [kg] - /// - Stability derivative, 2nd order, y component [kg/m] - /// - Stability derivative, 1st order, y component [kg] - /// - Stability derivative, 2nd order, z component [kg/m] - /// - Stability derivative, 1st order, z component [kg] - /// - Stability derivative, 2nd order, roll component [kg/m^2] - /// - Stability derivative, 1st order, roll component [kg/m] - /// - Stability derivative, 2nd order, pitch component [kg/m^2] - /// - Stability derivative, 1st order, pitch component [kg/m] - /// - Stability derivative, 2nd order, yaw component [kg/m^2] - /// - Stability derivative, 1st order, yaw component [kg/m] + /// * - Added mass in x direction [kg] + /// * - Added mass in y direction [kg] + /// * - Added mass in z direction [kg] + /// * - Added mass in roll direction [kgm^2] + /// * - Added mass in pitch direction [kgm^2] + /// * - Added mass in yaw direction [kgm^2] + /// * - Stability derivative, 2nd order, x component [kg/m] + /// * - Stability derivative, 1st order, x component [kg] + /// * - Stability derivative, 2nd order, y component [kg/m] + /// * - Stability derivative, 1st order, y component [kg] + /// * - Stability derivative, 2nd order, z component [kg/m] + /// * - Stability derivative, 1st order, z component [kg] + /// * - Stability derivative, 2nd order, roll component [kg/m^2] + /// * - Stability derivative, 1st order, roll component [kg/m] + /// * - Stability derivative, 2nd order, pitch component [kg/m^2] + /// * - Stability derivative, 1st order, pitch component [kg/m] + /// * - Stability derivative, 2nd order, yaw component [kg/m^2] + /// * - Stability derivative, 1st order, yaw component [kg/m] /// Additionally the system also supports the following parameters: - /// - The density of the fluid its moving in. + /// * - The density of the fluid its moving in. + /// Defaults to 998kgm^-3. [kgm^-3, deprecated] + /// * - The density of the fluid its moving in. /// Defaults to 998kgm^-3. [kgm^-3] - /// - The link of the model that is being subject to + /// * - The link of the model that is being subject to /// hydrodynamic forces. [Required] + /// * - This allows the robot to have an individual namespace + /// for current. This is useful when you have multiple vehicles in + /// different locations and you wish to set the currents of each vehicle + /// separately. If no namespace is given then the plugin listens on + /// the `/ocean_current` topic for a `Vector3d` message. Otherwise it + /// listens on `/model/{namespace name}/ocean_current`.[String, Optional] + /// * - A generic current. + /// [vector3d m/s, optional, default = [0,0,0]m/s] /// /// # Example /// An example configuration is provided in the examples folder. The example /// uses the LiftDrag plugin to apply steering controls. It also uses the /// thruster plugin to propel the craft and the buoyancy plugin for buoyant - /// force. To run th example run. + /// force. To run the example run. /// ``` /// ign gazebo auv_controls.sdf /// ``` @@ -86,6 +97,19 @@ namespace systems /// -m ignition.msgs.Double -p 'data: -31' /// ``` /// The vehicle should move in a circle. + /// + /// ## Ocean Currents + /// When underwater, vehicles are often subject to ocean currents. The + /// hydrodynamics plugin allows simulation of such currents. We can add + /// a current simply by publishing the following: + /// ``` + /// ign topic -t /ocean_current -m ignition.msgs.Vector3d -p 'x: 1, y:0, z:0' + /// ``` + /// You should observe your vehicle slowly drift to the side. + /// + /// # Citations + /// [1] Fossen, Thor I. _Guidance and Control of Ocean Vehicles_. + /// United Kingdom: Wiley, 1994. class Hydrodynamics: public ignition::gazebo::System, public ignition::gazebo::ISystemConfigure, diff --git a/tutorials.md.in b/tutorials.md.in index b02a44fbe7..934f01e3b3 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -29,6 +29,7 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage videorecorder "Video Recorder": Record videos from the 3D render window. * \subpage collada_world_exporter "Collada World Exporter": Export an entire world to a single Collada mesh. +* \subpage underwater_vehicles "Underwater Vehicles": Understand how to simulate underwater vehicles. **Migration from Gazebo classic** diff --git a/tutorials/underwater_vehicles.md b/tutorials/underwater_vehicles.md index 341c6c9017..37a5fe0eba 100644 --- a/tutorials/underwater_vehicles.md +++ b/tutorials/underwater_vehicles.md @@ -1,3 +1,4 @@ +\page underwater_vehicles Underwater vehicles # Simulating Autnomous Underwater Vehicles Ignition now supports basic simulation of underwater vehicles. @@ -14,7 +15,7 @@ The behaviour of a moving body through water is different from the behaviour of a ground based vehicle. In particular bodies moving underwater experience much more forces derived from drag, buoyancy and lift. The way these forces act on a body can be seen in the following diagram: -![force diagram](https://raw.githubusercontent.com/ignitionrobotics/ign-gazebo/main/tutorials/files/underwater/MBARI%20forces.png) +![force diagram](https://raw.githubusercontent.com/ignitionrobotics/ign-gazebo/ign-gazebo5/tutorials/files/underwater/MBARI%20forces.png) # Setting up the buoyancy plugin The buoyancy plugin in ignition uses the collision mesh to calculate the volume @@ -177,3 +178,13 @@ ign topic -t /model/tethys/joint/propeller_joint/cmd_pos \ -m ignition.msgs.Double -p 'data: -31' ``` The vehicle should move in a circle. + +# Ocean Currents + +When underwater, vehicles are often subject to ocean currents. The hydrodynamics +plugin allows simulation of such currents. We can add a current simply by +publishing the following: +``` +ign topic -t /ocean_current -m ignition.msgs.Vector3d -p 'x: 1, y:0, z:0' +``` +You should observe your vehicle slowly drift to the side.