Line data    Source code

       1             : #ifndef CONTROLS_ALLOCATOR_SOLVER_THRUST_ALLOCATION_SOLVER_INTERFACE_H
       2             : #define CONTROLS_ALLOCATOR_SOLVER_THRUST_ALLOCATION_SOLVER_INTERFACE_H
       3             : 
       4             : #include <utils/Types.h>
       5             : 
       6             : class ThrusterLayout;
       7             : 
       8             : /**
       9             :  * @brief Abstract interface for quadratic-programming thrust allocation solvers
      10             :  *
      11             :  * Concrete solvers implement force allocation for a specific DOF subset (3DOF
      12             :  * or 6DOF) by solving a QP that minimises thruster effort while satisfying the
      13             :  * desired body force equality constraints subject to per-thruster force bounds.
      14             :  */
      15           1 : class ThrustAllocationSolverInterface {
      16             : public:
      17           0 :     virtual ~ThrustAllocationSolverInterface() = default;
      18             : 
      19             :     /**
      20             :      * @brief Builds or rebuilds all internal QP matrices from a thruster layout
      21             :      *
      22             :      * Must be called before allocate() whenever the thruster configuration changes.
      23             :      *
      24             :      * @param layout [in] Thruster layout describing positions and force directions
      25             :      */
      26           1 :     virtual void build(const ThrusterLayout& layout) = 0;
      27             : 
      28             :     /**
      29             :      * @brief Solves the QP and returns per-thruster force magnitudes
      30             :      *
      31             :      * @param tau [in] Desired 6DOF body force/torque vector in N and Nm
      32             :      *
      33             :      * @return Nx1 vector of per-thruster forces in Newtons, indexed by thruster idx
      34             :      */
      35           1 :     virtual VectorXd allocate(const Vector6d& tau) = 0;
      36             : 
      37             :     /**
      38             :      * @brief Reconstructs the achieved body force from per-thruster forces
      39             :      *
      40             :      * @param thrusterForces [in] Nx1 vector of per-thruster forces in Newtons
      41             :      *
      42             :      * @return 6-element achieved body force/torque vector in N and Nm
      43             :      */
      44           1 :     virtual Vector6d getBodyForce(const VectorXd& thrusterForces) = 0;
      45             : 
      46             :     /**
      47             :      * @brief Sets the slack-variable penalty weight for a given body axis
      48             :      *
      49             :      * The diagonal QP weight entry is set to 10^val, so larger values penalise
      50             :      * force deficit on that axis more heavily.
      51             :      *
      52             :      * @param val  [in] Exponent for the weight (weight = 10^val)
      53             :      * @param axis [in] Body axis index: 0=X, 1=Y, 2=Z, 3=Roll, 4=Pitch, 5=Yaw
      54             :      */
      55           1 :     virtual void setQ(double val, uint8_t axis) = 0;
      56             : 
      57             :     /**
      58             :      * @brief Updates the per-thruster force bounds used in the QP inequality constraints
      59             :      *
      60             :      * @param min_force [in] Minimum (most negative) thruster force in Newtons
      61             :      * @param max_force [in] Maximum thruster force in Newtons
      62             :      */
      63           1 :     virtual void updateLimits(float min_force, float max_force) = 0;
      64             : };
      65             : 
      66             : #endif  // CONTROLS_ALLOCATOR_SOLVER_THRUST_ALLOCATION_SOLVER_INTERFACE_H