From 9599770b76add0ecc09d853514d40c58e427998f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 27 Apr 2021 02:32:45 +0800 Subject: [PATCH] Feature/hydrodynamics (#749) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Implement hydrodynamics and thruster plugin. Signed-off-by: Arjo Chakravarty Co-authored-by: Mabel Zhang Co-authored-by: Carlos Agüero --- examples/worlds/auv_controls.sdf | 146 ++++++++ src/systems/CMakeLists.txt | 2 + src/systems/hydrodynamics/CMakeLists.txt | 6 + src/systems/hydrodynamics/Hydrodynamics.cc | 352 ++++++++++++++++++++ src/systems/hydrodynamics/Hydrodynamics.hh | 119 +++++++ src/systems/thruster/CMakeLists.txt | 6 + src/systems/thruster/Thruster.cc | 279 ++++++++++++++++ src/systems/thruster/Thruster.hh | 105 ++++++ tutorials/files/underwater/MBARI forces.png | Bin 0 -> 33069 bytes tutorials/underwater_vehicles.md | 179 ++++++++++ 10 files changed, 1194 insertions(+) create mode 100644 examples/worlds/auv_controls.sdf create mode 100644 src/systems/hydrodynamics/CMakeLists.txt create mode 100644 src/systems/hydrodynamics/Hydrodynamics.cc create mode 100644 src/systems/hydrodynamics/Hydrodynamics.hh create mode 100644 src/systems/thruster/CMakeLists.txt create mode 100644 src/systems/thruster/Thruster.cc create mode 100644 src/systems/thruster/Thruster.hh create mode 100644 tutorials/files/underwater/MBARI forces.png create mode 100644 tutorials/underwater_vehicles.md diff --git a/examples/worlds/auv_controls.sdf b/examples/worlds/auv_controls.sdf new file mode 100644 index 0000000000..e82596c152 --- /dev/null +++ b/examples/worlds/auv_controls.sdf @@ -0,0 +1,146 @@ + + + + + + 0.0 1.0 1.0 + 0.0 0.7 0.8 + + + + 0.001 + 1.0 + + + + + + + + + + + + 1000 + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 0 0 1 0 0 1.57 + https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI%20Tethys%20LRAUV + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + tethys + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + + + + + + \ No newline at end of file diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 20a9894b6d..997e046f0e 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -98,6 +98,7 @@ add_subdirectory(camera_video_recorder) add_subdirectory(detachable_joint) add_subdirectory(diff_drive) add_subdirectory(follow_actor) +add_subdirectory(hydrodynamics) add_subdirectory(imu) add_subdirectory(joint_controller) add_subdirectory(joint_position_controller) @@ -121,6 +122,7 @@ add_subdirectory(pose_publisher) add_subdirectory(scene_broadcaster) add_subdirectory(sensors) add_subdirectory(thermal) +add_subdirectory(thruster) add_subdirectory(touch_plugin) add_subdirectory(triggered_publisher) add_subdirectory(user_commands) diff --git a/src/systems/hydrodynamics/CMakeLists.txt b/src/systems/hydrodynamics/CMakeLists.txt new file mode 100644 index 0000000000..8b672acf5c --- /dev/null +++ b/src/systems/hydrodynamics/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(hydrodynamics + SOURCES + Hydrodynamics.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/hydrodynamics/Hydrodynamics.cc b/src/systems/hydrodynamics/Hydrodynamics.cc new file mode 100644 index 0000000000..61f7dc115a --- /dev/null +++ b/src/systems/hydrodynamics/Hydrodynamics.cc @@ -0,0 +1,352 @@ +/* + * Copyright (C) 2021 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 "ignition/gazebo/components/AngularVelocity.hh" +#include "ignition/gazebo/components/LinearVelocity.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/Link.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/System.hh" +#include "ignition/gazebo/Util.hh" + +#include "Hydrodynamics.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Private Hydrodynamics data class. +class ignition::gazebo::systems::HydrodynamicsPrivateData +{ + /// \brief Values to set via Plugin Parameters. + /// Plugin Parameter: Added mass in surge, X_\dot{u}. + public: double paramXdotU; + + /// \brief Plugin Parameter: Added mass in sway, Y_\dot{v}. + public: double paramYdotV; + + /// \brief Plugin Parameter: Added mass in heave, Z_\dot{w}. + public: double paramZdotW; + + /// \brief Plugin Parameter: Added mass in roll, K_\dot{p}. + public: double paramKdotP; + + /// \brief Plugin Parameter: Added mass in pitch, M_\dot{q}. + public: double paramMdotQ; + + /// \brief Plugin Parameter: Added mass in yaw, N_\dot{r}. + public: double paramNdotR; + + /// \brief Plugin Parameter: Linear drag in surge. + public: double paramXu; + + /// \brief Plugin Parameter: Quadratic drag in surge. + public: double paramXuu; + + /// \brief Plugin Parameter: Linear drag in sway. + public: double paramYv; + + /// \brief Plugin Parameter: Quadratic drag in sway. + public: double paramYvv; + + /// \brief Plugin Parameter: Linear drag in heave. + public: double paramZw; + + /// \brief Plugin Parameter: Quadratic drag in heave. + public: double paramZww; + + /// \brief Plugin Parameter: Linear drag in roll. + public: double paramKp; + + /// \brief Plugin Parameter: Quadratic drag in roll. + public: double paramKpp; + + /// \brief Plugin Parameter: Linear drag in pitch. + public: double paramMq; + + /// \brief Plugin Parameter: Quadratic drag in pitch. + public: double paramMqq; + + /// \brief Plugin Parameter: Linear drag in yaw. + public: double paramNr; + + /// \brief Plugin Parameter: Quadratic drag in yaw. + public: double paramNrr; + + /// \brief Water density [kg/m^3]. + public: double waterDensity; + + /// \brief Added mass of vehicle; + /// See: https://en.wikipedia.org/wiki/Added_mass + Eigen::MatrixXd Ma; + + /// \brief Previous state. + public: Eigen::VectorXd prevState; + + /// Link entity + public: ignition::gazebo::Entity linkEntity; +}; + +///////////////////////////////////////////////// +void AddAngularVelocityComponent( + const ignition::gazebo::Entity &_entity, + ignition::gazebo::EntityComponentManager &_ecm) +{ + if (!_ecm.Component(_entity)) + { + _ecm.CreateComponent(_entity, + ignition::gazebo::components::AngularVelocity()); + } + + // Create an angular velocity component if one is not present. + if (!_ecm.Component( + _entity)) + { + _ecm.CreateComponent(_entity, + ignition::gazebo::components::WorldAngularVelocity()); + } +} + +///////////////////////////////////////////////// +void AddWorldPose( + const ignition::gazebo::Entity &_entity, + ignition::gazebo::EntityComponentManager &_ecm) +{ + if (!_ecm.Component(_entity)) + { + _ecm.CreateComponent(_entity, ignition::gazebo::components::WorldPose()); + } +} + +///////////////////////////////////////////////// +void AddWorldLinearVelocity( + const ignition::gazebo::Entity &_entity, + ignition::gazebo::EntityComponentManager &_ecm) +{ + if (!_ecm.Component( + _entity)) + { + _ecm.CreateComponent(_entity, + ignition::gazebo::components::WorldLinearVelocity()); + } +} + +///////////////////////////////////////////////// +double SdfParamDouble( + const std::shared_ptr &_sdf, + const std::string& _field, + double _default) +{ + return _sdf->Get(_field, _default).first; +} + +///////////////////////////////////////////////// +Hydrodynamics::Hydrodynamics() +{ + this->dataPtr = std::make_unique(); +} + +///////////////////////////////////////////////// +Hydrodynamics::~Hydrodynamics() +{ + // Do nothing +} + +///////////////////////////////////////////////// +void Hydrodynamics::Configure( + const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &/*_eventMgr*/ +) +{ + this->dataPtr->waterDensity = SdfParamDouble(_sdf, "waterDensity", 998); + this->dataPtr->paramXdotU = SdfParamDouble(_sdf, "xDotU" , 5); + this->dataPtr->paramYdotV = SdfParamDouble(_sdf, "yDotV" , 5); + this->dataPtr->paramZdotW = SdfParamDouble(_sdf, "zDotW" , 0.1); + this->dataPtr->paramKdotP = SdfParamDouble(_sdf, "kDotP" , 0.1); + this->dataPtr->paramMdotQ = SdfParamDouble(_sdf, "mDotQ" , 0.1); + this->dataPtr->paramNdotR = SdfParamDouble(_sdf, "nDotR" , 1); + this->dataPtr->paramXu = SdfParamDouble(_sdf, "xU" , 20); + this->dataPtr->paramXuu = SdfParamDouble(_sdf, "xUU" , 0); + this->dataPtr->paramYv = SdfParamDouble(_sdf, "yV" , 20); + this->dataPtr->paramYvv = SdfParamDouble(_sdf, "yVV" , 0); + this->dataPtr->paramZw = SdfParamDouble(_sdf, "zW" , 20); + this->dataPtr->paramZww = SdfParamDouble(_sdf, "zWW" , 0); + this->dataPtr->paramKp = SdfParamDouble(_sdf, "kP" , 20); + this->dataPtr->paramKpp = SdfParamDouble(_sdf, "kPP" , 0); + this->dataPtr->paramMq = SdfParamDouble(_sdf, "mQ" , 20); + this->dataPtr->paramMqq = SdfParamDouble(_sdf, "mQQ" , 0); + this->dataPtr->paramNr = SdfParamDouble(_sdf, "nR" , 20); + this->dataPtr->paramNrr = SdfParamDouble(_sdf, "nRR" , 0); + + // Create model object, to access convenient functions + auto model = ignition::gazebo::Model(_entity); + if(!_sdf->HasElement("link_name")) + { + ignerr << "You musk specify a for the hydrodynamic" + << " plugin to act upon"; + return; + } + auto linkName = _sdf->Get("link_name"); + this->dataPtr->linkEntity = model.LinkByName(_ecm, linkName); + if(!_ecm.HasEntity(this->dataPtr->linkEntity)) + { + ignerr << "Link name" << linkName << "does not exist"; + return; + } + + this->dataPtr->prevState = Eigen::VectorXd::Zero(6); + + AddWorldPose(this->dataPtr->linkEntity, _ecm); + AddAngularVelocityComponent(this->dataPtr->linkEntity, _ecm); + AddWorldLinearVelocity(this->dataPtr->linkEntity, _ecm); + + + // Added mass according to Fossen's equations (p 37) + this->dataPtr->Ma = Eigen::MatrixXd::Zero(6, 6); + + this->dataPtr->Ma(0, 0) = this->dataPtr->paramXdotU; + this->dataPtr->Ma(1, 1) = this->dataPtr->paramYdotV; + this->dataPtr->Ma(2, 2) = this->dataPtr->paramZdotW; + this->dataPtr->Ma(3, 3) = this->dataPtr->paramKdotP; + this->dataPtr->Ma(4, 4) = this->dataPtr->paramMdotQ; + this->dataPtr->Ma(5, 5) = this->dataPtr->paramNdotR; +} + +///////////////////////////////////////////////// +void Hydrodynamics::PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + if (_info.paused) + return; + + // These variables follow Fossen's scheme in "Guidance and Control + // of Ocean Vehicles." The `state` vector contains the ship's current velocity + // in the formate [x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel]. + // `stateDot` consists of the first derivative in time of the state vector. + // `Cmat` corresponds to the Centripetal matrix + // `Dmat` is the drag matrix + // `Ma` is the added mass. + Eigen::VectorXd stateDot = Eigen::VectorXd(6); + Eigen::VectorXd state = Eigen::VectorXd(6); + Eigen::MatrixXd Cmat = Eigen::MatrixXd::Zero(6, 6); + Eigen::MatrixXd Dmat = Eigen::MatrixXd::Zero(6, 6); + + // Get vehicle state + ignition::gazebo::Link baseLink(this->dataPtr->linkEntity); + auto linearVelocity = + _ecm.Component(this->dataPtr->linkEntity); + auto rotationalVelocity = baseLink.WorldAngularVelocity(_ecm); + + if (!linearVelocity) + { + ignerr << "no linear vel" <<"\n"; + return; + } + + // 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 localRotationalVelocity = pose->Rot().Inverse() * *rotationalVelocity; + + state(0) = localLinearVelocity.X(); + state(1) = localLinearVelocity.Y(); + state(2) = localLinearVelocity.Z(); + + state(3) = localRotationalVelocity.X(); + state(4) = localRotationalVelocity.Y(); + state(5) = localRotationalVelocity.Z(); + + auto dt = static_cast(_info.dt.count())/1e9; + stateDot = (state - this->dataPtr->prevState)/dt; + + this->dataPtr->prevState = state; + + // The added mass + const Eigen::VectorXd kAmassVec = this->dataPtr->Ma * stateDot; + + // 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, 5) = - this->dataPtr->paramXdotU * state(0); + Cmat(2, 3) = - this->dataPtr->paramYdotV * state(1); + 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, 4) = - this->dataPtr->paramNdotR * state(5); + 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, 5) = - this->dataPtr->paramKdotP * state(3); + 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); + const Eigen::VectorXd kCmatVec = - Cmat * state; + + // Damping forces (Fossen P. 43) + Dmat(1, 1) + = - this->dataPtr->paramYv - this->dataPtr->paramYvv * abs(state(1)); + Dmat(0, 0) + = - this->dataPtr->paramXu - this->dataPtr->paramXuu * abs(state(0)); + Dmat(2, 2) + = - this->dataPtr->paramZw - this->dataPtr->paramZww * abs(state(2)); + Dmat(3, 3) + = - this->dataPtr->paramKp - this->dataPtr->paramKpp * abs(state(3)); + Dmat(4, 4) + = - this->dataPtr->paramMq - this->dataPtr->paramMqq * abs(state(4)); + Dmat(5, 5) + = - this->dataPtr->paramNr - this->dataPtr->paramNrr * abs(state(5)); + + const Eigen::VectorXd kDvec = Dmat * state; + + const Eigen::VectorXd kTotalWrench = kAmassVec + kDvec + kCmatVec; + + ignition::math::Vector3d + totalForce(-kTotalWrench(0), -kTotalWrench(1), -kTotalWrench(2)); + ignition::math::Vector3d + totalTorque(-kTotalWrench(3), -kTotalWrench(4), -kTotalWrench(5)); + + baseLink.AddWorldWrench( + _ecm, + pose->Rot()*(totalForce), + pose->Rot()*totalTorque); +} + +IGNITION_ADD_PLUGIN( + Hydrodynamics, System, + Hydrodynamics::ISystemConfigure, + Hydrodynamics::ISystemPreUpdate +) + +IGNITION_ADD_PLUGIN_ALIAS( + Hydrodynamics, + "ignition::gazebo::systems::Hydrodynamics") diff --git a/src/systems/hydrodynamics/Hydrodynamics.hh b/src/systems/hydrodynamics/Hydrodynamics.hh new file mode 100644 index 0000000000..8a313282cc --- /dev/null +++ b/src/systems/hydrodynamics/Hydrodynamics.hh @@ -0,0 +1,119 @@ +/* + * Copyright (C) 2021 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 IGNITION_GAZEBO_SYSTEMS_HYDRODYNAMICS_HH_ +#define IGNITION_GAZEBO_SYSTEMS_HYDRODYNAMICS_HH_ + +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + class HydrodynamicsPrivateData; + + /// \brief This class provides hydrodynamic behaviour for underwater vehicles + /// It is shamelessly based off Brian Bingham's plugin for 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 + /// 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 + /// 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] + /// Additionally the system also supports the following parameters: + /// - The density of the fluid its moving in. + /// Defaults to 998kgm^-3. [kgm^-3] + /// - The link of the model that is being subject to + /// hydrodynamic forces. [Required] + /// + /// # 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. + /// ``` + /// 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 + /// -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 + /// -m ignition.msgs.Double -p 'data: -31' + /// ``` + /// The vehicle should move in a circle. + class Hydrodynamics: + public ignition::gazebo::System, + public ignition::gazebo::ISystemConfigure, + public ignition::gazebo::ISystemPreUpdate + { + /// \brief Constructor + public: Hydrodynamics(); + + /// \brief Destructor + public: ~Hydrodynamics() override; + + /// 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 + private: std::unique_ptr dataPtr; + }; +} +} +} +} +#endif diff --git a/src/systems/thruster/CMakeLists.txt b/src/systems/thruster/CMakeLists.txt new file mode 100644 index 0000000000..57285f81f5 --- /dev/null +++ b/src/systems/thruster/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(thruster + SOURCES + Thruster.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc new file mode 100644 index 0000000000..eefd5b0e90 --- /dev/null +++ b/src/systems/thruster/Thruster.cc @@ -0,0 +1,279 @@ +/* + * Copyright (C) 2021 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 "ignition/gazebo/components/AngularVelocity.hh" +#include "ignition/gazebo/components/ChildLinkName.hh" +#include "ignition/gazebo/components/LinearVelocity.hh" +#include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/Link.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" + +#include "Thruster.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::ThrusterPrivateData +{ + /// \brief Mutex for read/write access to class + public: std::mutex mtx; + + /// \brief Thrust output by propeller in N + public: double thrust = 0.0; + + /// \brief The link entity which will spin + public: ignition::gazebo::Entity linkEntity; + + /// \brief Axis along which the propeller spins + public: ignition::math::Vector3d jointAxis; + + /// \brief ignition node for handling transport + public: ignition::transport::Node node; + + /// \brief The PID which controls the rpm + public: ignition::math::PID rpmController; + + /// \brief maximum input force [N], default: 1000N + public: double cmdMax = 1000; + + /// \brief minimum input force [N], default: 1000N + public: double cmdMin = -1000; + + /// \brief Thrust coefficient relating the + /// propeller rpm to the thrust + public: double thrustCoefficient = 1; + + /// \brief Density of fluid in kgm^-3, default: 1000kgm^-3 + public: double fluidDensity = 1000; + + /// \brief Diameter of propeller in m, default: 0.02 + public: double propellerDiameter = 0.02; + + /// \brief callback for handling thrust update + public: void OnCmdThrust(const ignition::msgs::Double &_msg); + + /// \brief function which computes rpm from thrust + public: double ThrustToAngularVec(double thrust); +}; + +///////////////////////////////////////////////// +Thruster::Thruster() +{ + this->dataPtr = std::make_unique(); +} + +///////////////////////////////////////////////// +Thruster::~Thruster() +{ +} + +///////////////////////////////////////////////// +void Thruster::Configure( + const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &/*_eventMgr*/) +{ + // Get namespace + std::string ns {""}; + if (_sdf->HasElement("namespace")) + { + ns = _sdf->Get("namespace"); + } + + // Get joint name + if (!_sdf->HasElement("joint_name")) + { + ignerr << "No joint to treat as propeller found \n"; + return; + } + auto jointName = _sdf->Get("joint_name"); + + // Get thrust coefficient + if (!_sdf->HasElement("thrust_coefficient")) + { + ignerr << "Failed to get thrust_coefficient" << "\n"; + return; + } + this->dataPtr->thrustCoefficient = _sdf->Get("thrust_coefficient"); + + // Get propeller diameter + if (!_sdf->HasElement("propeller_diameter")) + { + ignerr << "Failed to get propeller_diameter \n"; + } + this->dataPtr->propellerDiameter = _sdf->Get("propeller_diameter"); + + // Get fluid density, default to water otherwise + if (_sdf->HasElement("fluid_density")) + { + this->dataPtr->fluidDensity = _sdf->Get("fluid_density"); + } + igndbg << "Setting fluid density to: " << this->dataPtr->fluidDensity << "\n"; + + // Create model object, to access convenient functions + auto model = ignition::gazebo::Model(_entity); + + auto jointEntity = model.JointByName(_ecm, jointName); + auto childLink = + _ecm.Component(jointEntity); + + this->dataPtr->jointAxis = + _ecm.Component(jointEntity) + ->Data().Xyz(); + + std::string thrusterTopic = ignition::transport::TopicUtils::AsValidTopic( + "/model/" + ns + "/joint/" + jointName + "/cmd_pos"); + + this->dataPtr->node.Subscribe( + thrusterTopic, + &ThrusterPrivateData::OnCmdThrust, + this->dataPtr.get()); + + // Get link entity + this->dataPtr->linkEntity = model.LinkByName(_ecm, childLink->Data()); + + // Create an angular velocity component if one is not present. + if (!_ecm.Component( + this->dataPtr->linkEntity)) + { + _ecm.CreateComponent(this->dataPtr->linkEntity, + ignition::gazebo::components::AngularVelocity()); + } + + // Create an angular velocity component if one is not present. + if (!_ecm.Component( + this->dataPtr->linkEntity)) + { + _ecm.CreateComponent(this->dataPtr->linkEntity, + ignition::gazebo::components::WorldAngularVelocity()); + } + + double p = 0.1; + double i = 0; + double d = 0; + double iMax = 1; + double iMin = -1; + double cmdMax = this->dataPtr->ThrustToAngularVec(this->dataPtr->cmdMax); + double cmdMin = this->dataPtr->ThrustToAngularVec(this->dataPtr->cmdMin); + double cmdOffset = 0; + + if (_sdf->HasElement("p_gain")) + { + p = _sdf->Get("p_gain"); + } + if (!_sdf->HasElement("i_gain")) + { + i = _sdf->Get("i_gain"); + } + if (!_sdf->HasElement("d_gain")) + { + d = _sdf->Get("d_gain"); + } + + this->dataPtr->rpmController.Init( + p, + i, + d, + iMax, + iMin, + cmdMax, + cmdMin, + cmdOffset); +} + +///////////////////////////////////////////////// +void ThrusterPrivateData::OnCmdThrust(const ignition::msgs::Double &_msg) +{ + std::lock_guard lock(mtx); + this->thrust = ignition::math::clamp(ignition::math::fixnan(_msg.data()), + this->cmdMin, this->cmdMax); +} + +///////////////////////////////////////////////// +double ThrusterPrivateData::ThrustToAngularVec(double _thrust) +{ + // 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 / + (this->fluidDensity + * this->thrustCoefficient * pow(this->propellerDiameter, 4)))); + + propAngularVelocity *= (_thrust > 0) ? 1: -1; + + return propAngularVelocity; +} + +///////////////////////////////////////////////// +void Thruster::PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + if (_info.paused) + return; + + ignition::gazebo::Link link(this->dataPtr->linkEntity); + + auto pose = worldPose(this->dataPtr->linkEntity, _ecm); + + // TODO(arjo129): add logic for custom coordinate frame + auto unitVector = pose.Rot().RotateVector( + this->dataPtr->jointAxis.Normalize()); + + double desiredThrust; + { + std::lock_guard lock(this->dataPtr->mtx); + desiredThrust = this->dataPtr->thrust; + } + // 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(desiredThrust); + auto currentAngular = (link.WorldAngularVelocity(_ecm))->Dot(unitVector); + auto angularError = currentAngular - desiredPropellerAngVel; + double torque = 0.0; + if (abs(angularError) > 0.1) + torque = this->dataPtr->rpmController.Update(angularError, _info.dt); + + link.AddWorldWrench( + _ecm, + unitVector * this->dataPtr->thrust, + unitVector * torque); +} + +IGNITION_ADD_PLUGIN( + Thruster, System, + Thruster::ISystemConfigure, + Thruster::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(Thruster, "ignition::gazebo::systems::Thruster") diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh new file mode 100644 index 0000000000..176cc82398 --- /dev/null +++ b/src/systems/thruster/Thruster.hh @@ -0,0 +1,105 @@ +/* + * Copyright (C) 2021 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 IGNITION_GAZEBO_SYSTEMS_THRUSTER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_THRUSTER_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class ThrusterPrivateData; + + /// \brief This class provides a class that simulates a maritime thruster for + /// boats and underwater vehicles. It uses the equations described in Fossen's + /// "Guidance and Control of Ocean Vehicles" in page 246. This plugin takes in + /// force in Newtons and applies it to the thruster. It also calculates the + /// theoretical RPM of the blades and spins them at that RPM. The rationale + /// for directly using force + /// + /// ## System Parameters + /// - The namespace in which the robot exists. The plugin will + /// listen on the topic `/model/{namespace}/joint/{joint_name}/cmd_pos`. + /// [Optional] + /// - This is the joint in the model which corresponds to the + /// propeller. [Required] + /// - This is the coefficient which relates the RPM to + /// actual thrust. [Required, no units] + /// - The fluid density of the liquid in which the thruster + /// is operating in. [Required, kgm^-3] + /// - The propeller diameter is the diameter of the prop + /// in meters. [Required, m] + /// + /// # 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 + /// 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 + /// -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 + /// -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 &_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 + private: std::unique_ptr dataPtr; + }; +} +} +} +} + +#endif diff --git a/tutorials/files/underwater/MBARI forces.png b/tutorials/files/underwater/MBARI forces.png new file mode 100644 index 0000000000000000000000000000000000000000..07e8e327317bc44f67afcf23134b50f3364c6bbd GIT binary patch literal 33069 zcmeFZ`9E7-v_DQ;^r5P)spj@+tBTN8(GqkXsCkaD6g7kzVn_@v)q$cDii)A8h?yja zP*t>Is39@LteQ$hOo=gkj_2O{+Oot*EoPbVRrMw-vuBtbkEwj$L~XD z&vGC8&b@DU0;XgAZcgnA1Bv@&a|n3T|M!QI$-VdCk(1G9_pT?*4jAuUujU;D>;sIt zdldZ77vBCIx^M4;zi%xb*t_10oBV6<`ckA>c<*}T-2Xk`|1kJJKlr~``2RT{d@=G` z{nKlebDd{v0nhwu&CO+!dj#Onp}!2XbjJ>B&!5LPZS)6EHuDMVa&w)L1mJp-+4<>8 zU_(l`_4v+r`7Q@j$_#JlZ4U;J*!0RWOq3UN-So<)JzKs$b7?WuHC;&~e3k z8+>r7x^LgNh495CaL)V1zql0U_K17_z(qAR#%s0i8gv={iqquj@O@nRT6+wwZDVAV z=m4K{WA_NCVE?MNrV3v}qji+ugs+(_v_78$wy+SXvfa2k#oK6(>7HaaiL$S&^IPdRV}}C>D*R)Z?bB zlZ5OktSlU>hgG-BB03QbR3xGsF`hjUwL6B8p%0ehOh4bYUHRQbP-&@GR#Q{!ft~?@ zy0n7lAFWM9HH`)K=!|8UFqu8F^9Fj zZ{csni~XQ%uQlyr1_mrLH2oZxFWS~V&RH83(%Y_BTYD{`R&qU-->Trggm+)wtIW{p zS6BcRM9*-Q8}S!MgcmkUSR1Ev1|kk$KA|hdekN>G0lfRPpSEY;Y3=Ekry8hN*h8+R zfWhO8Zrv>(Vn@gF_41ua24?Wx+EQllh^H0G?2KQuJlEB!|1mc$u3*vQJ(p(=slI{z z`FXM@f?o$<>KvO? zY`Jp15~3R6l}uuchp!kiBVWh_e^mDxPzWP*B6TPNL}MM61(L|km4SmIKwMlVKEXE^ zM=fN+e!XQcwaZcKj32~M=L)lYx8|#j8_SC5dZCTm%fv9>HO|@uBDHCgh8~}2c_E~_7|fr68x1hH zcpTkoiQC$sX1&LS?QBp9DU>ud4|&!7Tt35C1_ns!Du<6YuY52!`VpdH3{%rtsGVt` ztCRb!hbB>vQ41Iz)4_3_wNbSV9ON*}scF{K!I>4Z#b&(6EqNX)gn*o)I57NDn-t^C zjl)a6_=i7^G2wclhovu{@8Jvt6cg=mz6{P-nS(adFJb@KC(r(ZFdSm~)!BEcsAS75 z39jr%SZFy97gtInB-t_OZ?q?u37=Y=f1h&4alpQ7TB z-e5H5f|Cy|ZncO)6QLg=cf^%#-fV9%a1(4_G}+V;k~b*3aQzV1(Enf$<1TkR5c4*r z5X`&O%%Hw|j^3`1HLN*h=OqszcQ*%{8mx#-JDW;j+O}AqG!DZria02U3#4|BbJWw` zMjk$X{zTZA=0y06gVoQ6y6w_|uS3RiZ^bt>2}prO|7PKgejLlY_u|rpUQ{76z2QEn z3Bkam>MVah_?Nt&;X^v;7Q!itZ<&@$lA$IBOPy+6#rGaIqMQ^M4K z@ERj>7b@UB3Ft%JZ|KI`pEqZc^cskOakm^Eu|7>itYxm@jB=JIV}#&1`Zxix92`Kh zv<{oG5`g>s?^xHgq>UG=psoR9H{>+p1|H z7@y~Y5*NSW%UoLHKH-JAuzmj-DyFr6@pT#|`{F?ZrI&{KB$N{Z4`H=Gl^?t6n>|acP2ZnCV`!=r>W<@TKzo2z%GY>ftT1X|f{RpU}HLd59 z0N0S#nJD0qUsPN|1dtb_cEar%LjBjLGo#z$y3^H(+D!^v&Xjq{J&p_J#=8PKJ})~2 z2xGW{lVt8f7+^Q^0GZfm3Kr6hQFfVaD;qL8quprgV|kE)7tsH`J=G-g5zt@VPV0 zHT*(7u@OkjeuCmAo^bDPSgDKLUTTkZj0AxE2W)-WOmBY{e|54!SBk1e#?0Yy4^Bhn zPP%uB!{Qa`qooWP@Y(wxfdG$FDyz_q*c<{VyrD87ef`dWWZq(SR&JmRZA~s}`$A9H z1@*%L)8AfcxVF9iG$9ZfSchF_XC#xUoDt&eF0jl06%mer)W{j4sQj zxiVR+iTd*BsfI0s%b_&&b5~XEYkzJb0Mc`;9oYKIj7SFP-;YUHLxw~e@fh>C5U`VB zccXSTJf!p@!zYpcaE4YY>JNKorPQ{%O?$8?>aeL^sQbf*r=4QP%aP`5{eiTV{w;Ar z>f}~0XEl;#D{}L_OoEhVw>Bw0x^PDoZB@El)*tvMs-WlU>NY&0nHb${o|SnW8KrB{ z>n)=L4?^gp(T~_JgfbvG zIqk=ny$;>W$4r(Vy4e`54JuO^Vkwk`3&eCjoQ-83ta&kM|5#G840jz z-LGR(;LwV($Sp>2-@UWL37X@7c;bYO!M(_^-x7eVRvM;X`mxDo(0RZP!R%oV1PIC> zQ$^iIU;hIESfC zQL^s#t)1cp{E0r_w96@9yNnwP&d{VYT$6O%S@glReV)JsVq}Ad9fY|wj$!%YaY9;uPT)6wXE4&e%PKQ`>wn-4_p&}JnLa>0mX}g# zRSF4fq7xKOB*mSW}+!rWO5kt58Kisn&FRw3%RSAQT8xFCZTi zpA->cg@Pb86;V4YS!8?&bJCz9=f(^8Q`;mb0-OZ~M&;m<2}{x8toDz4)|i8!^p zv!=(?@vJK`$%$SYXGu5!Diz$LvldGHtjibc6=iF(4hCj{(QmBRwepcGuIC?aK4T0| zcbh;oy2&-A&R)`oCBG{q+uOA4&{#Qcg-{>T7j`89(5bw`4KU~O$GIsAJjT1>>AP!D zyQzl?z2?l0n=AZm#ut(9HDAX?EjIHJh$8%=&j-@;PW(yA#7{;G_$-Xp{g_0F-N`&< zKA@HVrg#!{^>_c={hMp1@oCZJvUU#?rNQ~vVtB`m3F5J8rWIxt0%nhUw|>qP#Tu@T zix!c*iWb`8M#*23AWjipw&1-)Yk~#d3|DE|4p3cuwJ*JUqb`5a7UN~fyhu39bISCK zxpO#>^fv6qAf1vv@0!0EPud;N3Sa&KZeDqBtXmncQvY4R4RDH;9U*w8Qv@qH1OW^4 z1z6!?C@m`YY9ZM=LWMtRGfFWiPWKc6C!pH!b3BYH!&ILOlZF2|SuVo9*<)w2Xkl$N z*Own3q6fMIq>S+)X(^e`YrY^QAO%{Ev|e15vs(kQQXrlDv(oDgqyRzdZn zNt6Rt4QFZBOT;wiCYqYWPm}n=fpvm>)-K7~iB+~Xx5C1c_Ov1_x^Q1mlB~{rF}iG7 zw;#xZNdn0JLgO{xa;k~*YD^&{MwE-y`n3K8TD{`Ni^J?qdN~5OhHs^*4ZwE1`&Q<5 zb9=6>>I(29-=8&R13(jirxM!DYc0@)NL2-CFy{htvk9HQK|`0bA^DuaBleLuowC=< z2Y2UGg*WZ8svAWf^%o?FZSe{G3q;N3ry|EgisTj)O_?V*2*v6`!H= z%!@jS5D^Z!{PCiNK=QuWN88Bywqi?k_9Ti*axGFSnUJvpp@Edk2}wDZ4P%ZIzD%Mj z6Zrf2-@%o2j6=aW^(-3JD@}ywRsu#O_9l$Ahm1YxjO~D6?fGVNP!_)}KU;F9=!jTe zkMXtp5A+?L#x$EZg)l})_)G|hr7H^NsPg&}SXRuaKYYeI*aap2ME)2_?P8TLn-h;d zEDi*mg3A)bObu`;f*3aM90&NGhzOV^p||9NBC>eeG8O%=!v{SURdSnB9GEadR; zN;@N2HME%QXiv0S<$SN|pB8@WyfuVuHqTMGeDsZR^AA56^EInjKG+${qMskH$mF4C z&BuJz9gU|9MM6cQgBF<+PM{cF*o1|%;n9Iem`wn1% zTsr4!@d6GmtCNMa26@tG#m@5lPxv?aqu^tk(j4H4o;Mg6r*+R7w)pXH%Wvpcc4n#M5lDd91ss z^WZF8*|7HWn*!A{t4)0f62jy>nZc@sCwXlg?%TbwtIA0t=`Xy{ig<8j+wAYV+}2rd zA@&@y8EnQfdmwcme?5Ex?l7f#U^VvH&ThSrkbJPsRM(7!ZBF1w`cO>U4EUSy1dE)$?AkoAH|KDx~zu zy|4?m+O9e9((&FwWYhL?e8ftZ=2U#8@D`AKYJ@>j7|_WtPsiuKnIhH1zI1!G&jU%b zwh>k!bEc`X%HI%zK27bzS8vD6ZKYQPf-IAJ;`Bpaaw20dOc*m71 zDJ%5TM6L4cKo*B-7`~>_#NuEZ$7ZnX?avJh*t#HGf;3R1tU$W}_~Jjq(4ehWyfQs6 zg8xt!J>x?q#(3h9Enys2L!?u-&z4o6QHw0VL%#uK$R`e>d*hdx10$K3noe_~*l{qy zyyR1Gw%ayc!9eeUpItx;3hQ!t4Ep)@-ZG*XSVnwVP&nHj;IaKMjcWB>t=XiO2(|hw z!rX+l?~e7&`AS{+Z`;l5ysd_wW7q7S%O5in@ZfIl_u~wpEPrb>k#{x|7O~F||T)*xIM z&p|zrIPU+(t#Xv4Tp?vl|U?{L~`cFe~Dc)C_j4`j{ zi|uZ^@KOw8jmz=S&lUz-*~xDBFLBd*Wz+w_$yX(0!-WWEoixyMb0V4$f4hIB!eKbh zL<08Vb9Y=m9_qhWB>vCw7y2V@nR*^`U2J3C9>dt4AZudTm1c6ndSAetuigBAkDoCM z=`I%rt;E4odTR&VTMAXoLn!JpPE~^F6olGL?)qL)^&behhz?a>svH{!`;5ndQ5^?! z7ExKA?ce5m#55_(6H!>{dWx9RzteSu=}TGSv5TT{k9(SHi0WE8hEUKMkdc=sgK!{s zAY8tb#Q}N57y88vHE09iwL@f06-KPF-V-qq@Sui1CRq-Hd7Vrga{vss|JUFrXFYnQ zi9Qj9`I)Oier!mxxb6*ZizdVvG^?T2s&l(FpsKn>D7N(Ge*wn@u2V|2wJJ>YiKyP+ z;4E<{tC?)fu`gf1T3@PQog$5CoZ2=S(*M_>6Dpd6RfVsytMo)%jlA2tW;NsW`v**5 zIEVi8q?E{Q$uVgB(SIlUZb#bM3pa2`e5tm-Tsf&aJHJHV`uoNI+CN35Fsudl{=b?Fl&biY(-W3it?>>%9; z5Qt*wo+SkGX8Wbsz3YLZ7O?N{3~x#qSHcX}?(+6Hudf=Umg>rr#9*EtbO1Eve?e2o z9p$O^xk1+ktqfG9kZSH%kw4ir_rYs$qcxZ*{0D_AD&RuBh|i@u53<0e$f-qC(_j&v z(T^J*v2DVCF$|$V_x6GRam1@f!Rr@vv^cH1hdYFRh)$$V^t#?yU1N>o^@9Mg%SH- zR?xyznkVZDhOc285%^ZNM47a*7T927F|7&nw!Pa`=#g`WR zhhx>Dj7iy)j7Ht?!DDAaI9dkG<+xHX>#AMO-R- z=X`xlWE_Xgf*ROi-T}{f^*^4&*E3lMvZO_7u@VKKfy@=aTp>3)x=>EnQmgg5lghKwjdha4-=Xe?QXWbR zP2YFCHZ@gkigyAqH}kpRt8FKW>A!Wus@;^4OkIVxuzg%lE(3loVwxy`lu!vR3v4z& z*{`bigcgzzwmnB)fi^!gJrntY$6iCSi~PZUZPD z*w{Mu4=z)ab@E4a34O}tELV-)wtkWA?n+NVcGW#~SI30Xqt$QAZsR4XJIG0|`_ol9RLzQ`4&&9y@#40=ht)=b z9ZftSSyzv4GzWGqArREcRiaS#rh@~N1cE_`c<69RT8epeeWs#H^X@8_6UL+Bf_2ra+;b7A!5g z@VdvvP^);{b9R|@brSkR6^D9=dtSX9O%23R^u@uc z_$PD80%=P7@|#4#9&(vp)q0OyJ&CEe`bd{foocaC-XNxQZ+E5Km-V=zRf8GtVpeev z?(}cUId$unZwc95T;L;)Rh$eT+&Am)SFsvaJ>R2=tT}4P#xcFW&EqAd>@IN37A-b% z`k5|Lhmc-JEpem9iE+c)qUI%rn23_D*g{)o2!*nGPuOQ?-WL0KLE6sZWgnvdb04Yy#R(*r)O2oCE4wb-Cw^3K zAsb3~pzjD`EKj`{TFxOr7qm-#Is`q2Uv5UNL{o8*9}hh$i!F!Nqo-BlgL zd&20lJWl#i*yGuhc(U(C=v-vT1_9zeO-xM52Ms1H)dy1aiA(WikTF;_P*YuD4^tRE zos&BnuFD7Pu%T~D?WzlICT#FNnf}LT%ogqG(bS9LU{W#a2`X!V)c$7yV{So;Q5*U0sEKABO??8S#H!> za8`fJVT-dSZMQy4#con|fLL(aLYqrvaDExGS3VDp3~ZIxT-Y7_&a0{H9YbRr_4mZwBlZW~eB-4mz5&m4ORiI|Ns44SxZNCy&kghSiZk znmFc@`X3<5pfse-_j0%~e(EJ*zD<8?c*aA{&jIP_8#B&y?!OI)he7I*tK9Qe|B!Oz zqCG9T@aydK=b4jg_VLH4m{0b&4ruLY5bD2Zz301GqHW`_QSPd1>4dys2-XiUE>@(!L=RF?`X zw`$Xpwrm>@VtK_{Abu|<lb)!2^-@Y`~r=)A7iNC+c^zulcBZY z=l$(s4N0fUWOaO8b$%Duq1*Sfo^|N->>eZiZ2=dYf`5&r;L!y`QAzhIrMmA`;#EU> zLP`;7M(CPOEcPqwO#lhE!=%and%IcyOkM6kDKsryOjY;Za(Hj^AGvd)zppO zFWLuVUL00+TT7+%FQClG5M)$U#E!mFhSoCQK7}V^y#GO?fe8Z0_@pt4K&lorE^;%! zNt_&y3+^tJs-B>I^ZBiqlp%GGinPUK}3a#HY}W>|+bPY;P87UM*`!(Jd7w?+fOcNq(;X^yXL!yTuac|(~Vg*Jh>al&Lu zs`W8qH{H&3YPP&+7PK`xQ+}aVF2CuK9#b)0r}J7Ib0@s@k+ zM@$H*nPgWsZe=-e;Q9rGrl}+ab-rZtgsI%7(MkaxYazge*kA|4^r zH5RBlhoqnL3Q+~|`C(q#q7mlvPvgz!=h}R2OKB{|TmPDL_i^;S-S4vbgY3?jbi|5ug3CirQ0^tPAbvyNH5Lua7N zO2?`=O(KZZbPELgn+T%(x!vKhiz{IP8}}-W)r0Hc5?eK1s9zWJ>GAm0TF3%M_R$8J z2~pj?&_AAMCpzu4sPIJBmQaG(SVNJw5>gpGP6c*WzsX5_d`~h(_oDDoi~y1d8TSp$ zn9n?vYVOtQ^FjNk49eCC-BuEn92zntbZly24)7k~LiHLCRb7g#*6eY7xxOvFdV6AC zC1FD=_q9&wO;5x(lIxwk5#cfsMUocE1Z8!PM<4U9h>nyyO|iWD?0ejj*d0}{RUEFW zDE5r;r{8zo5KlBO9lqsjeP{kan>JSi=HxHvw7_ zJWys9;Q)*1E%gq`8(Hzlv!RaohUqJ7J6al7Hp13|iR2w}VfTd3RhHQgH>F~Il?l=` z{W)2@)EF{Rqe3%~Ep1Vinfpq8n?E0wplC}il&wtbj|BSYhGv0<;of47svCiiu#57e zqksO6&UA-G&?-J^-b<42?j6#HAxL>(FU&w~$Ci+)TT)6Oca8lyzmmKOGgMibW6_}+ zqs^Q?p%0O%1fy46D696e#9U(%X-kst zipr180*e+ZbJhD}H*1cTaWw+}sxRzXOFj{nRqmaPy#~m0bNbaEoCPOlrg2TdmJcm& z#GG0PNurKf+8&Z=D7JJ-_eX`jIRrzhqxVBf#@!cJPc<>>_p5j0ik zW%HZD%^dfJ=V!fcX5W_gvZw$YHz>EwC%4VFU>bLC$(>G`d(c)t!J)U2;`Ds+^(glC zEfl%16amc~rPJ=4O3^cfM(bDYiwWWW#jFxJr%2B19rc3XVw_JxF!5uG>h5xpVqG|X z@Pbxl_x2kbW{r_orls*dn7K8*urbIh>|rtHazQk(QJh;>owg+QMI{DcZ0;3mH|Fic zp=#3`ytYx|7x6Wr-?emt@H((-YKY$@1>}ZnJorK&a2i_-!U_eKAYySQ&kVl zZ?oK-GIKqrjwELN`sR%SE6OTz%1XV`2AVxC-`~7;QTlHV*BtIVZ;b23OY+gK=g;%D zeo&8KDkv;rB(KeKJPQjTL~du1$=*Z00~G z`TT4L+nLqDgHA4>9-U^S9v+2xWbs25wHV^<7%8+h@iw24cFJIQW!9`=DTOA6Cck3O z&VW++QzNZyVhx&F&899MS0-F|V>vS;LAW|cLwhZqm+*~Ujd^(f>hHf2)mM1iooxof zbYdP{kQRw)uE^fMgnF8uQZ)B;vu>enGylmZOpQgcBG%ZGo z9yW$CZjouDGo>sELB_Cra_OyE0t2<|Hg79_-Y!+u4cbu%cW_k~E|~~G4Lhpi(e*7} zKKXjy`Ss1k0woZYATAWSBRYB(SJsG@)f=*iZ$D^#%G>AA`zi7ry_>xI^W>yCCp8{chMuot7c8Pc_LED+<*jILwag-jqa87Kn5|O#g@Xx5;LJ!8nfo} z8@zXhiprRjO%nQRhT6?P-+P=kiTuU!5&VOW9n=ZYcwGWfdHozB-{l zG4x{zAB;uCDnwbh*=+3BI|nC@bJ`=+YP4H+Y{31lb(``fb>RmWO@+N%Tk@jw4&RPB z*(a0~`FOPj2oGNsWnV?<=R&W6wJ`4Y0WV)}_3|g_O=%F5D2jIqdd+?@e5=4hiZV>- z;a-Rk%q?nAXA6si{XlM6GFdMDV;9v1N!$BU3K^Dg-%>e7QCez97-LFLoo69$JI zGZ0h7o>X~wcz!(*hS+wbKPGLEf6o`Nm(O7st)GRe#T6NzP{kCirebmxPAPw6E7`2! zLaJ9CK$TG?`9|W?FANgOSM8KVmDQE<>%VNeDy(3v)*MY4*UJWDMu>ZL5XwG}^nI_`pOE!I^Nm%#kbn8(ylbg*yuXrv% zqN#`9BqzJ|%Ntjn)X}TF#%yqB+9Pr>TWdeqUtg)08usgxvB9^Z2 z?ETEDtEC6iWvm=rI6n}VJd@<-mttnuNHZXzjeTy}bnnMinTxgeRGoPtN(5(#M>(r@ z@&f$tBVTq3OT)4yiyqsI+MiceSdSHovoz2&Cu*1yaaC46H$YnQ?lMvZkV3o}RNr`Q z_z*0s^*j(4dQop(7_y3P5$X9uoxDr4>J7@!p5@dCATG=y;^OjT=J3-+6!axlopvEd zm9Xme(dhF)eRv#mAtIzu4l!pKGO`xf(Vei-g&|)q(V&>xQ*;K~S zo)T7Yl)kY4){(=R2YMKHX4Lp1`dXg{g;N~y+dkfi&0cnge=?)%_Ut^X0|9yQS$}xz zPCkU%fg%$E^uf+nuWfX5gr4&f6LD{#g059I2f3}f^GT~D9JI4e3A+m#$1 zYFMa|GDGfzp}0_O}&&*cxD)DWI4bW51n#@F{&`w|@Mh8_u7+*X{ANpfLw}1-Kr?3C^PIXtZrTt8OlTnGVAIc&=4`7{rb4eMi_*$>m4+C5>uky4TC|;sn+zqbJiq5Bn z@O0grT)pm*xjq|JpKclAI=O16275Bc)5R;*;#Sq#ODetzRZ$Q-en8;z5U7YOp3{fR zx%hVpN#$mc=Q}SQS$#|zXXd2qtO5{>D~(2#-94#Qk<`4Q(c4S2djACDpkS#T90WX2LMG;;611?8FSA-u;)v;xe=Px3 zi{y|re1?uRj)-+;`R(6gV|AGroRDwrw{cwzb})UzKPINcECL6WX*sQV@w3 z;w+>Ix)2K+S@6tNGrYJ8+4x9!91hF4v@SrjZ<r^o>gXWbNs(d3u+z}u(9o@Os0hIat*kJ}^+`GIggKk+P z?k)33hQ8pTWB+qI7g8KCd;PbNy4kz;8-FgH(mJ?O-%^}xI%XrxeID4vI&zw@fPS^V zF1qcyJid(fMAiSNxDO220|gm8yQNcbn4e&)APma%n%X82%AD*upVsGie%@TShzGEP zXo+gEMkCP$Ukk*~3uAOm?fBpE@H8zxg_?TZIFKykUmDn4ZP#zHwOwhEp)({?o8-^8 z=z31kkHd=SgkxdNE87+s)ReT>`%?11T7+&KF28ZB_?s3T7hYT#jpdz9Rmxf z0G`Fth!{9H|NCI3U0D~8am30Fas2|BjkIGVdhdGQu~hKmTW}(GEL7u28||{eGuN~- z@5Mz}>+CQZWBjU33;|s+!jI#XU?XP22 z96G_@=2bgsC5$ij)lbx$D`NyQs;r=PkYnDp-h)z~N!>M5lGPQpfawv8V!NFul@;ta zK`)#6Wow@cMYo9RK2j;`7=GnN5I8#KusF{OE$-UrizrZeSR!=?aH_e-;zN!h%`4Fs z855@)cL1*fSw0>f+)N%hX*yQC(5Ai{bio+pUbsc;h#c3V6?r25KAvV>I8;%Lh7=1wUymx>LZ*b=;Y^` zxUeD$18C82`%D}|7s{)h6m$ZtbSVhG)_dZg|EZZav@k@w{EpjqjvvxNgFCH{F(I;=d3+Ar`&ZETkS5BA}n1MNLoc-=CT5N*!HtbUAYR#nA!!y9XUAe+F0{ABP#aDzkxN-KKq!0mp&A()yv88A@hb9NA|$RD#Mxx1`L-5{Ixw+2EbV zbHU5H=Yro-kfVT%p9RTP-wO0V=;&*B%yuNRZ%s66_|6ZNBF9oD>t=zn&@EvBC$9Mg zMT??GA$pq@jWUOln>571>TT6y^m4ml4!iophtNM3>>8v^W@HotX6A0-ZgWtZ`n} z43x?5-CS1HyjX{tOjCWti|(~LV5}3_vL0%jNe?VW5VEWjUvvbn!A=HFgia1zn`$jn zx%XiWCy-|?d!$-Ov9;ZI4zmSxFJ(q99^&hI92?_SyBzz4VfyN)&4)L(Cb2QpIJLvo zw$_*Yr>b+^)Ism5m1+l(7AHG|kP+Kr#!pz(eh6v_gV3Rep=9P;!-ixFqKf`uW0J-nP_TZkIsX5 zYWJ|qqJ5=HENfwmqD?*P0R3EwkVAART9h2$S?3ZtA=U`=(=Dy&?b0YoV{|OoociHV2wpIRz}m5TBKXWUs<~NY5o4QGDFJ;Up*cuI)%QjKh_)&Pg2;~ zT-B|XOvSR=7u+ONg5VA(_v6gtsb>!udkN%E4xjxwjhdWOy#eX2O87UVk| zNx&8JDGg1}O`K)KZDLmw#IO~EcQkh67c_d3Ba6jWDK;xxt>pqKQCq;SG$pHS=dpS} z=~E5|I=uP=8}M5d`<1jjF~cI^8S|CGbP9ufW;B z3AWp>k3XQ3p^+DJ4Nr#p);TWx{Bc-k{yejm`%4P+AA;OlesrXkJ9lM4GyLw{fR+IA z;};!&#=z&>;uN_a=B*U+?Wp7Ws0Wk481=dX!E0^VH5H@SrEU84=hs}jJ{lU`Kc8pL z|L4<3$+fvy(*%yial@Lp-x>CIxi6UR{JOq#;6|9dxL0lE_K(O;0>>WX zET$GE+-3!kw~8ib4mgD@YK$K@=ILoShc2WptVq*!jT2V#imrmBWUI$>e#CwyrZv5e zO>LQkZco4{`GN+N8s4btNZ|5Ck!b2~`Vw^nvcWIfFvG(YTRr#rwzH^=62$wKvF5=@a+AA+)O7Tpwe*Vi*jj(}nbKU^bg?iRQK)pUCUKM6VWB{pGx}O+ zv1E)srkfcTr6{Ueq@hq7U1+|cw6q>bYwMXxqgp6p_E~C2)|Cx1wr!!JA6N}LC<_ge z+vQ=)TGJiHM=6xAok93Ie!hX}vd0RbME!0r3Yhf!P-0kN_rab=`lbd^FNdk+1ta3S z`j{}oX}VFUt)jkX7}@oa4rvn&v!w_#1uDKVcTPWo47L>d^`$!R+#en(TOXvyUxM1z z$n) z46B#!j=uLCE7ly?58L8&mDX$P{`BAXEPmw|Qdm(tA|W;|CVh_3d6B!jFYXAFs^N6PBrD3D``I1$AQpWwRX^LCZF( zo`p}X{t3No$UAVeVl|Rie-Jg$huS(fx=95Z23C7!QGp?FDzikU>3*SUFKhe|VdvSs zJAOWyEO|pMcUOawiLdF0KED#{jjsPo9KJ8#&~kb*%uZ1{92c@dUD}@Swa8Ha9+CKY zjC%Qb?lO6r=|cW&Sj-am)UF1sgA8H^<3X0ok*{E;^lEY#lyCV{70NMOyMU2ybRi$= zD2HQgeUI%K>}a1SRZEVnKaEg^G9*=o{w>MUxzrG*Uc=-KD#wE`_t0bmKw8u`L z|CiZ?&}{!1`SB(0C%NZF0f?pSfWJ_X8N4YG%x+St@{dyP!JXPWz~3OVx?tlrhEP?O zQQ$fI?ISp>DniZYBM5SSqHnC(^!A8W28-UK0iAy&wf>t(W~RMhd?xnPl!;4wGd01T z^<=%D_15<=Jq|}0)WdC!C1TYNykuYxg7UQQO3}am3AGmXYQHh};U@iQ+Z)$v3m;>o$x>xKG*XYE|+Q0ekwF7#EYPvCQ*amS

H}D*E~WTam0f;9Cd*4hG^i?qQ7Oj4*n zhH)@&hmOX}nytSz5>@FyS@g~avJ63t6?_o;iDsY@aixIKrwW^)l=rI7P;k1wh`}E` zJ*hyJV`rV^vA;g3CQIJ10yuha>+bFzK&gK`bsac*k7O|@?TzcX<(y@rs^cGJg>11D zEOQFFvtTw-{$huT>&@YCx*5AWm}s9vs zB(r|fYu@zyYvAZ>qI*I4THA%LPa3TSyO+tE1a`Eb7poQ)aIJW6;SEDV?lb81VltyLg)dcDhOVR zfR`vJCG?U&K!gwoO;kWZI-!M7L|Q`cg!ZoZ-1mFV`4`T3@(T_}*n6?|+H=k|=bE4I zvvn$EMGM1iF8IaW0JJY-*iy&LRj|gjILoQ%-iL4QJvVINf><@|ScyjLF!pg~{Nx@8 ze#Sju3NO-pKwJu+32a}hVLH}QLYuTXXS~n)qEGD^Quf@*Pf(0^CfPjt8^Whwf=5j= zps{>63cVT9uzg>{=!&jcV~VO=lSXjmyA4&*rfIoXGA(}-&JE#(u2EwP)>J|z)3Cy9 zn*;)0VNWx&N5H@1oinNzhC@76w;ti@7NST8P7eNyZuT0K$4tjKD(I92wlvnP{oOD} z2TnoQ{_YlD+#=t@Z+3+2-XJv632%s~B^^5W&gK35!kIa%^oWJ^PS)%+)U#Y=gMSF0 z=z3NSA0@z}Q+I(7UN>+_#i{l2E^=Osdf4i>Ih1ZgdXb#rENtf;S=YaO9PUt1TxWHI zI2w4&JzcVCJ~8KafyPcY>=%nQ>HO`6qSKm%7LsV;yF_yeb1!7&MxG|R$&yjtRON!@ zlPviaJj8Op_3Xp5&xmkGqvxg(_b@tz63Pnl-IplGdYV+uv%hOb)6NN%LQJ1Hwa5Q- zXk_3=Jr}oY^oE*7>n=`92n6HvlB$`uIzYCG8iwA43vYc46&CKiW!6Y@@_to!n9GK8 z%^NI^S#eeYmSJgi;8;Y@O-+B;{s52Vu(8{2mP~VV6{1Vglb=`$*^9*mOgU{f&)p(@ zXRPzs{56JH&AszCTd5IR1=_TJ)$eaYmzF>qFERC(k?imp6}-{)AXtP0Ui)&*R6nUQ zh>xEkwS}J8ZO?FXe2=j#Z_J4h3G#Gh%^KoY2-5Z^znW6;qj06F?5Xm1)nnP+$X_*T zX81x5Kld)gS-?n=xGlVmWbSb4)MTQcKjxjAAofi8=*vX{Q&BV1oXtzmniR(YFLA;5 z$Mg9tFe}#KN>)a2`K%plCyEEPM~5mI5Fv}PC@;ro=RO3Zv)Qa z8>@yt3Cq(4PJ!`Dm+_u$hPW6I2B3O*D`~Kev>Q$_fvsE!VPfp_uIh!oarA!e=9bA` zss6SIL*)^Ab-)djVgiDyR{=RC2xP|Joy>kPFd_%VJucf<3$F;!MY%y^&k4smdVnR3L*=MQ~^~ zCoW0i1Lce9gk{0A6(-DA_xk=`y$c&q6QVhvr=ZoT}lOcHGhQE==`FV6mjaG&Q=ch+!9&QOW1)m3*J_NHr4@nwnFVjvc9- zi-I2_WDO3-E8%k!Mnade_i5TM1Yqek{%j7+5Ba^npY({**k^$dVZ^UIPNEa-xvA+0 z{?9k1B-P1mIWYMTD#6d$mxkI|8v0u`!k4+J{w)#3qplxNM;(6`W;ap7l?>6>;Qx3lNc1cUtErzrGmTA z(m&%T9^&7ork?dW*Zvy+N;*xIH2rJ4J^%GyWB7J)Dtqw0{_gTK=)$*}QhX&(x}%9a z+ND!YsV5~_57{`{2h#wieOS6s+qCVG|yH!HCLRi}Lezq=aQ@7XO^Zi#% zlw4)sQghKmKG+Sbzz4XLU`CwFUZy4XA~UmP#4ohsV#U)>lwmM-~imP1|Bv;5qM%DbVXr2}g! zyTx_=!>aunod+eZunf1W3)!X+5xAHYTTH#F#5>Hc=i&F8*C+n=C&r>V3of4}KiOBZ zV7A1i!OrBmX$jgOLvC+qTVc@?+(d6T_G1Ir&Wh-Ng)4nA&@Gmk7>XeB@h>W*LBH&) zPt}aa?#5~_&yQq2k6RR~T{>tC5!A}^kRgbShHH=scF?aeX{YR#Uczq%OD#~csSX+E z!eaVTC)!{iGmOGwHir6LM_+XCL^kjyFh>|ZXedKb9~Ir0XL!R34uzhY_%@p)m1=*U z_@R3PZcX}HU5=RM93xfLt1pmKWy&7B-}1BfQ9bH##_JHCBG1~I{vy|QO}Dq$`PA(t z&LeZ*v@eA1{j-409X@=5B(t{~@|o~)VQ*n>$`XyWs~$tyXDxxZH|H70b)-%BDQ9B zW8Jpf^|t-|#!#=TMF!O9O0M)SMJlTc-N2LHp**S~+&9p_UujO3%Z|uq$h-}jV6-%< zwNn0dq`I2mqZ8yRg<Q-#=#^_E6C3NeEaFHo_0&S)>XWsJb3i+gkh_ zxN7c(h7KzP=Wgcofx`6CE~$mM_2ctci}oBO=Q*yf;yHV$L{LO2%Mw~N`g2|Fz~w3f zAu-2vX(RlBd=y;KN00`e&EX3neMq^An7D*cYd}T!o+}%1ZRktE2ff%Du1u!D=d9~>v;tAu_bq? zli2GKob8qmX@9iqv&`)GT<77b6z^wLZVk!CmEuA=G0t1*=Itp0WeH@V zqdltU?neEtS)@J0+=*8?npH7r_L95z=0^upd6s);g~Ze9EfvaDOOIfhKVqPzIv9v) z->YHO?pKs=um-({NJpHG%l&roPcgc1r7Bpzk?@0WQY8}?s9!y%iuJP;py+qDcFV*a z%%Zz)yr!7gF;WO|lOx{HB*rm)a_B;k#(L+O&E}lO5Gr47@*>w=7v52+suI-#XpO4j zVB$;xDYfPCgMtG{FHY{-@pP|a0|U!Wo8|vnZF Yv^9RWmpMUwe&|&*Rnbjf~5+1 zqC`Y{XV#lOf6PE^#LwOzc+s&_va89EOGnyqnHro@vtemmOHRBy9O=~S5uSa;wd~~A z*|f}G9QE5B$6McBcpdKTX_ZP_dlYzf$>IDCC zUrv81IjVoEnHDg{+cKn%J8&`L80$?Dd^-dgzI6H?F?uWOn{3N_*tHLw1$+*};-^iC z!4zGLm-a%^ag2Ja3wsd^@l)y(t)xM^!XUE9%U$sPMcKbbhqb(WHK0lF&QW^@7Mp91 zOhxkuCd{H!l;ry#BdEc^+sjRLey7x+8PKl;^4Z!_(Q|T;et_A+Jt|| z^7NVh#b5p*SlF~YZ~f>f=5sQ8F#*qxsd&MT{!qWK^1A5C1BVQzjoV!_1{vaE{p-q> zoG$_zXEuLAoIS?t+C5f3{OhUy`}Ij3(Wia7pnv3u`SPCBaa63av^!f=CVJJ7LqE&% zqJQuy4bjnvSnP+DL}!noc>*XJ=_cSkhPmFwRfhh21g)I4ffAN^Qq?zZrQV#7-JR$y z+gvD|Q1CQ$2%%QTE6mPqZZ==WV)w0hhU&Dh*X7V01E6#2#XhI*g(=lZcBk7gZq$FP z#G^E_x%<8cSs<5)w5OEcRH(W9#kZVAw43uxMG$L0RxIU+0DFl>EscR18(AzvktjB$ z#(u?0PMy&wwH4I74V5+kPF&x5o@Ze9agB47Cuoj z$353iY4wMPn0vRgG%js4IM;QIAZ;{!7n;mbw0V+hv+m6+>)DfL{w?~3r2)DqD5Z_bg3HW|jA+Nx6({4|(x@7WlvRuuM%tzUc zQNDNHMVKQ+Uba?li1g!Fn4^_hkd+}d>V<2-3G07|+czWMS6Ry#;Yr49C7lwDxbsKrUc}$F z^HR1m@kxG>@S}EfgSYB%`B&$wX5TT3jnd7GBqNX27deQb$S0LgzOigovplr~J>m5C z(T@LU=alxc9pCc~Z`&j=Ny}(ye8$)|=k5mSg!Gl>tA?>?D2K>5=Dr2zSOj|{CB>@9 zW25zbWpEQKbhYgWRL(2DJ^diDec%06dxk>O^u3QBvLfNmvXNV=vSJQ#;hhGxE10_8 z@)zu!cb7zk%1QziaS9tUpSY1G6*Zk2jgVtg_2tH&Qrq5th99JO4zN7ipkFe}+ru}91^&(%x?2+Kxi&IINa;){A#srQI@khz5vbW|9O%Z$N9dNz#t-ZbT!x2YG zG8`Qdym^D~gSD&Wsyd|amB-954}AOR9?*2BvD2w%K4n~`TBGWL=gE>(1MR_@sjm3} z>j(PkOqL@X$=keAgc$xqdB29)6@qQf40$7ba5UhMHv-SI==S*X1FBMwS5W66(W^=UB@5!m_eiE!1uoJ|`x(=HCBT2tLT8SkY3<{a*N)>jWVJ)Wl z)_S(Xpf$noC1F}H1%HYj+BYkQtgjhy_^bLEqAncM&Cct{wVt-JC1K~ zAZBCnqZ4m^)bGs@Su@&tMV9uHnFj4S;dhLUmH5b;O8GahL^#im*S4wZSLq!}HQ*ob zdQlBI4%PjnUOHvZ2&ssPF@+GQ6AR;nAk``j|MZh#vSxe8bpvmsh0NE-H~D!5WN&Wo z?9?m`{(9+9O$`b!4Ba@m%}*{3{4_@0Y|xN6EHl&QJ-w$<8|czajhpRl3uo1tt`$#= z>TKz(4DJ3pE?Y57e%j;8$?%Gw?i{=NF9emq$NH>MkREG+D)>QT5clyN{iXI6>#c+%?`F$hW_xz&SL+?BtJdiq zsxB7lC2+n^->+9h432x&>y@sxrz)@vtPlSJ5UOO{OGN!qQlFs4lj1`GMy(s5$NcZY z7Czc(rM+y~buxMEjD7-uZwKrha^fF9USHS{C+Prm|te94XCS|t{A5NvlMb@j0%R3jpu8VmsK zrgrrHkf&T~iW=R0{e34QXI9)xidU|lRwEJv8h55Vrb70moc4!j#pyRb_cx4C3KC&* zHSHI9B~-c>l44Y1(;L3;UFCee9Oom zI%TiHUB4{SdyS6_CmlyoBgc6;(&|CK{oCQ)L5yNX`D^*2>=&S$m`jy@sZ6)!^=E!z zzYCg!I#f!Fr36O@Dn0M>s0%PKWDWuth!$gXl&g||Ow^1`@<64nlffW&b0C1X)Bo~J z@Mi%p6m})Z^R`<5>&k|T8nSg~x;qqqlRnm`^f;1>9gLASUcL8jAF;u2Lb7fS%zk=T z(En3jUfxuZ!1;ML1}!sf`e;FCwD+uK_lM5Bfxb$4rx!yCgrV%GTwXn8tbmEIk|J&ViCv0R?GgmK)BBi|Qg$_1?2Txo=&LBu`05OA?8H8P{P=$9 zgSwH)8&1u7COA?tpx6Q0YWu7U!;{SZBlOiUl0E(ea!L)(oF!|vZ4Kw|`gXIwBwmPR{#4`HrtS*&-jR*aHYvPbhA*qraQa?697AJ_fusoEu|N96!8tx07f6RR_t zheIXR(|oBnoD9~l=T-r()A+i+?69=kllM})Dw%k{b2mUbgEZn*cN#}eKg__ED8#d&bKbIC@}u>iw?WP1D2A^ z&hGb@XBPl8ETAPFpvx9<{wM;HO6aN%Sb%_PTp9qJJy?`qzrV!!CEg6q_+%BZvg0eQu^}_!0TCQFG%C@lljT3*>FzE^&Jv#7Y zcYQ8##J~ocIbMi+sAo46ozXz{&Bq4gOPz}y_|=d8;Q60k_+b$3Rpuw5qvVM1`r(>=qiH^tYwl9-J47-tB zOmK*ddN@ggFXdWl{q`3HsWdoA$0`144oNaI%O@O-`2Id)o51@;I~L7e5Qkao3GAE4)eOEO~fk$G$FZC6cu!- zt$vVL>CVko-p+4-e8ZVEZB&Vn%sSg34Bj}i@F-V*Ytl%hE5IC<&x`J`qagKpuAL~) zf8si(Vw>Me+f`k!2HzA@qA&NI6R4C5tz9v_;Ba#$=I@|xR=xJmW_PZh=HZ^7?-(dT zv|Bx7&BQF{dX3dDZUb_wh^SGQ2Z7Q9NNVOh^jExt1NFBGpCJ|m`fHy?5?Vd0CLdO_seG_KnMmSN1F-Me$8WgQ>CEclh{J=Q z2-IQ+MS!&|`za>L%!pB#4hN4Eee?A=N-RD=f4SH!Cx)ucmCs*;DGFO2`Q#X7AGSdp zYJU4F$ynT>K6EuO?s7Yt`JC1weS!WF4T2pxval8b@P$7StSDe5eB4SGxU}RCESz*Q zf+^xPtF4j?nNb|RGs}s*0^VO31G@Ej^yB3~^%}s3ZaW?|;ks3GDG0VBt6sogmHh^- z%q6E9R@Gzy&Hz076C&}GP7$Uc<962LRTq^MPjs)(^^!-}83JDa=jj7ZEM>F%!#$I8 z?1=c4R`-E~b6XO!(IU_&YS=s$NeMyU-+wPerW?C$=2d(}R0 zhy*yghRc0%6S|!0eas-CYF%XAF4YPCLA;iDaxUaqX`67+__IF&4AnXPM^@itjH6Za zhg-(<-=_hn!6e{6&k{CX3n?@+fp&C$r*sZ zyDolf?b7Vd@*j=g32U--)Mjk>As|JU&(F|Nq=dkw#r`SKfJ%pNzdS2Su$}xSx1|GM zc8TynP9e2HEI(iV0H2C~A?RaoZ;sFE|rjO}U&iD>sqY>VqlMIoh6<<~qgzsvo zYk%+gvt%=(Ut5xtAE~5fQ3bCt3t0Z2=Zijioj_?(=nNP0bKr$PCaiY2k`Pffpb7mTAlHB^$53{I5J4dvcRd>oiw?KeYM00tImAAzwc^#mO2 zfWQ#dz(o%L$povG{$v@nFxg;a<58}j`sJPR!kQYv_v0Ha7Hijx1MSe}c_TeykW`Dr zGwQ&ud+q`R-rL>!!-L%tXi0<=vVnLnB)Is_ONLxJ(Bpy?-6ET|{TVECDnG|;9>@5z zy+t@;zS(J7uLaYz{@&(6*@?3vRR})U?6rImRIGspEpAXaHbq6%+qT!Ttvi-Ek zRdaZu2zSrxwoy z3&XgD^gvMkd+U`VcmPn#uRS}umnXiJFNCxR23FnKTQ$^m*n5)<22!)3$2fl;-2F#; zs$!>0Cn_CFlA`A`^n8}V_y=#a2sp)ZOZhgy*{Kp%m@1?~@-^Gv(>BZSfD-$f{(Pg< zBxgY*DTn-_vHdfT*Je8LXN{FE?$mg8Q9gLrq1S$WxbcnU zik*=<#j|Osc=k?e=70tWbP)g%?RiOCuxK2!!>3kvtL_DGqM~@lCP5s4CPyCyR!N%< zoCFL1$LIFaXzYao>n?t;+U|N9MN)O@oy9yr%HjbgcQBHf&2HAh&>e(23*XP0RGhwS zP<7oF5cvmE&_DMpO5?kWS6Is-9rL+~#-D*t8S-fnOmOZV)+!MqD;n8}E!PsNSCaII?r5K`rxdiT@JH^Dk#Fui?B%}hWIn85zJ zkgZvlTiT0&QRtTd+F8tbt`VsP00qCjcCaqL13k&WB?OG{G5R@N7!h-odY9<>Uc$;3X%4R9enWUTcxw?s zZ=<1WAN-`5JzEAEB>@}RIv6oM%zMiUm&!;Nz}2>kMOSzX-E0|0uf;+nU$FB}3js&E zP|{Z1924Vy4efc`(74e;6MU0WIk&N)0k#&;)S{yb3Rk<*Etzjw8KgY1p48e)hb)w` z761_cVnN}V-a*d&l2)S*pxXc+{FH@r?`_6dV<{3wRwV>O4C`mRYTuGa8%ph5=*m1Vks8Pw}_<#pW?vcWeJOX+iCHzGMpw01v_v!DKbcZTO*GuR^ zW^`Ab-nSg@CEqiGWR5?X{^&~Bw-iS+o0HZpcF}GPbG-#MCphIoPwzi5?u}VZUOyf49M>Y!p za_=XV60UhpDj|D(uBMuDp1fm)zpNQoLtqpSqQqksQ)o`-=)_h%=3pb{J1<#G;#|#Q z3k!U1>c*V(+1;%vComPSF^UNU1TOcDoj_jfD@K{|QS)ck1Fx!ezuE$%Tf$6On^m}5 zrHP=dTUX|+g=+2==BMjw+t@tQs%|0^&aL~B@;&A+!uU0~)P-mPAfq8Yf<6g+H0}ZD zRc2O-*0H*sa>9iCq#|L{CYi6HIhn8pA~@C{WStqFfB%u{cwZuuH zdiyYXptm@iVxRSZ-!a8hzZ3qsT&&#`<3k}dTAxb!32m8@gW{h@6^NvpEwQ(+3ofwS zTfPi>_BZ{$o$^oXx9r>AIdk>yV>T3P9$hko9`bm4 z)FsspyJm!q3gqAj#oa!* zm@L)%dOmyNA~>cNh@Az)Qw@Y;K(|76t}b`N!+4Y-5PJLK2~5Nyj62h>7IQNry#EP zJXS>3Leaiq-BK5A{Vp>auP#HH9^22j41B~X-2wEoMlJV7hy#vEa3T( z1ztWj%YE!8%;2?=1%yy|2Hhd?vlEnm1GM#T5c5-$=2r9*x5S?Yr)WQr?S%8*RA>1F z|84_k6u%d^5|(_E{BkSsK>yrBe`Ut~#hRqKf@T9W+u)FEM!IuDt=k>$b_U-3H3L!S zuIi1^Y-;cku{~C>QA*ySM>8#8R|>2!B>QIw*ld%jlD8>kghtw@!W${U(xurl`}C($ zqhezWTwt0<;~Gf4a4h_LNA?*t4RYD?I4U~9C1k5Da*A6GiTII|ppjCG_5oRrs&VwE zLgycPvOnQFWeSMlsw6Yw&+u$b^1C1q@7Sf!mI@L-@+Drww88rdCxn>(aR>zw^LO;4 zHAB{48Jc4q!20n})~9A@)Y3pF-M+k*V`J327+MPH#PlM7@ z880BWWQbzY>NdR}=?Zpdwrow`aBV^_CZx;L( zJ$%X18N<#`81qZ|*tq`hXNguSG&s(NsM^F8y3!_Sr!%c8OKSm(c&Z;3!P&RvqU!u)|SggO^I#ISaNBx!8HkrD|b zi4aI66KuEWc}Nh8PBpK*By3Y~6=O>$z_O*;N0bAO zuu1LTwWXRMBC?!#p^#Jz(bP|5MN^A8QIPVEz*`o{cF4=-RhC=#!+gvo3y=IRG|K_R zfBWs_OAD|@%RoC^a1464*d>z7=x%NjGjk`1|Em!iL9pWYk;M?VnT~WS#Ew3t;z6{R z;d?z-%pgL*XI;vg-S*oL>%Go^OM&sSF&iVp`VJrp-;9tB<v94%oXU+xgy^y~gk=@++4bQP4UfFdLgRx;oWHUOVu*|L)*H z0Emh=2D|2SSWy%LKLr|<9mstBVnHFfaohGYoaAhVoL zOta8bvIy5I+;>_JPINbC@4og%HBm&n=jbhPdi%A8)dh&P_E{cxBeS@cBcImjz+Ln! z2-`Lj*R+Dyr~vR8WL(4MFgzzUu_9&$gfg>2yC3}nx#KQ|0cozBRJMCqPMO-}$Pj@C z7Mx#%=~V4p3HnYAY-j_p&w&Drv9DA+NEwjydF&s9>VdC*?_MX^-Xm(Zf5(#bw0nSD z8_#CN^QTER-91dh4Is*2w1XIS2a8rKrn-7PNDz^Cm)pu`Lm{8F$Rqwj z>IDP+4e*1#9Zx}ZtR{xr7MoRYf%V}Gog4l)y%cL(?lN>oH2n`m4x}p=B2x)OpqKz*=SttD8Awm1SY?eWR7ea-dZBH;-&y6M&!&9`KzhrazD*6!(k*5-ilK1Sjy+HzVv0x^u zAi|hymFxUDwPWmje=2vzmFe1!U>i%ym&UXDn&Um~73bandhEjKhz{?n-req9^W}AW zmmHvbful%J#ZQ`KXsCnzbV(J+OP7c2^%%-jZ_PNEV6pEdZ7M;su{smH+90b6IycVK zA@hI75e4BW1r;1c4hwB_kZ}ZZe}UwO@&{`by~chd@E@$6kYwXVBiU~fCqbg=J917C zByOmikh)+bkMus(?OI;5EylSoVfUA0(5vR82LVIQCHhtgR}03KJx7!jOV&3@UV)`q zi#UyYv>OFjC_dm&u#adh1@t1&Gaw_&NH1;3BZlQdVGIpS|9xnJ_kkYRXXl$Td=}_D zg8;ordHGHA>5gD*-~8m?ywm^843Mv*A9h)zuaVYgjrHA=R;>bI{_9gf`wRdYfI8}~IkLPnc4G$N%e#EMB!k100&4U&rl*-mN@k+odymlz}S%+>aq3 z(G?Xk+G=8tIz2GJy$Whh{LB;P{AaY6grG`G;BL;wx?MQ>9|zzU7?HqM;KnI zfyrlx?gOd7MTo}#y&oui|3jw$g3l>B;2(T0fWBk;PpxA5Z9ygZK7AE~PZ>Q04nB|m zBOLzyzrx}Fdx8I*M6fRYcOm_M=D@+|TsoMQ**kjLmjC~Mv1kzp4#{bzsZV2c6LCl1 LSg%CqVdVb-0I}l< literal 0 HcmV?d00001 diff --git a/tutorials/underwater_vehicles.md b/tutorials/underwater_vehicles.md new file mode 100644 index 0000000000..341c6c9017 --- /dev/null +++ b/tutorials/underwater_vehicles.md @@ -0,0 +1,179 @@ +# Simulating Autnomous Underwater Vehicles + +Ignition now supports basic simulation of underwater vehicles. +This capability is based on the equations described in Fossen's ["Guidance and +Control of Ocean Vehicles"](https://www.wiley.com/en-sg/Guidance+and+Control+of+Ocean+Vehicles-p-9780471941132). +This tutorial will guide you through the steps you +need to setup simulation of an underwater vehicle. In this tutorial, we will +guide you through the setup of the [MBARI Tethys](https://app.ignitionrobotics.org/accurrent/fuel/models/MBARI%20Tethys%20LRAUV). +One can find the final sdf file for this tutorial in the +`examples/worlds/auv_controls.sdf` file. + +# Understanding Hydrodynamic Forces +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) + +# Setting up the buoyancy plugin +The buoyancy plugin in ignition uses the collision mesh to calculate the volume +of the vehicle. Additionally, it needs to know the density of the fluid in which +it is moving. By default this is set to 1000kgm^-3. However, in real life this +may vary depending on many factors like depth, salinity of water etc. To add +the buoyancy plugin all one needs to do is add the following under the `` +tag: +```xml + + 1000 + +``` +# Setting up the thrusters +We need the vehicle to move, so we will be adding the `Thruster` plugin. The +thruster plugin takes in a force and applies it along with calculating the desired +rpm. Under the `` or `` tag add the following: +```xml + + tethys + propeller_joint + 0.004422 + 1000 + 0.2 + +``` +Now if we were to publish to `/model/tethys/joint/propeller_joint/cmd_pos` +``` +ign topic -t /model/tethys/joint/propeller_joint/cmd_pos \ + -m ignition.msgs.Double -p 'data: -31' +``` +we should see the model move. The thrusters are governed by the equation on +page 246 of Fossen's book. In particular it relates force to rpm as follows: +`Thrust = fluid_density * RPM^2 * thrust_constant * propeller blade size^4`. +The plugin takes in commands in newtons. So if you have a different thrust +curve you can still use the plugin with some type of adapter script. The thrust +constant is normally determined by individual manufacturers. In this case we are +using the Tethys's thrust coefficient. you may also build a test rig to measure +your thruster's thrust coefficient. + +# Adding Hydrodynamic behaviour +You may notice that the robot now keeps getting faster and faster. This is +because there is no drag to oppose the thruster's force. We will be using +Fossen's equations which describe the motion of a craft through the water for +this. For better understanding of the parameters here, I would refer you to +his book. Usually these parameters can be found via fluid simulation programs or +experimental tests in a water tub. +```xml + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + +``` + +# Control surfaces +Just like aeroplanes, an underwater vehicle may also use fins for stability and +control. Fortunately, Ignition already has a version of the LiftDrag plugin. In +this tutorial, we will simply add two liftdrag plugins to the rudder and +elevator of MBARI's Tethys. For more info about the liftdrag plugin inluding +what the parameters mean you may look +at [this gazebo classic tutorial](http://gazebosim.org/tutorials?tut=aerodynamics&cat=physics). +Essentially when we tilt the fins, we should experience a lift force which +will cause the vehicle to experience a torque and the vehicle should start +turning when we move. + +```xml + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + +``` +The number in this case were kindly provided by MBARI for the Tethys. +We also need to be able to control the position of the thruster fins so we will +use the joint controller plugin. +```xml + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + +``` +We should now be able to wiggle the fins using the following command: +``` +ign topic -t /model/tethys/joint/vertical_fins_joint/0/cmd_pos \ + -m ignition.msgs.Double -p 'data: -0.17' +``` + +# Testing the system out + +To control the rudder of the craft run the following +``` +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 \ +-m ignition.msgs.Double -p 'data: -31' +``` +The vehicle should move in a circle.