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 num_thrusters_;
 
    MatrixXd T_full_;
 
    MatrixXd Aeq_;
 
    MatrixXd Aineq_;
 
    VectorXd Bineq_;
 
    VectorXd C_;
 
    MatrixXd Q_;
 
    Eigen::QuadProgDense qp_;
 
    void build_qp_matrices(const MatrixXd& T_full);
 
    void set_bineq(float min_force, float max_force);
 
public:
    ThrustAllocation6DOFQPSolver() = default;
 
    void build(const ThrusterLayout& layout) override;
 
    VectorXd allocate(const Vector6d& tau) override;
 
    Vector6d get_body_force(const VectorXd& thruster_forces) override;
 
    void set_q(double val, uint8_t axis) override;
 
    void update_limits(float min_force, float max_force) override;
};
 
#endif  // CONTROLS_ALLOCATOR_SOLVER_THRUST_ALLOCATION_6DOF_QP_SOLVER_H