Line data    Source code

       1             : #ifndef CONTROLS_ALLOCATOR_SOLVER_THRUST_ALLOCATION_6DOF_QP_SOLVER_H
       2             : #define CONTROLS_ALLOCATOR_SOLVER_THRUST_ALLOCATION_6DOF_QP_SOLVER_H
       3             : 
       4             : #include <allocator/config/ThrusterLayout.h>
       5             : #include <allocator/solver/ThrustAllocationSolverInterface.h>
       6             : #include <eigen-quadprog/QuadProg.h>
       7             : 
       8             : #include <vector>
       9             : 
      10             : /**
      11             :  * @brief Quadratic-programming thrust allocator for full 6DOF underwater vehicles
      12             :  *
      13             :  * Solves a dense QP with N+6 variables (N thruster forces + 6 slack variables),
      14             :  * 6 equality constraints (full body force matching), and 2N inequality
      15             :  * constraints (per-thruster force bounds).  Slack variables absorb infeasibility
      16             :  * when the requested wrench exceeds the achievable thruster envelope.
      17             :  */
      18           1 : class ThrustAllocation6DOFQPSolver : public ThrustAllocationSolverInterface {
      19             : protected:
      20             :     /** @brief Number of thrusters in the current layout */
      21             :     int num_thrusters_;
      22             : 
      23             :     /** @brief Full 6xN allocation matrix retained for get_body_force() */
      24             :     MatrixXd T_full_;
      25             : 
      26             :     /** @brief 6x(N+6) equality-constraint matrix [T_full | I_6] */
      27             :     MatrixXd Aeq_;
      28             : 
      29             :     /** @brief 2Nx(N+6) inequality-constraint matrix encoding per-thruster bounds */
      30             :     MatrixXd Aineq_;
      31             : 
      32             :     /** @brief 2N-element inequality right-hand side vector */
      33             :     VectorXd Bineq_;
      34             : 
      35             :     /** @brief Linear cost vector (zero for pure quadratic minimisation) */
      36             :     VectorXd C_;
      37             : 
      38             :     /** @brief (N+6)x(N+6) quadratic cost matrix (thruster effort + slack penalties) */
      39             :     MatrixXd Q_;
      40             : 
      41             :     /** @brief Dense QP solver instance */
      42             :     Eigen::QuadProgDense qp_;
      43             : 
      44             :     /**
      45             :      * @brief Builds the equality and inequality QP matrices from the allocation matrix
      46             :      *
      47             :      * @param T_full [in] Full 6xN allocation matrix
      48             :      */
      49           1 :     void build_qp_matrices(const MatrixXd& T_full);
      50             : 
      51             :     /**
      52             :      * @brief Updates the inequality RHS vector with new per-thruster force bounds
      53             :      *
      54             :      * @param min_force [in] Minimum thruster force in Newtons
      55             :      * @param max_force [in] Maximum thruster force in Newtons
      56             :      */
      57           1 :     void set_bineq(float min_force, float max_force);
      58             : 
      59             : public:
      60           0 :     ThrustAllocation6DOFQPSolver() = default;
      61             : 
      62             :     /**
      63             :      * @brief Builds QP matrices and initialises the solver from a thruster layout
      64             :      *
      65             :      * @param layout [in] Thruster layout describing positions and force directions
      66             :      */
      67           1 :     void build(const ThrusterLayout& layout) override;
      68             : 
      69             :     /**
      70             :      * @brief Solves the 6DOF QP and returns per-thruster force magnitudes
      71             :      *
      72             :      * @param tau [in] Desired 6DOF body force/torque vector in N and Nm
      73             :      *
      74             :      * @return Nx1 vector of per-thruster forces in Newtons indexed by thruster idx
      75             :      */
      76           1 :     VectorXd allocate(const Vector6d& tau) override;
      77             : 
      78             :     /**
      79             :      * @brief Reconstructs the achieved 6DOF body force from per-thruster forces
      80             :      *
      81             :      * @param thruster_forces [in] Nx1 vector of per-thruster forces in Newtons
      82             :      *
      83             :      * @return 6-element achieved body force/torque vector in N and Nm
      84             :      */
      85           1 :     Vector6d get_body_force(const VectorXd& thruster_forces) override;
      86             : 
      87             :     /**
      88             :      * @brief Sets the slack-variable penalty weight for a given body axis
      89             :      *
      90             :      * @param val  [in] Exponent for the weight (weight = 10^val)
      91             :      * @param axis [in] Body axis index: 0=X, 1=Y, 2=Z, 3=Roll, 4=Pitch, 5=Yaw
      92             :      */
      93           1 :     void set_q(double val, uint8_t axis) override;
      94             : 
      95             :     /**
      96             :      * @brief Updates the per-thruster force bounds for the QP inequality constraints
      97             :      *
      98             :      * @param min_force [in] Minimum thruster force in Newtons
      99             :      * @param max_force [in] Maximum thruster force in Newtons
     100             :      */
     101           1 :     void update_limits(float min_force, float max_force) override;
     102             : };
     103             : 
     104             : #endif  // CONTROLS_ALLOCATOR_SOLVER_THRUST_ALLOCATION_6DOF_QP_SOLVER_H