diff --git a/lrauv_ignition_plugins/src/ThrusterPlugin.cc b/lrauv_ignition_plugins/src/ThrusterPlugin.cc index 7f259a4e..8de583bd 100644 --- a/lrauv_ignition_plugins/src/ThrusterPlugin.cc +++ b/lrauv_ignition_plugins/src/ThrusterPlugin.cc @@ -14,60 +14,98 @@ * limitations under the License. * */ - +#include #include +#include -#include -#include -#include -#include -#include -#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/JointVelocityCmd.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 "ThrusterPlugin.hh" -namespace tethys_thrusters -{ -class ThrusterPrivateData +using namespace tethys; + +class tethys::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 Joint entity + public: ignition::gazebo::Entity jointEntity; + + /// \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; - public: double thrustCoefficient; + /// \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; - public: double propellerDiameter; + /// \brief Diameter of propeller in m, default: 0.02 + public: double propellerDiameter = 0.02; + + /// \brief Velocity Control mode - this disables the PID and directly + /// bypasses the physics engine by writing the angular velocity to + /// the joint. + /// default: false + public: bool velocityControl = false; + /// \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); }; -ThrusterPlugin::ThrusterPlugin() - : dataPtr(std::make_unique()) +///////////////////////////////////////////////// +Thruster::Thruster() +{ + this->dataPtr = std::make_unique(); +} + +///////////////////////////////////////////////// +Thruster::~Thruster() { } -void ThrusterPlugin::Configure( +///////////////////////////////////////////////// +void Thruster::Configure( const ignition::gazebo::Entity &_entity, const std::shared_ptr &_sdf, ignition::gazebo::EntityComponentManager &_ecm, @@ -89,7 +127,7 @@ void ThrusterPlugin::Configure( auto jointName = _sdf->Get("joint_name"); // Get thrust coefficient - if(!_sdf->HasElement("thrust_coefficient")) + if (!_sdf->HasElement("thrust_coefficient")) { ignerr << "Failed to get thrust_coefficient" << "\n"; return; @@ -97,33 +135,44 @@ void ThrusterPlugin::Configure( this->dataPtr->thrustCoefficient = _sdf->Get("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("propeller_diameter"); // Get fluid density, default to water otherwise - if(_sdf->HasElement("fluid_density")) + if (_sdf->HasElement("fluid_density")) { this->dataPtr->fluidDensity = _sdf->Get("fluid_density"); } igndbg << "Setting fluid density to: " << this->dataPtr->fluidDensity << "\n"; + if (_sdf->HasElement("velocity_control")) + { + this->dataPtr->velocityControl = _sdf->Get("velocity_control"); + } + igndbg << "Setting velocity control to: " << this->dataPtr->velocityControl << "\n"; + // Create model object, to access convenient functions auto model = ignition::gazebo::Model(_entity); - auto jointEntity = model.JointByName(_ecm, jointName); + this->dataPtr->jointEntity = model.JointByName(_ecm, jointName); auto childLink = - _ecm.Component(jointEntity); + _ecm.Component( + this->dataPtr->jointEntity); this->dataPtr->jointAxis = - _ecm.Component(jointEntity) + _ecm.Component( + this->dataPtr->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->node.Subscribe( + thrusterTopic, + &ThrusterPrivateData::OnCmdThrust, this->dataPtr.get()); // Get link entity @@ -167,9 +216,18 @@ void ThrusterPlugin::Configure( d = _sdf->Get("d_gain"); } - this->dataPtr->rpmController.Init(p, i, d, iMax, iMin, cmdMax, cmdMin, cmdOffset); + 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); @@ -177,21 +235,23 @@ void ThrusterPrivateData::OnCmdThrust(const ignition::msgs::Double &_msg) this->cmdMin, this->cmdMax); } -double ThrusterPrivateData::ThrustToAngularVec(double thrust) +///////////////////////////////////////////////// +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 / + _thrust / (this->fluidDensity * this->thrustCoefficient * pow(this->propellerDiameter, 4)))); - propAngularVelocity *= (thrust > 0) ? 1: -1; + propAngularVelocity *= (_thrust > 0) ? 1: -1; return propAngularVelocity; } -void ThrusterPlugin::PreUpdate( +///////////////////////////////////////////////// +void Thruster::PreUpdate( const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) { @@ -202,30 +262,53 @@ void ThrusterPlugin::PreUpdate( auto pose = worldPose(this->dataPtr->linkEntity, _ecm); - //TODO: add logic for custom coordinate frame - auto unitVector = pose.Rot().RotateVector(this->dataPtr->jointAxis.Normalize()); + // TODO(arjo129): add logic for custom coordinate frame + auto unitVector = pose.Rot().RotateVector( + this->dataPtr->jointAxis.Normalize()); - std::lock_guard lock(this->dataPtr->mtx); - // Thrust is proprtional to the Rotation Rate squared + 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 desired = sqrt( - abs(this->dataPtr->thrust / - (this->dataPtr->fluidDensity - * this->dataPtr->thrustCoefficient * pow(this->dataPtr->propellerDiameter, 4)))); - - desired *= (this->dataPtr->thrust > 0) ? 1: -1; + auto desiredPropellerAngVel = + this->dataPtr->ThrustToAngularVec(desiredThrust); + + if(this->dataPtr->velocityControl) + { + auto velocityComp = + _ecm.Component( + this->dataPtr->jointEntity); + if (velocityComp == nullptr) + { + _ecm.CreateComponent(this->dataPtr->jointEntity, + ignition::gazebo::components::JointVelocityCmd( + {desiredPropellerAngVel})); + } + else + { + velocityComp->Data()[0] = desiredPropellerAngVel; + } + } + auto currentAngular = (link.WorldAngularVelocity(_ecm))->Dot(unitVector); - auto angularError = currentAngular - desired; + 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); + link.AddWorldWrench( + _ecm, + unitVector * this->dataPtr->thrust, + unitVector * torque); } -} //end namespace tethys thrusters IGNITION_ADD_PLUGIN( - tethys_thrusters::ThrusterPlugin, + tethys::Thruster, ignition::gazebo::System, - tethys_thrusters::ThrusterPlugin::ISystemConfigure, - tethys_thrusters::ThrusterPlugin::ISystemPreUpdate) + tethys::Thruster::ISystemConfigure, + tethys::Thruster::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(Thruster, "tethys::Thruster") diff --git a/lrauv_ignition_plugins/src/ThrusterPlugin.hh b/lrauv_ignition_plugins/src/ThrusterPlugin.hh index 741d22a8..2f69a375 100644 --- a/lrauv_ignition_plugins/src/ThrusterPlugin.hh +++ b/lrauv_ignition_plugins/src/ThrusterPlugin.hh @@ -17,16 +17,18 @@ #include -namespace tethys_thrusters +namespace tethys { class ThrusterPrivateData; - class ThrusterPlugin: + class Thruster: public ignition::gazebo::System, public ignition::gazebo::ISystemConfigure, public ignition::gazebo::ISystemPreUpdate { - public: ThrusterPlugin(); + public: Thruster(); + + public: ~Thruster(); public: void Configure( const ignition::gazebo::Entity &_entity, diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index 16393675..283fe947 100644 --- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf +++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf @@ -120,13 +120,14 @@ + filename="ThrusterPlugin" + name="tethys::Thruster"> tethys propeller_joint 0.004422 1000 0.2 + true