usv_gazebo_dynamics_plugin.cpp
Go to the documentation of this file.
00001 /* 
00002 
00003 Copyright (c) 2017, Brian Bingham
00004 All rights reserved
00005 
00006 This file is part of the usv_gazebo_dynamics_plugin package, known as this Package.
00007 
00008 This Package free software: you can redistribute it and/or modify
00009 it under the terms of the GNU General Public License as published by
00010 the Free Software Foundation, either version 3 of the License, or
00011 (at your option) any later version.
00012 
00013 This Package s distributed in the hope that it will be useful,
00014 but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016 GNU General Public License for more details.
00017 
00018 You should have received a copy of the GNU General Public License
00019 along with this package.  If not, see <http://www.gnu.org/licenses/>.
00020 
00021 */
00022 
00023 #include <boost/thread.hpp>
00024 #include <nav_msgs/Odometry.h>
00025 #include <sensor_msgs/JointState.h>
00026 #include <ros/time.h>
00027 #include <tf2/LinearMath/Transform.h>
00028 
00029 #include <usv_gazebo_plugins/usv_gazebo_dynamics_plugin.h>
00030 
00031 #define GRAVITY 9.815
00032 
00033 using namespace gazebo;
00034 
00035 UsvPlugin::UsvPlugin()
00036 {
00037 }
00038 
00039 UsvPlugin::~UsvPlugin()
00040 {
00041   rosnode_->shutdown();
00042   spinner_thread_->join();
00043   delete rosnode_;
00044   delete spinner_thread_;
00045 }
00046 
00047 void UsvPlugin::FiniChild()
00048 {
00049 }
00050     
00051 
00052 double UsvPlugin::getSdfParamDouble(sdf::ElementPtr sdfPtr, const std::string &param_name, double default_val)
00053 {
00054   double val = default_val;
00055   if (sdfPtr->HasElement(param_name) && sdfPtr->GetElement(param_name)->GetValue()) 
00056   {
00057     val = sdfPtr->GetElement(param_name)->Get<double>();
00058     ROS_INFO_STREAM("Parameter found - setting <" << param_name << "> to <" << val << ">.");
00059 
00060   }
00061   else{
00062     ROS_INFO_STREAM("Parameter <" << param_name << "> not found: Using default value of <" << val << ">.");
00063   }
00064   return val;
00065 }
00066                                     
00067 void UsvPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
00068 {
00069   ROS_INFO("Loading usv_gazebo_dynamics_plugin");
00070   model_ = _parent;
00071   world_ = model_->GetWorld();
00072   
00073   // Retrieve model paramters from SDF
00074   // Set default values
00075   node_namespace_ = "";
00076 
00077   water_level_ = 0.5;
00078   water_density_ = 997.7735;
00079 
00080   param_X_dot_u_ = 5;  // Added mass
00081   param_Y_dot_v_ = 5;
00082   param_N_dot_r_ = 1;
00083   param_X_u_ = 20;  // linear drag
00084   param_X_uu_ = 0.0;
00085   param_Y_v_ = 20;
00086   param_Y_vv_ = 0.0;
00087   param_Z_w_ = 20;
00088   param_K_p_ = 20.0;
00089   param_M_q_ = 20.0;
00090   param_N_r_ = 20;
00091   param_N_rr_ = 0.0;
00092 
00093   param_boat_width_ = 1.0;
00094   param_boat_length_ = 1.35;
00095 
00096   param_metacentric_length_ = 0.4 ; // From clearpath
00097   param_metacentric_width_ = 0.4;  // ditto
00098   param_boat_area_ = 0.48;  // Clearpath: 1.2m in length, 0.2m in width, 2 pontoons.
00099   
00100   //  Enumerating model
00101   ROS_INFO_STREAM("Enumerating Model...");
00102   ROS_INFO_STREAM("Model name = "<< model_->GetName());
00103   physics::Link_V links = model_->GetLinks();
00104   for (int ii=0; ii<links.size(); ii++){
00105     ROS_INFO_STREAM("Link: "<<links[ii]->GetName());
00106   }
00107 
00108   // Get parameters from SDF
00109   if (_sdf->HasElement("robotNamespace")) 
00110   {
00111     node_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
00112   }
00113 
00114   if (!_sdf->HasElement("bodyName") || !_sdf->GetElement("bodyName")->GetValue())
00115   {                       
00116     link_ = model_->GetLink();
00117     link_name_ = link_->GetName();
00118     ROS_INFO_STREAM("Did not find SDF parameter bodyName");
00119   } 
00120   else {
00121     link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
00122     //link_name_ = "thrust_link";
00123     link_ = model_->GetLink(link_name_);
00124 
00125     ROS_INFO_STREAM("Found SDF parameter bodyName as <"<<link_name_<<">");
00126   }
00127   if (!link_)
00128   {
00129     ROS_FATAL("usv_gazebo_dynamics_plugin error: bodyName: %s does not exist\n", link_name_.c_str());
00130     return;
00131   }
00132   else
00133   {
00134     ROS_INFO_STREAM("USV Model Link Name = " << link_name_);
00135   }
00136 
00137   water_level_ = getSdfParamDouble(_sdf,"waterLevel",water_level_);
00138   water_density_ = getSdfParamDouble(_sdf,"waterDensity",water_density_);
00139 
00140   param_X_dot_u_ = getSdfParamDouble(_sdf,"xDotU",param_X_dot_u_);
00141   param_Y_dot_v_ = getSdfParamDouble(_sdf,"yDotV",param_Y_dot_v_);
00142   param_N_dot_r_ = getSdfParamDouble(_sdf,"nDotR",param_N_dot_r_);
00143 
00144   param_X_u_ = getSdfParamDouble(_sdf,"xU",param_X_u_);
00145   param_X_uu_ = getSdfParamDouble(_sdf,"xUU",param_X_uu_);
00146   param_Y_v_ = getSdfParamDouble(_sdf,"yV",param_Y_v_);
00147   param_Y_vv_ = getSdfParamDouble(_sdf,"yVV",param_Y_vv_);
00148   param_Z_w_ = getSdfParamDouble(_sdf,"zW",param_Z_w_);
00149   param_K_p_ = getSdfParamDouble(_sdf,"kP",param_K_p_);
00150   param_M_q_ = getSdfParamDouble(_sdf,"mQ",param_M_q_);
00151   param_N_r_ = getSdfParamDouble(_sdf,"nR",param_N_r_);
00152   param_N_rr_ = getSdfParamDouble(_sdf,"nRR",param_N_rr_);
00153 
00154 
00155   param_boat_area_ = getSdfParamDouble(_sdf,"boatArea",param_boat_area_);
00156   param_boat_width_ = getSdfParamDouble(_sdf,"boatWidth",param_boat_width_);
00157   param_boat_length_ = getSdfParamDouble(_sdf,"boatLength",param_boat_length_);
00158 
00159   param_metacentric_length_ = getSdfParamDouble(_sdf,"metacentricLength",
00160                                                 param_metacentric_length_);
00161   param_metacentric_width_ = getSdfParamDouble(_sdf,"metacentricWidth",
00162                                                 param_metacentric_width_);
00163   
00164   // Wave parameters
00165   std::ostringstream buf;
00166   math::Vector2d tmpm;
00167   std::vector<float> tmpv(2,0);
00168   param_wave_n_ = _sdf->GetElement("wave_n")->Get<int>();
00169   for (int ii=0; ii < param_wave_n_; ii++){
00170     buf.str("");
00171     buf<<"wave_amp"<<ii;
00172     param_wave_amps_.push_back(_sdf->GetElement(buf.str())->Get<float>());
00173     ROS_INFO_STREAM("Wave Amplitude "<<ii<<": "<<param_wave_amps_[ii]);
00174     buf.str("");
00175     buf<<"wave_period"<<ii;
00176     param_wave_periods_.push_back(_sdf->GetElement(buf.str())->Get<float>());
00177     buf.str("");
00178     buf<<"wave_direction"<<ii;
00179     tmpm=_sdf->GetElement(buf.str())->Get<math::Vector2d>();
00180     tmpv[0] = tmpm.x;
00181     tmpv[1] = tmpm.y;
00182     param_wave_directions_.push_back(tmpv);
00183     ROS_INFO_STREAM("Wave Direction "<<ii<<": "<<param_wave_directions_[ii][0]
00184                     << ", " << param_wave_directions_[ii][1]);
00185 
00186   }
00187 
00188 
00189   // Get inertia and mass of vessel
00190   math::Vector3 inertia = link_->GetInertial()->GetPrincipalMoments();
00191   double mass = link_->GetInertial()->GetMass();
00192 
00193   // Report some of the pertinent parameters for verification
00194   ROS_INFO("USV Dynamics Parameters: From URDF XACRO model definition");
00195   ROS_INFO_STREAM("Vessel Mass (rigid-body): " << mass);
00196   ROS_INFO_STREAM("Vessel Inertia Vector (rigid-body): X:" << inertia[0] <<
00197                   " Y:"<<inertia[1] <<
00198                   " Z:"<<inertia[2]);
00199   ROS_INFO("USV Dynamics Parameters: Plugin Parameters");
00200 
00201   //initialize time and odometry position
00202   prev_update_time_ = this->world_->GetSimTime();
00203 
00204   // Initialize the ROS node
00205   int argc = 0;
00206   char** argv = NULL;
00207   ros::init(argc, argv, "usv_dynamics_gazebo", 
00208             ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00209   rosnode_ = new ros::NodeHandle( node_namespace_ );
00210 
00211   // Listen to the update event. This event is broadcast every
00212   // simulation iteration.
00213   this->spinner_thread_ = new boost::thread( boost::bind( &UsvPlugin::spin, this) );
00214   this->updateConnection = event::Events::ConnectWorldUpdateBegin(
00215     boost::bind(&UsvPlugin::UpdateChild, this));
00216 
00217   // Initialize Added Mass Matrix
00218   Ma_ = Eigen::MatrixXd(6,6);
00219   Ma_ << param_X_dot_u_ ,   0,   0, 0, 0, 0,
00220         0,   param_Y_dot_v_,   0, 0, 0, 0,
00221         0,   0,   0.1, 0, 0, 0,
00222         0,   0,   0, 0.1, 0, 0, 
00223         0,   0,   0, 0, 0.1, 0,  
00224         0,   0,   0, 0, 0, param_N_dot_r_ ;
00225 
00226   Cmat_ =  Eigen::MatrixXd::Zero(6,6);
00227   Dmat_ =  Eigen::MatrixXd::Zero(6,6);
00228   state_dot_ = Eigen::VectorXd(6);
00229   state_ = Eigen::VectorXd(6);
00230   amassVec_ = Eigen::VectorXd(6);
00231 
00232   int NN = 2; // must be factor of 2! - only 2 for now!!
00233   // x,y grid step increments
00234   dx_ = param_boat_length_/NN;
00235   dy_ = param_boat_width_/NN;
00236   // Vector for interating throug grid points on boat
00237   for (int ii=-NN/2; ii<0; ii++){
00238     II_.push_back(ii);
00239   }
00240   for (int ii=1; ii<=NN/2; ii++){
00241     II_.push_back(ii);
00242   }
00243   // Precalculate this to save some time.
00244   buoy_frac_ = (param_boat_area_/(NN*NN))*GRAVITY*water_density_;
00245   
00246 }
00247 
00248 
00249 void UsvPlugin::UpdateChild()
00250 {
00251   common::Time time_now = this->world_->GetSimTime();
00252   //common::Time step_time = time_now - prev_update_time_;
00253   double dt = (time_now - prev_update_time_).Double();
00254   prev_update_time_ = time_now;
00255   
00256   // Get Pose/Orientation from Gazebo (if no state subscriber is active)
00257   pose_ = link_->GetWorldPose();
00258   euler_ = pose_.rot.GetAsEuler();
00259 
00260   // Get body-centered linear and angular rates
00261   vel_linear_body_ = link_->GetRelativeLinearVel();  
00262   ROS_DEBUG_STREAM_THROTTLE(0.5,"Vel linear: " << vel_linear_body_);
00263   vel_angular_body_ = link_->GetRelativeAngularVel();
00264   ROS_DEBUG_STREAM_THROTTLE(0.5,"Vel angular: " << vel_angular_body_);
00265   // Estimate the linear and angular accelerations.
00266   // Note the the GetRelativeLinearAccel() and AngularAccel() functions
00267   // appear to be unreliable
00268   math::Vector3 accel_linear_body = (vel_linear_body_ - prev_lin_vel_) /dt;
00269   prev_lin_vel_ = vel_linear_body_;
00270   ROS_DEBUG_STREAM_THROTTLE(0.5,"Accel linear: " << accel_linear_body);
00271   math::Vector3 accel_angular_body = (vel_angular_body_ - prev_ang_vel_) /dt;
00272   prev_ang_vel_ = vel_angular_body_;
00273   ROS_DEBUG_STREAM_THROTTLE(0.5,"Accel angular: " << accel_angular_body);
00274 
00275   // Create state and derivative of state (accelerations)
00276   state_dot_ << accel_linear_body.x, accel_linear_body.y, accel_linear_body.z, 
00277     accel_angular_body.x, accel_angular_body.y, accel_angular_body.z;
00278 
00279   state_ << vel_linear_body_.x, vel_linear_body_.y, vel_linear_body_.z, 
00280     vel_angular_body_.x, vel_angular_body_.y, vel_angular_body_.z;
00281 
00282   // Added Mass
00283   amassVec_ = -1.0*Ma_*state_dot_;
00284   ROS_DEBUG_STREAM_THROTTLE(1.0,"state_dot_: \n" << state_dot_);
00285   ROS_DEBUG_STREAM_THROTTLE(1.0,"amassVec :\n" << amassVec_);
00286   
00287   // Coriolis - added mass components
00288   Cmat_(0,5) = param_Y_dot_v_ * vel_linear_body_.y;
00289   Cmat_(1,5) = param_X_dot_u_ * vel_linear_body_.x;
00290   Cmat_(5,0) = param_Y_dot_v_ * vel_linear_body_.y;
00291   Cmat_(5,1) = param_X_dot_u_ * vel_linear_body_.x;
00292   Cvec_ = -1.0*Cmat_*state_;
00293   ROS_DEBUG_STREAM_THROTTLE(1.0,"Cvec :\n" << Cvec_);
00294 
00295   // Drag
00296   //Eigen::MatrixXd Dmat = Eigen::MatrixXd(6,6);
00297   Dmat_(0,0) = param_X_u_ + param_X_uu_*std::abs(vel_linear_body_.x);
00298   Dmat_(1,1) = param_Y_v_ + param_Y_vv_*std::abs(vel_linear_body_.y);
00299   Dmat_(2,2) = param_Z_w_;
00300   Dmat_(3,3) = param_K_p_;
00301   Dmat_(4,4) = param_M_q_;
00302   Dmat_(5,5) = param_N_r_ + param_N_rr_*std::abs(vel_angular_body_.z);
00303   ROS_DEBUG_STREAM_THROTTLE(1.0,"Dmat :\n" << Dmat_);
00304   Dvec_ = -1.0*Dmat_*state_;
00305   ROS_DEBUG_STREAM_THROTTLE(1.0,"Dvec :\n" << Dvec_);
00306   
00307   // Vehicle frame transform
00308   tf2::Quaternion vq = tf2::Quaternion();
00309   tf2::Matrix3x3 m;
00310   m.setEulerYPR(euler_.z,euler_.y,euler_.x);
00311   m.getRotation(vq);
00312   tf2::Transform xform_v = tf2::Transform(vq);
00313 
00314   // Sum all forces - in body frame
00315   Eigen::VectorXd forceSum = amassVec_ + Dvec_;// + buoyVec;
00316   // Forces in fixed frame
00317   ROS_DEBUG_STREAM_THROTTLE(1.0,"forceSum :\n" << forceSum);
00318   
00319   // Add dynamic forces/torques to link at CG
00320   link_->AddRelativeForce(math::Vector3(forceSum(0),forceSum(1),forceSum(2)));
00321   link_->AddRelativeTorque(math::Vector3(forceSum(3),forceSum(4),forceSum(5)));
00322 
00323   // Distribute upward buoyancy force
00324   
00325   float ddx, ddy, ddz, buoy_force;
00326   double w, k, dz, Ddotx;
00327   math::Vector3 X;  // location of vehicle base link
00328   tf2::Vector3 bpnt(0,0,0);     // grid points on boat
00329   tf2::Vector3 bpnt_w(0,0,0);   // in world coordinates
00330 
00331   // Loop over boat grid points
00332   //for (std::list<int>::iterator ii=Ilist.begin();ii != Ilist.end(); ii++){
00333   for (std::vector<int>::iterator it=II_.begin(); it != II_.end(); ++it){
00334     bpnt.setX((*it)*dx_);  // grid point in boat fram
00335     for (std::vector<int>::iterator jt=II_.begin(); jt != II_.end(); ++jt){
00336       //for (std::list<int>::iterator jj=Jlist.begin(); jj != Jlist.end(); jj++)
00337       bpnt.setY((*jt)*dy_);
00338       
00339       // Transform from vessel to water/world frame
00340       bpnt_w = xform_v*bpnt;
00341 
00342       // Debug
00343       ROS_DEBUG_STREAM_THROTTLE(1.0,"[" << (*it) <<","<<(*jt)<< "] grid points"<<bpnt.x() <<","<<bpnt.y() <<","<<bpnt.z());
00344       ROS_DEBUG_STREAM_THROTTLE(1.0,"v frame euler "<<euler_);
00345       ROS_DEBUG_STREAM_THROTTLE(1.0,"in water frame"<<bpnt_w.x() <<","<<bpnt_w.y() <<","<<bpnt_w.z());
00346 
00347       // Vertical location of boat grid point in world frame
00348       ddz = pose_.pos.z+bpnt_w.z();
00349       ROS_DEBUG_STREAM("Z, pose: " << pose_.pos.z <<", bpnt: "<<bpnt_w.z() << ", dd: " << ddz);
00350       
00351       // Find vertical displacement of wave field
00352       X.x = pose_.pos.x+bpnt_w.x();  // World location of grid point
00353       X.y = pose_.pos.y+bpnt_w.y();
00354       // sum vertical dsplacement over all waves
00355       dz = 0.0;
00356       for (int ii=0; ii < param_wave_n_; ii++){
00357         Ddotx=param_wave_directions_[ii][0]*X.x
00358           +param_wave_directions_[ii][1]*X.y;
00359         w = 2.0*3.14159 / param_wave_periods_[ii];
00360         k = w*w/9.81;
00361         dz += param_wave_amps_[ii]*cos(k*Ddotx-w*time_now.Float());;    
00362       }
00363       ROS_DEBUG_STREAM_THROTTLE(1.0,"wave disp: " << dz);
00364 
00365       // Buoyancy force at grid point
00366       buoy_force = (((water_level_+dz)-(ddz))*(buoy_frac_));
00367       ROS_DEBUG_STREAM("buoy_force: " << buoy_force);
00368       // Apply force at grid point
00369       // From web, Appears that position is in the link frame
00370       // and force is in world frame
00371       link_->AddForceAtRelativePosition(math::Vector3(0,0,buoy_force),
00372                                         math::Vector3(bpnt.x(),bpnt.y(),bpnt.z()));
00373     }
00374   }
00375 }
00376 
00377 void UsvPlugin::spin()
00378 {
00379     while(ros::ok()) ros::spinOnce();
00380 }
00381 
00382 GZ_REGISTER_MODEL_PLUGIN(UsvPlugin);
00383 


usv_gazebo_plugins
Author(s): Brian Bingham
autogenerated on Thu Mar 7 2019 03:37:04