WPILibC++ 2024.3.2
frc::detail Namespace Reference

Classes

class  ListenerExecutor
 An executor for running listener tasks posted by Sendable listeners synchronously from the main application thread. More...
 
class  RecordingController
 
class  ShuffleboardInstance
 

Functions

const char * GetStringForWidgetType (BuiltInWidgets type)
 
std::shared_ptr< SendableCameraWrapper > & GetSendableCameraWrapper (std::string_view cameraName)
 
void AddToSendableRegistry (wpi::Sendable *sendable, std::string_view name)
 
WPILIB_DLLEXPORT int IncrementAndGetProfiledPIDControllerInstances ()
 
template<int States, int Inputs>
void CheckDARE_ABQ (const Eigen::Matrix< double, States, States > &A, const Eigen::Matrix< double, States, Inputs > &B, const Eigen::Matrix< double, States, States > &Q)
 Checks the preconditions of A, B, and Q for the DARE solver. More...
 
template<int States, int Inputs>
Eigen::Matrix< double, States, States > DARE (const Eigen::Matrix< double, States, States > &A, const Eigen::Matrix< double, States, Inputs > &B, const Eigen::Matrix< double, States, States > &Q, const Eigen::LLT< Eigen::Matrix< double, Inputs, Inputs > > &R_llt)
 Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation: More...
 
template<int States, int Inputs>
Eigen::Matrix< double, States, States > DARE (const Eigen::Matrix< double, States, States > &A, const Eigen::Matrix< double, States, Inputs > &B, const Eigen::Matrix< double, States, States > &Q, const Eigen::LLT< Eigen::Matrix< double, Inputs, Inputs > > &R_llt, const Eigen::Matrix< double, Inputs, Inputs > &N)
 Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation: More...
 

Variables

constexpr const char * kProtocol = "camera_server://"
 

Function Documentation

◆ AddToSendableRegistry()

void frc::detail::AddToSendableRegistry ( wpi::Sendable sendable,
std::string_view  name 
)

◆ CheckDARE_ABQ()

template<int States, int Inputs>
void frc::detail::CheckDARE_ABQ ( const Eigen::Matrix< double, States, States > &  A,
const Eigen::Matrix< double, States, Inputs > &  B,
const Eigen::Matrix< double, States, States > &  Q 
)

Checks the preconditions of A, B, and Q for the DARE solver.

Template Parameters
StatesNumber of states.
InputsNumber of inputs.
Parameters
AThe system matrix.
BThe input matrix.
QThe state cost matrix.
Exceptions
std::invalid_argumentif Q isn't symmetric positive semidefinite.
std::invalid_argumentif the (A, B) pair isn't stabilizable.
std::invalid_argumentif the (A, C) pair where Q = CᵀC isn't detectable.

◆ DARE() [1/2]

template<int States, int Inputs>
Eigen::Matrix< double, States, States > frc::detail::DARE ( const Eigen::Matrix< double, States, States > &  A,
const Eigen::Matrix< double, States, Inputs > &  B,
const Eigen::Matrix< double, States, States > &  Q,
const Eigen::LLT< Eigen::Matrix< double, Inputs, Inputs > > &  R_llt 
)

Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation:

AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0

This internal function skips expensive precondition checks for increased performance. The solver may hang if any of the following occur:

  • Q isn't symmetric positive semidefinite
  • R isn't symmetric positive definite
  • The (A, B) pair isn't stabilizable
  • The (A, C) pair where Q = CᵀC isn't detectable

Only use this function if you're sure the preconditions are met.

Template Parameters
StatesNumber of states.
InputsNumber of inputs.
Parameters
AThe system matrix.
BThe input matrix.
QThe state cost matrix.
R_lltThe LLT decomposition of the input cost matrix.

◆ DARE() [2/2]

template<int States, int Inputs>
Eigen::Matrix< double, States, States > frc::detail::DARE ( const Eigen::Matrix< double, States, States > &  A,
const Eigen::Matrix< double, States, Inputs > &  B,
const Eigen::Matrix< double, States, States > &  Q,
const Eigen::LLT< Eigen::Matrix< double, Inputs, Inputs > > &  R_llt,
const Eigen::Matrix< double, Inputs, Inputs > &  N 
)

Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation:

AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0

This is equivalent to solving the original DARE:

A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0

where A₂ and Q₂ are a change of variables:

A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ

This overload of the DARE is useful for finding the control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.

    ∞ [xₖ]ᵀ[Q  N][xₖ]
J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
   k=0

This is a more general form of the following. The linear-quadratic regulator is the feedback control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:

    ∞
J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
   k=0

This can be refactored as:

    ∞ [xₖ]ᵀ[Q 0][xₖ]
J = Σ [uₖ] [0 R][uₖ] ΔT
   k=0

This internal function skips expensive precondition checks for increased performance. The solver may hang if any of the following occur:

  • Q₂ isn't symmetric positive semidefinite
  • R isn't symmetric positive definite
  • The (A₂, B) pair isn't stabilizable
  • The (A₂, C) pair where Q₂ = CᵀC isn't detectable

Only use this function if you're sure the preconditions are met.

Template Parameters
StatesNumber of states.
InputsNumber of inputs.
Parameters
AThe system matrix.
BThe input matrix.
QThe state cost matrix.
R_lltThe LLT decomposition of the input cost matrix.
NThe state-input cross cost matrix.

◆ GetSendableCameraWrapper()

std::shared_ptr< SendableCameraWrapper > & frc::detail::GetSendableCameraWrapper ( std::string_view  cameraName)

◆ GetStringForWidgetType()

const char * frc::detail::GetStringForWidgetType ( BuiltInWidgets  type)

◆ IncrementAndGetProfiledPIDControllerInstances()

WPILIB_DLLEXPORT int frc::detail::IncrementAndGetProfiledPIDControllerInstances ( )

Variable Documentation

◆ kProtocol

constexpr const char* frc::detail::kProtocol = "camera_server://"
constexpr