Functions
double	degToRad (const double &_deg)
	The degToRad function converts degrees to radians. More...

double	radToDeg (const double &_rad)
	The radToDeg function converts radians to degrees. More...

double	wrapAngleToPi (const double &_rad)
	The wrapAngleToPi function wraps an input angle to the interval [-pi, pi). More...

double	wrapAngleTo2Pi (const double &_rad)
	The wrapAngleTo2Pi function wraps an input angle to the interval [0, 2pi). More...

double	angleDiff (const double &_ang1, const double &_ang2)
	The angleDiff function returns the difference in angle between two given angles in the interval [0, pi) More...

template<typename Matrix_TypeA , typename Matrix_TypeB >
static bool	dampedPseudoInverse (const Matrix_TypeA &_J, Matrix_TypeB &_dPinvJ, const double _lambda=0, const double _epsilon=std::numeric_limits< typename Matrix_TypeA::Scalar >::epsilon())
	The dampedPseudoInverse function calculates the damped pseudo-inverse of a matrix using SVD. More...

template<typename Matrix_TypeA , typename Matrix_TypeB >
static bool	nullSpaceProjector (const Matrix_TypeA &_A, Matrix_TypeB &_N, const double _alpha=1)
	The nullSpaceProjector function calculates the null-space projector of a given matrix. The null-space projector is calculated using the Moore-Penrose pseudo-inverse (N = I - alpha * pinvA * A). More...

template<typename Matrix_TypeA >
Eigen::Matrix< typename Matrix_TypeA::Scalar, Eigen::Dynamic, Eigen::Dynamic >	SVDNullSpaceProjector (const Matrix_TypeA &_A)
	The SVDNullSpaceProjector function calculates the null-space projector of a given matrix. The null-space projector is calculated using the singular-value decomposition (SVD). The null-space. More...

template<typename Vector_TypeA , typename Vector_TypeB >
Eigen::Matrix< typename Vector_TypeA::Scalar, Eigen::Dynamic, 1 >	boxMinus (const Vector_TypeA &_a, const Vector_TypeB &_b)
	The boxMinus function calculates the box-minus operation between two vectors of equal size. More...

Variables
static constexpr double	TWO_PI = boost::math::constants::two_pi<double>()
	Define 2PI. More...

static constexpr double	FOUR_THIRDS_PI = boost::math::constants::four_thirds_pi<double>()
	Define 4PI/3. More...

static constexpr double	PI = boost::math::constants::pi<double>()
	Define PI. More...

static constexpr double	THREE_QUARTERS_PI = boost::math::constants::three_quarters_pi<double>()
	Define 3PI/4. More...

static constexpr double	TWO_THIRDS_PI = boost::math::constants::two_thirds_pi<double>()
	Define 2PI/3. More...

static constexpr double	HALF_PI = boost::math::constants::half_pi<double>()
	Define PI/2. More...

static constexpr double	THIRD_PI = boost::math::constants::third_pi<double>()
	Define PI/3. More...

static constexpr double	SIXTH_PI = boost::math::constants::sixth_pi<double>()
	Define PI/6. More...

static constexpr double	ONE_DIV_TWO_PI = boost::math::constants::one_div_two_pi<double>()
	Define 1/2PI. More...

static constexpr double	INF = std::numeric_limits<double>::infinity()
	Define infinity. More...

Function Documentation

◆ degToRad()

double math_utils::degToRad ( const double & _deg )

The degToRad function converts degrees to radians.

Parameters

[in] _deg Angle in degrees.

Returns: Angle in radians.

Definition at line 33 of file angle_utils.cpp.

◆ radToDeg()

double math_utils::radToDeg ( const double & _rad )

The radToDeg function converts radians to degrees.

Parameters

[in] _rad Angle in radians.

Returns: Angle in degrees.

Definition at line 39 of file angle_utils.cpp.

◆ wrapAngleToPi()

double math_utils::wrapAngleToPi ( const double & _rad )

The wrapAngleToPi function wraps an input angle to the interval [-pi, pi).

Parameters

[in] _rad Angle in radians.

Returns: Angle in radians in the interval [-pi,pi)

Definition at line 45 of file angle_utils.cpp.

◆ wrapAngleTo2Pi()

double math_utils::wrapAngleTo2Pi ( const double & _rad )

The wrapAngleTo2Pi function wraps an input angle to the interval [0, 2pi).

Parameters

[in] _rad Angle in radians.

Returns: Angle in radians in the interval [0, 2pi).

Definition at line 59 of file angle_utils.cpp.

◆ angleDiff()

double math_utils::angleDiff	(	const double &	_ang1,
		const double &	_ang2
	)

The angleDiff function returns the difference in angle between two given angles in the interval [0, pi)

Parameters

[in]	_ang1	First angle in radians
[in]	_ang2	Second angle in radians

Returns: Angle difference from _ang1 relative _ang2

Definition at line 72 of file angle_utils.cpp.

◆ dampedPseudoInverse()

template<typename Matrix_TypeA , typename Matrix_TypeB >

static bool math_utils::dampedPseudoInverse	(	const Matrix_TypeA &	_J,
		Matrix_TypeB &	_dPinvJ,
		const double	_lambda = `0`,
		const double	_epsilon = `std::numeric_limits<typename Matrix_TypeA::Scalar>::epsilon()`
	)

static

The dampedPseudoInverse function calculates the damped pseudo-inverse of a matrix using SVD.

Parameters

[in]	_J	Input Matrix.
[out]	_dPinvJ	Output Matrix (damped pseudo-inverse of the input matrix).
[in]	_lambda	Damping factor. Defaults to zero, i.e. the moore-penrose pseudo-inverse.
[in]	_epsilon	Numerical precision (singular value threshold).

Returns: Returns true if the damped pseudo-inverse is calculated successfully.

Definition at line 50 of file linalg_utils.h.

◆ nullSpaceProjector()

template<typename Matrix_TypeA , typename Matrix_TypeB >

static bool math_utils::nullSpaceProjector	(	const Matrix_TypeA &	_A,
		Matrix_TypeB &	_N,
		const double	_alpha = `1`
	)

static

The nullSpaceProjector function calculates the null-space projector of a given matrix. The null-space projector is calculated using the Moore-Penrose pseudo-inverse (N = I - alpha * pinvA * A).

Parameters

[in]	_A	Input matrix A.
[out]	_N	Output matrix N (null-space projector of the input matrix).
[in]	_alpha	Scalar parameter between 0 and 1 used to regulate the priority between two tasks (if used in a hierarchical control approach).

Returns: Returns true if a null-space projector is calculated successfully.

Definition at line 104 of file linalg_utils.h.

◆ SVDNullSpaceProjector()

template<typename Matrix_TypeA >

Eigen::Matrix<typename Matrix_TypeA::Scalar, Eigen::Dynamic, Eigen::Dynamic> math_utils::SVDNullSpaceProjector ( const Matrix_TypeA & _A )

The SVDNullSpaceProjector function calculates the null-space projector of a given matrix. The null-space projector is calculated using the singular-value decomposition (SVD). The null-space.

projector is found as N(_A) = V_N^*, where the SVD comprises:

A = [U_R, U_N][S, 0;[V_R^*;

0, 0] V_N^*]

Parameters

[in] _A Input matrix A.

Returns: Returns the SVD null-space projector of the input matrix _A.

Definition at line 144 of file linalg_utils.h.

◆ boxMinus()

template<typename Vector_TypeA , typename Vector_TypeB >

Eigen::Matrix<typename Vector_TypeA::Scalar, Eigen::Dynamic, 1> math_utils::boxMinus	(	const Vector_TypeA &	_a,
		const Vector_TypeB &	_b
	)

The boxMinus function calculates the box-minus operation between two vectors of equal size.

Parameters

[in]	_a	LHS vector.
[in]	_b	RHS vector.

Returns: Returns the box-minus operation between the two vectors.

Definition at line 180 of file linalg_utils.h.

Variable Documentation

◆ TWO_PI

constexpr double math_utils::TWO_PI = boost::math::constants::two_pi<double>()

staticconstexpr

Define 2PI.

Definition at line 38 of file angle_utils.h.

◆ FOUR_THIRDS_PI

constexpr double math_utils::FOUR_THIRDS_PI = boost::math::constants::four_thirds_pi<double>()

staticconstexpr

Define 4PI/3.

Definition at line 41 of file angle_utils.h.

◆ PI

constexpr double math_utils::PI = boost::math::constants::pi<double>()

staticconstexpr

Define PI.

Definition at line 44 of file angle_utils.h.

◆ THREE_QUARTERS_PI

constexpr double math_utils::THREE_QUARTERS_PI = boost::math::constants::three_quarters_pi<double>()

staticconstexpr

Define 3PI/4.

Definition at line 47 of file angle_utils.h.

◆ TWO_THIRDS_PI

constexpr double math_utils::TWO_THIRDS_PI = boost::math::constants::two_thirds_pi<double>()

staticconstexpr

Define 2PI/3.

Definition at line 50 of file angle_utils.h.

◆ HALF_PI

constexpr double math_utils::HALF_PI = boost::math::constants::half_pi<double>()

staticconstexpr

Define PI/2.

Definition at line 53 of file angle_utils.h.

◆ THIRD_PI

constexpr double math_utils::THIRD_PI = boost::math::constants::third_pi<double>()

staticconstexpr

Define PI/3.

Definition at line 56 of file angle_utils.h.

◆ SIXTH_PI

constexpr double math_utils::SIXTH_PI = boost::math::constants::sixth_pi<double>()

staticconstexpr

Define PI/6.

Definition at line 59 of file angle_utils.h.

◆ ONE_DIV_TWO_PI

constexpr double math_utils::ONE_DIV_TWO_PI = boost::math::constants::one_div_two_pi<double>()

staticconstexpr

Define 1/2PI.

Definition at line 62 of file angle_utils.h.

◆ INF

constexpr double math_utils::INF = std::numeric_limits<double>::infinity()

staticconstexpr

Define infinity.

Definition at line 33 of file const_utils.h.

Functions

Variables

Function Documentation

◆ degToRad()

◆ radToDeg()

◆ wrapAngleToPi()

◆ wrapAngleTo2Pi()

◆ angleDiff()

◆ dampedPseudoInverse()

◆ nullSpaceProjector()

◆ SVDNullSpaceProjector()

projector is found as N(_A) = V_N^*, where the SVD comprises:

0, 0] V_N^*]

◆ boxMinus()

Variable Documentation

◆ TWO_PI

◆ FOUR_THIRDS_PI

◆ PI

◆ THREE_QUARTERS_PI

◆ TWO_THIRDS_PI

◆ HALF_PI

◆ THIRD_PI

◆ SIXTH_PI

◆ ONE_DIV_TWO_PI

◆ INF