00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
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 ¶m_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
00074
00075 node_namespace_ = "";
00076
00077 water_level_ = 0.5;
00078 water_density_ = 997.7735;
00079
00080 param_X_dot_u_ = 5;
00081 param_Y_dot_v_ = 5;
00082 param_N_dot_r_ = 1;
00083 param_X_u_ = 20;
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 ;
00097 param_metacentric_width_ = 0.4;
00098 param_boat_area_ = 0.48;
00099
00100
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
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
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
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
00190 math::Vector3 inertia = link_->GetInertial()->GetPrincipalMoments();
00191 double mass = link_->GetInertial()->GetMass();
00192
00193
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
00202 prev_update_time_ = this->world_->GetSimTime();
00203
00204
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
00212
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
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;
00233
00234 dx_ = param_boat_length_/NN;
00235 dy_ = param_boat_width_/NN;
00236
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
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
00253 double dt = (time_now - prev_update_time_).Double();
00254 prev_update_time_ = time_now;
00255
00256
00257 pose_ = link_->GetWorldPose();
00258 euler_ = pose_.rot.GetAsEuler();
00259
00260
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
00266
00267
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
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
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
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
00296
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
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
00315 Eigen::VectorXd forceSum = amassVec_ + Dvec_;
00316
00317 ROS_DEBUG_STREAM_THROTTLE(1.0,"forceSum :\n" << forceSum);
00318
00319
00320 link_->AddRelativeForce(math::Vector3(forceSum(0),forceSum(1),forceSum(2)));
00321 link_->AddRelativeTorque(math::Vector3(forceSum(3),forceSum(4),forceSum(5)));
00322
00323
00324
00325 float ddx, ddy, ddz, buoy_force;
00326 double w, k, dz, Ddotx;
00327 math::Vector3 X;
00328 tf2::Vector3 bpnt(0,0,0);
00329 tf2::Vector3 bpnt_w(0,0,0);
00330
00331
00332
00333 for (std::vector<int>::iterator it=II_.begin(); it != II_.end(); ++it){
00334 bpnt.setX((*it)*dx_);
00335 for (std::vector<int>::iterator jt=II_.begin(); jt != II_.end(); ++jt){
00336
00337 bpnt.setY((*jt)*dy_);
00338
00339
00340 bpnt_w = xform_v*bpnt;
00341
00342
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
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
00352 X.x = pose_.pos.x+bpnt_w.x();
00353 X.y = pose_.pos.y+bpnt_w.y();
00354
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
00366 buoy_force = (((water_level_+dz)-(ddz))*(buoy_frac_));
00367 ROS_DEBUG_STREAM("buoy_force: " << buoy_force);
00368
00369
00370
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