ThrustAllocation6DOFQPSolver Class Reference

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	get_body_force (const VectorXd &thruster_forces) override
	Reconstructs the achieved 6DOF body force from per-thruster forces. More...
void	set_q (double val, uint8_t axis) override
	Sets the slack-variable penalty weight for a given body axis. More...
void	update_limits (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	build_qp_matrices (const MatrixXd &T_full)
	Builds the equality and inequality QP matrices from the allocation matrix. More...
void	set_bineq (float min_force, float max_force)
	Updates the inequality RHS vector with new per-thruster force bounds. More...

Protected Attributes
int	num_thrusters_
	Number of thrusters in the current layout. More...
MatrixXd	T_full_
	Full 6xN allocation matrix retained for get_body_force() 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.

build_qp_matrices()

void ThrustAllocation6DOFQPSolver::build_qp_matrices ( const MatrixXd &  T_full )

protected

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

Parameters

T_full

[in] Full 6xN allocation matrix

get_body_force()

Vector6d ThrustAllocation6DOFQPSolver::get_body_force ( const VectorXd &  thruster_forces )

overridevirtual

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

Parameters

thruster_forces

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

Returns

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

Implements ThrustAllocationSolverInterface.

set_bineq()

void ThrustAllocation6DOFQPSolver::set_bineq ( 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

set_q()

void ThrustAllocation6DOFQPSolver::set_q ( 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.

update_limits()

void ThrustAllocation6DOFQPSolver::update_limits ( 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)

num_thrusters_

int ThrustAllocation6DOFQPSolver::num_thrusters_

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.

T_full_

MatrixXd ThrustAllocation6DOFQPSolver::T_full_

protected

Full 6xN allocation matrix retained for get_body_force()

---

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

• include/allocator/solver/ThrustAllocation6DOFQPSolver.h
• src/allocator/solver/ThrustAllocation6DOFQPSolver.cpp