ThrustAllocation6DOFQPSolver.h

Go to the documentation of this file.

#ifndef CONTROLS_ALLOCATOR_SOLVER_THRUST_ALLOCATION_6DOF_QP_SOLVER_H
#define CONTROLS_ALLOCATOR_SOLVER_THRUST_ALLOCATION_6DOF_QP_SOLVER_H
 
#include <allocator/config/ThrusterLayout.h>
#include <allocator/solver/ThrustAllocationSolverInterface.h>
#include <eigen-quadprog/QuadProg.h>
 
#include <vector>
 
class ThrustAllocation6DOFQPSolver : public ThrustAllocationSolverInterface {
protected:
    int numThrusters;
 
    std::vector<int> thrustersIdxMapping;
 
    ThrusterLayout layout;
 
    std::vector<Thruster> sorted_thrusters;
 
    MatrixXd Aeq;
 
    MatrixXd Aineq;
 
    VectorXd Bineq;
 
    VectorXd C;
 
    MatrixXd Q;
 
    Eigen::QuadProgDense qp;
 
    void buildQPMatrices(const MatrixXd& T_full);
 
    void setBineq(float min_force, float max_force);
 
public:
    ThrustAllocation6DOFQPSolver() = default;
 
    void build(const ThrusterLayout& layout) override;
 
    VectorXd allocate(const Vector6d& tau) override;
 
    Vector6d getBodyForce(const VectorXd& thrusterForces) override;
 
    void setQ(double val, uint8_t axis) override;
 
    void updateLimits(float min_force, float max_force) override;
};
 
#endif  // CONTROLS_ALLOCATOR_SOLVER_THRUST_ALLOCATION_6DOF_QP_SOLVER_H