Line data    Source code

       1             : #ifndef CONTROLS_ALLOCATOR_SOLVER_THRUST_ALLOCATION_3DOF_QP_SOLVER_H
       2             : #define CONTROLS_ALLOCATOR_SOLVER_THRUST_ALLOCATION_3DOF_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 3DOF (X, Y, Yaw) surface vehicles
      12             :  *
      13             :  * Solves a dense QP with N+3 variables (N thruster forces + 3 slack variables),
      14             :  * 3 equality constraints (X, Y, Yaw body force matching), and 2N inequality
      15             :  * constraints (per-thruster force bounds).  Slack variables allow the solver to
      16             :  * find a feasible solution even when the requested force is unachievable.
      17             :  *
      18             :  * The Z, Roll, and Pitch axes of tau are ignored during allocation.
      19             :  */
      20           1 : class ThrustAllocation3DOFQPSolver : public ThrustAllocationSolverInterface {
      21             : protected:
      22             :     /** @brief Number of thrusters in the current layout */
      23             :     int numThrusters;
      24             : 
      25             :     /** @brief Maps QP variable index i to the global thruster idx */
      26             :     std::vector<int> thrustersIdxMapping;
      27             : 
      28             :     /** @brief Copy of the layout passed to build() */
      29             :     ThrusterLayout layout;
      30             : 
      31             :     /** @brief Thrusters sorted by idx for deterministic column ordering */
      32             :     std::vector<Thruster> sorted_thrusters;
      33             : 
      34             :     /** @brief Full 6xN allocation matrix retained for getBodyForce() */
      35             :     MatrixXd T_full;
      36             : 
      37             :     /** @brief 3x(N+3) equality-constraint matrix [T_xy_yaw | I_3] */
      38             :     MatrixXd Aeq;
      39             : 
      40             :     /** @brief 2Nx(N+3) inequality-constraint matrix encoding per-thruster bounds */
      41             :     MatrixXd Aineq;
      42             : 
      43             :     /** @brief 2N-element inequality right-hand side vector */
      44             :     VectorXd Bineq;
      45             : 
      46             :     /** @brief Linear cost vector (zero for pure quadratic minimisation) */
      47             :     VectorXd C;
      48             : 
      49             :     /** @brief (N+3)x(N+3) quadratic cost matrix (thruster effort + slack penalties) */
      50             :     MatrixXd Q;
      51             : 
      52             :     /** @brief Dense QP solver instance */
      53             :     Eigen::QuadProgDense qp;
      54             : 
      55             :     /**
      56             :      * @brief Builds the equality and inequality QP matrices from the allocation matrix
      57             :      *
      58             :      * @param T_full [in] Full 6xN allocation matrix
      59             :      */
      60           1 :     void buildQPMatrices(const MatrixXd& T_full);
      61             : 
      62             :     /**
      63             :      * @brief Updates the inequality RHS vector with new per-thruster force bounds
      64             :      *
      65             :      * @param min_force [in] Minimum thruster force in Newtons
      66             :      * @param max_force [in] Maximum thruster force in Newtons
      67             :      */
      68           1 :     void setBineq(float min_force, float max_force);
      69             : 
      70             : public:
      71           0 :     ThrustAllocation3DOFQPSolver() = default;
      72             : 
      73             :     /**
      74             :      * @brief Builds QP matrices and initialises the solver from a thruster layout
      75             :      *
      76             :      * @param layout [in] Thruster layout describing positions and force directions
      77             :      */
      78           1 :     void build(const ThrusterLayout& layout) override;
      79             : 
      80             :     /**
      81             :      * @brief Solves the 3DOF QP and returns per-thruster force magnitudes
      82             :      *
      83             :      * @param tau [in] Desired 6DOF body force vector (only X, Y, Yaw are used)
      84             :      *
      85             :      * @return Nx1 vector of per-thruster forces in Newtons indexed by thruster idx
      86             :      */
      87           1 :     VectorXd allocate(const Vector6d& tau) override;
      88             : 
      89             :     /**
      90             :      * @brief Reconstructs the achieved 6DOF body force from per-thruster forces
      91             :      *
      92             :      * @param thrusterForces [in] Nx1 vector of per-thruster forces in Newtons
      93             :      *
      94             :      * @return 6-element achieved body force/torque vector in N and Nm
      95             :      */
      96           1 :     Vector6d getBodyForce(const VectorXd& thrusterForces) override;
      97             : 
      98             :     /**
      99             :      * @brief Sets the slack-variable penalty weight for a 3DOF-relevant axis
     100             :      *
     101             :      * Only axes 0 (X), 1 (Y), and 5 (Yaw) are mapped to slack variables;
     102             :      * other axes are silently ignored.
     103             :      *
     104             :      * @param val  [in] Exponent for the weight (weight = 10^val)
     105             :      * @param axis [in] Body axis index: 0=X, 1=Y, 5=Yaw
     106             :      */
     107           1 :     void setQ(double val, uint8_t axis) override;
     108             : 
     109             :     /**
     110             :      * @brief Updates the per-thruster force bounds for the QP inequality constraints
     111             :      *
     112             :      * @param min_force [in] Minimum thruster force in Newtons
     113             :      * @param max_force [in] Maximum thruster force in Newtons
     114             :      */
     115           1 :     void updateLimits(float min_force, float max_force) override;
     116             : };
     117             : 
     118             : #endif  // CONTROLS_ALLOCATOR_SOLVER_THRUST_ALLOCATION_3DOF_QP_SOLVER_H