Quadratic-programming thrust allocator for 3DOF (X, Y, Yaw) surface vehicles. More...

#include <ThrustAllocation3DOFQPSolver.h>

Inheritance diagram for ThrustAllocation3DOFQPSolver:

Public Member Functions
	ThrustAllocation3DOFQPSolver ()=default

void	build (const ThrusterLayout &layout) override
	Builds QP matrices and initialises the solver from a thruster layout. More...

VectorXd	allocate (const Vector6d &tau) override
	Solves the 3DOF QP and returns per-thruster force magnitudes. More...

Vector6d	getBodyForce (const VectorXd &thrusterForces) override
	Reconstructs the achieved 6DOF body force from per-thruster forces. More...

void	setQ (double val, uint8_t axis) override
	Sets the slack-variable penalty weight for a 3DOF-relevant axis. More...

void	updateLimits (float min_force, float max_force) override
	Updates the per-thruster force bounds for the QP inequality constraints. More...

Public Member Functions inherited from ThrustAllocationSolverInterface
virtual	~ThrustAllocationSolverInterface ()=default

Protected Member Functions
void	buildQPMatrices (const MatrixXd &T_full)
	Builds the equality and inequality QP matrices from the allocation matrix. More...

void	setBineq (float min_force, float max_force)
	Updates the inequality RHS vector with new per-thruster force bounds. More...

Protected Attributes
int	numThrusters
	Number of thrusters in the current layout. More...

std::vector< int >	thrustersIdxMapping
	Maps QP variable index i to the global thruster idx. More...

ThrusterLayout	layout
	Copy of the layout passed to build() More...

std::vector< Thruster >	sorted_thrusters
	Thrusters sorted by idx for deterministic column ordering. More...

MatrixXd	T_full
	Full 6xN allocation matrix retained for getBodyForce() More...

MatrixXd	Aeq
	3x(N+3) equality-constraint matrix [T_xy_yaw \| I_3] More...

MatrixXd	Aineq
	2Nx(N+3) inequality-constraint matrix encoding per-thruster bounds More...

VectorXd	Bineq
	2N-element inequality right-hand side vector More...

VectorXd	C
	Linear cost vector (zero for pure quadratic minimisation) More...

MatrixXd	Q
	(N+3)x(N+3) quadratic cost matrix (thruster effort + slack penalties) More...

Eigen::QuadProgDense	qp
	Dense QP solver instance. More...

Detailed Description

Quadratic-programming thrust allocator for 3DOF (X, Y, Yaw) surface vehicles.

Solves a dense QP with N+3 variables (N thruster forces + 3 slack variables), 3 equality constraints (X, Y, Yaw body force matching), and 2N inequality constraints (per-thruster force bounds). Slack variables allow the solver to find a feasible solution even when the requested force is unachievable.

The Z, Roll, and Pitch axes of tau are ignored during allocation.

Constructor & Destructor Documentation

◆ ThrustAllocation3DOFQPSolver()

ThrustAllocation3DOFQPSolver::ThrustAllocation3DOFQPSolver ( )

default

Member Function Documentation

◆ allocate()

VectorXd ThrustAllocation3DOFQPSolver::allocate ( const Vector6d & tau )

overridevirtual

Solves the 3DOF QP and returns per-thruster force magnitudes.

Parameters

tau	[in] Desired 6DOF body force vector (only X, Y, Yaw are used)

Returns: Nx1 vector of per-thruster forces in Newtons indexed by thruster idx

Implements ThrustAllocationSolverInterface.

◆ build()

void ThrustAllocation3DOFQPSolver::build ( const ThrusterLayout & layout )

overridevirtual

Builds QP matrices and initialises the solver from a thruster layout.

Parameters

layout [in] Thruster layout describing positions and force directions

Implements ThrustAllocationSolverInterface.

◆ buildQPMatrices()

void ThrustAllocation3DOFQPSolver::buildQPMatrices ( const MatrixXd & T_full )

protected

Builds the equality and inequality QP matrices from the allocation matrix.

Parameters

T_full [in] Full 6xN allocation matrix

◆ getBodyForce()

Vector6d ThrustAllocation3DOFQPSolver::getBodyForce ( const VectorXd & thrusterForces )

overridevirtual

Reconstructs the achieved 6DOF body force from per-thruster forces.

Parameters

thrusterForces [in] Nx1 vector of per-thruster forces in Newtons

Returns: 6-element achieved body force/torque vector in N and Nm

Implements ThrustAllocationSolverInterface.

◆ setBineq()

void ThrustAllocation3DOFQPSolver::setBineq	(	float	min_force,
		float	max_force
	)

protected

Updates the inequality RHS vector with new per-thruster force bounds.

Parameters

min_force	[in] Minimum thruster force in Newtons
max_force	[in] Maximum thruster force in Newtons

◆ setQ()

void ThrustAllocation3DOFQPSolver::setQ	(	double	val,
		uint8_t	axis
	)

overridevirtual

Sets the slack-variable penalty weight for a 3DOF-relevant axis.

Only axes 0 (X), 1 (Y), and 5 (Yaw) are mapped to slack variables; other axes are silently ignored.

Parameters

val	[in] Exponent for the weight (weight = 10^val)
axis	[in] Body axis index: 0=X, 1=Y, 5=Yaw

Implements ThrustAllocationSolverInterface.

◆ updateLimits()

void ThrustAllocation3DOFQPSolver::updateLimits	(	float	min_force,
		float	max_force
	)

overridevirtual

Updates the per-thruster force bounds for the QP inequality constraints.

Parameters

min_force	[in] Minimum thruster force in Newtons
max_force	[in] Maximum thruster force in Newtons

Implements ThrustAllocationSolverInterface.

Member Data Documentation

◆ Aeq

MatrixXd ThrustAllocation3DOFQPSolver::Aeq

protected

3x(N+3) equality-constraint matrix [T_xy_yaw | I_3]

◆ Aineq

MatrixXd ThrustAllocation3DOFQPSolver::Aineq

protected

2Nx(N+3) inequality-constraint matrix encoding per-thruster bounds

◆ Bineq

VectorXd ThrustAllocation3DOFQPSolver::Bineq

protected

2N-element inequality right-hand side vector

◆ C

VectorXd ThrustAllocation3DOFQPSolver::C

protected

Linear cost vector (zero for pure quadratic minimisation)

◆ layout

ThrusterLayout ThrustAllocation3DOFQPSolver::layout

protected

Copy of the layout passed to build()

◆ numThrusters

int ThrustAllocation3DOFQPSolver::numThrusters

protected

Number of thrusters in the current layout.

◆ Q

MatrixXd ThrustAllocation3DOFQPSolver::Q

protected

(N+3)x(N+3) quadratic cost matrix (thruster effort + slack penalties)

◆ qp

Eigen::QuadProgDense ThrustAllocation3DOFQPSolver::qp

protected

Dense QP solver instance.

◆ sorted_thrusters

std::vector<Thruster> ThrustAllocation3DOFQPSolver::sorted_thrusters

protected

Thrusters sorted by idx for deterministic column ordering.

◆ T_full

MatrixXd ThrustAllocation3DOFQPSolver::T_full

protected

Full 6xN allocation matrix retained for getBodyForce()

◆ thrustersIdxMapping

std::vector<int> ThrustAllocation3DOFQPSolver::thrustersIdxMapping

protected

Maps QP variable index i to the global thruster idx.

The documentation for this class was generated from the following files:

include/allocator/solver/ThrustAllocation3DOFQPSolver.h
src/allocator/solver/ThrustAllocation3DOFQPSolver.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ ThrustAllocation3DOFQPSolver()

Member Function Documentation

◆ allocate()

◆ build()

◆ buildQPMatrices()

◆ getBodyForce()

◆ setBineq()

◆ setQ()

◆ updateLimits()

Member Data Documentation

◆ Aeq

◆ Aineq

◆ Bineq

◆ C

◆ layout

◆ numThrusters

◆ Q

◆ qp

◆ sorted_thrusters

◆ T_full

◆ thrustersIdxMapping