HydrodynamicModel.hh

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.


#ifndef __UUV_GAZEBO_HYDRO_MODEL_HH__
#define __UUV_GAZEBO_HYDRO_MODEL_HH__

#include <gazebo/gazebo.hh>
#include <gazebo/physics/Link.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo/physics/Collision.hh>
#include <gazebo/physics/Shape.hh>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

#include <string>
#include <vector>
#include <map>

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


namespace gazebo
{
class HydrodynamicModel : public BuoyantObject
{
  protected: HydrodynamicModel(sdf::ElementPtr _sdf, physics::LinkPtr _link);

  public: virtual std::string GetType() = 0;

  public: virtual void ApplyHydrodynamicForces(
    double time, const ignition::math::Vector3d &_flowVelWorld) = 0;

  public: virtual void Print(std::string _paramName,
    std::string _message = std::string()) = 0;

  public: virtual bool GetParam(std::string _tag,
    std::vector<double>& _output) = 0;

  public: virtual bool GetParam(std::string _tag,
    double& _output) = 0;

  public: virtual bool SetParam(std::string _tag, double _input) = 0;

  protected: void ComputeAcc(Eigen::Vector6d _velRel,
                            double _time,
                            double _alpha = 0.3);

  protected: bool CheckParams(sdf::ElementPtr _sdf);

  protected: ignition::math::Vector3d ToNED(ignition::math::Vector3d _vec);

  protected: ignition::math::Vector3d FromNED(ignition::math::Vector3d _vec);

  protected: Eigen::Vector6d filteredAcc;

  protected: double lastTime;

  protected: Eigen::Vector6d lastVelRel;

  protected: std::vector<std::string> params;

  protected: double Re;

  protected: double temperature;
};

typedef boost::shared_ptr<HydrodynamicModel> HydrodynamicModelPtr;

typedef HydrodynamicModel* (*HydrodynamicModelCreator)(sdf::ElementPtr, \
                                                       physics::LinkPtr);

class HydrodynamicModelFactory
{
  public: HydrodynamicModel* CreateHydrodynamicModel(sdf::ElementPtr _sdf,
                                                     physics::LinkPtr _link);

  public: static HydrodynamicModelFactory& GetInstance();

  public: bool RegisterCreator(const std::string& _identifier,
                               HydrodynamicModelCreator _creator);

  private: HydrodynamicModelFactory() {}

  private: std::map<std::string, HydrodynamicModelCreator> creators_;
};

#define REGISTER_HYDRODYNAMICMODEL(type) static const bool registeredWithFactory

#define REGISTER_HYDRODYNAMICMODEL_CREATOR(type, creator) \
  const bool type::registeredWithFactory = \
  HydrodynamicModelFactory::GetInstance().RegisterCreator( \
  type::IDENTIFIER, creator);

class HMFossen : public HydrodynamicModel
{
  public: static HydrodynamicModel* create(sdf::ElementPtr _sdf,
      physics::LinkPtr _link);

  public: virtual std::string GetType() { return IDENTIFIER; }

  public: virtual void Print(std::string _paramName,
                             std::string _message = std::string());

  public: virtual bool GetParam(std::string _tag,
                                std::vector<double>& _output);

  public: virtual bool GetParam(std::string _tag, double& _output);

  public: virtual bool SetParam(std::string _tag, double _input);

  protected: REGISTER_HYDRODYNAMICMODEL(HMFossen);

  protected: static const std::string IDENTIFIER;

  protected: HMFossen(sdf::ElementPtr _sdf, physics::LinkPtr _link);

  public: virtual void ApplyHydrodynamicForces(double time,
                            const ignition::math::Vector3d &_flowVelWorld);

  protected: void ComputeAddedCoriolisMatrix(const Eigen::Vector6d& _vel,
                                             const Eigen::Matrix6d& _Ma,
                                             Eigen::Matrix6d &_Ca) const;

  protected: void ComputeDampingMatrix(const Eigen::Vector6d& _vel,
                                       Eigen::Matrix6d &_D) const;

  protected: Eigen::Matrix6d GetAddedMass() const;

  protected: Eigen::Matrix6d Ma;

  protected: double scalingAddedMass;

  protected: double offsetAddedMass;

  protected: Eigen::Matrix6d Ca;

  protected: Eigen::Matrix6d D;

  protected: double scalingDamping;

  protected: double offsetLinearDamping;

  protected: double offsetLinForwardSpeedDamping;

  protected: double offsetNonLinDamping;

  protected: Eigen::Matrix6d DLin;

  protected: Eigen::Matrix6d DLinForwardSpeed;

  protected: Eigen::Matrix6d DNonLin;

  protected: std::vector<double> linearDampCoef;

  protected: std::vector<double> quadDampCoef;
};

class HMSphere : public HMFossen
{
  public: static HydrodynamicModel* create(sdf::ElementPtr _sdf,
      physics::LinkPtr _link);

  public: virtual std::string GetType() { return IDENTIFIER; }

  public: virtual void Print(std::string _paramName,
                             std::string _message = std::string());

  protected: REGISTER_HYDRODYNAMICMODEL(HMSphere);

  protected: static const std::string IDENTIFIER;

  protected: HMSphere(sdf::ElementPtr _sdf, physics::LinkPtr _link);

  protected: double radius;

  protected: double Cd;

  protected: double areaSection;
};

class HMCylinder : public HMFossen
{
  public: static HydrodynamicModel* create(sdf::ElementPtr _sdf,
      physics::LinkPtr _link);

  public: virtual std::string GetType() { return IDENTIFIER; }

  public: virtual void Print(std::string _paramName,
                             std::string _message = std::string());

  private: REGISTER_HYDRODYNAMICMODEL(HMCylinder);

  protected: static const std::string IDENTIFIER;

  protected: HMCylinder(sdf::ElementPtr _sdf, physics::LinkPtr _link);

  protected: double length;

  protected: double radius;

  protected: std::string axis;

  protected: double dimRatio;

  protected: double cdCirc;

  protected: double cdLength;
};

class HMSpheroid : public HMFossen
{
  public: static HydrodynamicModel* create(sdf::ElementPtr _sdf,
      physics::LinkPtr _link);

  public: virtual std::string GetType() { return IDENTIFIER; }

  public: virtual void Print(std::string _paramName,
                             std::string _message = std::string());

  private: REGISTER_HYDRODYNAMICMODEL(HMSpheroid);

  protected: static const std::string IDENTIFIER;

  protected: HMSpheroid(sdf::ElementPtr _sdf, physics::LinkPtr _link);

  protected: double length;

  protected: double radius;
};

class HMBox : public HMFossen
{
  public: static HydrodynamicModel* create(sdf::ElementPtr _sdf,
      physics::LinkPtr _link);

  public: virtual std::string GetType() { return IDENTIFIER; }

  public: virtual void Print(std::string _paramName,
                             std::string _message = std::string());

  private: REGISTER_HYDRODYNAMICMODEL(HMBox);

  protected: static const std::string IDENTIFIER;

  protected: HMBox(sdf::ElementPtr _sdf, physics::LinkPtr _link);

  protected: double Cd;

  protected: double length;

  protected: double width;

  protected: double height;
};
}

#endif  // __UUV_GAZEBO_HYDRO_MODEL_HH__