Dynamics vehicle control

glider_hybrid_whoi_description/urdf/glider_hybrid_whoi_base_kinematics.xacro

@@ -157,6 +157,33 @@
         <f_thruster_power_w1>-0.020919</f_thruster_power_w1>
         <f_thruster_power_w2>1.4699</f_thruster_power_w2>
         <f_thruster_power_w3>0.97895</f_thruster_power_w3>
+
+        <!-- Dynamics properties -->
+        <use_dynamics>true</use_dynamics>
+        <hard_code_input>false</hard_code_input>
+        <dynamics>
+          <hull_mass>48.2</hull_mass>
+          <hull_length>2.0</hull_length>
+          <hull_radius>0.105</hull_radius>
+          <shifter_mass>7.8</shifter_mass>
+          <ballast_radius>0.07</ballast_radius>
+          <default_mass_position>0.4</default_mass_position>
+          <default_ballast_position>0.711</default_ballast_position>
+          <initial_mass_position>-0.018</initial_mass_position>
+          <initial_ballast_position>-0.01024</initial_ballast_position>
+          <max_mass_position>0.03</max_mass_position>
+          <max_ballast_volume>2.4e-04</max_ballast_volume>
+          <center_of_buoyancy>0 0 0</center_of_buoyancy>
+          <center_of_gravity>0 0 0.0054</center_of_gravity>
+          <linear_damping>
+            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
+          </linear_damping>
+        </dynamics>
       </plugin>
       <plugin name="{namespace}_dave_ocean_current_plugin" filename="libdave_ocean_current_model_plugin.so">
         <flow_velocity_topic>hydrodynamics/current_velocity/${namespace}</flow_velocity_topic>
@@ -176,7 +203,7 @@
             <noiseFreq>0.0</noiseFreq>
           </velocity_down>
         </transient_current>
-        <tide_oscillation>true</tide_oscillation>
+        <tide_oscillation>false</tide_oscillation>
       </plugin>
     </gazebo>
 

glider_hybrid_whoi_gazebo/worlds/seafloor_underwater_stratified_current.world

@@ -121,7 +121,7 @@
         <!-- Database tag can accept full path or filename for .csv within the uuv_dave/worlds folder  -->
         <databasefilePath>transientOceanCurrentDatabase.csv</databasefilePath>
         <!-- Absolute path in local machine -->
-        <!-- <databasefilePath>/home/glider-sim/uuv_ws/src/glider_hybrid_whoi/glider_hybrid_whoi_gazebo/worlds/transientOceanCurrentDatabase.csv</databasefilePath> -->
+        <!-- <databasefilePath>/home/woensug/uuv_ws/src/glider_hybrid_whoi/glider_hybrid_whoi_gazebo/worlds/transientOceanCurrentDatabase_zero.csv</databasefilePath> -->
         <!-- Absolute path in docker environment -->
         <!-- <databasefilePath>/home/ros/glider_hybrid_whoi/install/share/glider_hybrid_whoi_gazebo/worlds/transientOceanCurrentDatabase.csv</databasefilePath> -->
       </transient_current>

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<double>& _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__