Merge pull request #8 from Robotic-Decision-Making-Lab/feat-model

Modified construction of rigid body inertia and coriolis matrices

include/hydrodynamics/hydrodynamics.hpp

@@ -53,6 +53,17 @@ struct Inertia
   /// - the added mass coefficients in the surge, sway, heave, roll, pitch, and yaw directions.
   Inertia(double mass, const Eigen::Vector3d & moments, const Eigen::Vector6d & added_mass);
 
+  /// Create a new wrapper for inertial parameters (including added mass and rigid body inertia) using:
+  /// - the mass of the vehicle,
+  /// - the moments of inertia about the x, y, and z axes,
+  /// - the added mass coefficients in the surge, sway, heave, roll, pitch, and yaw directions.
+  /// - the center of gravity of the vehicle.
+  Inertia(
+    double mass,
+    const Eigen::Vector3d & moments,
+    const Eigen::Vector6d & added_mass,
+    const Eigen::Vector3d & center_of_gravity);
+
   Eigen::Matrix6d rigid_body_matrix;
   Eigen::Matrix6d added_mass_matrix;
   Eigen::Matrix6d mass_matrix;
@@ -84,6 +95,13 @@ struct Coriolis
   /// - the added mass coefficients in the surge, sway, heave, roll, pitch, and yaw directions.
   Coriolis(double mass, const Eigen::Vector3d & moments, Eigen::Vector6d added_mass);
 
+  /// Create a new wrapper for the Coriolis and centripetal force parameters using:
+  /// - the mass of the vehicle,
+  /// - the moments of inertia about the x, y, and z axes,
+  /// - the added mass coefficients in the surge, sway, heave, roll, pitch, and yaw directions.
+  /// - the center of gravity of the vehicle.
+  Coriolis(double mass, const Eigen::Vector3d & moments, Eigen::Vector6d added_mass, Eigen::Vector3d center_of_gravity);
+
   /// Calculate the rigid body Coriolis matrix using the angular velocity of the vehicle.
   [[nodiscard]] auto calculate_rigid_body_coriolis_matrix(const Eigen::Vector3d & angular_velocity) const
     -> Eigen::Matrix6d;
@@ -97,6 +115,7 @@ struct Coriolis
   double mass;
   Eigen::Matrix3d moments;
   Eigen::Vector6d added_mass_coeff;
+  Eigen::Vector3d center_of_gravity;
 };
 
 struct Damping

src/hydrodynamics.cpp

@@ -75,6 +75,26 @@ Inertia::Inertia(double mass, const Eigen::Vector3d & moments, const Eigen::Vect
   mass_matrix = rigid_body_matrix + added_mass_matrix;
 }
 
+Inertia::Inertia(
+  double mass,
+  const Eigen::Vector3d & moments,
+  const Eigen::Vector6d & added_mass,
+  const Eigen::Vector3d & center_of_gravity)
+{
+  // Construct the rigid body inertia matrix
+  Eigen::Matrix6d rigid_body_matrix = Eigen::Matrix6d::Zero();
+  rigid_body_matrix.topLeftCorner(3, 3) = mass * Eigen::Matrix3d::Identity();
+  rigid_body_matrix.topRightCorner(3, 3) = -mass * make_skew_symmetric_matrix(center_of_gravity);
+  rigid_body_matrix.bottomLeftCorner(3, 3) = mass * make_skew_symmetric_matrix(center_of_gravity);
+  rigid_body_matrix.bottomRightCorner(3, 3) = moments.asDiagonal().toDenseMatrix();
+
+  // Construct the added mass matrix
+  added_mass_matrix = -added_mass.asDiagonal().toDenseMatrix();
+
+  // The complete mass matrix is the sum of the rigid body and added mass matrices
+  mass_matrix = rigid_body_matrix + added_mass_matrix;
+}
+
 Coriolis::Coriolis(
   double mass,
   double Ixx,
@@ -88,14 +108,28 @@ Coriolis::Coriolis(
   double Ndr)
 : mass(mass),
   moments(Eigen::Vector3d(Ixx, Iyy, Izz).asDiagonal().toDenseMatrix()),
-  added_mass_coeff(Eigen::Vector6d(Xdu, Ydv, Zdw, Kdp, Mdq, Ndr))
+  added_mass_coeff(Eigen::Vector6d(Xdu, Ydv, Zdw, Kdp, Mdq, Ndr)),
+  center_of_gravity(Eigen::Vector3d::Zero())
 {
 }
 
 Coriolis::Coriolis(double mass, const Eigen::Vector3d & moments, Eigen::Vector6d added_mass)
 : mass(mass),
   moments(moments.asDiagonal().toDenseMatrix()),
-  added_mass_coeff(std::move(added_mass))
+  added_mass_coeff(std::move(added_mass)),
+  center_of_gravity(Eigen::Vector3d::Zero())
+{
+}
+
+Coriolis::Coriolis(
+  double mass,
+  const Eigen::Vector3d & moments,
+  Eigen::Vector6d added_mass,
+  Eigen::Vector3d center_of_gravity)
+: mass(mass),
+  moments(moments.asDiagonal().toDenseMatrix()),
+  added_mass_coeff(std::move(added_mass)),
+  center_of_gravity(std::move(center_of_gravity))
 {
 }
 
@@ -106,6 +140,10 @@ auto Coriolis::calculate_rigid_body_coriolis_matrix(const Eigen::Vector3d & angu
   const Eigen::Vector3d moments_v2 = moments * angular_velocity;
 
   coriolis.topLeftCorner(3, 3) = mass * make_skew_symmetric_matrix(angular_velocity);
+  coriolis.topRightCorner(3, 3) =
+    -mass * make_skew_symmetric_matrix(angular_velocity) * make_skew_symmetric_matrix(center_of_gravity);
+  coriolis.bottomLeftCorner(3, 3) =
+    mass * make_skew_symmetric_matrix(center_of_gravity) * make_skew_symmetric_matrix(angular_velocity);
   coriolis.bottomRightCorner(3, 3) = -make_skew_symmetric_matrix(moments_v2);
 
   return coriolis;