ThrusterPlugin.cc

Go to the documentation of this file.

// Copyright (c) 2016 The UUV Simulator Authors.
// All rights reserved.
//
// 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 <boost/algorithm/string.hpp>
#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>

#include <limits>

#include <gazebo/gazebo.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/physics/Link.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo/physics/PhysicsEngine.hh>
#include <gazebo/physics/World.hh>
#include <gazebo/transport/TransportTypes.hh>
#include <sdf/sdf.hh>

#include <math.h>

#include <uuv_gazebo_plugins/ThrusterPlugin.hh>
#include <uuv_gazebo_plugins/Def.hh>


GZ_REGISTER_MODEL_PLUGIN(gazebo::ThrusterPlugin)

namespace gazebo {

ThrusterPlugin::ThrusterPlugin() : thrustForce(0),
  inputCommand(0),
  clampMin(std::numeric_limits<double>::lowest()),
  clampMax(std::numeric_limits<double>::max()),
  thrustMin(std::numeric_limits<double>::lowest()),
  thrustMax(std::numeric_limits<double>::max()),
  gain(1.0),
  isOn(true),
  thrustEfficiency(1.0),
  propellerEfficiency(1.0),
  thrusterID(-1)
{
}

ThrusterPlugin::~ThrusterPlugin()
{
  if (this->updateConnection)
  {
#if GAZEBO_MAJOR_VERSION >= 8
    this->updateConnection.reset();
#else
    event::Events::DisconnectWorldUpdateBegin(this->updateConnection);
#endif
  }
}

void ThrusterPlugin::Load(physics::ModelPtr _model,
                          sdf::ElementPtr _sdf)
{
  GZ_ASSERT(_model != NULL, "Invalid model pointer");

  // Initializing the transport node
  this->node = transport::NodePtr(new transport::Node());
#if GAZEBO_MAJOR_VERSION >= 8
  this->node->Init(_model->GetWorld()->Name());
#else
  this->node->Init(_model->GetWorld()->GetName());
#endif

  // Retrieve the link name on which the thrust will be applied
  GZ_ASSERT(_sdf->HasElement("linkName"), "Could not find linkName.");
  std::string linkName = _sdf->Get<std::string>("linkName");
  this->thrusterLink = _model->GetLink(linkName);
  GZ_ASSERT(this->thrusterLink, "thruster link is invalid");

  // Reading thruster ID
  GZ_ASSERT(_sdf->HasElement("thrusterID"), "Thruster ID was not provided");
  this->thrusterID = _sdf->Get<int>("thrusterID");

  // Thruster dynamics configuration:
  GZ_ASSERT(_sdf->HasElement("dynamics"), "Could not find dynamics.");
  this->thrusterDynamics.reset(
        DynamicsFactory::GetInstance().CreateDynamics(
          _sdf->GetElement("dynamics")));

  // Thrust conversion function
  GZ_ASSERT(_sdf->HasElement("conversion"), "Could not find dynamics");
  this->conversionFunction.reset(
        ConversionFunctionFactory::GetInstance().CreateConversionFunction(
          _sdf->GetElement("conversion")));

  // Optional paramters:
  // Rotor joint, used for visualization if available.
  if (_sdf->HasElement("jointName"))
    this->joint = _model->GetJoint(_sdf->Get<std::string>("jointName"));

  // Clamping interval
  if (_sdf->HasElement("clampMin"))
    this->clampMin = _sdf->Get<double>("clampMin");

  if (_sdf->HasElement("clampMax"))
    this->clampMax = _sdf->Get<double>("clampMax");

  if (this->clampMin >= this->clampMax)
  {
    gzmsg << "clampMax must be greater than clampMin, returning to default values..." << std::endl;
    this->clampMin = std::numeric_limits<double>::lowest();
    this->clampMax = std::numeric_limits<double>::max();
  }

  // Thrust force interval
  if (_sdf->HasElement("thrustMin"))
    this->thrustMin = _sdf->Get<double>("thrustMin");

  if (_sdf->HasElement("thrustMax"))
    this->thrustMax = _sdf->Get<double>("thrustMax");

  if (this->thrustMin >= this->thrustMax)
  {
    gzmsg << "thrustMax must be greater than thrustMin, returning to default values..." << std::endl;
    this->thrustMin = std::numeric_limits<double>::lowest();
    this->thrustMax = std::numeric_limits<double>::max();
  }

  // Gain (1.0 by default)
  if (_sdf->HasElement("gain"))
    this->gain = _sdf->Get<double>("gain");

  if (_sdf->HasElement("thrust_efficiency"))
  {
    this->thrustEfficiency = _sdf->Get<double>("thrust_efficiency");
    if (this->thrustEfficiency < 0.0 || this->thrustEfficiency > 1.0)
    {
      gzmsg << "Invalid thrust efficiency factor, setting it to 100%"
        << std::endl;
      this->thrustEfficiency = 1.0;
    }
  }

  if (_sdf->HasElement("propeller_efficiency"))
  {
    this->propellerEfficiency = _sdf->Get<double>("propeller_efficiency");
    if (this->propellerEfficiency < 0.0 || this->propellerEfficiency > 1.0)
    {
      gzmsg <<
        "Invalid propeller dynamics efficiency factor, setting it to 100%"
        << std::endl;
      this->propellerEfficiency = 1.0;
    }
  }
  // Root string for topics
  std::stringstream strs;
  strs << "/" << _model->GetName() << "/thrusters/" << this->thrusterID << "/";
  this->topicPrefix = strs.str();

  // Advertise the thrust topic
  this->thrustTopicPublisher =
      this->node->Advertise<msgs::Vector3d>(this->topicPrefix + "thrust");

  // Subscribe to the input signal topic

  this->commandSubscriber =
    this->node->Subscribe(this->topicPrefix + "input",
        &ThrusterPlugin::UpdateInput,
        this);

  // Connect the update event
  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
        boost::bind(&ThrusterPlugin::Update,
                    this, _1));
#if GAZEBO_MAJOR_VERSION >= 8
  this->thrusterAxis = this->joint->WorldPose().Rot().RotateVectorReverse(this->joint->GlobalAxis(0));
#else
  this->thrusterAxis = this->joint->GetWorldPose().rot.Ign().RotateVectorReverse(this->joint->GetGlobalAxis(0).Ign());
#endif
}

void ThrusterPlugin::Init()
{
}

void ThrusterPlugin::Reset()
{
    this->thrusterDynamics->Reset();
}

void ThrusterPlugin::Update(const common::UpdateInfo &_info)
{
  GZ_ASSERT(!std::isnan(this->inputCommand),
            "nan in this->inputCommand");

  double dynamicsInput;
  double dynamicState;
  // Test if the thruster has been turned off
  if (this->isOn)
  {
    double clamped = this->inputCommand;
    clamped = std::min(clamped, this->clampMax);
    clamped = std::max(clamped, this->clampMin);

    dynamicsInput = this->gain*clamped;
  }
  else
  {
    // In case the thruster is turned off in runtime, the dynamic state
    // will converge to zero
    dynamicsInput = 0.0;
  }
  dynamicState = this->propellerEfficiency *
    this->thrusterDynamics->update(dynamicsInput, _info.simTime.Double());

  GZ_ASSERT(!std::isnan(dynamicState), "Invalid dynamic state");
  // Multiply the output force magnitude with the efficiency
  this->thrustForce = this->thrustEfficiency *
    this->conversionFunction->convert(dynamicState);
  GZ_ASSERT(!std::isnan(this->thrustForce), "Invalid thrust force");

  // Use the thrust force limits
  this->thrustForce = std::max(this->thrustForce, this->thrustMin);
  this->thrustForce = std::min(this->thrustForce, this->thrustMax);

  this->thrustForceStamp = _info.simTime;
  ignition::math::Vector3d force(this->thrustForce*this->thrusterAxis);

  this->thrusterLink->AddRelativeForce(force);

  if (this->joint)
  {
    // Let joint rotate with correct angular velocity.
    this->joint->SetVelocity(0, dynamicState);
  }

  // Publish thrust:
  msgs::Vector3d thrustMsg;
  msgs::Set(&thrustMsg, ignition::math::Vector3d(this->thrustForce, 0., 0.));
  this->thrustTopicPublisher->Publish(thrustMsg);
}

void ThrusterPlugin::UpdateInput(ConstDoublePtr &_msg)
{
  this->inputCommand = _msg->value();
}
}