Minor tweaks

Signed-off-by: Carlos Agüero <caguero@openrobotics.org>

examples/worlds/auv_controls.sdf

@@ -2,10 +2,8 @@
 <sdf version="1.6">
   <world name="buoyant_tethys">
     <scene>
-      <!-- For turquoise ambient to match particle effect -->
+      <!-- For turquoise ambient -->
       <ambient>0.0 1.0 1.0</ambient>
-      <!-- For default gray ambient -->
-      <!--background>0.8 0.8 0.8</background-->
       <background>0.0 0.7 0.8</background>
     </scene>
 
@@ -51,7 +49,6 @@
     </light>
 
     <include>
-      <!-- Uncomment if you use the ground plane for debugging -->
       <pose>0 0 1 0 0 1.57</pose>
       <uri>https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI%20Tethys%20LRAUV</uri>
 
@@ -118,7 +115,6 @@
         <cp>0 0 0</cp>
       </plugin>
 
-
       <!-- hydrodynamics plugin-->
       <plugin
         filename="ignition-gazebo-hydrodynamics-system"

src/systems/hydrodynamics/Hydrodynamics.cc

@@ -14,7 +14,6 @@
  * limitations under the License.
  *
  */
-#include <mutex>
 #include <string>
 
 #include <Eigen/Eigen>
@@ -97,6 +96,7 @@ class ignition::gazebo::systems::HydrodynamicsPrivateData
   /// \brief Water density [kg/m^3].
   public: double waterDensity;
 
+  /// \brief Previous state.
   public: Eigen::VectorXd prevState;
 
   /// Link entity
@@ -108,13 +108,13 @@ void AddAngularVelocityComponent(
   const ignition::gazebo::Entity &_entity,
   ignition::gazebo::EntityComponentManager &_ecm)
 {
-  if (!_ecm.Component<ignition::gazebo::components::AngularVelocity>(
-      _entity))
+  if (!_ecm.Component<ignition::gazebo::components::AngularVelocity>(_entity))
   {
     _ecm.CreateComponent(_entity,
       ignition::gazebo::components::AngularVelocity());
   }
-    // Create an angular velocity component if one is not present.
+
+  // Create an angular velocity component if one is not present.
   if (!_ecm.Component<ignition::gazebo::components::WorldAngularVelocity>(
       _entity))
   {
@@ -128,11 +128,9 @@ void AddWorldPose(
   const ignition::gazebo::Entity &_entity,
   ignition::gazebo::EntityComponentManager &_ecm)
 {
-  if (!_ecm.Component<ignition::gazebo::components::WorldPose>(
-      _entity))
+  if (!_ecm.Component<ignition::gazebo::components::WorldPose>(_entity))
   {
-    _ecm.CreateComponent(_entity,
-      ignition::gazebo::components::WorldPose());
+    _ecm.CreateComponent(_entity, ignition::gazebo::components::WorldPose());
   }
 }
 
@@ -204,8 +202,8 @@ void Hydrodynamics::Configure(
 
   // Create model object, to access convenient functions
   auto model = ignition::gazebo::Model(_entity);
-  auto link_name = _sdf->Get<std::string>("link_name");
-  this->dataPtr->linkEntity = model.LinkByName(_ecm, link_name);
+  auto linkName = _sdf->Get<std::string>("link_name");
+  this->dataPtr->linkEntity = model.LinkByName(_ecm, linkName);
 
   this->dataPtr->prevState = Eigen::VectorXd::Zero(6);
 
@@ -233,7 +231,7 @@ void Hydrodynamics::PreUpdate(
   Eigen::VectorXd state    = Eigen::VectorXd(6);
   Eigen::MatrixXd Cmat     = Eigen::MatrixXd::Zero(6, 6);
   Eigen::MatrixXd Dmat     = Eigen::MatrixXd::Zero(6, 6);
-  Eigen::MatrixXd Ma = Eigen::MatrixXd::Zero(6, 6);
+  Eigen::MatrixXd Ma       = Eigen::MatrixXd::Zero(6, 6);
 
   // Get vehicle state
   ignition::gazebo::Link baseLink(this->dataPtr->linkEntity);
@@ -243,7 +241,7 @@ void Hydrodynamics::PreUpdate(
 
   if (!linearVelocity)
   {
-    ignerr <<"no linear vel" <<"\n";
+    ignerr << "no linear vel" <<"\n";
     return;
   }
 
@@ -276,27 +274,27 @@ void Hydrodynamics::PreUpdate(
   Ma(5, 5) = this->dataPtr->paramNdotR;
   const Eigen::VectorXd kAmassVec = Ma * stateDot;
 
-  // Coriollis and Centripetal forces for under water vehicles (Fossen P. 37)
+  // Coriolis and Centripetal forces for under water vehicles (Fossen P. 37)
   // Note: this is significantly different from VRX because we need to account
   // for the under water vehicle's additional DOF
   Cmat(0, 4) = - this->dataPtr->paramZdotW * state(2);
   Cmat(0, 5) = - this->dataPtr->paramYdotV * state(1);
-  Cmat(1, 3) = this->dataPtr->paramZdotW * state(2);
+  Cmat(1, 3) =   this->dataPtr->paramZdotW * state(2);
   Cmat(1, 5) = - this->dataPtr->paramXdotU * state(0);
   Cmat(2, 3) = - this->dataPtr->paramYdotV * state(1);
-  Cmat(2, 4) = this->dataPtr->paramXdotU * state(0);
+  Cmat(2, 4) =   this->dataPtr->paramXdotU * state(0);
   Cmat(3, 1) = - this->dataPtr->paramZdotW * state(2);
-  Cmat(3, 2) = this->dataPtr->paramYdotV * state(1);
+  Cmat(3, 2) =   this->dataPtr->paramYdotV * state(1);
   Cmat(3, 4) = - this->dataPtr->paramNdotR * state(5);
-  Cmat(3, 5) = this->dataPtr->paramMdotQ * state(4);
-  Cmat(4, 0) = this->dataPtr->paramZdotW * state(2);
+  Cmat(3, 5) =   this->dataPtr->paramMdotQ * state(4);
+  Cmat(4, 0) =   this->dataPtr->paramZdotW * state(2);
   Cmat(4, 2) = - this->dataPtr->paramXdotU * state(0);
-  Cmat(4, 3) = this->dataPtr->paramNdotR * state(5);
+  Cmat(4, 3) =   this->dataPtr->paramNdotR * state(5);
   Cmat(4, 5) = - this->dataPtr->paramKdotP * state(3);
-  Cmat(5, 0) = this->dataPtr->paramZdotW * state(2);
-  Cmat(5, 1) = this->dataPtr->paramXdotU * state(0);
+  Cmat(5, 0) =   this->dataPtr->paramZdotW * state(2);
+  Cmat(5, 1) =   this->dataPtr->paramXdotU * state(0);
   Cmat(5, 3) = - this->dataPtr->paramMdotQ * state(4);
-  Cmat(5, 4) = this->dataPtr->paramKdotP * state(3);
+  Cmat(5, 4) =   this->dataPtr->paramKdotP * state(3);
   const Eigen::VectorXd kCmatVec = - Cmat * state;
 
   // Damping forces (Fossen P. 43)
@@ -318,9 +316,9 @@ void Hydrodynamics::PreUpdate(
   const Eigen::VectorXd kTotalWrench = kAmassVec + kDvec + kCmatVec;
 
   ignition::math::Vector3d
-    totalForce(-kTotalWrench(0),  -kTotalWrench(1), -kTotalWrench(2));
+    totalForce(-kTotalWrench(0), -kTotalWrench(1), -kTotalWrench(2));
   ignition::math::Vector3d
-    totalTorque(-kTotalWrench(3),  -kTotalWrench(4), -kTotalWrench(5));
+    totalTorque(-kTotalWrench(3), -kTotalWrench(4), -kTotalWrench(5));
 
   baseLink.AddWorldWrench(
     _ecm,

src/systems/hydrodynamics/Hydrodynamics.hh

@@ -49,18 +49,18 @@ namespace systems
   ///   <kDotP> - Added mass in roll direction
   ///   <mDotQ> - Added mass in pitch direction
   ///   <nDotR> - Added mass in yaw direction
-  ///   <xUU> - Stability derivative, 2nd order, x component
-  ///   <xU> - Stability derivative, 1st order, x component
-  ///   <yVV> - Stability derivative, 2nd order, y component
-  ///   <yV> - Stability derivative, 1st order, y component
-  ///   <zWW> - Stability derivative, 2nd order, z component
-  ///   <zW> - Stability derivative, 1st order, z component
-  ///   <kPP> - Stability derivative, 2nd order, roll component
-  ///   <kP> - Stability derivative, 1st order, roll component
-  ///   <mQQ> - Stability derivative, 2nd order, pitch component
-  ///   <mQ> - Stability derivative, 1st order, pitch component
-  ///   <nRR> - Stability derivative, 2nd order, yaw component
-  ///   <nR> - Stability derivative, 1st order, yaw component
+  ///   <xUU>   - Stability derivative, 2nd order, x component
+  ///   <xU>    - Stability derivative, 1st order, x component
+  ///   <yVV>   - Stability derivative, 2nd order, y component
+  ///   <yV>    - Stability derivative, 1st order, y component
+  ///   <zWW>   - Stability derivative, 2nd order, z component
+  ///   <zW>    - Stability derivative, 1st order, z component
+  ///   <kPP>   - Stability derivative, 2nd order, roll component
+  ///   <kP>    - Stability derivative, 1st order, roll component
+  ///   <mQQ>   - Stability derivative, 2nd order, pitch component
+  ///   <mQ>    - Stability derivative, 1st order, pitch component
+  ///   <nRR>   - Stability derivative, 2nd order, yaw component
+  ///   <nR>    - Stability derivative, 1st order, yaw component
   /// Additionally the system also supports the following parameters:
   ///   <waterDensity> - The density of the fluid its moving in.
   ///     Defaults to 998kgm^-2.
@@ -70,19 +70,19 @@ namespace systems
   /// # 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 propell the craft and the buoyancy plugin for buoyant
+  /// thruster plugin to propel the craft and the buoyancy plugin for buoyant
   /// force. To run th example run.
   /// ```
   /// ign gazebo auv_controls.sdf
   /// ```
   /// To control the rudder of the craft run the following
   /// ```
-  /// ign topic -t /model/tethys/joint/vertical_fins_joint/0/cmd_pos \
+  /// ign topic -t /model/tethys/joint/vertical_fins_joint/0/cmd_pos
   ///    -m ignition.msgs.Double -p 'data: -0.17'
   /// ```
   /// To apply a thrust you may run the following command
   /// ```
-  /// ign topic -t /model/tethys/joint/propeller_joint/cmd_pos \
+  /// ign topic -t /model/tethys/joint/propeller_joint/cmd_pos
   /// -m ignition.msgs.Double -p 'data: -31'
   /// ```
   /// The vehicle should move in a circle.

src/systems/thruster/Thruster.cc

@@ -71,16 +71,18 @@ class ignition::gazebo::systems::ThrusterPrivateData
   public: double ThrustToAngularVec(double thrust);
 };
 
+/////////////////////////////////////////////////
 Thruster::Thruster()
 {
   this->dataPtr = std::make_unique<ThrusterPrivateData>();
 }
 
+/////////////////////////////////////////////////
 Thruster::~Thruster()
 {
-
 }
 
+/////////////////////////////////////////////////
 void Thruster::Configure(
   const ignition::gazebo::Entity &_entity,
   const std::shared_ptr<const sdf::Element> &_sdf,
@@ -103,22 +105,22 @@ void Thruster::Configure(
   auto jointName = _sdf->Get<std::string>("joint_name");
 
   // Get thrust coefficient
-  if(!_sdf->HasElement("thrust_coefficient"))
+  if (!_sdf->HasElement("thrust_coefficient"))
   {
     ignerr << "Failed to get thrust_coefficient" << "\n";
     return;
   }
   this->dataPtr->thrustCoefficient = _sdf->Get<double>("thrust_coefficient");
 
   // Get propeller diameter
-  if(!_sdf->HasElement("propeller_diameter"))
+  if (!_sdf->HasElement("propeller_diameter"))
   {
     ignerr << "Failed to get propeller_diameter \n";
   }
   this->dataPtr->propellerDiameter = _sdf->Get<double>("propeller_diameter");
 
   // Get fluid density, default to water otherwise
-  if(_sdf->HasElement("fluid_density"))
+  if (_sdf->HasElement("fluid_density"))
   {
     this->dataPtr->fluidDensity = _sdf->Get<double>("fluid_density");
   }
@@ -195,16 +197,18 @@ void Thruster::Configure(
     cmdOffset);
 }
 
+/////////////////////////////////////////////////
 void ThrusterPrivateData::OnCmdThrust(const ignition::msgs::Double &_msg)
 {
   std::lock_guard<std::mutex> lock(mtx);
   this->thrust = ignition::math::clamp(ignition::math::fixnan(_msg.data()),
     this->cmdMin, this->cmdMax);
 }
 
+/////////////////////////////////////////////////
 double ThrusterPrivateData::ThrustToAngularVec(double _thrust)
 {
-  // Thrust is proprtional to the Rotation Rate squared
+  // Thrust is proportional to the Rotation Rate squared
   // See Thor I Fossen's  "Guidance and Control of ocean vehicles" p. 246
   auto propAngularVelocity = sqrt(abs(
     _thrust /
@@ -216,6 +220,7 @@ double ThrusterPrivateData::ThrustToAngularVec(double _thrust)
   return propAngularVelocity;
 }
 
+/////////////////////////////////////////////////
 void Thruster::PreUpdate(
   const ignition::gazebo::UpdateInfo &_info,
   ignition::gazebo::EntityComponentManager &_ecm)
@@ -232,15 +237,15 @@ void Thruster::PreUpdate(
     this->dataPtr->jointAxis.Normalize());
 
   std::lock_guard<std::mutex> lock(this->dataPtr->mtx);
-  // Thrust is proprtional to the Rotation Rate squared
+  // Thrust is proportional to the Rotation Rate squared
   // See Thor I Fossen's  "Guidance and Control of ocean vehicles" p. 246
   auto desiredPropellerAngVel =
     this->dataPtr->ThrustToAngularVec(this->dataPtr->thrust);
 
   auto currentAngular = (link.WorldAngularVelocity(_ecm))->Dot(unitVector);
   auto angularError = currentAngular - desiredPropellerAngVel;
   double torque = 0.0;
-  if(abs(angularError) > 0.1)
+  if (abs(angularError) > 0.1)
     torque = this->dataPtr->rpmController.Update(angularError, _info.dt);
 
   link.AddWorldWrench(
@@ -249,7 +254,6 @@ void Thruster::PreUpdate(
     unitVector * torque);
 }
 
-
 IGNITION_ADD_PLUGIN(
   Thruster, System,
   Thruster::ISystemConfigure,

src/systems/thruster/Thruster.hh

@@ -54,42 +54,46 @@ namespace systems
   /// # 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 propell the craft and the buoyancy plugin for buoysnt
+  /// thruster plugin to propell the craft and the buoyancy plugin for buoyant
   /// force. To run th example run.
   /// ```
   /// ign gazebo auv_controls.sdf
   /// ```
   /// To control the rudder of the craft run the following
   /// ```
-  /// ign topic -t /model/tethys/joint/vertical_fins_joint/0/cmd_pos \
+  /// ign topic -t /model/tethys/joint/vertical_fins_joint/0/cmd_pos
   ///    -m ignition.msgs.Double -p 'data: -0.17'
   /// ```
   /// To apply a thrust you may run the following command
   /// The vehicle should move in a circle.
   /// ```
-  /// ign topic -t /model/tethys/joint/propeller_joint/cmd_pos \
+  /// ign topic -t /model/tethys/joint/propeller_joint/cmd_pos
   /// -m ignition.msgs.Double -p 'data: -31'
   /// ```
   class Thruster:
     public ignition::gazebo::System,
     public ignition::gazebo::ISystemConfigure,
     public ignition::gazebo::ISystemPreUpdate
   {
+    /// \brief Constructor
     public: Thruster();
 
+    /// \brief Destructor
     public: ~Thruster() override;
 
+    /// Documentation inherited
     public: void Configure(
         const ignition::gazebo::Entity &_entity,
         const std::shared_ptr<const sdf::Element> &_sdf,
         ignition::gazebo::EntityComponentManager &_ecm,
-        ignition::gazebo::EventManager &/*_eventMgr*/
-    );
+        ignition::gazebo::EventManager &/*_eventMgr*/);
 
+    /// Documentation inherited
     public: void PreUpdate(
         const ignition::gazebo::UpdateInfo &_info,
         ignition::gazebo::EntityComponentManager &_ecm);
 
+    /// \brief Private data pointer
     private: std::unique_ptr<ThrusterPrivateData> dataPtr;
   };
 }