Quadratic-programming thrust allocator for full 6DOF underwater vehicles. More...

#include <ThrustAllocation6DOFQPSolver.h>

Inheritance diagram for ThrustAllocation6DOFQPSolver:

Public Member Functions
	ThrustAllocation6DOFQPSolver ()=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 6DOF 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 given body 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	Aeq
	6x(N+6) equality-constraint matrix [T_full \| I_6] More...

MatrixXd	Aineq
	2Nx(N+6) 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+6)x(N+6) quadratic cost matrix (thruster effort + slack penalties) More...

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

Detailed Description

Quadratic-programming thrust allocator for full 6DOF underwater vehicles.

Solves a dense QP with N+6 variables (N thruster forces + 6 slack variables), 6 equality constraints (full body force matching), and 2N inequality constraints (per-thruster force bounds). Slack variables absorb infeasibility when the requested wrench exceeds the achievable thruster envelope.

Constructor & Destructor Documentation

◆ ThrustAllocation6DOFQPSolver()

ThrustAllocation6DOFQPSolver::ThrustAllocation6DOFQPSolver ( )

default

Member Function Documentation

◆ allocate()

VectorXd ThrustAllocation6DOFQPSolver::allocate ( const Vector6d & tau )

overridevirtual

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

Parameters

tau	[in] Desired 6DOF body force/torque vector in N and Nm

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

Implements ThrustAllocationSolverInterface.

◆ build()

void ThrustAllocation6DOFQPSolver::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 ThrustAllocation6DOFQPSolver::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 ThrustAllocation6DOFQPSolver::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 ThrustAllocation6DOFQPSolver::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 ThrustAllocation6DOFQPSolver::setQ	(	double	val,
		uint8_t	axis
	)

overridevirtual

Sets the slack-variable penalty weight for a given body axis.

Parameters

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

Implements ThrustAllocationSolverInterface.

◆ updateLimits()

void ThrustAllocation6DOFQPSolver::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 ThrustAllocation6DOFQPSolver::Aeq

protected

6x(N+6) equality-constraint matrix [T_full | I_6]

◆ Aineq

MatrixXd ThrustAllocation6DOFQPSolver::Aineq

protected

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

◆ Bineq

VectorXd ThrustAllocation6DOFQPSolver::Bineq

protected

2N-element inequality right-hand side vector

◆ C

VectorXd ThrustAllocation6DOFQPSolver::C

protected

Linear cost vector (zero for pure quadratic minimisation)

◆ layout

ThrusterLayout ThrustAllocation6DOFQPSolver::layout

protected

Copy of the layout passed to build()

◆ numThrusters

int ThrustAllocation6DOFQPSolver::numThrusters

protected

Number of thrusters in the current layout.

◆ Q

MatrixXd ThrustAllocation6DOFQPSolver::Q

protected

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

◆ qp

Eigen::QuadProgDense ThrustAllocation6DOFQPSolver::qp

protected

Dense QP solver instance.

◆ sorted_thrusters

std::vector<Thruster> ThrustAllocation6DOFQPSolver::sorted_thrusters

protected

Thrusters sorted by idx for deterministic column ordering.

◆ thrustersIdxMapping

std::vector<int> ThrustAllocation6DOFQPSolver::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/ThrustAllocation6DOFQPSolver.h
src/allocator/solver/ThrustAllocation6DOFQPSolver.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ ThrustAllocation6DOFQPSolver()

Member Function Documentation

◆ allocate()

◆ build()

◆ buildQPMatrices()

◆ getBodyForce()

◆ setBineq()

◆ setQ()

◆ updateLimits()

Member Data Documentation

◆ Aeq

◆ Aineq

◆ Bineq

◆ C

◆ layout

◆ numThrusters

◆ Q

◆ qp

◆ sorted_thrusters

◆ thrustersIdxMapping