Use tf2_eigen for msg conversions

blue_control/CMakeLists.txt

@@ -26,6 +26,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   std_srvs
   eigen3_cmake_module
   Eigen3
+  tf2_eigen
 )
 
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})

blue_control/include/blue_control/ismc.hpp

@@ -45,8 +45,8 @@ class ISMC : public Controller
 
 private:
   // Hyperparameters used by the ISMC
-  Eigen::VectorXd total_velocity_error_;
-  Eigen::MatrixXd convergence_rate_;
+  blue::dynamics::Vector6d total_velocity_error_;
+  blue::dynamics::Matrix6d convergence_rate_;
   double sliding_gain_;
   double boundary_thickness_;
 

blue_control/package.xml

@@ -15,6 +15,7 @@
   <depend>eigen</depend>
   <depend>blue_msgs</depend>
   <depend>std_srvs</depend>
+  <depend>tf2_eigen</depend>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

blue_control/src/ismc.cpp

@@ -22,6 +22,7 @@
 
 #include <algorithm>
 #include <cmath>
+#include <tf2_eigen/tf2_eigen.hpp>
 
 #include "blue_dynamics/thruster_dynamics.hpp"
 
@@ -43,7 +44,7 @@ ISMC::ISMC()
   convergence_rate_ = convergence_diag.asDiagonal().toDenseMatrix();
   sliding_gain_ = this->get_parameter("sliding_gain").as_double();
   boundary_thickness_ = this->get_parameter("boundary_thickness").as_double();
-  total_velocity_error_ = Eigen::VectorXd::Zero(6);
+  total_velocity_error_ = blue::dynamics::Vector6d::Zero();
 
   // Update the reference signal when a new command is received
   cmd_sub_ = this->create_subscription<blue_msgs::msg::Reference>(
@@ -52,43 +53,44 @@ ISMC::ISMC()
 
 mavros_msgs::msg::OverrideRCIn ISMC::update()
 {
-  Eigen::VectorXd velocity(6);  // NOLINT
-  velocity << odom_.twist.twist.linear.x, odom_.twist.twist.linear.y, odom_.twist.twist.linear.z,
-    odom_.twist.twist.angular.x, odom_.twist.twist.angular.y, odom_.twist.twist.angular.z;
+  blue::dynamics::Vector6d velocity;
+  tf2::fromMsg(odom_.twist.twist, velocity);
 
   // Calculate the velocity error
-  Eigen::VectorXd velocity_error(6);
-  velocity_error << cmd_.twist.linear.x, cmd_.twist.linear.y, cmd_.twist.linear.z,
-    cmd_.twist.angular.x, cmd_.twist.angular.y, cmd_.twist.angular.z;
+  blue::dynamics::Vector6d velocity_error;
+  tf2::fromMsg(cmd_.twist, velocity_error);
   velocity_error -= velocity;
 
-  Eigen::VectorXd desired_accel(6);  // NOLINT
+  // There is no suitable tf2_eigen function for Accel types :(
+  blue::dynamics::Vector6d desired_accel;  // NOLINT
   desired_accel << cmd_.accel.linear.x, cmd_.accel.linear.y, cmd_.accel.linear.z,
     cmd_.accel.angular.x, cmd_.accel.angular.y, cmd_.accel.angular.z;
 
   // Get the current rotation of the vehicle in the inertial frame
-  Eigen::Quaterniond orientation = Eigen::Quaterniond(
-    odom_.pose.pose.orientation.w, odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y,
-    odom_.pose.pose.orientation.z);
+  Eigen::Quaterniond orientation;
+  tf2::fromMsg(odom_.pose.pose.orientation, orientation);
 
   // Make sure to update the velocity error integral term BEFORE calculating the sliding surface
   // (the integral is up to time "t")
   total_velocity_error_ += velocity_error * dt_;
 
   // Calculate the sliding surface
-  Eigen::VectorXd surface = velocity_error + convergence_rate_ * total_velocity_error_;  // NOLINT
+  blue::dynamics::Vector6d surface =
+    velocity_error + convergence_rate_ * total_velocity_error_;  // NOLINT
 
   // Apply the sign function to the surface
   surface.unaryExpr([this](double x) { return tanh(x / boundary_thickness_); });
 
-  const Eigen::VectorXd forces =
+  const blue::dynamics::Vector6d forces =
     hydrodynamics_.inertia.getInertia() * (desired_accel + convergence_rate_ * velocity_error) +
     hydrodynamics_.coriolis.calculateCoriolis(velocity) * velocity +
     hydrodynamics_.damping.calculateDamping(velocity) * velocity +
     hydrodynamics_.restoring_forces.calculateRestoringForces(orientation.toRotationMatrix()) +
     sliding_gain_ * surface;
 
   // Multiply the desired forces by the pseudoinverse of the thruster configuration matrix
+  // The size of this vector will depend on the number of thrusters so we don't assign it to a
+  // fixed-size matrix
   const Eigen::VectorXd pwms = tcm_.completeOrthogonalDecomposition().pseudoInverse() * forces;
 
   // Convert the thruster forces to PWM values
@@ -106,7 +108,7 @@ mavros_msgs::msg::OverrideRCIn ISMC::update()
   const std::tuple<int, int> deadband = blue::dynamics::calculateDeadZone(battery_state_.voltage);
 
   for (uint16_t i = 0; i < pwms.size(); i++) {
-    uint16_t pwm = static_cast<uint16_t>(pwms[i]);
+    auto pwm = static_cast<uint16_t>(pwms[i]);
 
     // Apply the deadband to the PWM values
     if (pwm > std::get<0>(deadband) && pwm < std::get<1>(deadband)) {

blue_dynamics/include/blue_dynamics/hydrodynamics.hpp

@@ -25,6 +25,12 @@
 namespace blue::dynamics
 {
 
+// Used to represent a 6x6 matrix: commonly used in hydrodynamics.
+using Matrix6d = Eigen::Matrix<double, 6, 6>;
+
+// Used to represent a 6x1 matrix: commonly used in hydrodynamics.
+using Vector6d = Eigen::Matrix<double, 6, 1>;
+
 /**
  * @brief Create a skew-symmetric matrix from the provided coefficients.
  *
@@ -62,8 +68,7 @@ class Inertia
    *        M_dot{q}, N_dot{r})`.
    */
   Inertia(
-    double mass, const Eigen::Vector3d & inertia_tensor_coeff,
-    const Eigen::VectorXd & added_mass_coeff);
+    double mass, const Eigen::Vector3d & inertia_tensor_coeff, const Vector6d & added_mass_coeff);
 
   /**
    * @brief Get the vehicle's inertia matrix.
@@ -78,10 +83,10 @@ class Inertia
    *
    * @return The inertia matrix `M`.
    */
-  [[nodiscard]] Eigen::MatrixXd getInertia() const;
+  [[nodiscard]] Matrix6d getInertia() const;
 
 private:
-  Eigen::MatrixXd inertia_matrix_;
+  Matrix6d inertia_matrix_;
 };
 
 /**
@@ -104,8 +109,7 @@ class Coriolis
    *        M_dot{q}, N_dot{r})`.
    */
   Coriolis(
-    double mass, const Eigen::Vector3d & inertia_tensor_coeff,
-    const Eigen::VectorXd & added_mass_coeff);
+    double mass, const Eigen::Vector3d & inertia_tensor_coeff, const Vector6d & added_mass_coeff);
 
   /**
    * @brief Calculate the Coriolis and centripetal forces for the vehicle.
@@ -116,7 +120,7 @@ class Coriolis
    * @param velocity The current velocity of the vehicle in the body frame.
    * @return The Coriolis and centripetal force matrix `C`.
    */
-  [[nodiscard]] Eigen::MatrixXd calculateCoriolis(const Eigen::VectorXd & velocity) const;
+  [[nodiscard]] Matrix6d calculateCoriolis(const Vector6d & velocity) const;
 
   /**
    * @brief Calculate the rigid body Coriolis matrix.
@@ -134,8 +138,7 @@ class Coriolis
    * @param angular_velocity The current angular velocity of the vehicle in the body frame (rad/s).
    * @return The rigid body Coriolis matrix `C_RB`.
    */
-  [[nodiscard]] Eigen::MatrixXd calculateRigidBodyCoriolis(
-    const Eigen::Vector3d & angular_velocity) const;
+  [[nodiscard]] Matrix6d calculateRigidBodyCoriolis(const Eigen::Vector3d & angular_velocity) const;
 
   /**
    * @brief Calculate the added Coriolis matrix.
@@ -151,12 +154,12 @@ class Coriolis
    * @param velocity The current velocity of the vehicle in the body frame.
    * @return The added Coriolis matrix `C_A`.
    */
-  [[nodiscard]] Eigen::MatrixXd calculateAddedCoriolis(const Eigen::VectorXd & velocity) const;
+  [[nodiscard]] Matrix6d calculateAddedCoriolis(const Vector6d & velocity) const;
 
 private:
   double mass_{0.0};
   Eigen::Matrix3d moments_;
-  Eigen::VectorXd added_mass_coeff_;
+  Vector6d added_mass_coeff_;
 };
 
 /**
@@ -177,8 +180,7 @@ class Damping
    * @param quadratic_damping_coeff The nonlinear damping coefficients `(X_u|u|, Y_v|v|, Z_w|w|,
    *        K_p|p|, M_q|q|, N_r|r|)`.
    */
-  Damping(
-    const Eigen::VectorXd & linear_damping_coeff, const Eigen::VectorXd & quadratic_damping_coeff);
+  Damping(const Vector6d & linear_damping_coeff, const Vector6d & quadratic_damping_coeff);
 
   /**
    * @brief Calculate the damping forces for the vehicle.
@@ -190,19 +192,19 @@ class Damping
    * @param velocity The current velocity of the vehicle in the body frame.
    * @return The damping matrix `D`.
    */
-  [[nodiscard]] Eigen::MatrixXd calculateDamping(const Eigen::VectorXd & velocity) const;
+  [[nodiscard]] Matrix6d calculateDamping(const Vector6d & velocity) const;
 
 private:
-  Eigen::MatrixXd linear_damping_;
-  Eigen::VectorXd quadratic_damping_coeff_;
+  Matrix6d linear_damping_;
+  Vector6d quadratic_damping_coeff_;
 
   /**
    * @brief Calculate the nonlinear damping matrix.
    *
    * @param velocity The current velocity of the vehicle in the body frame.
    * @return The nonlinear damping matrix.
    */
-  [[nodiscard]] Eigen::MatrixXd calculateNonlinearDamping(const Eigen::VectorXd & velocity) const;
+  [[nodiscard]] Matrix6d calculateNonlinearDamping(const Vector6d & velocity) const;
 };
 
 /**
@@ -239,7 +241,7 @@ class RestoringForces
    * @param rotation The current rotation of the vehicle in the inertial frame.
    * @return The vector of restoring forces.
    */
-  [[nodiscard]] Eigen::VectorXd calculateRestoringForces(const Eigen::Matrix3d & rotation) const;
+  [[nodiscard]] Vector6d calculateRestoringForces(const Eigen::Matrix3d & rotation) const;
 
 private:
   double weight_{0.0};
@@ -264,18 +266,18 @@ class CurrentEffects
    *
    * @param current_velocity The velocity of the fluid in the inertial frame.
    */
-  explicit CurrentEffects(const Eigen::VectorXd & current_velocity);
+  explicit CurrentEffects(const Vector6d & current_velocity);
 
   /**
    * @brief Calculate the current in the body frame.
    *
    * @param rotation The current rotation of the vehicle in the inertial frame.
    * @return The velocity of the current in the body frame.
    */
-  [[nodiscard]] Eigen::VectorXd calculateCurrentEffects(const Eigen::Matrix3d & rotation) const;
+  [[nodiscard]] Vector6d calculateCurrentEffects(const Eigen::Matrix3d & rotation) const;
 
 private:
-  Eigen::VectorXd current_velocity_;
+  Vector6d current_velocity_;
 };
 
 /**

blue_dynamics/src/hydrodynamics.cpp

@@ -40,42 +40,40 @@ namespace blue::dynamics
 }
 
 Inertia::Inertia(
-  double mass, const Eigen::Vector3d & inertia_tensor_coeff,
-  const Eigen::VectorXd & added_mass_coeff)
+  double mass, const Eigen::Vector3d & inertia_tensor_coeff, const Vector6d & added_mass_coeff)
 {
-  Eigen::MatrixXd rigid_body = Eigen::MatrixXd::Zero(6, 6);
+  Matrix6d rigid_body = Matrix6d::Zero();
 
-  rigid_body.topLeftCorner(3, 3) = mass * Eigen::MatrixXd::Identity(3, 3);
+  rigid_body.topLeftCorner(3, 3) = mass * Eigen::Matrix3d::Identity();
   rigid_body.bottomRightCorner(3, 3) = inertia_tensor_coeff.asDiagonal().toDenseMatrix();
 
-  Eigen::MatrixXd added_mass = -added_mass_coeff.asDiagonal().toDenseMatrix();
+  Matrix6d added_mass = -added_mass_coeff.asDiagonal().toDenseMatrix();
 
   inertia_matrix_ = rigid_body + added_mass;
 }
 
-[[nodiscard]] Eigen::MatrixXd Inertia::getInertia() const { return inertia_matrix_; }
+[[nodiscard]] Matrix6d Inertia::getInertia() const { return inertia_matrix_; }
 
 Coriolis::Coriolis(
-  double mass, const Eigen::Vector3d & inertia_tensor_coeff,
-  const Eigen::VectorXd & added_mass_coeff)
+  double mass, const Eigen::Vector3d & inertia_tensor_coeff, const Vector6d & added_mass_coeff)
 : mass_(mass),
   moments_(inertia_tensor_coeff.asDiagonal().toDenseMatrix()),
   added_mass_coeff_(std::move(added_mass_coeff))
 {
 }
 
-[[nodiscard]] Eigen::MatrixXd Coriolis::calculateCoriolis(const Eigen::VectorXd & velocity) const
+[[nodiscard]] Matrix6d Coriolis::calculateCoriolis(const Vector6d & velocity) const
 {
   // The rigid body Coriolis matrix only needs the angular velocity
   const Eigen::Vector3d angular_velocity = velocity.bottomRows(3);
 
   return calculateRigidBodyCoriolis(angular_velocity) + calculateAddedCoriolis(velocity);
 }
 
-[[nodiscard]] Eigen::MatrixXd Coriolis::calculateRigidBodyCoriolis(
+[[nodiscard]] Matrix6d Coriolis::calculateRigidBodyCoriolis(
   const Eigen::Vector3d & angular_velocity) const
 {
-  Eigen::MatrixXd rigid = Eigen::MatrixXd::Zero(6, 6);
+  Matrix6d rigid = Matrix6d::Zero();
 
   const Eigen::Vector3d moments_v2 = moments_ * angular_velocity;
 
@@ -85,10 +83,9 @@ Coriolis::Coriolis(
   return rigid;
 }
 
-[[nodiscard]] Eigen::MatrixXd Coriolis::calculateAddedCoriolis(
-  const Eigen::VectorXd & velocity) const
+[[nodiscard]] Matrix6d Coriolis::calculateAddedCoriolis(const Vector6d & velocity) const
 {
-  Eigen::MatrixXd added = Eigen::MatrixXd::Zero(6, 6);
+  Matrix6d added = Matrix6d::Zero();
 
   Eigen::Matrix3d linear_vel = createSkewSymmetricMatrix(
     added_mass_coeff_(0) * velocity(0), added_mass_coeff_(1) * velocity(1),
@@ -105,20 +102,18 @@ Coriolis::Coriolis(
   return added;
 }
 
-Damping::Damping(
-  const Eigen::VectorXd & linear_damping_coeff, const Eigen::VectorXd & quadratic_damping_coeff)
+Damping::Damping(const Vector6d & linear_damping_coeff, const Vector6d & quadratic_damping_coeff)
 : linear_damping_(-linear_damping_coeff.asDiagonal().toDenseMatrix()),
   quadratic_damping_coeff_(std::move(quadratic_damping_coeff))
 {
 }
 
-[[nodiscard]] Eigen::MatrixXd Damping::calculateDamping(const Eigen::VectorXd & velocity) const
+[[nodiscard]] Matrix6d Damping::calculateDamping(const Vector6d & velocity) const
 {
   return linear_damping_ + calculateNonlinearDamping(velocity);
 }
 
-[[nodiscard]] Eigen::MatrixXd Damping::calculateNonlinearDamping(
-  const Eigen::VectorXd & velocity) const
+[[nodiscard]] Matrix6d Damping::calculateNonlinearDamping(const Vector6d & velocity) const
 {
   return -(quadratic_damping_coeff_.asDiagonal().toDenseMatrix() * velocity.cwiseAbs())
             .asDiagonal()
@@ -135,13 +130,13 @@ RestoringForces::RestoringForces(
 {
 }
 
-[[nodiscard]] Eigen::VectorXd RestoringForces::calculateRestoringForces(
+[[nodiscard]] Vector6d RestoringForces::calculateRestoringForces(
   const Eigen::Matrix3d & rotation) const
 {
   const Eigen::Vector3d fg(0, 0, weight_);
   const Eigen::Vector3d fb(0, 0, -buoyancy_);
 
-  Eigen::VectorXd g_rb(6);
+  Vector6d g_rb;
 
   g_rb.topRows(3) = rotation * (fg + fb);
   g_rb.bottomRows(3) =
@@ -152,15 +147,19 @@ RestoringForces::RestoringForces(
   return g_rb;
 }
 
-CurrentEffects::CurrentEffects(const Eigen::VectorXd & current_velocity)
+CurrentEffects::CurrentEffects(const Vector6d & current_velocity)
 : current_velocity_(std::move(current_velocity))
 {
 }
 
-[[nodiscard]] Eigen::VectorXd CurrentEffects::calculateCurrentEffects(
+[[nodiscard]] Vector6d CurrentEffects::calculateCurrentEffects(
   const Eigen::Matrix3d & rotation) const
 {
-  return rotation * current_velocity_;
+  Vector6d rotated_current_effects;
+  rotated_current_effects.topRows(3) = rotation * current_velocity_.topRows(3);
+  rotated_current_effects.bottomRows(3) = rotation * current_velocity_.bottomRows(3);
+
+  return rotated_current_effects;
 }
 
 HydrodynamicParameters::HydrodynamicParameters(