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 num_thrusters_;
      24             : 
      25             :     /** @brief Full 6xN allocation matrix retained for get_body_force() */
      26             :     MatrixXd T_full_;
      27             : 
      28             :     /** @brief 3x(N+3) equality-constraint matrix [T_xy_yaw | I_3] */
      29             :     MatrixXd Aeq_;
      30             : 
      31             :     /** @brief 2Nx(N+3) inequality-constraint matrix encoding per-thruster bounds */
      32             :     MatrixXd Aineq_;
      33             : 
      34             :     /** @brief 2N-element inequality right-hand side vector */
      35             :     VectorXd Bineq_;
      36             : 
      37             :     /** @brief Linear cost vector (zero for pure quadratic minimisation) */
      38             :     VectorXd C_;
      39             : 
      40             :     /** @brief (N+3)x(N+3) quadratic cost matrix (thruster effort + slack penalties) */
      41             :     MatrixXd Q_;
      42             : 
      43             :     /** @brief Dense QP solver instance */
      44             :     Eigen::QuadProgDense qp_;
      45             : 
      46             :     /**
      47             :      * @brief Builds the equality and inequality QP matrices from the allocation matrix
      48             :      *
      49             :      * @param T_full [in] Full 6xN allocation matrix
      50             :      */
      51           1 :     void build_qp_matrices(const MatrixXd& T_full);
      52             : 
      53             :     /**
      54             :      * @brief Updates the inequality RHS vector with new per-thruster force bounds
      55             :      *
      56             :      * @param min_force [in] Minimum thruster force in Newtons
      57             :      * @param max_force [in] Maximum thruster force in Newtons
      58             :      */
      59           1 :     void set_bineq(float min_force, float max_force);
      60             : 
      61             : public:
      62           0 :     ThrustAllocation3DOFQPSolver() = default;
      63             : 
      64             :     /**
      65             :      * @brief Builds QP matrices and initialises the solver from a thruster layout
      66             :      *
      67             :      * @param layout [in] Thruster layout describing positions and force directions
      68             :      */
      69           1 :     void build(const ThrusterLayout& layout) override;
      70             : 
      71             :     /**
      72             :      * @brief Solves the 3DOF QP and returns per-thruster force magnitudes
      73             :      *
      74             :      * @param tau [in] Desired 6DOF body force vector (only X, Y, Yaw are used)
      75             :      *
      76             :      * @return Nx1 vector of per-thruster forces in Newtons indexed by thruster idx
      77             :      */
      78           1 :     VectorXd allocate(const Vector6d& tau) override;
      79             : 
      80             :     /**
      81             :      * @brief Reconstructs the achieved 6DOF body force from per-thruster forces
      82             :      *
      83             :      * Uses the full 6xN allocation matrix, so the Z, Roll, and Pitch components are
      84             :      * reported even though they are not constrained during 3DOF allocation.
      85             :      *
      86             :      * @param thruster_forces [in] Nx1 vector of per-thruster forces in Newtons
      87             :      *
      88             :      * @return 6-element achieved body force/torque vector in N and Nm
      89             :      */
      90           1 :     Vector6d get_body_force(const VectorXd& thruster_forces) override;
      91             : 
      92             :     /**
      93             :      * @brief Sets the slack-variable penalty weight for a 3DOF-relevant axis
      94             :      *
      95             :      * Only axes 0 (X), 1 (Y), and 5 (Yaw) are mapped to slack variables;
      96             :      * other axes are silently ignored.
      97             :      *
      98             :      * @param val  [in] Exponent for the weight (weight = 10^val)
      99             :      * @param axis [in] Body axis index: 0=X, 1=Y, 5=Yaw
     100             :      */
     101           1 :     void set_q(double val, uint8_t axis) override;
     102             : 
     103             :     /**
     104             :      * @brief Updates the per-thruster force bounds for the QP inequality constraints
     105             :      *
     106             :      * @param min_force [in] Minimum thruster force in Newtons
     107             :      * @param max_force [in] Maximum thruster force in Newtons
     108             :      */
     109           1 :     void update_limits(float min_force, float max_force) override;
     110             : };
     111             : 
     112             : #endif  // CONTROLS_ALLOCATOR_SOLVER_THRUST_ALLOCATION_3DOF_QP_SOLVER_H