Line data    Source code

       1             : #ifndef CONTROLS_CONTROLLER_BUOYANCY_CONTROLLER_H
       2             : #define CONTROLS_CONTROLLER_BUOYANCY_CONTROLLER_H
       3             : 
       4             : #include <controller/BaseControllerInterface.h>
       5             : 
       6             : /**
       7             :  * @brief Controller that compensates for buoyancy and gravity restoring forces
       8             :  *
       9             :  * Produces two additive components:
      10             :  *
      11             :  * 1. **Static model-based compensation** — the gravitational/buoyancy restoring
      12             :  *    wrench G(q) evaluated at the goal orientation, identical to the original
      13             :  *    behaviour.
      14             :  *
      15             :  * 2. **Adaptive bias estimation** (opt-in via `buoyancy_adaptive`) — a slow
      16             :  *    integrator on the heave axis that learns the residual buoyancy force not
      17             :  *    captured by the static model.  The bias is only updated when the vehicle
      18             :  *    is in approximate steady state (small depth error and small heave
      19             :  *    velocity), so it does not fight the PD controller during transients.
      20             :  *    Inspired by the adaptive feedforward in Eng You Hong et al., "Depth
      21             :  *    Control of an Autonomous Underwater Vehicle, STARFISH", NUS 2010.
      22             :  *
      23             :  * Returns zero near the surface when surface gain scheduling is enabled.
      24             :  */
      25           1 : class BuoyancyController : public BaseControllerInterface {
      26             :     static constexpr double MAX_DT            = 0.1;
      27             :     static constexpr double EXPLODE_THRESHOLD = 1e4;
      28             : 
      29             : private:
      30             :     /** @brief Accumulated adaptive heave force bias in Newtons */
      31             :     double buoyancy_bias_ = 0.0;
      32             : 
      33             :     /** @brief Snapshot of the bias saved before a trajectory, for restore on completion */
      34             :     double saved_buoyancy_bias_ = 0.0;
      35             : 
      36             : public:
      37           0 :     using BaseControllerInterface::BaseControllerInterface;
      38             : 
      39             :     /**
      40             :      * @brief Resets the adaptive buoyancy bias to zero
      41             :      */
      42           1 :     void reset() override;
      43             : 
      44             :     /**
      45             :      * @brief Computes the total buoyancy compensation force
      46             :      *
      47             :      * Returns G(q) from the dynamics model plus, when adaptive estimation is
      48             :      * enabled, an adaptive heave-axis bias that slowly tracks residual depth
      49             :      * error during steady state.
      50             :      *
      51             :      * @param goal [in] Desired vehicle state (orientation and position Z are used)
      52             :      * @param curr [in] Current vehicle state (position Z and twist Z are used)
      53             :      * @param dt   [in] Time step in seconds (used by the adaptive estimator)
      54             :      *
      55             :      * @return 6-element restoring + adaptive force/torque vector in N and Nm
      56             :      */
      57           1 :     Vector6d getForce(const State& goal, const State& curr, double dt) override;
      58             : 
      59             :     /**
      60             :      * @brief Saves the current adaptive bias for later restoration
      61             :      */
      62           1 :     void saveBias();
      63             : 
      64             :     /**
      65             :      * @brief Restores the adaptive bias from the most recent saved snapshot
      66             :      */
      67           1 :     void loadBias();
      68             : 
      69             :     /**
      70             :      * @brief Returns the current adaptive bias value in Newtons
      71             :      */
      72           1 :     double getBias() const;
      73             : };
      74             : 
      75             : #endif  // CONTROLS_CONTROLLER_BUOYANCY_CONTROLLER_H