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 numThrusters;
      22             : 
      23             :     /** @brief Maps QP variable index i to the global thruster idx */
      24             :     std::vector<int> thrustersIdxMapping;
      25             : 
      26             :     /** @brief Copy of the layout passed to build() */
      27             :     ThrusterLayout layout;
      28             : 
      29             :     /** @brief Thrusters sorted by idx for deterministic column ordering */
      30             :     std::vector<Thruster> sorted_thrusters;
      31             : 
      32             :     /** @brief 6x(N+6) equality-constraint matrix [T_full | I_6] */
      33             :     MatrixXd Aeq;
      34             : 
      35             :     /** @brief 2Nx(N+6) inequality-constraint matrix encoding per-thruster bounds */
      36             :     MatrixXd Aineq;
      37             : 
      38             :     /** @brief 2N-element inequality right-hand side vector */
      39             :     VectorXd Bineq;
      40             : 
      41             :     /** @brief Linear cost vector (zero for pure quadratic minimisation) */
      42             :     VectorXd C;
      43             : 
      44             :     /** @brief (N+6)x(N+6) quadratic cost matrix (thruster effort + slack penalties) */
      45             :     MatrixXd Q;
      46             : 
      47             :     /** @brief Dense QP solver instance */
      48             :     Eigen::QuadProgDense qp;
      49             : 
      50             :     /**
      51             :      * @brief Builds the equality and inequality QP matrices from the allocation matrix
      52             :      *
      53             :      * @param T_full [in] Full 6xN allocation matrix
      54             :      */
      55           1 :     void buildQPMatrices(const MatrixXd& T_full);
      56             : 
      57             :     /**
      58             :      * @brief Updates the inequality RHS vector with new per-thruster force bounds
      59             :      *
      60             :      * @param min_force [in] Minimum thruster force in Newtons
      61             :      * @param max_force [in] Maximum thruster force in Newtons
      62             :      */
      63           1 :     void setBineq(float min_force, float max_force);
      64             : 
      65             : public:
      66           0 :     ThrustAllocation6DOFQPSolver() = default;
      67             : 
      68             :     /**
      69             :      * @brief Builds QP matrices and initialises the solver from a thruster layout
      70             :      *
      71             :      * @param layout [in] Thruster layout describing positions and force directions
      72             :      */
      73           1 :     void build(const ThrusterLayout& layout) override;
      74             : 
      75             :     /**
      76             :      * @brief Solves the 6DOF QP and returns per-thruster force magnitudes
      77             :      *
      78             :      * @param tau [in] Desired 6DOF body force/torque vector in N and Nm
      79             :      *
      80             :      * @return Nx1 vector of per-thruster forces in Newtons indexed by thruster idx
      81             :      */
      82           1 :     VectorXd allocate(const Vector6d& tau) override;
      83             : 
      84             :     /**
      85             :      * @brief Reconstructs the achieved 6DOF body force from per-thruster forces
      86             :      *
      87             :      * @param thrusterForces [in] Nx1 vector of per-thruster forces in Newtons
      88             :      *
      89             :      * @return 6-element achieved body force/torque vector in N and Nm
      90             :      */
      91           1 :     Vector6d getBodyForce(const VectorXd& thrusterForces) override;
      92             : 
      93             :     /**
      94             :      * @brief Sets the slack-variable penalty weight for a given body axis
      95             :      *
      96             :      * @param val  [in] Exponent for the weight (weight = 10^val)
      97             :      * @param axis [in] Body axis index: 0=X, 1=Y, 2=Z, 3=Roll, 4=Pitch, 5=Yaw
      98             :      */
      99           1 :     void setQ(double val, uint8_t axis) override;
     100             : 
     101             :     /**
     102             :      * @brief Updates the per-thruster force bounds for the QP inequality constraints
     103             :      *
     104             :      * @param min_force [in] Minimum thruster force in Newtons
     105             :      * @param max_force [in] Maximum thruster force in Newtons
     106             :      */
     107           1 :     void updateLimits(float min_force, float max_force) override;
     108             : };
     109             : 
     110             : #endif  // CONTROLS_ALLOCATOR_SOLVER_THRUST_ALLOCATION_6DOF_QP_SOLVER_H