Added thruster dynamics without battery state model

blue_bringup/config/bluerov2.yaml

@@ -26,6 +26,7 @@ ismc:
     msg_ids: [31, 32]
     msg_rates: [100.0, 100.0]
     control_loop_freq: 200.0
+    use_battery_state: false
 
 aruco_marker_localizer:
   ros__parameters:

blue_bringup/config/bluerov2_heavy.yaml

@@ -26,6 +26,7 @@ ismc:
     msg_ids: [31, 32]
     msg_rates: [100.0, 100.0]
     control_loop_freq: 200.0
+    use_battery_state: false
 
 aruco_marker_localizer:
   ros__parameters:

blue_control/include/blue_control/ismc.hpp

@@ -50,6 +50,7 @@ class ISMC : public Controller
   double sliding_gain_;
   double boundary_thickness_;
 
+  bool use_battery_state_;
   blue_msgs::msg::Reference cmd_;
   rclcpp::Subscription<blue_msgs::msg::Reference>::SharedPtr cmd_sub_;
 };

blue_control/src/ismc.cpp

@@ -38,13 +38,15 @@ ISMC::ISMC()
     "convergence_rate", std::vector<double>({100.0, 100.0, 100.0, 100.0, 100.0, 100.0}));
   this->declare_parameter("sliding_gain", 0.0);
   this->declare_parameter("boundary_thickness", 0.0);
+  this->declare_parameter("use_battery_state", false);
 
   Eigen::VectorXd convergence_diag =
     convertVectorToEigenVector(this->get_parameter("convergence_rate").as_double_array());
   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_ = blue::dynamics::Vector6d::Zero();
+  use_battery_state_ = this->get_parameter("use_battery_state").as_bool();
 
   // Update the reference signal when a new command is received
   cmd_sub_ = this->create_subscription<blue_msgs::msg::Reference>(
@@ -94,9 +96,13 @@ mavros_msgs::msg::OverrideRCIn ISMC::update()
   const Eigen::VectorXd pwms = tcm_.completeOrthogonalDecomposition().pseudoInverse() * forces;
 
   // Convert the thruster forces to PWM values
-  pwms.unaryExpr([this](double x) {
-    return blue::dynamics::calculatePwmFromThrustSurface(x, battery_state_.voltage);
-  });
+  if (use_battery_state_) {
+    pwms.unaryExpr([this](double x) {
+      return blue::dynamics::calculatePwmFromThrustSurface(x, battery_state_.voltage);
+    });
+  } else {
+    pwms.unaryExpr([this](double x) { return blue::dynamics::calculatePwmFromThrustCurve(x); });
+  }
 
   mavros_msgs::msg::OverrideRCIn msg;
 
@@ -105,8 +111,16 @@ mavros_msgs::msg::OverrideRCIn ISMC::update()
     channel = mavros_msgs::msg::OverrideRCIn::CHAN_NOCHANGE;
   }
 
-  const std::tuple<int, int> deadband = blue::dynamics::calculateDeadZone(battery_state_.voltage);
+  // Calculate the deadzone band
+  std::tuple<int, int> deadband;
+
+  if (use_battery_state_) {
+    deadband = blue::dynamics::calculateDeadZone(battery_state_.voltage);
+  } else {
+    deadband = blue::dynamics::calculateDeadZone();
+  }
 
+  // Set the PWM values
   for (uint16_t i = 0; i < pwms.size(); i++) {
     auto pwm = static_cast<uint16_t>(pwms[i]);
 

blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp

@@ -45,15 +45,22 @@ const uint16_t kMaxPwm = 1900;
 const uint16_t kNoThrustPwm = 1500;
 
 /**
- * @brief Calculates the T200 deadzone band for a given voltage.
+ * @brief Calculate the T200 deadzone band for a given voltage.
  *
  * @param voltage The current battery voltage.
  * @return The minimum and maximum PWM values for the deadzone band.
  */
 [[nodiscard]] std::tuple<int, int> calculateDeadZone(double voltage);
 
 /**
- * @brief Approximates the PWM input needed to achieve a desired force given the current battery
+ * @brief Calculate the general T200 deadzone band.
+ *
+ * @return The minimum and maximum PWM values for the deadzone band.
+ */
+[[nodiscard]] std::tuple<int, int> calculateDeadZone();
+
+/**
+ * @brief Approximate the PWM input needed to achieve a desired force given the current battery
  * voltage.
  *
  * @note This is a simplified technique for calculating the PWM input. A more accurate method would
@@ -67,4 +74,17 @@ const uint16_t kNoThrustPwm = 1500;
  */
 [[nodiscard]] int calculatePwmFromThrustSurface(double force, double voltage);
 
+/**
+ * @brief Approximate the PWM input needed to achieve a desired force.
+ *
+ * @note This is a simplified technique for calculating the PWM input. A more accurate method would
+ * perform a characterization of the thruster dynamics. The issue with that solution, however, is
+ * that there is no feedback from the BlueROV2 thrusters to use as the state variables for the
+ * system.
+ *
+ * @param force The desired force (N).
+ * @return The PWM input needed to achieve the desired force.
+ */
+[[nodiscard]] int calculatePwmFromThrustCurve(double force);
+
 }  // namespace blue::dynamics

blue_dynamics/src/thruster_dynamics.cpp

@@ -20,6 +20,7 @@
 
 #include "blue_dynamics/thruster_dynamics.hpp"
 
+#include <algorithm>
 #include <cmath>
 
 namespace blue::dynamics
@@ -41,6 +42,12 @@ namespace blue::dynamics
   return std::tuple<int, int>(min_deadzone, max_deadzone);
 }
 
+[[nodiscard]] std::tuple<int, int> calculateDeadZone()
+{
+  // The deadband zone was identified using the thrust curve data with an 18v battery
+  return std::tuple<int, int>(1470, 1530);
+}
+
 [[nodiscard]] int calculatePwmFromThrustSurface(double force, double voltage)
 {
   // Coefficients for the surface identified by fitting a surface to the thrust curves for the
@@ -63,4 +70,27 @@ namespace blue::dynamics
   return static_cast<int>(std::round(pwm));
 }
 
+[[nodiscard]] int calculatePwmFromThrustCurve(double force)
+{
+  // The thrust curve is only accurate over the following force range, so we restrict the input
+  // forces to that range
+  const double min_force = -40.0;
+  const double max_force = 60.0;
+
+  force = std::clamp(force, min_force, max_force);
+
+  // Coefficients for the 4th-degree polynomial fit to the thrust curve for the T200 run using
+  // a battery at 18v. The polynomial was identified using Matlab's `fit` function.
+  const double p00 = 1498;
+  const double p01 = 12.01;
+  const double p02 = -0.04731;
+  const double p03 = -0.002098;
+  const double p04 = 0.00002251;
+
+  const double pwm = p00 + p01 * force + p02 * std::pow(force, 2) + p03 * std::pow(force, 3) +
+                     p04 * std::pow(force, 4);
+
+  return static_cast<int>(std::round(pwm));
+}
+
 }  // namespace blue::dynamics

blue_dynamics/test/test_thruster_dynamics.cpp

@@ -36,20 +36,27 @@ TEST(ThrusterDynamicsTest, TestDeadzoneModel)
 
   const double voltage = 20.0;
 
-  const std::tuple<int, int> actual = calculateDeadZone(voltage);
+  // Calculate the deadzone using the battery voltage
+  std::tuple<int, int> actual = calculateDeadZone(voltage);
 
   ASSERT_NEAR(expected_min, std::get<0>(actual), 1);
   ASSERT_NEAR(expected_max, std::get<1>(actual), 1);
 }
 
-TEST(ThrusterDynamicsTest, TestThrustSurfaceModel)
+TEST(ThrusterDynamicsTest, TestThrustModel)
 {
   // These are measurements obtained from the BlueROV2 T200 characterization
   const int expected_pwm = 1280;
   const double force = -1.86 * 9.8;  // Convert from KgF to N
   const double voltage = 20.0;
 
-  const int actual = calculatePwmFromThrustSurface(force, voltage);
+  // Calculate the PWM values using the battery voltage
+  int actual = calculatePwmFromThrustSurface(force, voltage);
+
+  ASSERT_NEAR(expected_pwm, actual, 15);
+
+  // Now calculate the PWM values without the battery voltage
+  actual = calculatePwmFromThrustCurve(force);
 
   ASSERT_NEAR(expected_pwm, actual, 15);
 }