From c9ca26391adcc916c17d9ac9170879d9282a9151 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 19 Jun 2023 17:52:49 -0700 Subject: [PATCH 1/2] Started implementing thruster dynamics without battery state --- blue_control/include/blue_control/ismc.hpp | 1 + blue_control/src/ismc.cpp | 22 ++++++++++++--- .../blue_dynamics/thruster_dynamics.hpp | 28 ++++++++++++++++--- blue_dynamics/src/thruster_dynamics.cpp | 8 ++++-- 4 files changed, 49 insertions(+), 10 deletions(-) diff --git a/blue_control/include/blue_control/ismc.hpp b/blue_control/include/blue_control/ismc.hpp index 040438ef..071ec3bb 100644 --- a/blue_control/include/blue_control/ismc.hpp +++ b/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::SharedPtr cmd_sub_; }; diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index c6297e60..26482468 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -38,6 +38,7 @@ ISMC::ISMC() "convergence_rate", std::vector({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()); @@ -45,6 +46,7 @@ ISMC::ISMC() 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( @@ -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 deadband = blue::dynamics::calculateDeadZone(battery_state_.voltage); + // Calculate the deadzone band + std::tuple 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(pwms[i]); diff --git a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp index 3a289a23..d5019987 100644 --- a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp +++ b/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 calculateDeadZone(double voltage); +[[nodiscard]] inline std::tuple 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]] inline std::tuple 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 @@ -65,6 +72,19 @@ const uint16_t kNoThrustPwm = 1500; * @param voltage The current battery voltage (V). * @return The PWM input needed to achieve the desired force. */ -[[nodiscard]] int calculatePwmFromThrustSurface(double force, double voltage); +[[nodiscard]] inline int calculatePwmFromThrustSurface(double force, double voltage); + +/** + * @brief Approximate the PWM inpute 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]] inline int calculatePwmFromThrustCurve(double force); } // namespace blue::dynamics diff --git a/blue_dynamics/src/thruster_dynamics.cpp b/blue_dynamics/src/thruster_dynamics.cpp index bc0330c6..c04d1361 100644 --- a/blue_dynamics/src/thruster_dynamics.cpp +++ b/blue_dynamics/src/thruster_dynamics.cpp @@ -25,7 +25,7 @@ namespace blue::dynamics { -[[nodiscard]] std::tuple calculateDeadZone(double voltage) +[[nodiscard]] inline std::tuple calculateDeadZone(double voltage) { // The minimum PWM in the deadzone increases as the voltage increases // This is best represented by a 3rd order polynomial @@ -41,7 +41,9 @@ namespace blue::dynamics return std::tuple(min_deadzone, max_deadzone); } -[[nodiscard]] int calculatePwmFromThrustSurface(double force, double voltage) +[[nodiscard]] inline std::tuple calculateDeadZone() {} + +[[nodiscard]] inline int calculatePwmFromThrustSurface(double force, double voltage) { // Coefficients for the surface identified by fitting a surface to the thrust curves for the // voltages 10, 12, 14, 16, 18, 20 using Matlab's `fit` function with the `Poly23` surface @@ -63,4 +65,6 @@ namespace blue::dynamics return static_cast(std::round(pwm)); } +[[nodiscard]] inline int calculatePwmFromThrustCurve(double force) {} + } // namespace blue::dynamics From 83f8ddb112a2577f8302f762606f9157538f26de Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 20 Jun 2023 13:02:34 -0700 Subject: [PATCH 2/2] Added thruster models without battery state --- blue_bringup/config/bluerov2.yaml | 1 + blue_bringup/config/bluerov2_heavy.yaml | 1 + .../blue_dynamics/thruster_dynamics.hpp | 10 +++--- blue_dynamics/src/thruster_dynamics.cpp | 34 ++++++++++++++++--- blue_dynamics/test/test_thruster_dynamics.cpp | 13 +++++-- 5 files changed, 47 insertions(+), 12 deletions(-) diff --git a/blue_bringup/config/bluerov2.yaml b/blue_bringup/config/bluerov2.yaml index 415902d1..168cadee 100644 --- a/blue_bringup/config/bluerov2.yaml +++ b/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: diff --git a/blue_bringup/config/bluerov2_heavy.yaml b/blue_bringup/config/bluerov2_heavy.yaml index 87df36ee..8ce1deca 100644 --- a/blue_bringup/config/bluerov2_heavy.yaml +++ b/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: diff --git a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp index d5019987..18d0486a 100644 --- a/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp +++ b/blue_dynamics/include/blue_dynamics/thruster_dynamics.hpp @@ -50,14 +50,14 @@ const uint16_t kNoThrustPwm = 1500; * @param voltage The current battery voltage. * @return The minimum and maximum PWM values for the deadzone band. */ -[[nodiscard]] inline std::tuple calculateDeadZone(double voltage); +[[nodiscard]] std::tuple calculateDeadZone(double voltage); /** * @brief Calculate the general T200 deadzone band. * * @return The minimum and maximum PWM values for the deadzone band. */ -[[nodiscard]] inline std::tuple calculateDeadZone(); +[[nodiscard]] std::tuple calculateDeadZone(); /** * @brief Approximate the PWM input needed to achieve a desired force given the current battery @@ -72,10 +72,10 @@ const uint16_t kNoThrustPwm = 1500; * @param voltage The current battery voltage (V). * @return The PWM input needed to achieve the desired force. */ -[[nodiscard]] inline int calculatePwmFromThrustSurface(double force, double voltage); +[[nodiscard]] int calculatePwmFromThrustSurface(double force, double voltage); /** - * @brief Approximate the PWM inpute needed to achieve a desired force. + * @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 @@ -85,6 +85,6 @@ const uint16_t kNoThrustPwm = 1500; * @param force The desired force (N). * @return The PWM input needed to achieve the desired force. */ -[[nodiscard]] inline int calculatePwmFromThrustCurve(double force); +[[nodiscard]] int calculatePwmFromThrustCurve(double force); } // namespace blue::dynamics diff --git a/blue_dynamics/src/thruster_dynamics.cpp b/blue_dynamics/src/thruster_dynamics.cpp index c04d1361..f7774bd1 100644 --- a/blue_dynamics/src/thruster_dynamics.cpp +++ b/blue_dynamics/src/thruster_dynamics.cpp @@ -20,12 +20,13 @@ #include "blue_dynamics/thruster_dynamics.hpp" +#include #include namespace blue::dynamics { -[[nodiscard]] inline std::tuple calculateDeadZone(double voltage) +[[nodiscard]] std::tuple calculateDeadZone(double voltage) { // The minimum PWM in the deadzone increases as the voltage increases // This is best represented by a 3rd order polynomial @@ -41,9 +42,13 @@ namespace blue::dynamics return std::tuple(min_deadzone, max_deadzone); } -[[nodiscard]] inline std::tuple calculateDeadZone() {} +[[nodiscard]] std::tuple calculateDeadZone() +{ + // The deadband zone was identified using the thrust curve data with an 18v battery + return std::tuple(1470, 1530); +} -[[nodiscard]] inline int calculatePwmFromThrustSurface(double force, double voltage) +[[nodiscard]] int calculatePwmFromThrustSurface(double force, double voltage) { // Coefficients for the surface identified by fitting a surface to the thrust curves for the // voltages 10, 12, 14, 16, 18, 20 using Matlab's `fit` function with the `Poly23` surface @@ -65,6 +70,27 @@ namespace blue::dynamics return static_cast(std::round(pwm)); } -[[nodiscard]] inline int calculatePwmFromThrustCurve(double force) {} +[[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(std::round(pwm)); +} } // namespace blue::dynamics diff --git a/blue_dynamics/test/test_thruster_dynamics.cpp b/blue_dynamics/test/test_thruster_dynamics.cpp index 3e4fcdfa..83e8fc3a 100644 --- a/blue_dynamics/test/test_thruster_dynamics.cpp +++ b/blue_dynamics/test/test_thruster_dynamics.cpp @@ -36,20 +36,27 @@ TEST(ThrusterDynamicsTest, TestDeadzoneModel) const double voltage = 20.0; - const std::tuple actual = calculateDeadZone(voltage); + // Calculate the deadzone using the battery voltage + std::tuple 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); }