diff --git a/glider_hybrid_whoi_description/urdf/glider_hybrid_whoi_base_kinematics.xacro b/glider_hybrid_whoi_description/urdf/glider_hybrid_whoi_base_kinematics.xacro
index 40f7dfd8..4b06e468 100644
--- a/glider_hybrid_whoi_description/urdf/glider_hybrid_whoi_base_kinematics.xacro
+++ b/glider_hybrid_whoi_description/urdf/glider_hybrid_whoi_base_kinematics.xacro
@@ -157,6 +157,33 @@
-0.020919
1.4699
0.97895
+
+
+ true
+ false
+
+ 48.2
+ 2.0
+ 0.105
+ 7.8
+ 0.07
+ 0.4
+ 0.711
+ -0.018
+ -0.01024
+ 0.03
+ 2.4e-04
+ 0 0 0
+ 0 0 0.0054
+
+ 0 0 0 0 0 0
+ 0 -50 0 0 0 0
+ 0 0 -50 0 0 0
+ 0 0 0 0 0 0
+ 0 0 0 0 -12 0
+ 0 0 0 0 0 -12
+
+
hydrodynamics/current_velocity/${namespace}
@@ -176,7 +203,7 @@
0.0
- true
+ false
diff --git a/glider_hybrid_whoi_gazebo/worlds/seafloor_underwater_stratified_current.world b/glider_hybrid_whoi_gazebo/worlds/seafloor_underwater_stratified_current.world
index fc318d8c..4653a69d 100644
--- a/glider_hybrid_whoi_gazebo/worlds/seafloor_underwater_stratified_current.world
+++ b/glider_hybrid_whoi_gazebo/worlds/seafloor_underwater_stratified_current.world
@@ -121,7 +121,7 @@
transientOceanCurrentDatabase.csv
-
+
diff --git a/kinematics_ros_plugins/include/kinematics_ros_plugins/KinematicsROSPlugin.hh b/kinematics_ros_plugins/include/kinematics_ros_plugins/KinematicsROSPlugin.hh
index 26556fd3..4db99283 100644
--- a/kinematics_ros_plugins/include/kinematics_ros_plugins/KinematicsROSPlugin.hh
+++ b/kinematics_ros_plugins/include/kinematics_ros_plugins/KinematicsROSPlugin.hh
@@ -225,6 +225,93 @@ namespace kinematics_ros
protected: double C_D;
protected: double C_L;
+ /// ============================================= ///
+ /// =========== DYNAMICS (for pitch) ======== ///
+ /// ============================================= ///
+ /// \brief A flag for dynamics calculations
+ private: bool dynamics;
+
+ /// \brief A flag for hardcoded input
+ private: bool hardCodeInput;
+
+ /// \brief Buoyancy pump position
+ protected: double pumpPos;
+
+ /// \brief Sliding mass position
+ protected: double massPos;
+
+ /// \brief total_mass
+ protected: double m;
+
+ /// \brief Center of gravity
+ protected: ignition::math::Vector3d cog;
+
+ /// \brief Center of buoyancy in the body frame
+ protected: ignition::math::Vector3d cob;
+
+ /// \brief ballast_radius
+ protected: double r_w;
+
+ /// \brief hull_length
+ protected: double l_h;
+
+ /// \brief hull_radius
+ protected: double r_h;
+
+ /// \brief hull_mass
+ protected: double m_h;
+
+ /// \brief shifter_mass
+ protected: double m_s;
+
+ /// \brief initial_mass_position
+ protected: double x_s_o;
+
+ /// \brief initial_ballast_position
+ protected: double x_w_o;
+
+ /// \brief max_mass_position
+ protected: double x_s_o_max;
+
+ /// \brief max_ballast_volume
+ protected: double vol_w_max;
+
+ /// \brief Added-mass matrix
+ protected: Eigen::Matrix6d Ma;
+
+ /// \brief Added-mass associated Coriolis matrix
+ protected: Eigen::Matrix6d Ca;
+
+ /// \brief Damping matrix
+ protected: Eigen::Matrix6d D;
+
+ /// \brief Linear damping matrix
+ protected: Eigen::Matrix6d DLin;
+
+ /// \brief Last timestamp (in seconds)
+ protected: double lastTime;
+
+ /// \brief Dynamics post/vel variables
+ protected: Eigen::Vector6d eta, eta_last, eta_dot;
+ protected: Eigen::Vector6d nu, nu_last, nu_dot;
+
+ /// \brief Get parameters (read matrix form defenitions)
+ protected: void GetParam(std::string _tag, std::vector& _output);
+
+ /// \brief Filtered linear & angular acceleration vector in link frame.
+ /// This is used to prevent the model to become unstable given that Gazebo
+ /// only calls the update function at the beginning or at the end of a
+ /// iteration of the physics engine
+ protected: Eigen::Vector6d filteredAcc;
+
+ /// \brief Convey commands to calculate dynamics
+ protected: virtual void ConveyDynamicsCommands
+ (const frl_vehicle_msgs::UwGliderCommand::ConstPtr &_msg);
+
+ /// \brief Calculate dynamics when commands conveyed and every updates
+ protected: virtual void CalculateDynamics
+ (double _massPos, double _pumpVol, double _thrustPower);
+
/// ============================================= ///
/// ===========-=== Visual Effects ======-======= ///
/// ============================================= ///
@@ -369,6 +456,23 @@ inline ignition::math::Matrix3d Mat3dToGazebo(const Eigen::Matrix3d &_x)
_x(1, 0), _x(1, 1), _x(1, 2),
_x(2, 0), _x(2, 1), _x(2, 2));
}
+
+inline Eigen::Matrix6d Jacobian(const double &phi,
+ const double &theta,
+ const double &psi)
+{
+ Eigen::Matrix6d out;
+ out << cos(psi)*cos(theta),
+ -sin(psi)*cos(phi)+cos(psi)*sin(theta)*sin(phi),
+ sin(psi)*sin(phi)+cos(psi)*cos(phi)*sin(theta), 0, 0, 0,
+ sin(psi)*cos(theta), cos(psi)*cos(phi)+sin(psi)*sin(theta)*sin(phi),
+ -cos(psi)*sin(phi)+sin(psi)*sin(theta)*cos(phi), 0, 0, 0,
+ -sin(theta), cos(theta)*sin(phi), cos(theta)*cos(phi), 0, 0, 0,
+ 0, 0, 0, 1, sin(phi)*tan(theta), cos(phi)*tan(theta),
+ 0, 0, 0, 0, cos(phi), -sin(phi),
+ 0, 0, 0, 0, sin(phi)/cos(theta), cos(phi)/cos(theta);
+ return out;
+}
}
#endif // __KINEMATICS_ROS_PLUGIN_HH__
diff --git a/kinematics_ros_plugins/src/KinematicsROSPlugin.cc b/kinematics_ros_plugins/src/KinematicsROSPlugin.cc
index 4b4ef344..c2504732 100644
--- a/kinematics_ros_plugins/src/KinematicsROSPlugin.cc
+++ b/kinematics_ros_plugins/src/KinematicsROSPlugin.cc
@@ -192,10 +192,58 @@ void KinematicsROSPlugin::Load(gazebo::physics::ModelPtr _model,
else
this->f_thruster_power_w3 = 0.97895;
+ // ---------------- READ Dynamics coefficients ---------------- //
+ if (_sdf->HasElement("use_dynamics"))
+ this->dynamics = _sdf->Get("use_dynamics");
+ else
+ this->dynamics = false; // default off
+
+ if (this->dynamics)
+ {
+ sdf::ElementPtr sdfDynamics = _sdf->GetElement("dynamics");
+ this->r_w = sdfDynamics->Get("ballast_radius");
+ this->l_h = sdfDynamics->Get("hull_length");
+ this->r_h = sdfDynamics->Get("hull_radius");
+ this->m_h = sdfDynamics->Get("hull_mass");
+ this->m_s = sdfDynamics->Get("shifter_mass");
+ this->x_s_o = sdfDynamics->Get("default_mass_position");
+ this->x_w_o = sdfDynamics->Get("default_ballast_position");
+ this->x_s_o_max = sdfDynamics->Get("max_mass_position");
+ this->vol_w_max = (sdfDynamics->Get("max_ballast_volume"))/(M_PI*(this->r_w)*(this->r_w));
+ this->cog = Str2Vector(sdfDynamics->Get("center_of_gravity"));
+ this->cob = Str2Vector(sdfDynamics->Get("center_of_buoyancy"));
+
+ // Load linear damping coefficients, if provided. Otherwise, the linear
+ // damping matrix is set to zero
+ if (sdfDynamics->HasElement("linear_damping"))
+ this->DLin = Str2Matrix6(sdfDynamics->Get("linear_damping"));
+ else
+ gzmsg << "Using linear damping NULL" << std::endl;
+
+ // Initial calculation
+ this->massPos = sdfDynamics->Get("initial_mass_position");
+ this->pumpPos = sdfDynamics->Get("initial_ballast_position");
+ this->motorPower = 0.0;
+
+ // Initialize position/orientation and velocity;
+ this->eta.setZero();
+ this->eta_last.setZero();
+ this->eta_dot.setZero();
+ this->nu.setZero();
+ this->nu_last.setZero();
+ this->nu_dot.setZero();
+
+ if (_sdf->HasElement("hard_code_input"))
+ this->hardCodeInput = _sdf->Get("hard_code_input");
+ else
+ this->hardCodeInput = false;
+ }
+
// Initiate variables
this->prev_pitch = 0.0;
this->prev_yaw = 0.0;
this->prev_motorPower = 0.0;
+ this->flowVelocity = ignition::math::Vector3d(0.0, 0.0, 0.0);
// Free surface detection
this->buoyancyFlag = true; // Initialize buoyancy engine
@@ -217,9 +265,18 @@ void KinematicsROSPlugin::Load(gazebo::physics::ModelPtr _model,
// Initiate variables
this->prev_pitch = 0.0;
- this->prev_yaw = 0.0;
this->prev_motorPower = 0.0;
- this->modelVel = ignition::math::Vector3d(0.0, 0.0, 0.0);
+ // this->modelVel = ignition::math::Vector3d(0.0, 0.0, 0.0);
+ ignition::math::Pose3d model_pose = this->model->WorldPose();
+ ignition::math::Vector3 model_RPY =
+ calculateRPY(model_pose.Rot().X(), model_pose.Rot().Y(),
+ model_pose.Rot().Z(), model_pose.Rot().W());
+ this->prev_yaw = model_RPY.Z();
+ this->eta_last << model_pose.Pos().X(), model_pose.Pos().Y(), model_pose.Pos().Z(),
+ model_RPY.X(), model_RPY.Y(), model_RPY.Z();
+
+ // this->targetLinearVel_prev = Eigen::Vector3d(0.0, 0.0, 0.0);
+ // this->targetAngularVel_prev = Eigen::Vector3d(0.0, 0.0, 0.0);
// Free surface detection
this->buoyancyFlag = true; // Initialize buoyancy engine
@@ -289,8 +346,9 @@ void KinematicsROSPlugin::Update(const gazebo::common::UpdateInfo &)
// Update model state
this->updateModelState();
- // Apply ocean current
- this->applyOceanCurrent();
+ // Dynamics Calculation
+ if (this->dynamics)
+ this->CalculateDynamics(this->massPos, this->pumpPos, this->motorPower);
// Send status
this->ConveyModelState();
@@ -338,11 +396,340 @@ void KinematicsROSPlugin::ConveyCommands(
const frl_vehicle_msgs::UwGliderCommand::ConstPtr &_msg)
{
// Convey commands to functions
- this->ConveyKinematicsCommands(_msg);
+ if(this->dynamics)
+ {
+ // Calculate Dynamics
+ this->ConveyDynamicsCommands(_msg);
+ }
+ else
+ {
+ // Calculate Kinematics
+ this->ConveyKinematicsCommands(_msg);
+
+ // Apply ocean current
+ this->applyOceanCurrent();
+ }
this->controlMsgDetected = true;
}
+/////////////////////////////////////////////////
+void KinematicsROSPlugin::ConveyDynamicsCommands(
+ const frl_vehicle_msgs::UwGliderCommand::ConstPtr &_msg)
+{
+ // Motor/thruster
+ this->motorPower = this->prev_motorPower;
+ switch(_msg->motor_cmd_type)
+ {
+ case frl_vehicle_msgs::UwGliderCommand::MOTOR_CMD_VOLTAGE:
+ this->calcThrusterForce(_msg->motor_cmd_type,
+ _msg->target_motor_cmd);
+ this->prev_motorPower = this->motorPower;
+ break;
+
+ case frl_vehicle_msgs::UwGliderCommand::MOTOR_CMD_POWER:
+ this->calcThrusterForce(_msg->motor_cmd_type,
+ _msg->target_motor_cmd);
+ this->prev_motorPower = this->motorPower;
+ break;
+
+ case frl_vehicle_msgs::UwGliderCommand::MOTOR_CMD_NONE:
+ this->motorPower = this->prev_motorPower;
+ break;
+
+ default:
+ gzmsg << "WRONG THRUSTER COMMAND TYPE "
+ << "(1 : voltage control, 2: power control)"
+ << std::endl;
+ break;
+ }
+
+ // Pitch by battery(mass) position
+ switch(_msg->pitch_cmd_type)
+ {
+ case frl_vehicle_msgs::UwGliderCommand::PITCH_CMD_BATT_POS:
+ {
+ this->massPos = _msg->target_pitch_value/39.3701*1000; // [m to inch]
+ break;
+ }
+
+ default:
+ gzmsg << "WRONG PITCH COMMAND TYPE "
+ << " For dynamics, only (1 : battery position) can be used"
+ << std::endl;
+ break;
+ }
+
+ // Ballast pump
+ if (_msg->target_pumped_volume != 0.0)
+ {
+ gzmsg << "Pump Position control Received" << std::endl;
+ this->pumpPos = _msg->target_pumped_volume/1000000.0/(M_PI*(this->r_w)*(this->r_w));
+ }
+
+ // Heading control (rudder)
+ double target_yaw = 1000.0;
+ switch(_msg->rudder_control_mode)
+ {
+ // TODO : No control algorithm included yet for target heading
+ // case frl_vehicle_msgs::UwGliderCommand::RUDDER_CONTROL_HEADING:
+ // target_yaw = M_PI/2 - _msg->target_heading;
+ // break;
+
+ case frl_vehicle_msgs::UwGliderCommand::RUDDER_CONTROL_ANGLE:
+ {
+ double rudderAngleZero = 0.0;
+ double rudderAnglePort = M_PI/6.0; // 30 degree
+ double rudderAngleStbd = -M_PI/6.0; // 30 degree
+ switch(_msg->rudder_angle)
+ {
+ case frl_vehicle_msgs::UwGliderCommand::RUDDER_ANGLE_CENTER:
+ target_yaw = rudderAngleZero;
+ break;
+
+ case frl_vehicle_msgs::UwGliderCommand::RUDDER_ANGLE_PORT:
+ target_yaw = rudderAnglePort;
+ break;
+
+ case frl_vehicle_msgs::UwGliderCommand::RUDDER_ANGLE_STBD:
+ target_yaw = rudderAngleStbd;
+ break;
+
+ case frl_vehicle_msgs::UwGliderCommand::RUDDER_ANGLE_DIRECT:
+ target_yaw = -_msg->target_rudder_angle; // positive turn stbd
+ break;
+
+ default:
+ gzmsg << "WRONG RUDDER ANGLE COMMAND "
+ << "(1: center, 2: port, 3: staboard, 4: direct)"
+ << std::endl; break;
+ }
+ break;
+ }
+
+ case frl_vehicle_msgs::UwGliderCommand::RUDDER_CONTROL_NONE:
+ break;
+
+ default:
+ gzmsg << "WRONG RUDDER COMMAND TYPE "
+ << "For dynamics, only 2: control angle is available"
+ << std::endl;
+ break;
+ }
+
+ // Assign target yaw orientation if msg received
+ if (target_yaw != 1000.0)
+ this->prev_yaw = target_yaw;
+}
+
+/////////////////////////////////////////////////
+void KinematicsROSPlugin::CalculateDynamics(
+ double _massPos, double _pumpPos, double _thrustPower)
+{
+ // Hard input data without ros topic for plugin testing
+ if (this->hardCodeInput){
+ double hardInputPumpVol, hardInputMassPos;
+ double hardInputYaw, hardInputThrustPower;
+ double _time = this->time.Double();
+ double Period = 400.0;
+
+ hardInputPumpVol = 1.0e-04 * cos(_time/Period);
+ hardInputMassPos = 1.0e-03 * cos(_time/Period);
+ hardInputYaw = 30.0/180.0*M_PI;
+ hardInputThrustPower = 0.5;
+
+ // Max value limiter
+ if (hardInputMassPos > x_s_o_max)
+ hardInputMassPos = x_s_o_max;
+ if (hardInputMassPos < -x_s_o_max)
+ hardInputMassPos = -x_s_o_max;
+ if (hardInputPumpVol > vol_w_max)
+ hardInputPumpVol = vol_w_max;
+ if (hardInputPumpVol < -vol_w_max)
+ hardInputPumpVol = -vol_w_max;
+
+ // Export
+ _pumpPos = -hardInputPumpVol/(M_PI*(this->r_w)*(this->r_w));
+ _massPos = hardInputMassPos;
+ this->prev_yaw = hardInputYaw;
+ _thrustPower = hardInputThrustPower;
+ }
+
+ // ---- Mass calculation ----- //
+ // Ballast volume and mass
+ double V_B = -_pumpPos*M_PI*(this->r_w)*(this->r_w);
+ double m_w = V_B*this->fluidDensity;
+
+ // Total vehicle mass
+ this->m = this->m_h + this->m_s + m_w;
+
+ // Moving mass position
+ double x_s = this->x_s_o + _massPos;
+
+ // Hull mass center (uniform hull mass distribution assumed)
+ double x_h = -this->m_s/this->m_h*this->x_s_o;
+
+ // Ballast tank mass center (pushing from front)
+ double x_w = this->x_w_o+(V_B/(M_PI*(this->r_w)*(this->r_w)));
+
+ // Center of gravity
+ (this->cog).X() = (x_h*this->m_h + x_s*this->m_s + x_w*m_w)/(this->m_h+this->m_s+m_w);
+
+ // ---- Rigid Body Matricies ----- //
+ double a = this->l_h/2.0; // half the length
+ double b = this->r_h; // hull radius
+ // inertial matrix (Fossen p.42 (2.156))
+ double I_yy = 4.0/15.0*m*(b*b+a*a); double I_zz = I_yy;double I_xx = 4.0/15.0*m*(b*b+b*b);
+ // M_RB = Fossen (2.91)
+ Eigen::Matrix3d m_S_r_cg = this->m * CrossProductOperator(cog);
+ Eigen::Matrix6d M_RB = Eigen::Matrix6d::Identity() * this->m;
+ M_RB(3,3) = I_xx; M_RB(4,4) = I_yy; M_RB(5,5) = I_zz;
+ Eigen::Matrix6d M_RB_NoDiag = M_RB;
+ M_RB(0,3) = -m_S_r_cg(0,0); M_RB(0,4) = -m_S_r_cg(0,1); M_RB(0,5) = -m_S_r_cg(0,2);
+ M_RB(1,3) = -m_S_r_cg(1,0); M_RB(1,4) = -m_S_r_cg(1,1); M_RB(1,5) = -m_S_r_cg(1,2);
+ M_RB(2,3) = -m_S_r_cg(2,0); M_RB(2,4) = -m_S_r_cg(2,1); M_RB(2,5) = -m_S_r_cg(2,2);
+ M_RB(3,0) = m_S_r_cg(0,0); M_RB(3,1) = m_S_r_cg(0,1); M_RB(3,2) = m_S_r_cg(0,2);
+ M_RB(4,0) = m_S_r_cg(1,0); M_RB(4,1) = m_S_r_cg(1,1); M_RB(4,2) = m_S_r_cg(1,2);
+ M_RB(5,0) = m_S_r_cg(2,0); M_RB(5,1) = m_S_r_cg(2,1); M_RB(5,2) = m_S_r_cg(2,2);
+ // Rigid Body Coriolis diagonal term
+ Eigen::Matrix6d C_RB = Eigen::Matrix6d::Zero();
+ Eigen::Vector6d ab = M_RB * nu_last;
+ Eigen::Matrix3d Sa = -1.0 * CrossProductOperator(ab.head<3>());
+ C_RB << Eigen::Matrix3d::Zero(), Sa,
+ Sa, -1.0 * CrossProductOperator(ab.tail<3>());
+
+ // ---- Added mass matrix calculation ----- //
+ // computed according to the procedure in Fossen p. 41
+ double e = 1.0-std::pow(b/a,2);
+ double alpha_o = (2.0*(1.0-e*e)/std::pow(e,3))*(0.5*std::log((1.0+e)/(1.0-e))-e);
+ double beta_o = 1.0/std::pow(e,2)-((1-std::pow(e,2))/(2.0*std::pow(e,3)))*std::log((1.0+e)/(1.0-e));
+ double X_udot = -(alpha_o/(2.0-alpha_o))*this->m;
+ double Y_vdot = -(beta_o/(2.0-beta_o))*this->m;
+ double Z_wdot = Y_vdot;
+ double K_pdot = 0.0;
+ double M_qdot = -0.2*this->m*(std::pow(std::pow(b,2)-std::pow(a,2),2)*(alpha_o-beta_o))/
+ (2.0*(std::pow(b,2)-std::pow(a,2))+(std::pow(b,2)+std::pow(a,2))*(beta_o-alpha_o));
+ double N_rdot = M_qdot;
+
+ // M_A_cg
+ Eigen::Matrix6d Ma_cg = Eigen::Matrix6d::Identity();
+ Ma_cg(0,0) = -X_udot; Ma_cg(1,1) = -Y_vdot; Ma_cg(2,2) = -Z_wdot;
+ Ma_cg(3,3) = -K_pdot; Ma_cg(4,4) = -M_qdot; Ma_cg(5,5) = -N_rdot;
+
+ // M_A = LeftSide*M_A_CG*RightSide
+ Eigen::Vector3d cog = Eigen::Vector3d((this->cog).X(),(this->cog).Y(),(this->cog).Z());
+ Eigen::Matrix3d S_r_cg = CrossProductOperator(cog);
+ Eigen::Matrix6d LeftSide = Eigen::Matrix6d::Identity();
+ LeftSide(0,3) = S_r_cg(0,0); LeftSide(0,4) = S_r_cg(0,1); LeftSide(0,5) = S_r_cg(0,2);
+ LeftSide(1,3) = S_r_cg(1,0); LeftSide(1,4) = S_r_cg(1,1); LeftSide(1,5) = S_r_cg(1,2);
+ LeftSide(2,3) = S_r_cg(2,0); LeftSide(2,4) = S_r_cg(2,1); LeftSide(2,5) = S_r_cg(2,2);
+ Eigen::Matrix6d RightSide = Eigen::Matrix6d::Identity();
+ Eigen::Matrix3d S_r_cg_T = S_r_cg.transpose();
+ RightSide(0,3) = S_r_cg_T(0,0); RightSide(0,4) = S_r_cg_T(0,1); RightSide(0,5) = S_r_cg_T(0,2);
+ RightSide(1,3) = S_r_cg_T(1,0); RightSide(1,4) = S_r_cg_T(1,1); RightSide(1,5) = S_r_cg_T(1,2);
+ RightSide(2,3) = S_r_cg_T(2,0); RightSide(2,4) = S_r_cg_T(2,1); RightSide(2,5) = S_r_cg_T(2,2);
+ Eigen::Matrix6d M_A = LeftSide*Ma_cg*RightSide;
+
+ // Added mass Coriolis diagonal term
+ Eigen::Matrix6d C_A = Eigen::Matrix6d::Zero();
+ ab = M_A * nu_last;
+ Sa = -1.0 * CrossProductOperator(ab.head<3>());
+ C_A << Eigen::Matrix3d::Zero(), Sa,
+ Sa, -1.0 * CrossProductOperator(ab.tail<3>());
+
+ // ---- Hydrostatic restoring forces ----- //
+ double W = this->m*this->gravity; // Gravitational force acting on center of gravity
+ double B = (this->m_h+this->m_s)*this->gravity; // Buoyancy force acting on center of buoyancy
+ Eigen::Vector6d tau_R;
+ tau_R << (W-B)*sin(eta(4)), -(W-B)*cos(eta(4))*sin(eta(3)), -(W-B)*cos(eta(4))*cos(eta(3)),
+ -((this->cog).Y()*W-(this->cob).Y()*B)*cos(eta(4))*cos(eta(3))+((this->cog).Z()*W-(this->cob).Z()*B)*cos(eta(4))*sin(eta(3)),
+ ((this->cog).Z()*W-(this->cob).Z()*B)*sin(eta(4))+((this->cog).X()*W-(this->cob).X()*B)*cos(eta(4))*cos(eta(3)),
+ -((this->cog).X()*W-(this->cob).X()*B)*cos(eta(4))*sin(eta(3))-((this->cog).Y()*W-(this->cob).Y()*B)*sin(eta(4));
+
+ // --- Calculate hull hydrodynamic force (Graver) --- //
+ //angle of attack of hull
+ double alpha_h;
+ if (abs(nu_last(0)) > 0.0)
+ alpha_h = atan(nu_last(2)/nu_last(0));
+ else
+ alpha_h = 0.0;
+ // Graver hydrodynamic forces for hull
+ double A_h = M_PI*this->r_h*this->r_h;
+ double C_D_h = 0.214 + 32.3*alpha_h*alpha_h;
+ double C_L_h = 11.76*alpha_h + 4.6 * alpha_h*alpha_h;
+ double C_M_h = 0.63*alpha_h;
+ double F_h_kernel = std::pow(std::pow(nu_last(0)*nu_last(0)+nu_last(1)*nu_last(1)+nu_last(2)*nu_last(2),0.5),2);
+ double F_D_h = 0.5*this->fluidDensity*A_h*F_h_kernel*C_D_h;
+ double F_L_h = 0.5*this->fluidDensity*A_h*F_h_kernel*C_L_h;
+ double F_M_h = 0.5*this->fluidDensity*A_h*F_h_kernel*C_M_h;
+ Eigen::Vector6d tau_H_h; tau_H_h << F_L_h*sin(alpha_h)-F_D_h*cos(alpha_h), 0.0, -F_L_h*cos(alpha_h)-F_D_h*sin(alpha_h), 0.0, F_M_h, 0.0;
+
+ // --- Calculate rudder hydrodynamic force (Graver) --- //
+ //angle of attack of rudder
+ double alpha_r;
+ if (abs(nu_last(0)) > 0.0)
+ alpha_r = this->prev_yaw;
+ else
+ alpha_r = 0.0;
+ // Graver hydrodynamic forces for rudder
+ // TODO : Demo hydordynamic forces coefficient used here using the template of the hull
+ double A_r = M_PI*this->r_h*this->r_h / 10.0;
+ double C_D_r = (0.214 + 32.3*alpha_r*alpha_r) / 10.0;
+ double C_L_r = (11.76*alpha_r + 4.6 * alpha_r*alpha_r) / 10.0;
+ double C_M_r = 0.63*alpha_r * 10.0;
+ double F_r_kernel = std::pow(std::pow(nu_last(0)*nu_last(0)+nu_last(1)*nu_last(1)+nu_last(2)*nu_last(2),0.5),2);
+ double F_D_r = 0.5*this->fluidDensity*A_r*F_r_kernel*C_D_r;
+ double F_L_r = 0.5*this->fluidDensity*A_r*F_r_kernel*C_L_r;
+ double F_M_r = 0.5*this->fluidDensity*A_r*F_r_kernel*C_M_r;
+ Eigen::Vector6d tau_H_r; tau_H_r << F_L_r*sin(alpha_r)-F_D_r*cos(alpha_r),
+ -F_L_r*cos(alpha_r)-F_D_r*sin(alpha_r), 0.0, 0.0, 0.0, F_M_r;
+
+ // Hydrodynamic forces by the propeller
+ Eigen::Vector6d tau_H_p; tau_H_p << _thrustPower, 0.0, 0.0, 0.0, 0.0, 0.0;
+
+ // --- Solve --- //
+ // totla matricies and forces
+ Eigen::Matrix6d M_tot = M_RB + M_A;
+ Eigen::Matrix6d C_tot = C_RB + C_A;
+ Eigen::Matrix6d D_tot = -1.0 * this->DLin;
+ Eigen::Vector6d tau_H = tau_H_h + tau_H_r + tau_H_p;
+
+ // Solve for nu_dot
+ this->nu_dot = (M_tot).colPivHouseholderQr().solve(tau_H-(C_tot)*nu_last-(D_tot)*nu_last-tau_R);
+
+ // --- Update vehicle position --- //
+ // Caculate position
+ double dt = this->time.Double() - this->lastTime;
+ this->nu = this->nu_last + dt*this->nu_dot;
+ this->eta_dot = Jacobian(this->eta_last(3),this->eta_last(4),this->eta_last(5))*this->nu;
+ this->eta = this->eta_last + dt*this->eta_dot;
+
+ // Apply ocean current
+ double dx = this->flowVelocity.Y() * dt;
+ double dy = this->flowVelocity.X() * dt;
+ double dz = -this->flowVelocity.Z() * dt;
+ this->eta(0) += dx; this->eta(1) += dy; this->eta(2) += dz;
+
+ // Update pose
+ ignition::math::Pose3d model_pose;
+ model_pose.Pos().X() = this->eta(0);
+ model_pose.Pos().Y() = this->eta(1);
+ model_pose.Pos().Z() = this->eta(2);
+ ignition::math::Quaterniond target_orientation;
+ target_orientation.Euler(this->eta(3), this->eta(4), this->eta(5));
+ model_pose.Rot() = target_orientation;
+ this->model->SetWorldPose(model_pose);
+
+ // update time and variables
+ this->lastTime = this->time.Double();
+ this->nu_last = this->nu;
+ this->eta_last = this->eta;
+
+ // Eliminate Gazebo physics
+ this->link->ResetPhysicsStates();
+}
+
+
/////////////////////////////////////////////////
void KinematicsROSPlugin::ConveyKinematicsCommands(
const frl_vehicle_msgs::UwGliderCommand::ConstPtr &_msg)
@@ -362,10 +749,12 @@ void KinematicsROSPlugin::ConveyKinematicsCommands(
{
case frl_vehicle_msgs::UwGliderCommand::PITCH_CMD_BATT_POS:
{
- this->battpos = _msg->target_pitch_value/39.3701*1000; // [m to inch]
- double target_pitch_rad
- = this->f_pitch_battpos_cal_m * this->battpos + this->f_pitch_battpos_cal_b;
- target_pitch = target_pitch_rad;
+ // Commented out : Use dynamics flag for battery position command
+ gzmsg << "USE DYNAMICS for battery position command " << std::endl;
+ // this->battpos = _msg->target_pitch_value/39.3701*1000; // [m to inch]
+ // double target_pitch_rad
+ // = this->f_pitch_battpos_cal_m * this->battpos + this->f_pitch_battpos_cal_b;
+ // target_pitch = target_pitch_rad;
break;
}
@@ -552,11 +941,11 @@ void KinematicsROSPlugin::ConveyKinematicsCommands(
targetPose.Rot().W() = cmd_msg.request.model_state.pose.orientation.w;
this->model->SetWorldPose(targetPose);
- ignition::math::v4::Vector3d targetLinearTwist;
+ ignition::math::Vector3d targetLinearTwist;
targetLinearTwist.X() = cmd_msg.request.model_state.twist.linear.x;
targetLinearTwist.Y() = cmd_msg.request.model_state.twist.linear.y;
targetLinearTwist.Z() = cmd_msg.request.model_state.twist.linear.z;
- ignition::math::v4::Vector3d targetAngularTwist(0.0, 0.0, 0.0);
+ ignition::math::Vector3d targetAngularTwist(0.0, 0.0, 0.0);
this->model->SetWorldTwist(targetLinearTwist, targetAngularTwist);
this->modelState = cmd_msg.request.model_state;
@@ -583,7 +972,7 @@ void KinematicsROSPlugin::applyOceanCurrent()
this->link->SetLinearVel(ignition::math::Vector3d(
this->modelVel.X() + this->flowVelocity.Y(),
this->modelVel.Y() + this->flowVelocity.X(),
- this->modelVel.Z() + this->flowVelocity.Z()));
+ this->modelVel.Z() - this->flowVelocity.Z()));
}
/////////////////////////////////////////////////
@@ -652,7 +1041,7 @@ void KinematicsROSPlugin::ConveyModelState()
void KinematicsROSPlugin::writeCSVLog()
{
// CSV log write stream
-if (this->writeLogFlag && this->controlMsgDetected)
+if (this->writeLogFlag)
{
double time = this->time.Double();
if (this->writeCounter == 0)
@@ -844,6 +1233,5 @@ void KinematicsROSPlugin::CheckSubmergence()
}
}
-
GZ_REGISTER_MODEL_PLUGIN(KinematicsROSPlugin)
}
diff --git a/matlab_dynamics/CBSoct.mat b/matlab_dynamics/CBSoct.mat
new file mode 100755
index 00000000..98d641d8
Binary files /dev/null and b/matlab_dynamics/CBSoct.mat differ
diff --git a/matlab_dynamics/Comparisons.png b/matlab_dynamics/Comparisons.png
new file mode 100755
index 00000000..6b9d0265
Binary files /dev/null and b/matlab_dynamics/Comparisons.png differ
diff --git a/matlab_dynamics/GliderDynamics.m b/matlab_dynamics/GliderDynamics.m
new file mode 100755
index 00000000..41e9f51a
--- /dev/null
+++ b/matlab_dynamics/GliderDynamics.m
@@ -0,0 +1,317 @@
+clear;clc;
+close all;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%% Hybrid Glider Simulator (6DOF) %%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%% 22-02-08 Woen-Sug Choi %%%%%%
+%%%% Mixture of Brian and Isa %%%%%%
+%%%% Uses Brian's exp input data %%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%---------------------------%
+%---- Input Variables ----%
+%---------------------------%
+% Damping force for hull
+% Estimated by Brian Claus. Cannot be determined analyitically.
+Xu = 0; Yv = 0; Zw = -14;
+Kp = 0; Mq = -12; Nr = 0;
+D_h = -diag([Xu,Yv,Zw,Kp,Mq,Nr]);
+
+% User setup variables
+t_max = 3000;
+t_min = 120;
+timestep = 0.1;
+
+%-----------------------------------%
+%---- Calculation variables ----%
+%-----------------------------------%
+% Time array construction
+time = 0:timestep:t_max;
+% Thruster force array construction
+F_T = zeros(length(time),1);
+
+% Initial conditions:
+x_s_o = 0.4; % Initial Movable Mass position
+x_w_o = 0.711; % Initial Ballast Tank position
+alpha_e_control = zeros(6,length(time));
+alpha_r_control = zeros(6,length(time));
+eta = zeros(6,length(time));
+eta_dot = zeros(6,length(time));
+nu = zeros(6,length(time));
+nu_dot = zeros(6,length(time));
+tau = zeros(6,length(time));
+epsilon = zeros(6,length(time));
+eta(:,1) = [0; 0; 0; 0; 0; 0;];
+eta_dot(:,1) = [0; 0; 0; 0; 0; 0;];
+nu_dot(:,1) = [0; 0; 0; 0; 0; 0;];
+tau(:,1) = [0; 0; 0; 0; 0; 0;];
+epsilon(:,1) = [0; 0; 0; 0; 0; 0;];
+
+%-----------------------------------%
+%---- Environmental variables ----%
+%-----------------------------------%
+% Physical constants
+rho = 1024; % density [kg/m3]
+g = 9.81; % gravity
+
+% Vehicle properties
+length_h = 2; % Vehicle length [m]
+r_h = 0.105; % radius of vehicle [m]
+A_h = pi*r_h^2; % Area [m2]
+A_e = A_h/10; % Elevator Area [m2]
+m_h = 48.2; % vehicle mass
+m_s = 7.8; % mass shifter mass
+max_cbattpos = 0.015;
+max_delta_battpos = 0.004;
+x_cb = 0; y_cb = 0; z_cb = 0; % Equal to Ballast Tank position fixed
+x_cg = 0; y_cg = 0; z_cg = 0.0054;
+r_w = 0.07; % Ballast Tank radius
+max_V_B = 2.4e-4;
+
+%---------------------------%
+%---- Load variables ----%
+%---------------------------%
+% Load data
+load CBSoct
+m_present_time = CBSoct(:,445)-CBSoct(t_min/5,445);
+i_tmax = find(m_present_time > t_max,1);
+m_present_time = m_present_time(1:i_tmax);
+m_wallast_pumped = CBSoct(1:i_tmax,316).*0.000001;
+m_wattpos = -CBSoct(1:i_tmax,321).*0.0254;
+m_depth = CBSoct(1:i_tmax,337);
+m_depth_rate = CBSoct(1:i_tmax,338);
+m_pitch = CBSoct(1:i_tmax,442);
+t_max = max(m_present_time);
+% Remove nans
+i = ~isnan(m_wallast_pumped)&~isnan(m_wattpos)&~isnan(m_depth)&~isnan(m_pitch)&~isnan(m_depth_rate);
+m_wallast_pumped = m_wallast_pumped(i);
+m_wattpos = m_wattpos(i);
+m_present_time = m_present_time(i);
+m_depth = m_depth(i);
+m_depth_rate = m_depth_rate(i);
+m_pitch = m_pitch(i);
+
+% Interpolate control variables to have the same length
+m_wattposi = interp1(m_present_time,m_wattpos,time);
+m_wallast_pumpedi = interp1(m_present_time,m_wallast_pumped,time);
+m_depthi = interp1(m_present_time,m_depth,time);
+m_depth_ratei = interp1(m_present_time,m_depth_rate,time);
+m_pitchi = interp1(m_present_time,m_pitch,time);
+
+% Data allocations for Variable mass position and ballast pump
+x_s_c = m_wattposi;
+V_B = m_wallast_pumpedi;
+
+%-------------------------------------%
+%---- Glider Kinmatics Matrices ----%
+%-------------------------------------%
+% Functions
+J = @(phi,theta,psi) [cos(psi)*cos(theta), ...
+ -sin(psi)*cos(phi)+cos(psi)*sin(theta)*sin(phi), ...
+ sin(psi)*sin(phi)+cos(psi)*cos(phi)*sin(theta), 0, 0, 0; ...
+ sin(psi)*cos(theta), cos(psi)*cos(phi)+sin(psi)*sin(theta)*sin(phi), ...
+ -cos(psi)*sin(phi)+sin(psi)*sin(theta)*cos(phi), 0, 0, 0; ...
+ -sin(theta), cos(theta)*sin(phi), cos(theta)*cos(phi), 0, 0, 0; ...
+ 0, 0, 0, 1, sin(phi)*tan(theta), cos(phi)*tan(theta); ...
+ 0, 0, 0, 0, cos(phi), -sin(phi); 0, 0, 0, 0, sin(phi)/cos(theta), cos(phi)/cos(theta);];
+skew = @(x1,x2,x3) [0,-x3,x2; x3,0,-x1; -x2,x1,0;];
+
+% Initiate nu
+nu(:,1) = J(eta(4,1),eta(5,1),eta(6,1))\eta_dot(:,1);
+
+% Main loop
+for i = 1:length(time)-1
+
+ % ---- Mass Components ---- %
+ % Ballast masss
+ m_w = V_B(i)*rho;
+ % Total vehicle mass
+ m = m_h + m_s + m_w;
+ % Moving mass position
+ x_s = x_s_o + x_s_c(i);
+ % Hull mass center (uniform hull mass distribution assumed)
+ x_h = -m_s/m_h*x_s_o;
+ % Ballast tank mass center (pushing from front)
+ x_w = x_w_o+(V_B(i)/(pi()*r_w^2));
+ % Center of gravity
+ x_cg = (x_h*m_h + x_s*m_s + x_w*m_w)/(m_h+m_s+m_w);
+
+ %-----------------------------------%
+ %------- Rigid Body Matrices -------%
+ %-----------------------------------%
+ %-- Mass matrix --%
+ a = length_h/2; % half the length
+ b = r_h; % hull radius
+ % inertial matrix (Fossen p.42 (2.156))
+ I_yy = 4/15*m*(b^2+a^2); I_zz = I_yy;
+ I_xx = 4/15*m*(b^2+b^2);
+ I_o = [I_xx,0,0;0,I_yy,0;0,0,I_zz];
+ % M_RB = Fossen (2.91)
+ M_RB = zeros(6,6);
+ M_RB(1:3,1:3) = m*diag([1,1,1]);
+ M_RB(1:3,4:6) = -m*skew(x_cg,y_cg,z_cg);
+ M_RB(4:6,1:3) = m*skew(x_cg,y_cg,z_cg);
+ M_RB(4:6,4:6) = I_o;
+
+ %-- Centripal and Coriolis matrix --%
+ % C_RB = Fossen (2.93)
+ C_RB = zeros(6,6);
+ C_RB(1:3,1:3) = zeros(3,3);
+ temp = M_RB(1:3,1:3)*nu(1:3,i)+M_RB(1:3,4:6)*nu(4:6,i);
+ C_RB(1:3,4:6) = -skew(temp(1),temp(2),temp(3));
+ temp = M_RB(1:3,1:3)*nu(1:3,i)+M_RB(1:3,4:6)*nu(4:6,i);
+ C_RB(4:6,1:3) = -skew(temp(1),temp(2),temp(3));
+ temp = M_RB(4:6,1:3)*nu(1:3,i)+M_RB(4:6,4:6)*nu(4:6,i);
+ C_RB(4:6,4:6) = -skew(temp(1),temp(2),temp(3));
+
+ %-----------------------------------%
+ %------- Added Mass Forces -------%
+ %-----------------------------------%
+ %-- Mass matrix --%
+ % computed according to the procedure in Fossen p. 41
+ e = 1-(b/a)^2;
+ alpha_o = (2*(1-e^2)/e^3)*(0.5*log((1+e)/(1-e))-e);
+ beta_o = 1/e^2-((1-e^2)/(2*e^3))*log((1+e)/(1-e));
+ X_udot = -(alpha_o/(2-alpha_o))*m;
+ Y_vdot = -(beta_o/(2-beta_o))*m; %-480 is due to wings
+ Z_wdot = Y_vdot;
+ K_pdot = 0;
+ M_qdot = -0.2*m*((b^2-a^2)^2*(alpha_o-beta_o))/...
+ (2*(b^2-a^2)+(b^2+a^2)*(beta_o-alpha_o));
+ N_rdot = M_qdot;
+ % Isa, eq. 21,32
+ M_A_cg = -diag([X_udot, Y_vdot, Z_wdot, K_pdot, M_qdot, N_rdot]);
+
+ M_A_Left(1:3,1:3) = diag([1,1,1]);
+ M_A_Left(1:3,4:6) = skew(x_cg,y_cg,z_cg);
+ M_A_Left(4:6,1:3) = zeros(3,3);
+ M_A_Left(4:6,4:6) = diag([1,1,1]);
+ M_A_Right(1:3,1:3) = diag([1,1,1]);
+ M_A_Right(1:3,4:6) = skew(x_cg,y_cg,z_cg)';
+ M_A_Right(4:6,1:3) = zeros(3,3);
+ M_A_Right(4:6,4:6) = diag([1,1,1]);
+ M_A = M_A_Left*M_A_cg*M_A_Right;
+
+ %-- Centripal and Coriolis matrix --%
+ % C_RB = Fossen (2.93)
+ C_A = zeros(6,6);
+ C_A(1:3,1:3) = zeros(3,3);
+ temp = M_A(1:3,1:3)*nu(1:3,i)+M_A(1:3,4:6)*nu(4:6,i);
+ C_A(1:3,4:6) = -skew(temp(1),temp(2),temp(3));
+ temp = M_A(1:3,1:3)*nu(1:3,i)+M_A(1:3,4:6)*nu(4:6,i);
+ C_A(4:6,1:3) = -skew(temp(1),temp(2),temp(3));
+ temp = M_A(4:6,1:3)*nu(1:3,i)+M_A(4:6,4:6)*nu(4:6,i);
+ C_A(4:6,4:6) = -skew(temp(1),temp(2),temp(3));
+
+ %-----------------------------------%
+ %------- Hydrostatic Forces -------%
+ %-----------------------------------%
+ % Hydrostatic restoring forces
+ % Fossen (2.168)
+ W = m*g; % Gravitational force acting on center of gravity
+ B = (m_h+m_s)*g; % Buoyancy force acting on center of buoyancy
+ g_eta= [(W-B)*sin(eta(5,i)); ...
+ -(W-B)*cos(eta(5,i))*sin(eta(4,i)); ...
+ -(W-B)*cos(eta(5,i))*cos(eta(4,i)); ...
+ -(y_cg*W-y_cb*B)*cos(eta(5,i))*cos(eta(4,i))+(z_cg*W-z_cb*B)*cos(eta(5,i))*sin(eta(4,i)); ...
+ (z_cg*W-z_cb*B)*sin(eta(5,i))+(x_cg*W-x_cb*B)*cos(eta(5,i))*cos(eta(4,i)); ...
+ -(x_cg*W-x_cb*B)*cos(eta(5,i))*sin(eta(4,i))-(y_cg*W-y_cb*B)*sin(eta(5,i));];
+ % Hystostatic force
+ tau_R = -g_eta;
+
+ %-----------------------------------%
+ %------- Hydrodynamic Forces -------%
+ %-----------------------------------%
+ % ------------ Hull------------ %
+ % angle of attack of hull
+ % Graver hydrodynamic forces for hull
+ if abs(nu(1,i)) > 0
+ alpha_h = atan(nu(3,i)/nu(1,i));
+ C_D_h = 0.214 + 32.3*alpha_h^2;
+ C_L_h = 11.76*alpha_h + 4.6 * alpha_h^2;
+ C_M_h = 0.63*alpha_h;
+ F_D_h = 0.5*rho*A_h*((nu(1,i)^2+nu(2,i)^2+nu(3,i)^2)^0.5)^2*C_D_h;
+ F_L_h = 0.5*rho*A_h*((nu(1,i)^2+nu(2,i)^2+nu(3,i)^2)^0.5)^2*C_L_h;
+ F_M_h = 0.5*rho*A_h*((nu(1,i)^2+nu(2,i)^2+nu(3,i)^2)^0.5)^2*C_M_h;
+ tau_H_h = [F_L_h*sin(alpha_h)-F_D_h*cos(alpha_h);0; ...
+ -F_L_h*cos(alpha_h)-F_D_h*sin(alpha_h);0;F_M_h;0;];
+ else
+ alpha_h = 0;
+ tau_H_h = zeros(6,1);
+ end
+
+ % % ------------ Elevator ------------ %
+ % % Hydrodynamic forces by Elevator
+ % % angle of attack of Elevator
+ % if abs(nu(1,i)) > 0
+ % alpha_e = atan(nu(3,i)/nu(1,i)) + alpha_e_control(i);
+ % else
+ % alpha_e = 0;
+ % end
+ % C_D_e = 0.214 + 32.3*alpha_e^2;
+ % C_L_e = 11.76*alpha_e + 4.6 * alpha_e^2;
+ % C_M_e = 2.0*0.63*alpha_e; % multipled by 2
+ % F_D_e = 0.5*rho*A_e*((nu(1,i)^2+nu(3,i)^2)^0.5)^2*C_D_e;
+ % F_L_e = 0.5*rho*A_e*((nu(1,i)^2+nu(3,i)^2)^0.5)^2*C_L_e;
+ % F_M_e = 0.5*rho*A_e*((nu(1,i)^2+nu(3,i)^2)^0.5)^2*C_M_e;
+ % tau_H_e = [ F_L_e*sin(alpha_e)-F_D_e*cos(alpha_e);0; ...
+ % -F_L_e*cos(alpha_e)-F_D_e*sin(alpha_e);0;F_M_e;0;];
+
+ % ------------ Rudder ------------ %
+ % Hydrodynamic forces by Rudder
+ % angle of attack of Rudder
+ if abs(nu(1,i)) > 0
+ alpha_r = alpha_r_control(i);
+ else
+ alpha_r = 0.0;
+ end
+
+ C_D_r = (0.214 + 32.3*alpha_r^2) / 10;
+ C_L_r = (11.76*alpha_r + 4.6 * alpha_r^2) / 10;
+ C_M_r = 0.63*alpha_r * 10;
+
+ F_D_r = 0.5*rho*A_e*((nu(1,i)^2+nu(2,i)^2+nu(3,i)^2)^0.5)^2*C_D_r;
+ F_L_r = 0.5*rho*A_e*((nu(1,i)^2+nu(2,i)^2+nu(3,i)^2)^0.5)^2*C_L_r;
+ F_M_r = 0.5*rho*A_e*((nu(1,i)^2+nu(2,i)^2+nu(3,i)^2)^0.5)^2*C_M_r;
+
+ tau_H_r = [F_L_r*sin(alpha_r)-F_D_r*cos(alpha_r); ...
+ -F_L_r*cos(alpha_r)-F_D_r*sin(alpha_r); 0; ...
+ 0;0;-F_M_r;];
+
+ % Hydrodynamic forces by propeller
+ tau_H_p = [F_T(i);0;0;0;0;0];
+
+ %-----------------------------------%
+ %------- Solver (Euler) -------%
+ %-----------------------------------%
+ % Total matrices and forces
+ M_tot = M_RB+M_A;
+ C_tot = C_RB+C_A;
+ D_tot = D_h;
+ tau_H = tau_H_h + tau_H_r + tau_H_p;
+
+ % Solve for nu_dot
+ h = time(i+1)-time(i);
+ nu_dot(:,i) = M_tot\(-C_tot*nu(:,i)-D_tot*nu(:,i) + tau_R + tau_H);
+ nu(:,i+1)=nu(:,i) + h.*nu_dot(:,i);
+ eta_dot(:,i) = J(eta(4,i),eta(5,i),eta(6,i))*nu(:,i+1);
+ eta(:,i+1) = eta(:,i) + (h)*eta_dot(:,i);
+end
+
+figure
+subplot(3,1,1)
+plot(m_present_time,-m_pitch.*180./(2*pi()),'k'); hold on;
+plot(time,eta(5,:).*180./(2*pi()),'r')
+grid on; ylim([-25 25])
+legend('\theta_{glider}','\theta_{sim}')
+
+subplot(3,1,2)
+plot(m_present_time,-m_depth,' k'); hold on;
+plot(time,eta(3,:),'r')
+grid on; ylim([-50 5])
+legend('z_{glider}','z_{sim}')
+
+subplot(3,1,3)
+plot(m_present_time,-m_depth_rate.*100,'k'); hold on;
+plot(time,eta_dot(3,:).*100,'r')
+grid on; ylim([-50 50])
+legend('z_{dot glider}','z_{dot sim}')