diff --git a/include/gazebo_uuv_plugin.h b/include/gazebo_uuv_plugin.h index 9360d67552..5e5989312d 100644 --- a/include/gazebo_uuv_plugin.h +++ b/include/gazebo_uuv_plugin.h @@ -1,5 +1,7 @@ /* - * Copyright 2016 Austin Buchan, Nils Rottmann, TUHH Hamburg, Germany + * Copyright 2020 Daniel Duecker, TU Hamburg, Germany + * Copyright 2020 Philipp Hastedt, TU Hamburg, Germany + * based on prior work by Nils Rottmann and Austin Buchan * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -14,81 +16,89 @@ * limitations under the License. */ -#include -#include -#include + + +#include +#include +#include #include #include - -#include "common.h" +#include +#include +#include +#include "CommandMotorSpeed.pb.h" #include "gazebo/transport/transport.hh" #include "gazebo/msgs/msgs.hh" -#include "CommandMotorSpeed.pb.h" +#include "MotorSpeed.pb.h" +#include "Float.pb.h" +#include "common.h" -namespace gazebo { -// Default values -static const std::string kDefaultNamespace = ""; -static const std::string kDefaultCommandSubTopic = "/gazebo/command/motor_speed"; -static const std::string kDefaultLinkName = "base_link"; // the link name of the base hippocampus, see sdf file +namespace turning_direction { +const static int CCW = 1; +const static int CW = -1; +} +namespace gazebo { typedef const boost::shared_ptr CommandMotorSpeedPtr; -// define class GazeboUUVPlugin -class GazeboUUVPlugin : public ModelPlugin { +class GazeboUUVPlugin : public MotorModel, public ModelPlugin { + public: + GazeboUUVPlugin() + : ModelPlugin(), + MotorModel(){ + } + + virtual ~GazeboUUVPlugin(); + virtual void InitializeParams(); + virtual void Publish(); + protected: + virtual void UpdateForcesAndMoments(); + virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + virtual void OnUpdate(const common::UpdateInfo &); + + private: + std::string namespace_; + std::string command_sub_topic_; + std::string link_base_; + std::string link_prop_0_; + std::string link_prop_1_; + std::string link_prop_2_; + std::string link_prop_3_; + + int turning_directions_[4]; + double motor_commands_[4]; + double motor_force_constant_; + double motor_torque_constant_; + double dead_zone_; + + /* Hydro coefficients - FOSSEN 2011 */ + double X_u_; + double Y_v_; + double Z_w_; + double K_p_; + double M_q_; + double N_r_; + + double X_udot_; + double Y_vdot_; + double Z_wdot_; + double K_pdot_; + double M_qdot_; + double N_rdot_; + + transport::NodePtr node_handle_; + transport::SubscriberPtr command_sub_; + physics::ModelPtr model_; + physics::LinkPtr baseLink_; + physics::LinkPtr prop0Link_; + physics::LinkPtr prop1Link_; + physics::LinkPtr prop2Link_; + physics::LinkPtr prop3Link_; + event::ConnectionPtr updateConnection_; + boost::thread callback_queue_thread_; + void QueueThread(); + void VelocityCallback(CommandMotorSpeedPtr &rot_velocities); - public: - GazeboUUVPlugin() : - ModelPlugin(), - namespace_(kDefaultNamespace), // definde namespace, topic and link_name - command_sub_topic_(kDefaultCommandSubTopic), - link_name_(kDefaultLinkName) {} - - virtual ~GazeboUUVPlugin(); - - protected: - void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); - void OnUpdate(const common::UpdateInfo&); - - private: - event::ConnectionPtr update_connection_; - - std::string namespace_; - std::string command_sub_topic_; - std::string link_name_; - - transport::NodePtr node_handle_; - transport::SubscriberPtr command_sub_; - - physics::LinkPtr link_; - physics::Link_V rotor_links_; - - void CommandCallback(CommandMotorSpeedPtr &command); - double command_[4]; - - double last_time_; - double time_delta_; - - double motor_force_constant_; - double motor_torque_constant_; - - double X_u_; - double Y_v_; - double Z_w_; - double K_p_; - double M_q_; - double N_r_; - - double X_udot_; - double Y_vdot_; - double Z_wdot_; - double K_pdot_; - double M_qdot_; - double N_rdot_; - - // variables for debugging - double time_; - double counter_; }; - } diff --git a/models/hippocampus/hippocampus.sdf b/models/hippocampus/hippocampus.sdf deleted file mode 100644 index 27c0fc28d0..0000000000 --- a/models/hippocampus/hippocampus.sdf +++ /dev/null @@ -1,888 +0,0 @@ - - - - - - 0 0 0 0 0 0 - - 0 0 0 0 0 0 - - 3 - - 0.007 - 0 - 0 - 0.027 - 0 - 0.027 - - - - 0 - 0 - - 0 - - - 0 0 0 0 0 0 - - - model://hippocampus/meshes/hippocampus.stl - - - - 1 - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - 0 0 0 1 - - __default__ - - - 1 - 0 - - - - - 0.1 0 0 0 1.57 0 - - - 0.35 - 0.005 - - - - 0.0 0.0 0.0 1 - 0.0 0.0 0.0 1 - 0.0 0.0 0.0 1 - 1.0 0.0 0.0 1 - - - - 0 0.1 0 -1.57 0 0 - - - 0.25 - 0.005 - - - - 0.0 0.0 0.0 1 - 0.0 0.0 0.0 1 - 0.0 0.0 0.0 1 - 0.0 1.0 0.0 1 - - - - 0 0 0.1 0 0 0 - - - 0.25 - 0.005 - - - - 0.0 0.0 0.0 1 - 0.0 0.0 0.0 1 - 0.0 0.0 0.0 1 - 0.0 0.0 1.0 1 - - - - - - 0 - - 0 - 10 - 0 0 0 0 1.570796 0 - - - 0.03 - 0.36 - - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - - - 0 0 0 3.1415 0 0 - - 0 0 0 0 0 0 - 0.015 - - 1e-05 - 0 - 0 - 1e-05 - 0 - 1e-05 - - - 0 - 0 - 0 - - - - - - -0.05 -0.0481 -0.0481 0 0 0 - 0 - 0 - - 0 - - 0 0 0 0 0 0 - - - 1 1 1 - model://hippocampus/meshes/hippocampus_prop.stl - - - - - 0 0 1 1 - 0 0 1 1 - 0 0 1 1 - 0 0 1 1 - - __default__ - - - 1 - 0 - - - 0 - 10 - 0 0 0.02 0 0 0 - - - 0.02 - 0.06 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - - -0.05 +0.0481 -0.0481 0 0 0 - 0 - 0 - - 0 - - 0 0 0 0 0 0 - - - 1 1 1 - model://hippocampus/meshes/hippocampus_prop.stl - - - - - 0 0 1 1 - 0 0 1 1 - 0 0 1 1 - 0 0 1 1 - - __default__ - - - 1 - 0 - - - 0 - 10 - 0 0 0.02 0 0 0 - - - 0.02 - 0.06 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - - -0.05 0.0481 0.0481 0 0 0 - 0 - 0 - - 0 - - 0 0 0 0 0 0 - - - 1 1 1 - model://hippocampus/meshes/hippocampus_prop.stl - - - - - 0 0 1 1 - 0 0 1 1 - 0 0 1 1 - 0 0 1 1 - - __default__ - - - 1 - 0 - - - 0 - 10 - 0 0 0.02 0 0 0 - - - 0.02 - 0.06 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - - -0.05 -0.0481 +0.0481 0 0 0 - 0 - 0 - - 0 - - 0 0 0 0 0 0 - - - 1 1 1 - model://hippocampus/meshes/hippocampus_prop.stl - - - - - 0 0 1 1 - 0 0 1 1 - 0 0 1 1 - 0 0 1 1 - - __default__ - - - 1 - 0 - - - 0 - 10 - 0 0 0.02 0 0 0 - - - 0.02 - 0.06 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - - - base_link - hippocampus/imu_link - - 1 0 0 - 1 - - 0 - 0 - 0 - 0 - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - base_link - rotor_0 - - 1 0 0 - 1 - - -1e+16 - 1e+16 - -1 - -1 - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - base_link - rotor_1 - - 1 0 0 - 1 - - -1e+16 - 1e+16 - -1 - -1 - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - base_link - rotor_2 - - 1 0 0 - 1 - - -1e+16 - 1e+16 - -1 - -1 - - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - base_link - rotor_3 - - 1 0 0 - 1 - - -1e+16 - 1e+16 - -1 - -1 - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - - 0 - 1 - - - true - - - - - 20 - 0.0004 - 6.4e-06 - 600 - /mag - - - - - hippocampus - /imu - /gps - /mag - /baro - INADDR_ANY - 14560 - false - /dev/ttyACM0 - 921600 - INADDR_ANY - 14550 - false - false - true - true - /gazebo/command/motor_speed - - - - - 0 - 0 - 1 - 0 - 0 - velocity - - - 1 - 0 - 1 - 0 - 0 - velocity - - - 2 - 0 - 1 - 0 - 0 - velocity - - - 3 - 0 - 1 - 0 - 0 - velocity - - - - - - - - hippocampus - /gazebo/command/motor_speed - base_link - 3 - 0.02 - - - - - 1.11 2.8 2.8 - 0.00451 0.0163 0.0163 - - - 5.39 17.36 17.36 - 0.00114 0.007 0.007 - - - - - - hippocampus - hippocampus/imu_link - /imu - 0.0003394 - 3.8785e-05 - 1000.0 - 0.0087 - 0.004 - 0.006 - 300.0 - 0.196 - - - - - 10 - /baro - - - - - hippocampus - rotor_0_joint - rotor_0 - ccw - 0.0125 - 0.025 - 838 - 0.0 - 0.0 - /gazebo/command/motor_speed - 0 - 0.0 - 0.0 - motor_speed/0 - 0.05 - - - hippocampus - rotor_1_joint - rotor_1 - cw - 0.0125 - 0.025 - 838 - 0.0 - 0.0 - /gazebo/command/motor_speed - 1 - 0.0 - 0.0 - motor_speed/1 - 0.05 - - - hippocampus - rotor_2_joint - rotor_2 - ccw - 0.0125 - 0.025 - 838 - 0.0 - 0.0 - /gazebo/command/motor_speed - 2 - 0.0 - 0.0 - motor_speed/2 - 0.05 - - - hippocampus - rotor_3_joint - rotor_3 - cw - 0.0125 - 0.025 - 838 - 0.0 - 0.0 - /gazebo/command/motor_speed - 3 - 0.0 - 0.0 - motor_speed/3 - 0.05 - - - diff --git a/models/hippocampus/meshes/hippocampus.stl b/models/uuv_hippocampus/meshes/uuv_hippocampus.stl similarity index 100% rename from models/hippocampus/meshes/hippocampus.stl rename to models/uuv_hippocampus/meshes/uuv_hippocampus.stl diff --git a/models/hippocampus/meshes/hippocampus_prop.stl b/models/uuv_hippocampus/meshes/uuv_hippocampus_prop.stl similarity index 100% rename from models/hippocampus/meshes/hippocampus_prop.stl rename to models/uuv_hippocampus/meshes/uuv_hippocampus_prop.stl diff --git a/models/hippocampus/model.config b/models/uuv_hippocampus/model.config similarity index 60% rename from models/hippocampus/model.config rename to models/uuv_hippocampus/model.config index 1f93e322a2..8d560fb11b 100644 --- a/models/hippocampus/model.config +++ b/models/uuv_hippocampus/model.config @@ -1,9 +1,17 @@ - hippocampus + uuv_hippocampus 1.0 - hippocampus.sdf + uuv_hippocampus.sdf + + Daniel Duecker + daniel.duecker@tuhh.de + + + Philipp Hastedt + philipp.hastedt@tuhh.de + Nils Rottmann Nils.Rottmann@tuhh.de diff --git a/models/uuv_hippocampus/uuv_hippocampus.sdf b/models/uuv_hippocampus/uuv_hippocampus.sdf new file mode 100644 index 0000000000..61d274796c --- /dev/null +++ b/models/uuv_hippocampus/uuv_hippocampus.sdf @@ -0,0 +1,664 @@ + + + + + + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + + 1.47 + + 0.0024 + 0 + 0 + 0.010717 + 0 + 0.010717 + + + + 0 0 0 0 1.570796 0 + + + 0.03 + 0.36 + + + + + + 0.001 + 0 + + + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + model://uuv_hippocampus/meshes/uuv_hippocampus.stl + + + + + + + 0 + + + + + + + + + 0 0 0 0 0 0 + + + 0 0 0 0 0 0 + 0.015 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + + 0 + + + + + uuv_hippocampus/imu_link + base_link + + 1 0 0 + + 0 + 0 + 0 + 0 + + + 0 + 0 + + 1 + + + + + + + + -0.05 0.0481 0.0481 0 0 0 + + 0 0 0 0 -0 0 + 0.005 + + 9.75e-07 + 0 + 0 + 0.000273104 + 0 + 0.000274004 + + + + 0 0 0.02 0 0 0 + + + 0.02 + 0.06 + + + + + + + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + model://uuv_hippocampus/meshes/uuv_hippocampus_prop.stl + + + + + + + 0 + + + + + + base_link + rotor_0 + + 1 0 0 + + -1e+16 + 1e+16 + + + 0 + 0 + + 1 + + + + + + + -0.05 -0.0481 0.0481 0 0 0 + + 0 0 0 0 -0 0 + 0.005 + + 9.75e-07 + 0 + 0 + 0.000273104 + 0 + 0.000274004 + + + + 0 0 0.02 0 0 0 + + + 0.02 + 0.06 + + + + + + + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + model://uuv_hippocampus/meshes/uuv_hippocampus_prop.stl + + + + + + + 0 + + + + + + rotor_1 + base_link + + 1 0 0 + + -1e+16 + 1e+16 + + + 0 + 0 + + 1 + + + + + + + -0.05 -0.0481 -0.0481 0 0 0 + + 0 0 0 0 -0 0 + 0.005 + + 9.75e-07 + 0 + 0 + 0.000273104 + 0 + 0.000274004 + + + + 0 0 0.02 0 0 0 + + + 0.02 + 0.06 + + + + + + + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + model://uuv_hippocampus/meshes/uuv_hippocampus_prop.stl + + + + + + + 0 + + + + + + rotor_2 + base_link + + 1 0 0 + + -1e+16 + 1e+16 + + + 0 + 0 + + 1 + + + + + + + -0.05 0.0481 -0.0481 0 0 0 + + 0 0 0 0 -0 0 + 0.005 + + 9.75e-07 + 0 + 0 + 0.000273104 + 0 + 0.000274004 + + + + 0 0 0.02 0 0 0 + + + 0.02 + 0.06 + + + + + + + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + model://uuv_hippocampus/meshes/uuv_hippocampus_prop.stl + + + + + + + 0 + + + + + + rotor_3 + base_link + + 1 0 0 + + -1e+16 + 1e+16 + + + 0 + 0 + + 1 + + + + + 0 + + + + base_link + 0.05 + + + + + + base_link + rotor_0 + rotor_1 + rotor_2 + rotor_3 + ccw + cw + ccw + cw + 8.5 + 0.0024 + 0 + 1.11 2.8 2.8 + 0.00451 0.0163 0.0163 + 5.39 17.36 17.36 + 0.00114 0.007 0.007 + /gazebo/command/motor_speed + + + + + + rotor_0_joint + rotor_0 + ccw + 0.0125 + 0.025 + 1100 + 0.0 + 0.00 + 0.0 + 0.0 + /gazebo/command/motor_speed + 0 + /motor_speed/0 + 0.05 + + + + rotor_1_joint + rotor_1 + cw + 0.0125 + 0.025 + 1100 + 0.0 + 0.0 + 0.0 + 0.0 + /gazebo/command/motor_speed + 1 + /motor_speed/1 + 0.05 + + + + rotor_2_joint + rotor_2 + ccw + 0.0125 + 0.025 + 1100 + 0.0 + 0.0 + 0.0 + 0.0 + /gazebo/command/motor_speed + 0 + /motor_speed/0 + 0.05 + + + + rotor_3_joint + rotor_3 + cw + 0.0125 + 0.025 + 1100 + 0.0 + 0.0 + 0.0 + 0.0 + /gazebo/command/motor_speed + 0 + /motor_speed/0 + 0.05 + + + + + true + + + + + 20 + 0.0004 + 6.4e-06 + 600 + /mag + + + + + 10 + /baro + + + + + /imu + /gps + /mag + /baro + INADDR_ANY + 14560 + 4560 + 0 + /dev/ttyACM0 + 921600 + INADDR_ANY + 14550 + INADDR_ANY + 14540 + false + false + false + true + false + true + true + /gazebo/command/motor_speed + + + + + 0 + 0 + 1 + 0 + 0 + velocity + top_port_motor_joint + + + 1 + 0 + 1 + 0 + 0 + velocity + top_starboard_joint + + + 2 + 0 + 1 + 0 + 0 + velocity + bottom_starboard_joint + + + 3 + 0 + 1 + 0 + 0 + velocity + bottom_port_joint + + + + + 0 + + + uuv_hippocampus/imu_link + /imu + 0.0003394 + 3.8785e-05 + 1000.0 + 0.0087 + 0.004 + 0.006 + 300.0 + 0.196 + + + diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index f758e08656..e5ce8cbd06 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -1105,7 +1105,7 @@ void GazeboMavlinkInterface::VisionCallback(OdomPtr& odom_message) { odom.time_usec = odom_message->time_usec(); - odom.frame_id = MAV_FRAME_LOCAL_FRD; + odom.frame_id = MAV_FRAME_LOCAL_NED; odom.child_frame_id = MAV_FRAME_BODY_FRD; odom.x = position.X(); diff --git a/src/gazebo_uuv_plugin.cpp b/src/gazebo_uuv_plugin.cpp index 3e7268c936..c3314753b0 100644 --- a/src/gazebo_uuv_plugin.cpp +++ b/src/gazebo_uuv_plugin.cpp @@ -1,5 +1,7 @@ /* - * Copyright 2016 Austin Buchan, Nils Rottmann TUHH Hamburg, Germany + * Copyright 2020 Daniel Duecker, TU Hamburg, Germany + * Copyright 2020 Philipp Hastedt, TU Hamburg, Germany + * based on prior work by Nils Rottmann and Austin Buchan * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -12,237 +14,216 @@ * 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. - * */ -/* - * This scripts simulates underwater froces and torques for the hippocampus model. The HippoCampus is an autonomous - * underwater vehicle (AUV) designed by the Technical University Hamburg-Harburg (TUHH). - * https://www.tuhh.de/mum/forschung/forschungsgebiete-und-projekte/flow-field-estimation-with-a-swarm-of-auvs.html +/** + * This plugin is an extension with regard to hydrodynamic effects. + * It + * - computes the hydrodynamic effects (according to FOSSEN 2011 - Handbook of Marine Craft Hydrodynamics and Motion Control) + * - applies the Hydro effects as forces/moments and are than applied to the vehicle + * - computes the forces/moments based on the motor commands */ #include "gazebo_uuv_plugin.h" #include - +#include +#include namespace gazebo { GazeboUUVPlugin::~GazeboUUVPlugin() { - update_connection_->~Connection(); + updateConnection_->~Connection(); } -// Load necessary data from the .sdf file +void GazeboUUVPlugin::InitializeParams() {} + +void GazeboUUVPlugin::Publish() {} + void GazeboUUVPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { + model_ = _model; + namespace_.clear(); - getSdfParam( - _sdf, "robotNamespace", namespace_, namespace_, true); + + namespace_ = _sdf->GetElement("robotNamespace")->Get(); node_handle_ = transport::NodePtr(new transport::Node()); node_handle_->Init(namespace_); - getSdfParam(_sdf, "linkName", link_name_, link_name_, true); - - // get the base link, thus the base hippocampus model - link_ = _model->GetLink(link_name_); - // get the child links, these are the links which represents the rotors of the hippocampus - rotor_links_ = link_->GetChildJointsLinks(); - for(int i = 0; i < rotor_links_.size(); i++) { - std::cout << "Rotor Link:" << rotor_links_[i]->GetScopedName() << "\n"; - command_[i] = 0.0; + //Get links + link_base_ = _sdf->GetElement("baseLinkName")->Get(); + baseLink_ = model_->GetLink(link_base_); + link_prop_0_ = _sdf->GetElement("prop0LinkName")->Get(); + prop0Link_ = model_->GetLink(link_prop_0_); + link_prop_1_ = _sdf->GetElement("prop1LinkName")->Get(); + prop1Link_ = model_->GetLink(link_prop_1_); + link_prop_2_ = _sdf->GetElement("prop2LinkName")->Get(); + prop2Link_ = model_->GetLink(link_prop_2_); + link_prop_3_ = _sdf->GetElement("prop3LinkName")->Get(); + prop3Link_ = model_->GetLink(link_prop_3_); + + //Get turning directions + std::string turning_direction_0 = _sdf->GetElement("turningDirection0")->Get(); + if (turning_direction_0 == "cw") { + turning_directions_[0] = turning_direction::CW; + } else if (turning_direction_0 == "ccw") { + turning_directions_[0] = turning_direction::CCW; + } + std::string turning_direction_1 = _sdf->GetElement("turningDirection1")->Get(); + if (turning_direction_1 == "cw") { + turning_directions_[1] = turning_direction::CW; + } else if (turning_direction_1 == "ccw") { + turning_directions_[1] = turning_direction::CCW; + } + std::string turning_direction_2 = _sdf->GetElement("turningDirection2")->Get(); + if (turning_direction_2 == "cw") { + turning_directions_[2] = turning_direction::CW; + } else if (turning_direction_2 == "ccw") { + turning_directions_[2] = turning_direction::CCW; + } + std::string turning_direction_3 = _sdf->GetElement("turningDirection3")->Get(); + if (turning_direction_3 == "cw") { + turning_directions_[3] = turning_direction::CW; + } else if (turning_direction_3 == "ccw") { + turning_directions_[3] = turning_direction::CCW; } - getSdfParam( - _sdf, "commandSubTopic", command_sub_topic_, command_sub_topic_); - - // subscribe to the commands (actuator outputs from the mixer from PX4) - command_sub_ = node_handle_->Subscribe( - "~/" + _model->GetName() + command_sub_topic_, &GazeboUUVPlugin::CommandCallback, this); - - update_connection_ = event::Events::ConnectWorldUpdateBegin( - boost::bind(&GazeboUUVPlugin::OnUpdate, this, _1)); - - // get force and torque parameters for force and torque calculations of the rotors from motor_speed - getSdfParam( - _sdf, "motorForceConstant", motor_force_constant_, motor_force_constant_); - getSdfParam( - _sdf, "motorTorqueConstant", motor_torque_constant_, motor_torque_constant_); + //Get force and moment constant + getSdfParam(_sdf, "motorForceConstant", motor_force_constant_, motor_force_constant_); + getSdfParam(_sdf, "motorTorqueConstant", motor_torque_constant_, motor_torque_constant_); + getSdfParam(_sdf, "deadZone", dead_zone_, dead_zone_); - // parameters for added mass and damping + //Get parameters for added mass and damping ignition::math::Vector3d added_mass_linear(0,0,0); - getSdfParam( - _sdf, "addedMassLinear", added_mass_linear, added_mass_linear); + getSdfParam(_sdf, "addedMassLinear", added_mass_linear, added_mass_linear); X_udot_ = added_mass_linear[0]; Y_vdot_ = added_mass_linear[1]; Z_wdot_ = added_mass_linear[2]; - ignition::math::Vector3d added_mass_angular(0,0,0); - getSdfParam( - _sdf, "addedMassAngular", added_mass_angular, added_mass_angular); + getSdfParam( _sdf, "addedMassAngular", added_mass_angular, added_mass_angular); K_pdot_ = added_mass_angular[0]; M_qdot_ = added_mass_angular[1]; N_rdot_ = added_mass_angular[2]; - ignition::math::Vector3d damping_linear(0,0,0); - getSdfParam( - _sdf, "dampingLinear", damping_linear, damping_linear); + getSdfParam(_sdf, "dampingLinear", damping_linear, damping_linear); X_u_ = damping_linear[0]; Y_v_ = damping_linear[1]; Z_w_ = damping_linear[2]; - ignition::math::Vector3d damping_angular(0,0,0); - getSdfParam( - _sdf, "dampingAngular", damping_angular, damping_angular); + getSdfParam(_sdf, "dampingAngular", damping_angular, damping_angular); K_p_ = damping_angular[0]; M_q_ = damping_angular[1]; N_r_ = damping_angular[2]; + getSdfParam(_sdf, "commandSubTopic", command_sub_topic_, command_sub_topic_); - // variables for debugging - time_ = 0.0; - counter_ = 0.0; - -} + // Listen to the update event. This event is broadcast every + // simulation iteration. + updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboUUVPlugin::OnUpdate, this, _1)); -// function to get the motor speed -void GazeboUUVPlugin::CommandCallback(CommandMotorSpeedPtr &command) { - for (int i = 0; i < 4; i++) { - command_[i] = command->motor_speed(i); + command_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + command_sub_topic_, &GazeboUUVPlugin::VelocityCallback, this); } - /*std::cout << "UUV Command Callback:" - << command_[0] << "," - << command_[1] << "," - << command_[2] << "," - << command_[3] << "," - << "\n"; */ - -} -// Update function, this runs in every circle +// This gets called by the world update start event. void GazeboUUVPlugin::OnUpdate(const common::UpdateInfo& _info) { - double now = _info.simTime.Double(); - time_delta_ = now - last_time_; - last_time_ = now; - time_ = time_ + time_delta_; - - //std::cout << "UUV Update at " << now << ", delta " << time_delta_ << "\n"; - - double forces[4]; - double torques[4]; - - // Apply forces and torques at rotor joints - for(int i = 0; i < 4; i++) { - - // Currently a rotor index hack to get over IMU link being first, since rotor_links_[0] would be the IMU - ignition::math::Vector3d rotor_force(motor_force_constant_ * command_[i] * std::abs(command_[i]), 0, 0); - rotor_links_[i+1]->AddRelativeForce(rotor_force); - - forces[i] = rotor_force[0]; - //std::cout << "Applying force " << rotor_force[2] << " to rotor " << i << "\n"; - - // CCW 1, CW 2, CCW 3 and CW 4. Apply drag torque - // directly to main body X axis - int propeller_direction = ((i+1)%2==0)?1:-1; // ternary operator: (condition) ? (if_true) : (if_false) - ignition::math::Vector3d rotor_torque( - propeller_direction * motor_torque_constant_ * command_[i] * std::abs(command_[i]), 0, 0); - link_->AddRelativeTorque(rotor_torque); - - //std::cout << "Applying torque " << rotor_torque[2] << " to rotor " << i << "\n"; - torques[i] = rotor_torque[0]; - } + sampling_time_ = _info.simTime.Double() - prev_sim_time_; + prev_sim_time_ = _info.simTime.Double(); + UpdateForcesAndMoments(); // Hydrodynamics are computed here + Publish(); +} - // for debugging - /*if (counter_ < time_) { - - counter_ = counter_ + 1.0; - - std::cout << "UUV Command Callback:" - << command_[0] << "," - << command_[1] << "," - << command_[2] << "," - << command_[3] << "," - << "\n"; - - double thrust = 0; - for(int i = 0; i<4; i++) thrust = thrust + forces[i]; - std::cout << "Thrust:"; - std::cout << thrust; - std::cout << "\n"; - - double L = 0.0481; - double roll = -torques[0] + torques[1] - torques[2] + torques[3]; - double pitch = (-forces[0] - forces[1] + forces[2] + forces[3])*L; - double yaw = (forces[0] - forces[1] - forces[2] + forces[3])*L; - std::cout << "Torques:"; - std::cout << roll << "," << pitch << "," << yaw; - std::cout << "\n"; - } */ - - // Calculate and apply body Coriolis and Drag forces and torques -#if GAZEBO_MAJOR_VERSION >= 9 - ignition::math::Vector3d linear_velocity = link_->RelativeLinearVel(); -#else - ignition::math::Vector3d linear_velocity = ignitionFromGazeboMath(link_->GetRelativeLinearVel()); -#endif - double u = linear_velocity[0]; - double v = linear_velocity[1]; - double w = linear_velocity[2]; - -#if GAZEBO_MAJOR_VERSION >= 9 - ignition::math::Vector3d angular_velocity = link_->RelativeAngularVel(); -#else - ignition::math::Vector3d angular_velocity = ignitionFromGazeboMath(link_->GetRelativeAngularVel()); -#endif - double p = angular_velocity[0]; - double q = angular_velocity[1]; - double r = angular_velocity[2]; - - //std::cout << "Vels:" << linear_velocity << ":" << angular_velocity << "\n"; - - // Linear Damping Matrix, with minus already multiplied - ignition::math::Matrix3d D_FL( - -X_u_, 0, 0, - 0, -Y_v_, 0, - 0, 0, -Z_w_ - ); - - // Angular Damping Matrix, with minus already multiplied - ignition::math::Matrix3d D_FA( - -K_p_, 0, 0, - 0, -M_q_, 0, - 0, 0, -N_r_ - ); - - ignition::math::Vector3d damping_force = - D_FL*linear_velocity; - ignition::math::Vector3d damping_torque = - D_FA*angular_velocity; - - // Corriolis Forces and Torques - // upper right and bottom left matrix, with minus already multiplied - ignition::math::Matrix3d C_AD_FA( - 0, Z_wdot_ * w, -Y_vdot_ * v, - -Z_wdot_ * w, 0, X_udot_ * u, - Y_vdot_ * v, -X_udot_ * u, 0 - ); - - // Torques from angular velocity, with minus already multiplied - ignition::math::Matrix3d C_AD_TA( - 0, N_rdot_ * r, -M_qdot_ * q, - -N_rdot_ * r, 0, K_pdot_ * p, - M_qdot_ * q, -K_pdot_ * p, 0 - ); - - ignition::math::Vector3d coriolis_force = - C_AD_FA*angular_velocity; - ignition::math::Vector3d coriolis_torque = - (C_AD_FA*linear_velocity) + (C_AD_TA*angular_velocity); - - //std::cout << C_AD_FA << "\n"; - //std::cout << "Linear:" << coriolis_force << "\n"; - //std::cout << "Angular:" << angular_velocity << "\n"; - - link_->AddRelativeForce(damping_force + coriolis_force); - link_->AddRelativeTorque(damping_torque + coriolis_torque); +void GazeboUUVPlugin::VelocityCallback(CommandMotorSpeedPtr &rot_velocities) { + motor_commands_[0] = static_cast(rot_velocities->motor_speed(0)); + motor_commands_[1] = static_cast(rot_velocities->motor_speed(1)); + motor_commands_[2] = static_cast(rot_velocities->motor_speed(2)); + motor_commands_[3] = static_cast(rot_velocities->motor_speed(3)); } -GZ_REGISTER_MODEL_PLUGIN(GazeboUUVPlugin) +void GazeboUUVPlugin::UpdateForcesAndMoments() { +/** + * This method: + * - computes the hydrodynamic effects (according to FOSSEN 2011 - Handbook of Marine Craft Hydrodynamics and Motion Control) + * - applies the Hydro effects as forces/moments and are than applied to the vehicle + * - computes the forces/moments based on the motor commands + */ + + //Calculate and apply rotor thrust and drag + double forces[4]; + double torques[4]; + double totalTorque = 0; + for(int i = 0; i < 4; i++){ + if(std::abs(motor_commands_[i]) < dead_zone_){ + forces[i] = 0.0; + } else { + forces[i] = motor_force_constant_ * motor_commands_[i] * std::abs(motor_commands_[i]); + //std::cout << "Force:" << i <<" , "<< forces[i] << ", "; + //std::cout << "Motor Speed:" << motor_commands_[i] << "\n"; + } + + torques[i] = -turning_directions_[i] * forces[i] * motor_torque_constant_; + totalTorque = totalTorque + torques[i]; + } + prop0Link_->AddRelativeForce(ignition::math::Vector3d(forces[0], 0, 0)); + prop1Link_->AddRelativeForce(ignition::math::Vector3d(forces[1], 0, 0)); + prop2Link_->AddRelativeForce(ignition::math::Vector3d(forces[2], 0, 0)); + prop3Link_->AddRelativeForce(ignition::math::Vector3d(forces[3], 0, 0)); + baseLink_->AddRelativeTorque(ignition::math::Vector3d(totalTorque, 0, 0)); + + //Calculate and add hydrodynamic forces + #if GAZEBO_MAJOR_VERSION >= 9 + ignition::math::Vector3d linear_velocity = baseLink_->RelativeLinearVel(); + ignition::math::Vector3d angular_velocity = baseLink_->RelativeAngularVel(); + #else + ignition::math::Vector3d linear_velocity = ignitionFromGazeboMath(baseLink_->GetRelativeLinearVel()); + ignition::math::Vector3d angular_velocity = ignitionFromGazeboMath(baseLink_->GetRelativeAngularVel()); + #endif + double u = linear_velocity[0]; + double v = linear_velocity[1]; + double w = linear_velocity[2]; + double p = angular_velocity[0]; + double q = angular_velocity[1]; + double r = angular_velocity[2]; + + // Linear Damping Matrix, with minus already multiplied + ignition::math::Matrix3d D_FL( + -X_u_, 0, 0, + 0, -Y_v_, 0, + 0, 0, -Z_w_ + ); + + // Angular Damping Matrix, with minus already multiplied + ignition::math::Matrix3d D_FA( + -K_p_, 0, 0, + 0, -M_q_, 0, + 0, 0, -N_r_ + ); + + ignition::math::Vector3d damping_force = D_FL*linear_velocity; + ignition::math::Vector3d damping_torque = D_FA*angular_velocity; + + // Corriolis Forces and Torques + // upper right and bottom left matrix, with minus already multiplied + ignition::math::Matrix3d C_AD_FA( + 0, Z_wdot_ * w, -Y_vdot_ * v, + -Z_wdot_ * w, 0, X_udot_ * u, + Y_vdot_ * v, -X_udot_ * u, 0 + ); + + // Torques from angular velocity, with minus already multiplied + ignition::math::Matrix3d C_AD_TA( + 0, N_rdot_ * r, -M_qdot_ * q, + -N_rdot_ * r, 0, K_pdot_ * p, + M_qdot_ * q, -K_pdot_ * p, 0 + ); + + ignition::math::Vector3d coriolis_force = C_AD_FA*angular_velocity; + ignition::math::Vector3d coriolis_torque = (C_AD_FA*linear_velocity) + (C_AD_TA*angular_velocity); + + //std::cout << C_AD_FA << "\n"; + //std::cout << "Linear:" << coriolis_force << "\n"; + //std::cout << "Angular:" << angular_velocity << "\n"; + + baseLink_->AddRelativeForce(damping_force + coriolis_force); + baseLink_->AddRelativeTorque(damping_torque + coriolis_torque); + } + +GZ_REGISTER_MODEL_PLUGIN(GazeboUUVPlugin); } diff --git a/worlds/uuv_hippocampus.world b/worlds/uuv_hippocampus.world new file mode 100644 index 0000000000..6a73ff5413 --- /dev/null +++ b/worlds/uuv_hippocampus.world @@ -0,0 +1,116 @@ + + + + + + + + + true + 0 0 2 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.001 0.001 -0.9 + + + + + + model://uuv_hippocampus + 0.5 0.5 0 0 0 3.1415 + + + + + false + + + true + 0 0 0 0 0 0 + + + 0 10 0 1.570796 0 0 + + + 0.0025 + 20 + + + + + + + + 10 0 0 0 1.570796 0 + + + 0.0025 + 20 + + + + + + + + 0 0 -10 0 0 0 + + + 0.0025 + 20 + + + + + + + + + + + + 0 0 -9.8066 + + + quick + 10 + 1.3 + 0 + + + 0 + 0.2 + 100 + 0.001 + + + 0.004 + 1 + 250 + 6.0e-6 2.3e-5 -4.2e-5 + + +