drake::math Namespace Reference

Classes
class	BarycentricMesh
	Represents a multi-linear function (from vector inputs to vector outputs) by interpolating between points on a mesh using (triangular) barycentric interpolation. More...
class	BsplineBasis
	Given a set of non-descending breakpoints t₀ ≤ t₁ ≤ ⋅⋅⋅ ≤ tₘ, a B-spline basis of order k is a set of n + 1 (where n = m - k) piecewise polynomials of degree k - 1 defined over those breakpoints. More...
struct	GetSubMatrixGradientArray
struct	GetSubMatrixGradientSingleElement
struct	Gradient
	Recursively defined template specifying a matrix type of the correct size for a gradient of a matrix function with respect to nq variables, of any order. More...
struct	Gradient< Derived, nq, 1 >
	Base case for recursively defined gradient template. More...
struct	GrayCodesMatrix
	GrayCodesMatrix::type returns an Eigen matrix of integers. More...
struct	GrayCodesMatrix< Eigen::Dynamic >
class	LinearSolver
	Solves a linear system of equations A*x=b. More...
struct	MatGradMult
struct	MatGradMultMat
class	NumericalGradientOption
class	RigidTransform
	This class represents a proper rigid transform between two frames which can be regarded in two ways. More...
class	RollPitchYaw
	This class represents the orientation between two arbitrary frames A and D associated with a Space-fixed (extrinsic) X-Y-Z rotation by "roll-pitch-yaw" angles [r, p, y], which is equivalent to a Body-fixed (intrinsic) Z-Y-X rotation by "yaw-pitch-roll" angles [y, p, r]. More...
class	RotationMatrix
	This class represents a 3x3 rotation matrix between two arbitrary frames A and B and helps ensure users create valid rotation matrices. More...

Typedefs
template<typename Derived , int nq>
using	AutoDiffMatrixType = MatrixLikewise< Eigen::AutoDiffScalar< Vector< typename Derived::Scalar, nq > >, Derived >
	The appropriate AutoDiffScalar matrix type given the value type and the number of derivatives at compile time. More...

Enumerations
enum	NumericalGradientMethod { kForward, kBackward, kCentral }
enum	KnotVectorType { kUniform, kClampedUniform }
	Enum representing types of knot vectors. More...

Functions
template<typename Derived >
MatrixLikewise< typename Derived::Scalar::Scalar, Derived >	ExtractValue (const Eigen::MatrixBase< Derived > &auto_diff_matrix)
	Extracts the value() portion from a matrix of AutoDiffScalar entries. More...
template<typename Derived >
decltype(auto)	DiscardGradient (const Eigen::MatrixBase< Derived > &matrix)
	B = DiscardGradient(A) enables casting from a matrix of AutoDiffScalars to AutoDiffScalar::Scalar type, explicitly throwing away any gradient information. More...
template<typename Derived , typename DerivedAutoDiff >
void	InitializeAutoDiff (const Eigen::MatrixBase< Derived > &value, std::optional< int > num_derivatives, std::optional< int > deriv_num_start, Eigen::MatrixBase< DerivedAutoDiff > *auto_diff_matrix)
	Initializes a single AutoDiff matrix given the corresponding value matrix. More...
template<typename Derived , typename DerivedAutoDiff >
void	InitializeAutoDiff (const Eigen::MatrixBase< Derived > &value, Eigen::MatrixBase< DerivedAutoDiff > *auto_diff_matrix)
	Alternate signature provides default values for the number of derivatives (dynamic, determined at run time) and the starting index (0). More...
template<int nq = Eigen::Dynamic, typename Derived >
AutoDiffMatrixType< Derived, nq >	InitializeAutoDiff (const Eigen::MatrixBase< Derived > &value, std::optional< int > num_derivatives={}, std::optional< int > deriv_num_start={})
	Initializes a single AutoDiff matrix given the corresponding value matrix. More...
template<typename... Deriveds>
auto	InitializeAutoDiffTuple (const Eigen::MatrixBase< Deriveds > &... args)
template<typename Derived >
Eigen::Matrix< typename Derived::Scalar::Scalar, Derived::SizeAtCompileTime, Eigen::Dynamic >	ExtractGradient (const Eigen::MatrixBase< Derived > &auto_diff_matrix, std::optional< int > num_derivatives={})
	Extracts the derivatives() portion from a matrix of AutoDiffScalar entries. More...
template<typename DerivedValue , typename DerivedGradient , typename DerivedAutoDiff >
void	InitializeAutoDiff (const Eigen::MatrixBase< DerivedValue > &value, const Eigen::MatrixBase< DerivedGradient > &gradient, Eigen::MatrixBase< DerivedAutoDiff > *auto_diff_matrix)
	Initializes an AutoDiff matrix given a matrix of values and a gradient matrix. More...
template<typename DerivedValue , typename DerivedGradient >
AutoDiffMatrixType< DerivedValue, DerivedGradient::ColsAtCompileTime >	InitializeAutoDiff (const Eigen::MatrixBase< DerivedValue > &value, const Eigen::MatrixBase< DerivedGradient > &gradient)
	Returns an AutoDiff matrix given a matrix of values and a gradient matrix. More...
template<typename Derived >
decltype(auto)	DiscardZeroGradient (const Eigen::MatrixBase< Derived > &auto_diff_matrix, double precision=Eigen::NumTraits< double >::dummy_precision())
	B = DiscardZeroGradient(A, precision) enables casting from a matrix of AutoDiffScalars to AutoDiffScalar::Scalar type, but first checking that the gradient matrix is empty or zero. More...
template<typename Derived >
std::enable_if<!std::is_same_v< typename Derived::Scalar, double >, int >::type	GetDerivativeSize (const Eigen::MatrixBase< Derived > &A)
	Given a matrix of AutoDiffScalars, returns the size of the derivatives. More...
bool	AreAutoDiffVecXdEqual (const Eigen::Ref< const VectorX< AutoDiffXd >> &a, const Eigen::Ref< const VectorX< AutoDiffXd >> &b)
	Determines if a and b are equal. More...
int	BinomialCoefficient (int n, int k)
	Computes the binomial coefficient n-choose-k efficiently using a dynamic programming recursion. More...
template<typename DerivedX , typename DerivedY , typename DerivedCalcX >
Eigen::Matrix< typename DerivedX::Scalar, DerivedY::RowsAtCompileTime, DerivedX::RowsAtCompileTime >	ComputeNumericalGradient (std::function< void(const DerivedCalcX &, DerivedY *y)> calc_func, const DerivedX &x, const NumericalGradientOption &option=NumericalGradientOption{ NumericalGradientMethod::kForward})
	Compute the gradient of a function f(x) through numerical difference. More...
Eigen::MatrixXd	ContinuousAlgebraicRiccatiEquation (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::MatrixXd > &B, const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::MatrixXd > &R)
	Computes the unique stabilizing solution S to the continuous-time algebraic Riccati equation: More...
Eigen::MatrixXd	ContinuousAlgebraicRiccatiEquation (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::MatrixXd > &B, const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::LLT< Eigen::MatrixXd > &R_cholesky)
	This is functionally the same as ContinuousAlgebraicRiccatiEquation(A, B, Q, R). More...
Eigen::MatrixXd	RealContinuousLyapunovEquation (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::MatrixXd > &Q)
template<typename v_Type , typename DtB_v_Type , typename w_AB_Type >
Vector3< typename v_Type::Scalar >	ConvertTimeDerivativeToOtherFrame (const Eigen::MatrixBase< v_Type > &v_E, const Eigen::MatrixBase< DtB_v_Type > &DtB_v_E, const Eigen::MatrixBase< w_AB_Type > &w_AB_E)
	Given ᴮd/dt(v) (the time derivative in frame B of an arbitrary 3D vector v) and given ᴬωᴮ (frame B's angular velocity in another frame A), this method computes ᴬd/dt(v) (the time derivative in frame A of v) by: ᴬd/dt(v) = ᴮd/dt(v) + ᴬωᴮ x v. More...
template<typename Derived >
drake::Matrix3< typename Derived::Scalar >	VectorToSkewSymmetric (const Eigen::MatrixBase< Derived > &p)
template<typename Derived >
Derived::Scalar	DifferentiableNorm (const Eigen::MatrixBase< Derived > &x)
	The 2-norm function |x| is not differentiable at x=0 (its gradient is x/|x|, which has a division-by-zero problem). More...
Eigen::MatrixXd	DiscreteAlgebraicRiccatiEquation (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::MatrixXd > &B, const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::MatrixXd > &R)
	Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation: More...
Eigen::MatrixXd	DiscreteAlgebraicRiccatiEquation (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::MatrixXd > &B, const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::MatrixXd > &R, const Eigen::Ref< const Eigen::MatrixXd > &N)
	Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation: More...
Eigen::MatrixXd	RealDiscreteLyapunovEquation (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::MatrixXd > &Q)
template<typename Scalar , int Options, typename StorageIndex >
std::vector< Eigen::Triplet< Scalar > >	SparseMatrixToTriplets (const Eigen::SparseMatrix< Scalar, Options, StorageIndex > &matrix)
	For a sparse matrix, return a vector of triplets, such that we can reconstruct the matrix using setFromTriplet function. More...
template<typename Derived >
void	SparseMatrixToRowColumnValueVectors (const Derived &matrix, std::vector< Eigen::Index > &row_indices, std::vector< Eigen::Index > &col_indices, std::vector< typename Derived::Scalar > &val)
	For a sparse matrix, return the row indices, the column indices, and value of the non-zero entries. More...
Eigen::Matrix3Xd	UniformPtsOnSphereFibonacci (int num_points)
	Deterministically generates approximate evenly distributed points on a unit sphere. More...
template<std::size_t Size>
std::array< int, Size >	intRange (int start)
template<typename Derived >
Derived::PlainObject	transposeGrad (const Eigen::MatrixBase< Derived > &dX, typename Derived::Index rows_X)
template<typename DerivedA , typename DerivedB , typename DerivedDA , typename DerivedDB >
MatGradMultMat< DerivedA, DerivedB, DerivedDA >::type	matGradMultMat (const Eigen::MatrixBase< DerivedA > &A, const Eigen::MatrixBase< DerivedB > &B, const Eigen::MatrixBase< DerivedDA > &dA, const Eigen::MatrixBase< DerivedDB > &dB)
template<typename DerivedDA , typename DerivedB >
MatGradMult< DerivedDA, DerivedB >::type	matGradMult (const Eigen::MatrixBase< DerivedDA > &dA, const Eigen::MatrixBase< DerivedB > &B)
template<typename Derived >
Eigen::Matrix< typename Derived::Scalar, Eigen::Dynamic, Eigen::Dynamic >	getSubMatrixGradient (const Eigen::MatrixBase< Derived > &dM, const std::vector< int > &rows, const std::vector< int > &cols, typename Derived::Index M_rows, int q_start=0, typename Derived::Index q_subvector_size=-1)
template<int QSubvectorSize, typename Derived , std::size_t NRows, std::size_t NCols>
GetSubMatrixGradientArray< QSubvectorSize, Derived, NRows, NCols >::type	getSubMatrixGradient (const Eigen::MatrixBase< Derived > &dM, const std::array< int, NRows > &rows, const std::array< int, NCols > &cols, typename Derived::Index M_rows, int q_start=0, typename Derived::Index q_subvector_size=QSubvectorSize)
template<int QSubvectorSize, typename Derived >
GetSubMatrixGradientSingleElement< QSubvectorSize, Derived >::type	getSubMatrixGradient (const Eigen::MatrixBase< Derived > &dM, int row, int col, typename Derived::Index M_rows, typename Derived::Index q_start=0, typename Derived::Index q_subvector_size=QSubvectorSize)
template<typename DerivedA , typename DerivedB >
void	setSubMatrixGradient (Eigen::MatrixBase< DerivedA > &dM, const Eigen::MatrixBase< DerivedB > &dM_submatrix, const std::vector< int > &rows, const std::vector< int > &cols, typename DerivedA::Index M_rows, typename DerivedA::Index q_start=0, typename DerivedA::Index q_subvector_size=-1)
template<int QSubvectorSize, typename DerivedA , typename DerivedB , std::size_t NRows, std::size_t NCols>
void	setSubMatrixGradient (Eigen::MatrixBase< DerivedA > &dM, const Eigen::MatrixBase< DerivedB > &dM_submatrix, const std::array< int, NRows > &rows, const std::array< int, NCols > &cols, typename DerivedA::Index M_rows, typename DerivedA::Index q_start=0, typename DerivedA::Index q_subvector_size=QSubvectorSize)
template<int QSubvectorSize, typename DerivedDM , typename DerivedDMSub >
void	setSubMatrixGradient (Eigen::MatrixBase< DerivedDM > &dM, const Eigen::MatrixBase< DerivedDMSub > &dM_submatrix, int row, int col, typename DerivedDM::Index M_rows, typename DerivedDM::Index q_start=0, typename DerivedDM::Index q_subvector_size=QSubvectorSize)
template<int NumDigits = Eigen::Dynamic>
GrayCodesMatrix< NumDigits >::type	CalculateReflectedGrayCodes (int num_digits=NumDigits)
	Returns a matrix whose i'th row is the Gray code for integer i. More...
int	GrayCodeToInteger (const Eigen::Ref< const Eigen::VectorXi > &gray_code)
	Converts the Gray code to an integer. More...
template<typename T >
const Eigen::Quaternion< T >	HopfCoordinateToQuaternion (const T &theta, const T &phi, const T &psi)
	Transforms Hopf coordinates to a quaternion w, x, y, z as w = cos(θ/2)cos(ψ/2) x = cos(θ/2)sin(ψ/2) y = sin(θ/2)cos(φ+ψ/2) z = sin(θ/2)sin(φ+ψ/2) The user can refer to equation 5 of Generating Uniform Incremental Grids on SO(3) Using the Hopf Fibration by Anna Yershova, Steven LaValle and Julie Mitchell, 2008. More...
template<typename T >
Vector3< T >	QuaternionToHopfCoordinate (const Eigen::Quaternion< T > &quaternion)
	Convert a unit-length quaternion (w, x, y, z) (with the requirement w >= 0) to Hopf coordinate as ψ = 2*atan2(x, w) φ = mod(atan2(z, y) - ψ/2, 2pi) θ = 2*atan2(√(y²+z²), √(w²+x²)) ψ is in the range of [-pi, pi]. More...
template<int MaxChunkSize = 10, class F , class Arg >
decltype(auto)	jacobian (F &&f, Arg &&x)
	Computes a matrix of AutoDiffScalars from which both the value and the Jacobian of a function \[ f:\mathbb{R}^{n\times m}\rightarrow\mathbb{R}^{p\times q} \] (f: R^n*m -> R^p*q) can be extracted. More...
template<int MaxChunkSizeOuter = 10, int MaxChunkSizeInner = 10, class F , class Arg >
decltype(auto)	hessian (F &&f, Arg &&x)
	Computes a matrix of AutoDiffScalars from which the value, Jacobian, and Hessian of a function \[ f:\mathbb{R}^{n\times m}\rightarrow\mathbb{R}^{p\times q} \] (f: R^n*m -> R^p*q) can be extracted. More...
template<typename Derived >
bool	IsSymmetric (const Eigen::MatrixBase< Derived > &matrix)
	Determines if a matrix is symmetric. More...
template<typename Derived >
bool	IsSymmetric (const Eigen::MatrixBase< Derived > &matrix, const typename Derived::Scalar &precision)
	Determines if a matrix is symmetric based on whether the difference between matrix(i, j) and matrix(j, i) is smaller than precision for all i, j. More...
template<typename Derived >
drake::MatrixX< typename Derived::Scalar >	ToSymmetricMatrixFromLowerTriangularColumns (const Eigen::MatrixBase< Derived > &lower_triangular_columns)
	Given a column vector containing the stacked columns of the lower triangular part of a square matrix, returning a symmetric matrix whose lower triangular part is the same as the original matrix. More...
template<int rows, typename Derived >
Eigen::Matrix< typename Derived::Scalar, rows, rows >	ToSymmetricMatrixFromLowerTriangularColumns (const Eigen::MatrixBase< Derived > &lower_triangular_columns)
	Given a column vector containing the stacked columns of the lower triangular part of a square matrix, returning a symmetric matrix whose lower triangular part is the same as the original matrix. More...
template<typename Derived >
drake::VectorX< typename Derived::Scalar >	ToLowerTriangularColumnsFromMatrix (const Eigen::MatrixBase< Derived > &matrix)
	Given a square matrix, extract the lower triangular part as a stacked column vector. More...
template<typename Derived >
bool	IsPositiveDefinite (const Eigen::MatrixBase< Derived > &matrix, double eigenvalue_tolerance=0.0, double symmetry_tolerance=0.0)
	Checks if a matrix is symmetric (with tolerance symmetry_tolerance – see IsSymmetric) and has all eigenvalues greater than eigenvalue_tolerance. More...
template<typename T >
std::vector< MatrixX< T > >	EigenToStdVector (const Eigen::Ref< const MatrixX< T >> &mat)
	Converts a MatrixX<T> into a std::vector<MatrixX<T>>, taking each column of the m-by-n matrix mat into an m-by-1 element of the returned std::vector. More...
template<typename T >
MatrixX< T >	StdVectorToEigen (const std::vector< MatrixX< T >> &vec)
	Converts a std::vector<MatrixX<T>> into a MatrixX<T>, composing each element of vec into a column of the returned matrix. More...
template<typename Derived >
MatrixX< typename Derived::Scalar >	ExtractPrincipalSubmatrix (const Eigen::MatrixBase< Derived > &mat, const std::set< int > &indices)
	Extracts the principal submatrix from the ordered set of indices. More...
template<typename Derived >
void	NormalizeVector (const Eigen::MatrixBase< Derived > &x, typename Derived::PlainObject &x_norm, typename drake::math::Gradient< Derived, Derived::RowsAtCompileTime, 1 >::type *dx_norm=nullptr, typename drake::math::Gradient< Derived, Derived::RowsAtCompileTime, 2 >::type *ddx_norm=nullptr)
	Computes the normalized vector, optionally with its gradient and second derivative. More...
Eigen::MatrixXd	DecomposePSDmatrixIntoXtransposeTimesX (const Eigen::Ref< const Eigen::MatrixXd > &Y, double zero_tol, bool return_empty_if_not_psd=false)
	For a symmetric positive semidefinite matrix Y, decompose it into XᵀX, where the number of rows in X equals to the rank of Y. More...
std::pair< Eigen::MatrixXd, Eigen::MatrixXd >	DecomposePositiveQuadraticForm (const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::VectorXd > &b, double c, double tol=0)
	Rewrite a quadratic form xᵀQx + bᵀx + c to (Rx+d)ᵀ(Rx+d) where. More...
Eigen::MatrixXd	BalanceQuadraticForms (const Eigen::Ref< const Eigen::MatrixXd > &S, const Eigen::Ref< const Eigen::MatrixXd > &P)
	Given two quadratic forms, x'Sx > 0 and x'Px, (with P symmetric and full rank), finds a change of variables x = Ty, which simultaneously diagonalizes both forms (as inspired by "balanced truncation" in model-order reduction [1]). More...
template<typename Scalar >
Eigen::Quaternion< Scalar >	ClosestQuaternion (const Eigen::Quaternion< Scalar > &quat1, const Eigen::Quaternion< Scalar > &quat2)
	Returns a unit quaternion that represents the same orientation as quat2, and has the "shortest" geodesic distance on the unit sphere to quat1. More...
template<typename Derived >
Vector4< typename Derived::Scalar >	quatConjugate (const Eigen::MatrixBase< Derived > &q)
template<typename Derived1 , typename Derived2 >
Vector4< typename Derived1::Scalar >	quatProduct (const Eigen::MatrixBase< Derived1 > &q1, const Eigen::MatrixBase< Derived2 > &q2)
template<typename DerivedQ , typename DerivedV >
Vector3< typename DerivedV::Scalar >	quatRotateVec (const Eigen::MatrixBase< DerivedQ > &q, const Eigen::MatrixBase< DerivedV > &v)
template<typename Derived1 , typename Derived2 >
Vector4< typename Derived1::Scalar >	quatDiff (const Eigen::MatrixBase< Derived1 > &q1, const Eigen::MatrixBase< Derived2 > &q2)
template<typename Derived1 , typename Derived2 , typename DerivedU >
Derived1::Scalar	quatDiffAxisInvar (const Eigen::MatrixBase< Derived1 > &q1, const Eigen::MatrixBase< Derived2 > &q2, const Eigen::MatrixBase< DerivedU > &u)
template<typename T >
boolean< T >	is_quaternion_in_canonical_form (const Eigen::Quaternion< T > &quat)
	This function tests whether a quaternion is in "canonical form" meaning that it tests whether the quaternion [w, x, y, z] has a non-negative w value. More...
template<typename T >
Eigen::Quaternion< T >	QuaternionToCanonicalForm (const Eigen::Quaternion< T > &quat)
	This function returns a quaternion in its "canonical form" meaning that it returns a quaternion [w, x, y, z] with a non-negative w. More...
template<typename T >
boolean< T >	AreQuaternionsEqualForOrientation (const Eigen::Quaternion< T > &quat1, const Eigen::Quaternion< T > &quat2, const T tolerance)
	This function tests whether two quaternions represent the same orientation. More...
template<typename T >
Vector4< T >	CalculateQuaternionDtFromAngularVelocityExpressedInB (const Eigen::Quaternion< T > &quat_AB, const Vector3< T > &w_AB_B)
	This function calculates a quaternion's time-derivative from its quaternion and angular velocity. More...
template<typename T >
Vector3< T >	CalculateAngularVelocityExpressedInBFromQuaternionDt (const Eigen::Quaternion< T > &quat_AB, const Vector4< T > &quatDt)
	This function calculates angular velocity from a quaternion and its time- derivative. More...
template<typename T >
T	CalculateQuaternionDtConstraintViolation (const Eigen::Quaternion< T > &quat, const Vector4< T > &quatDt)
	This function calculates how well a quaternion and its time-derivative satisfy the quaternion time-derivative constraint specified in [Kane, 1983] Section 1.13, equations 12-13, page 59. More...
template<typename T >
boolean< T >	IsQuaternionValid (const Eigen::Quaternion< T > &quat, const double tolerance)
	This function tests if a quaternion satisfies the quaternion constraint specified in [Kane, 1983] Section 1.3, equation 4, page 12, i.e., a quaternion [w, x, y, z] must satisfy: w^2 + x^2 + y^2 + z^2 = 1. More...
template<typename T >
boolean< T >	IsBothQuaternionAndQuaternionDtOK (const Eigen::Quaternion< T > &quat, const Vector4< T > &quatDt, const double tolerance)
	This function tests if a quaternion satisfies the time-derivative constraint specified in [Kane, 1983] Section 1.13, equation 13, page 59. More...
template<typename T >
bool	IsQuaternionAndQuaternionDtEqualAngularVelocityExpressedInB (const Eigen::Quaternion< T > &quat, const Vector4< T > &quatDt, const Vector3< T > &w_B, const double tolerance)
	This function tests if a quaternion and a quaternion's time derivative can calculate and match an angular velocity to within a tolerance. More...
template<typename T = double, class Generator = RandomGenerator>
Eigen::Quaternion< T >	UniformlyRandomQuaternion (Generator *generator)
	Generates a rotation (in the quaternion representation) that rotates a point on the unit sphere to another point on the unit sphere with a uniform distribution over the sphere. More...
template<typename T = double, class Generator = RandomGenerator>
Eigen::AngleAxis< T >	UniformlyRandomAngleAxis (Generator *generator)
	Generates a rotation (in the axis-angle representation) that rotates a point on the unit sphere to another point on the unit sphere with a uniform distribution over the sphere. More...
template<typename T = double, class Generator = RandomGenerator>
RotationMatrix< T >	UniformlyRandomRotationMatrix (Generator *generator)
	Generates a rotation (in the rotation matrix representation) that rotates a point on the unit sphere to another point on the unit sphere with a uniform distribution over the sphere. More...
template<typename T = double, class Generator = RandomGenerator>
Vector3< T >	UniformlyRandomRPY (Generator *generator)
	Generates a rotation (in the roll-pitch-yaw representation) that rotates a point on the unit sphere to another point on the unit sphere with a uniform distribution over the sphere. More...
template<typename Derived >
drake::math::Gradient< Eigen::Matrix< typename Derived::Scalar, 3, 3 >, 4 >::type	dquat2rotmat (const Eigen::MatrixBase< Derived > &quaternion)
	Computes the gradient of the function that converts a unit length quaternion to a rotation matrix. More...
template<typename DerivedR , typename DerivedDR >
drake::math::Gradient< Eigen::Matrix< typename DerivedR::Scalar, 3, 1 >, DerivedDR::ColsAtCompileTime >::type	drotmat2rpy (const Eigen::MatrixBase< DerivedR > &R, const Eigen::MatrixBase< DerivedDR > &dR)
	Computes the gradient of the function that converts a rotation matrix to body-fixed z-y'-x'' Euler angles. More...
template<typename DerivedR , typename DerivedDR >
drake::math::Gradient< Eigen::Matrix< typename DerivedR::Scalar, 4, 1 >, DerivedDR::ColsAtCompileTime >::type	drotmat2quat (const Eigen::MatrixBase< DerivedR > &R, const Eigen::MatrixBase< DerivedDR > &dR)
	Computes the gradient of the function that converts rotation matrix to quaternion. More...
double	ProjectMatToRotMatWithAxis (const Eigen::Matrix3d &M, const Eigen::Vector3d &axis, double angle_lb, double angle_ub)
	Projects an approximate 3 x 3 rotation matrix M onto an orthonormal matrix R so that R is a rotation matrix associated with a angle-axis rotation by an angle θ about a vector direction axis, with angle_lb <= θ <= angle_ub. More...
template<typename T >
T	SoftOverMax (const std::vector< T > &x, double alpha=1.0)
	Computes a smooth over approximation of max function, namely SoftOverMax(x) >= max(x). More...
template<typename T >
T	SoftUnderMax (const std::vector< T > &x, double alpha=1.0)
	Computes a smooth under approximation of max function, namely SoftUnderMax(x) <= max(x). More...
template<typename T >
T	SoftOverMin (const std::vector< T > &x, double alpha=1.0)
	Computes a smooth over approximation of min function, namely SoftOverMin(x) >= min(x). More...
template<typename T >
T	SoftUnderMin (const std::vector< T > &x, double alpha=1.0)
	Computes a smooth under approximation of min function, namely SoftUnderMin(x) <= min(x). More...
template<class T1 , class T2 >
T1	wrap_to (const T1 &value, const T2 &low, const T2 &high)
	For variables that are meant to be periodic, (e.g. More...
solve linear system of equations with a given solver.
Solve linear system of equations A * x = b. Where A is an Eigen matrix of double/AutoDiffScalar/symbolic::Expression, and b is an Eigen matrix of double/AutoDiffScalar/symbolic::Expression. Notice that if either A or b contains symbolic::Expression, then the other has to contain symbolic::Expression. This 3-argument version allows the user to re-use linear_solver when b changes or the gradient of A changes. When either A or b contains AutoDiffScalar, we use implicit function theorem to find the gradient in x as ∂x/∂zᵢ = A⁻¹(∂b/∂zᵢ - ∂A/∂zᵢ * x) where z is the variable we take gradient with. Note When both A and b are Eigen matrix of double, this function is almost as fast as calling linear_solver.solve(b) directly. When either A or b contains AutoDiffScalar, this function is a lot faster than first instantiating the linear solver of AutoDiffScalar, and then solving the equation with this autodiffable linear solver. Template Parameters LinearSolver The type of linear solver, for example Eigen::LLT<Eigen::Matrix2d> DerivedA An Eigen Matrix. DerivedB An Eigen Vector. Parameters linear_solver The linear solver constructed with the double-version of A. A The matrix A. b The vector b. Here is an example code. Eigen::Matrix<AutoDiffd<3>, 2, 2> A_ad; // Set the value and gradient in A_ad with arbitrary values; Eigen::Matrix2d A_val; A_val << 1, 2, 3, 4; // Gradient of A.col(0). Eigen::Matrix<double, 2, 3> A0_gradient; A0_gradient << 1, 2, 3, 4, 5, 6; A_ad.col(0) = InitializeAutoDiff(A_val.col(0), A0_gradient); // Gradient of A.col(1) Eigen::Matrix<double, 2, 3> A1_gradient; A1_gradient << 7, 8, 9, 10, 11, 12; A_ad.col(1) = InitializeAutoDiff(A_val.col(1), A1_gradient); // Set the value and gradient of b to arbitrary value. const Eigen::Vector2d b_val(2, 3); Eigen::Matrix<double, 2, 3> b_gradient; b_gradient << 1, 3, 5, 7, 9, 11; const auto b_ad = InitializeAutoDiff(b_val, b_gradient); // Solve the linear system A_val * x_val = b_val. Eigen::PartialPivLU<Eigen::Matrix2d> linear_solver(A_val); const auto x_val = SolveLinearSystem(linear_solver, A_val, b_val); // Solve the linear system A*x=b, together with the gradient. // x_ad contains both the value of the solution A*x=b, together with its // gradient. const auto x_ad = SolveLinearSystem(linear_solver, A_ad, b_ad);
template<typename LinearSolver , typename DerivedA , typename DerivedB >
std::enable_if< internal::is_double_or_symbolic_v< typename DerivedA::Scalar > &&internal::is_double_or_symbolic_v< typename DerivedB::Scalar > &&std::is_same_v< typename DerivedA::Scalar, typename DerivedB::Scalar >, Eigen::Matrix< typename DerivedA::Scalar, DerivedA::RowsAtCompileTime, DerivedB::ColsAtCompileTime > >::type	SolveLinearSystem (const LinearSolver &linear_solver, const Eigen::MatrixBase< DerivedA > &A, const Eigen::MatrixBase< DerivedB > &b)
	Specialized when A and b are both double or symbolic::Expression matrices. More...
template<typename LinearSolver , typename DerivedB >
std::enable_if< internal::is_double_or_symbolic_v< typename LinearSolver::MatrixType::Scalar > &&internal::is_double_or_symbolic_v< typename DerivedB::Scalar > &&std::is_same_v< typename LinearSolver::MatrixType::Scalar, typename DerivedB::Scalar >, Eigen::Matrix< typename LinearSolver::MatrixType::Scalar, DerivedB::RowsAtCompileTime, DerivedB::ColsAtCompileTime > >::type	SolveLinearSystem (const LinearSolver &linear_solver, const Eigen::MatrixBase< DerivedB > &b)
	Specialized when the matrix in linear_solver and b are both double or symbolic::Expression matrices. More...
template<typename LinearSolver , typename DerivedB >
std::enable_if< std::is_same_v< typename LinearSolver::MatrixType::Scalar, double > &&internal::is_autodiff_v< typename DerivedB::Scalar >, Eigen::Matrix< typename DerivedB::Scalar, DerivedB::RowsAtCompileTime, DerivedB::ColsAtCompileTime > >::type	SolveLinearSystem (const LinearSolver &linear_solver, const Eigen::MatrixBase< DerivedB > &b)
	Specialized the matrix in linear_solver is a double-valued matrix and b is an AutoDiffScalar-valued matrix. More...
template<typename LinearSolver , typename DerivedA , typename DerivedB >
std::enable_if< std::is_same_v< typename DerivedA::Scalar, double > &&internal::is_autodiff_v< typename DerivedB::Scalar >, Eigen::Matrix< typename DerivedB::Scalar, DerivedA::RowsAtCompileTime, DerivedB::ColsAtCompileTime > >::type	SolveLinearSystem (const LinearSolver &linear_solver, const Eigen::MatrixBase< DerivedA > &A, const Eigen::MatrixBase< DerivedB > &b)
	Specialized when A is a double-valued matrix and b is an AutoDiffScalar-valued matrix. More...
template<typename LinearSolver , typename DerivedA , typename DerivedB >
std::enable_if< internal::is_autodiff_v< typename DerivedA::Scalar >, Eigen::Matrix< typename DerivedA::Scalar, DerivedA::RowsAtCompileTime, DerivedB::ColsAtCompileTime > >::type	SolveLinearSystem (const LinearSolver &linear_solver, const Eigen::MatrixBase< DerivedA > &A, const Eigen::MatrixBase< DerivedB > &b)
	Specialized when A is an AutoDiffScalar-valued matrix, and b can contain either AutoDiffScalar or double. More...
Get linear solver
Create the linear solver for a given matrix A, which will be used to solve the linear system of equations A * x = b. The following table indicate the scalar type of the matrix in the returned linear solver, depending on the scalar type in matrix A A double ADS Expr solver double double Expr where ADS stands for Eigen::AutoDiffScalar, and Expr stands for symbolic::Expression. Here is the example code Eigen::Matrix2d A_val; A_val << 1, 2, 2, 5; Eigen::Vector2d b_val(3, 4); const Eigen::Vector2d x_val = SolveLinearSystem(GetLinearSolver<Eigen::LLT>(A_val), A_val, b_val); Eigen::Matrix<AutoDiffXd, 2, 2> A_ad; A_ad(0, 0).value() = A_val(0, 0); A_ad(0, 0).derivatives() = Eigen::Vector3d(1, 2, 3); A_ad(0, 1).value() = A_val(0, 1); A_ad(0, 1).derivatives() = Eigen::Vector3d(2, 3, 4); A_ad(1, 0).value() = A_val(1, 0); A_ad(1, 0).derivatives() = Eigen::Vector3d(3, 4, 5); A_ad(1, 1).value() = A_val(1, 1); A_ad(1, 1).derivatives() = Eigen::Vector3d(4, 5, 6); // Solve A * x = b with A containing gradient. const Eigen::Matrix<AutoDiffXd, 2, 1> x_ad1 = SolveLinearSystem(GetLinearSolver<Eigen::LLT>(A_ad), A_ad, b_val); Eigen::Matrix<AutoDiffXd, 2, 1> b_ad; b_ad(0).value() = b_val(0); b_ad(0).derivatives() = Eigen::Vector3d(5, 6, 7); b_ad(1).value() = b_val(1); b_ad(1).derivatives() = Eigen::Vector3d(6, 7, 8); // Solve A * x = b with b containing gradient. const Eigen::Matrix<AutoDiffXd, 2, 1> x_ad2 = SolveLinearSystem(GetLinearSolver<Eigen::LLT>(A_val), A_val, b_ad); // Solve A * x = b with both A and b containing gradient. const Eigen::Matrix<AutoDiffXd, 2, 1> x_ad3 = SolveLinearSystem(GetLinearSolver<Eigen::LLT>(A_ad), A_ad, b_ad); {cc}
template<template< typename, int... > typename LinearSolverType, typename DerivedA >
internal::EigenLinearSolver< LinearSolverType, DerivedA >	GetLinearSolver (const Eigen::MatrixBase< DerivedA > &A)
	Get the linear solver for a matrix A. More...
solve linear system of equations
Solve linear system of equations A * x = b. Where A is an Eigen matrix of double/AutoDiffScalar/symbolic::Expression, and b is an Eigen matrix of double/AutoDiffScalar/symbolic::Expression. Note that when either A or b contains symbolic::Expression, the other has to contain symbolic::Expression as well. When either A or b contains AutoDiffScalar, we use implicit function theorem to find the gradient in x as ∂x/∂zᵢ = A⁻¹(∂b/∂zᵢ - ∂A/∂zᵢ * x) where z is the variable we take gradient with. The following table indicate the scalar type of x with A/b containing the specified scalar type. The entries with NA are not supported. b ＼ A double ADS Expr double double ADS NA ADS ADS ADS NA Expr NA NA Expr where ADS stands for Eigen::AutoDiffScalar, and Expr stands for symbolic::Expression. TODO(hongkai.dai): support one of A/b being a double matrix and the other being a symbolic::Expression matrix. Note When both A and b are Eigen matrix of double, this function is almost as fast as calling linear_solver.solve(b) directly; when either A or b contains AutoDiffScalar, this function is a lot faster than first instantiating the linear solver of AutoDiffScalar, and then solving the equation with this autodiffable linear solver. Template Parameters LinearSolverType The type of linear solver, for example Eigen::LLT. Notice that this is just specifies the solver type (such as Eigen::LLT), not the matrix type (like Eigen::LLT<Eigen::Matrix2d>). All Eigen solvers we care about are templated on the matrix type. Some are further templated on configuration ints. The int... will account for zero or more of these ints, providing a common interface for both types of solvers. DerivedA An Eigen Matrix. DerivedB An Eigen Vector. Parameters A The matrix A. b The vector b. Here is an example code. Eigen::Matrix<AutoDiffd<3>, 2, 2> A_ad; // Set the value and gradient in A_ad with arbitrary values; Eigen::Matrix2d A_val; A_val << 1, 2, 3, 4; // Gradient of A.col(0). Eigen::Matrix<double, 2, 3> A0_gradient; A0_gradient << 1, 2, 3, 4, 5, 6; A_ad.col(0) = InitializeAutoDiff(A_val.col(0), A0_gradient); // Gradient of A.col(1) Eigen::Matrix<double, 2, 3> A1_gradient; A1_gradient << 7, 8, 9, 10, 11, 12; A_ad.col(1) = InitializeAutoDiff(A_val.col(1), A1_gradient); // Set the value and gradient of b to arbitrary value. const Eigen::Vector2d b_val(2, 3); Eigen::Matrix<double, 2, 3> b_gradient; b_gradient << 1, 3, 5, 7, 9, 11; const auto b_ad = InitializeAutoDiff(b_val, b_gradient); // Solve the linear system A*x=b without the gradient. const auto x_val = SolveLinearSystem<Eigen::PartialPivLU>(A_val, b_val); // Solve the linear system A*x=b, together with the gradient. // x_ad contains both the value of the solution A*x=b, together with its // gradient. const auto x_ad = SolveLinearSystem<Eigen::PartialPivLU>(A_ad, b_ad);
template<template< typename, int... > typename LinearSolverType, typename DerivedA , typename DerivedB >
internal::Solution< DerivedA, DerivedB >	SolveLinearSystem (const Eigen::MatrixBase< DerivedA > &A, const Eigen::MatrixBase< DerivedB > &b)
	Solves system A*x=b. More...

Typedef Documentation

AutoDiffMatrixType

using AutoDiffMatrixType = MatrixLikewise<Eigen::AutoDiffScalar<Vector<typename Derived::Scalar, nq> >, Derived>

The appropriate AutoDiffScalar matrix type given the value type and the number of derivatives at compile time.

Enumeration Type Documentation

KnotVectorType

enum KnotVectorType

strong

Enum representing types of knot vectors.

"Uniform" refers to the spacing between the knots. "Clamped" indicates that the first and last knots have multiplicity equal to the order of the spline.

Reference: http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node17.html

Enumerator
kUniform
kClampedUniform

NumericalGradientMethod

enum NumericalGradientMethod

strong

Enumerator
kForward	Compute the gradient as (f(x + Δx) - f(x)) / Δx, with Δx > 0.
kBackward	Compute the gradient as (f(x) - f(x - Δx)) / Δx, with Δx > 0.
kCentral	Compute the gradient as (f(x + Δx) - f(x - Δx)) / (2Δx), with Δx > 0.

Function Documentation

AreAutoDiffVecXdEqual()

bool drake::math::AreAutoDiffVecXdEqual	(	const Eigen::Ref< const VectorX< AutoDiffXd >> &	a,
		const Eigen::Ref< const VectorX< AutoDiffXd >> &	b
	)

Determines if a and b are equal.

a equals to b if they have the same value and gradients. TODO(hongkai.dai) implement and use std::equal_to<> for comparing Eigen vector of AutoDiffXd.

AreQuaternionsEqualForOrientation()

boolean<T> drake::math::AreQuaternionsEqualForOrientation	(	const Eigen::Quaternion< T > &	quat1,
		const Eigen::Quaternion< T > &	quat2,
		const T	tolerance
	)

This function tests whether two quaternions represent the same orientation.

This function converts each quaternion to its canonical form and tests whether the absolute value of the difference in corresponding elements of these canonical quaternions is within tolerance.

Parameters

quat1	Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat is analogous to the rotation matrix R_AB.
quat2	Quaternion with a description analogous to quat1.
tolerance	Nonnegative real scalar defining the allowable difference in the orientation described by quat1 and quat2.

Returns

true if quat1 and quat2 represent the same orientation (to within tolerance), otherwise false.

BalanceQuadraticForms()

Eigen::MatrixXd drake::math::BalanceQuadraticForms	(	const Eigen::Ref< const Eigen::MatrixXd > &	S,
		const Eigen::Ref< const Eigen::MatrixXd > &	P
	)

Given two quadratic forms, x'Sx > 0 and x'Px, (with P symmetric and full rank), finds a change of variables x = Ty, which simultaneously diagonalizes both forms (as inspired by "balanced truncation" in model-order reduction [1]).

In this note, we use abs(M) to indicate the elementwise absolute value.

Adapting from [1], we observe that there is a family of coordinate systems that can simultaneously diagonalize T'ST and T'PT. Using D to denote a diagonal matrix, we call the result S-normal if T'ST = I and abs(T'PT) = D⁻², call it P-normal if T'ST = D² and abs(T'PT) = I, and call it "balanced" if T'ST = D and abs(T'PT) = D⁻¹. Note that if P > 0, then T'PT = D⁻¹.

We find x=Ty such that T'ST = D and abs(T'PT) = D⁻¹, where D is diagonal. The recipe is:

• Factorize S = LL', and choose R=L⁻¹.
• Take svd(RPR') = UΣV', and note that U=V for positive definite matrices, and V is U up to a sign flip of the singular vectors for all symmetric matrices.
• Choose T = R'U Σ^{-1/4}, where the matrix exponent can be taken elementwise because Σ is diagonal. This gives T'ST = Σ^{-1/2} (by using U'U=I), and abs(T'PT) = Σ^{1/2}. If P > 0, then T'PT = Σ^{1/2}.

Note that the numerical "balancing" can address the absolute scaling of the quadratic forms, but not the relative scaling. To understand this, consider the scalar case: we have two quadratic functions, sx² and px², with s>0, p>0. We'd like to choose x=Ty so that sT²y² and pT²y² are "balanced" (we'd like them both to be close to y²). We'll choose T=p^{-1/4}s^{-1/4}, which gives sx² = sqrt(s/p)y², and px² = sqrt(p/s)y². For instance if s=1e8 and p=1e8, then t=1e-4 and st^2 = pt^2 = 1. But if s=10, p=1e7, then t=0.01, and st^2 = 1e-3, pt^2 = 1e3.

In the matrix case, the absolute scaling is important – it ensures that the two quadratic forms have the same matrix condition number and makes them as close as possible to 1. Besides absolute scaling, in the matrix case the balancing transform diagonalizes both quadratic forms.

[1] B. Moore, “Principal component analysis in linear systems: Controllability, observability, and model reduction,” IEEE Trans. Automat. Contr., vol. 26, no. 1, pp. 17–32, Feb. 1981.

BinomialCoefficient()

int drake::math::BinomialCoefficient	(	int	n,
		int	k
	)

Computes the binomial coefficient n-choose-k efficiently using a dynamic programming recursion.

https://en.wikipedia.org/wiki/Binomial_coefficient

Precondition

k >= 0

CalculateAngularVelocityExpressedInBFromQuaternionDt()

Vector3<T> drake::math::CalculateAngularVelocityExpressedInBFromQuaternionDt	(	const Eigen::Quaternion< T > &	quat_AB,
		const Vector4< T > &	quatDt
	)

This function calculates angular velocity from a quaternion and its time- derivative.

Algorithm from [Kane, 1983] Section 1.13, Pages 58-59.

• [Kane, 1983] "Spacecraft Dynamics," McGraw-Hill Book Co., New York, 1983. (with P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637

Parameters

quat_AB	Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat_AB is analogous to the rotation matrix R_AB.
quatDt	Time-derivative of quat_AB, i.e. [ẇ, ẋ, ẏ, ż].

Return values

w_AB_B

B's angular velocity in A, expressed in B.

CalculateQuaternionDtConstraintViolation()

T drake::math::CalculateQuaternionDtConstraintViolation	(	const Eigen::Quaternion< T > &	quat,
		const Vector4< T > &	quatDt
	)

This function calculates how well a quaternion and its time-derivative satisfy the quaternion time-derivative constraint specified in [Kane, 1983] Section 1.13, equations 12-13, page 59.

For a quaternion [w, x, y, z], the quaternion must satisfy: w^2 + x^2 + y^2 + z^2 = 1, hence its time-derivative must satisfy: 2*(w*ẇ + x*ẋ + y*ẏ + z*ż) = 0.

• [Kane, 1983] "Spacecraft Dynamics," McGraw-Hill Book Co., New York, 1983. (with P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637

Parameters

quat	Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB.
quatDt	Time-derivative of quat, i.e., [ẇ, ẋ, ẏ, ż].

Return values

quaternionDt_constraint_violation

The amount the time- derivative of the quaternion constraint has been violated, which may be positive or negative (0 means the constraint is perfectly satisfied).

CalculateQuaternionDtFromAngularVelocityExpressedInB()

Vector4<T> drake::math::CalculateQuaternionDtFromAngularVelocityExpressedInB	(	const Eigen::Quaternion< T > &	quat_AB,
		const Vector3< T > &	w_AB_B
	)

This function calculates a quaternion's time-derivative from its quaternion and angular velocity.

Algorithm from [Kane, 1983] Section 1.13, Pages 58-59.

• [Kane, 1983] "Spacecraft Dynamics," McGraw-Hill Book Co., New York, 1983. (With P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637

Parameters

quat_AB	Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat_AB is analogous to the rotation matrix R_AB.
w_AB_B	B's angular velocity in A, expressed in B.

Return values

quatDt

Time-derivative of quat_AB, i.e., [ẇ, ẋ, ẏ, ż].

CalculateReflectedGrayCodes()

GrayCodesMatrix<NumDigits>::type drake::math::CalculateReflectedGrayCodes

(

int

num_digits = NumDigits

)

Returns a matrix whose i'th row is the Gray code for integer i.

Template Parameters

NumDigits

The number of digits in the Gray code.

Parameters

num_digits

The number of digits in the Gray code.

Returns

M. M is a matrix of size 2ᵏ x k, where k is num_digits. M.row(i) is the Gray code for integer i.

ClosestQuaternion()

Eigen::Quaternion<Scalar> drake::math::ClosestQuaternion	(	const Eigen::Quaternion< Scalar > &	quat1,
		const Eigen::Quaternion< Scalar > &	quat2
	)

Returns a unit quaternion that represents the same orientation as quat2, and has the "shortest" geodesic distance on the unit sphere to quat1.

ComputeNumericalGradient()

Eigen::Matrix<typename DerivedX::Scalar, DerivedY::RowsAtCompileTime, DerivedX::RowsAtCompileTime> drake::math::ComputeNumericalGradient	(	std::function< void(const DerivedCalcX &, DerivedY *y)>	calc_func,
		const DerivedX &	x,
		const NumericalGradientOption &	option = NumericalGradientOption{ NumericalGradientMethod::kForward}
	)

Compute the gradient of a function f(x) through numerical difference.

Parameters

calc_func	calc_func(x, &y) computes the value of f(x), and stores the value in y. calc_func is responsible for properly resizing the output y when it consists of an Eigen vector of Eigen::Dynamic size.
x	The point at which the numerical gradient is computed.
option	The options for computing numerical gradient.

Template Parameters

DerivedX	an Eigen column vector.
DerivedY	an Eigen column vector.
DerivedCalcX	The type of x in the calc_func. Must be an Eigen column vector. It is possible to have DerivedCalcX being different from DerivedX, for example, calc_func could be solvers::EvaluatorBase(const Eigen::Ref<const Eigen::VectorXd>&, Eigen::VectorXd*), but x could be of type Eigen::VectorXd. TODO(hongkai.dai): understand why the default template DerivedCalcX = DerivedX doesn't compile when I instantiate ComputeNumericalGradient<DerivedX, DerivedY>(calc_func, x);

Return values

gradient

a matrix of size x.rows() x y.rows(). gradient(i, j) is ∂f(i) / ∂x(j)

Examples:

// Create a std::function from a lambda expression.
std::function<void (const Eigen::Vector2d&, Vector3d*)> foo = [](const
Eigen::Vector2d& x, Vector3d*y) { (*y)(0) = x(0); (*y)(1) = x(0) * x(1);
(*y)(2) = x(0) * std::sin(x(1));};
Eigen::Vector3d x_eval(1, 2, 3);
auto J = ComputeNumericalGradient(foo, x_eval);
// Note that if we pass in a lambda to ComputeNumericalGradient, then
// ComputeNumericalGradient has to instantiate the template types explicitly,
// as in this example. The issue of template deduction with std::function is
// explained in
//
https://stackoverflow.com/questions/48529410/template-arguments-deduction-failed-passing-func-pointer-to-stdfunction
auto bar = [](const Eigen::Vector2d& x, Eigen::Vector2d* y) {*y = x; };
auto J2 = ComputeNumericalGradient<Eigen::Vector2d,
Eigen::Vector2d, Eigen::Vector2d>(bar, Eigen::Vector2d(2, 3));

ContinuousAlgebraicRiccatiEquation() [1/2]

Eigen::MatrixXd drake::math::ContinuousAlgebraicRiccatiEquation	(	const Eigen::Ref< const Eigen::MatrixXd > &	A,
		const Eigen::Ref< const Eigen::MatrixXd > &	B,
		const Eigen::Ref< const Eigen::MatrixXd > &	Q,
		const Eigen::Ref< const Eigen::MatrixXd > &	R
	)

Computes the unique stabilizing solution S to the continuous-time algebraic Riccati equation:

\[ S A + A' S - S B R^{-1} B' S + Q = 0 \]

Exceptions

std::exception	if the Hamiltanoian matrix ⌈A BR⁻¹Bᵀ⌉ ⌊Q −Aᵀ⌋ is not invertible.
std::exception	if R is not positive definite.

Note

the pair (A, B) should be stabilizable, and (Q, A) should be detectable. For more information, please refer to page 526-527 of Linear Systems by Thomas Kailath.

Based on the Matrix Sign Function method outlined in this paper: http://www.engr.iupui.edu/~skoskie/ECE684/Riccati_algorithms.pdf

ContinuousAlgebraicRiccatiEquation() [2/2]

Eigen::MatrixXd drake::math::ContinuousAlgebraicRiccatiEquation	(	const Eigen::Ref< const Eigen::MatrixXd > &	A,
		const Eigen::Ref< const Eigen::MatrixXd > &	B,
		const Eigen::Ref< const Eigen::MatrixXd > &	Q,
		const Eigen::LLT< Eigen::MatrixXd > &	R_cholesky
	)

This is functionally the same as ContinuousAlgebraicRiccatiEquation(A, B, Q, R).

The Cholesky decomposition of R is passed in instead of R.

ConvertTimeDerivativeToOtherFrame()

Vector3<typename v_Type::Scalar> drake::math::ConvertTimeDerivativeToOtherFrame	(	const Eigen::MatrixBase< v_Type > &	v_E,
		const Eigen::MatrixBase< DtB_v_Type > &	DtB_v_E,
		const Eigen::MatrixBase< w_AB_Type > &	w_AB_E
	)

Given ᴮd/dt(v) (the time derivative in frame B of an arbitrary 3D vector v) and given ᴬωᴮ (frame B's angular velocity in another frame A), this method computes ᴬd/dt(v) (the time derivative in frame A of v) by: ᴬd/dt(v) = ᴮd/dt(v) + ᴬωᴮ x v.

This mathematical operation is known as the "Transport Theorem" or the "Golden Rule for Vector Differentiation" [Mitiguy 2016, §7.3]. It was discovered by Euler in 1758. Its explicit notation with superscript frames was invented by Thomas Kane in 1950. Its use as the defining property of angular velocity was by Mitiguy in 1993.

In source code and comments, we use the following monogram notations: DtA_v = ᴬd/dt(v) denotes the time derivative in frame A of the vector v. DtA_v_E = [ᴬd/dt(v)]_E denotes the time derivative in frame A of vector v, with the resulting new vector quantity expressed in a frame E.

In source code, this mathematical operation is performed with all vectors expressed in the same frame E as [ᴬd/dt(v)]ₑ = [ᴮd/dt(v)]ₑ + [ᴬωᴮ]ₑ x [v]ₑ which in monogram notation is:

  DtA_v_E = DtB_v_E + w_AB_E x v_E

[Mitiguy 2016] Mitiguy, P., 2016. Advanced Dynamics & Motion Simulation.

DecomposePositiveQuadraticForm()

std::pair<Eigen::MatrixXd, Eigen::MatrixXd> drake::math::DecomposePositiveQuadraticForm	(	const Eigen::Ref< const Eigen::MatrixXd > &	Q,
		const Eigen::Ref< const Eigen::VectorXd > &	b,
		double	c,
		double	tol = 0
	)

Rewrite a quadratic form xᵀQx + bᵀx + c to (Rx+d)ᵀ(Rx+d) where.

RᵀR = Q
Rᵀd = b / 2
dᵀd = c

This decomposition requires the matrix

⌈Q     b/2⌉
⌊bᵀ/2    c⌋

to be positive semidefinite.

We return R and d with the minimal number of rows, namely the rows of R and d equal to the rank of the matrix

⌈Q     b/2⌉
⌊bᵀ/2    c⌋

Notice that R might have more rows than Q, For example, the quadratic expression x² + 2x + 5 =(x+1)² + 2², it can be decomposed as

⎛⌈1⌉ * x + ⌈1⌉⎞ᵀ * ⎛⌈1⌉ * x + ⌈1⌉⎞
⎝⌊0⌋       ⌊2⌋⎠    ⎝⌊0⌋       ⌊2⌋⎠

Here R has 2 rows while Q only has 1 row.

On the other hand the quadratic expression x² + 2x + 1 can be decomposed as (x+1) * (x+1), where R has 1 row, same as Q.

Also notice that this decomposition is not unique. For example, with any permutation matrix P, we can define

R₁ = P*R
d₁ = P*d

Then (R₁*x+d₁)ᵀ(R₁*x+d₁) gives the same quadratic form.

Parameters

Q	The square matrix.
b	The vector containing the linear coefficients.
c	The constant term.
tol	We will determine if this quadratic form is always non-negative, by checking the Eigen values of the matrix [Q b/2] [bᵀ/2 c] are all greater than -tol. Default: is 0.

Return values

(R,d).

R and d have the same number of rows. R.cols() == x.rows(). R.rows() equals to the rank of the matrix [Q b/2] [bᵀ/2 c]

Precondition

1. The quadratic form is always non-negative, namely the matrix

        [Q    b/2]
        [bᵀ/2   c]
        

is positive semidefinite.

1. Q and b are of the correct size.
2. tol is non-negative.

Exceptions

std::exception

if the precondition is not satisfied.

DecomposePSDmatrixIntoXtransposeTimesX()

Eigen::MatrixXd drake::math::DecomposePSDmatrixIntoXtransposeTimesX	(	const Eigen::Ref< const Eigen::MatrixXd > &	Y,
		double	zero_tol,
		bool	return_empty_if_not_psd = false
	)

For a symmetric positive semidefinite matrix Y, decompose it into XᵀX, where the number of rows in X equals to the rank of Y.

Notice that this decomposition is not unique. For any orthonormal matrix U, s.t UᵀU = identity, X_prime = UX also satisfies X_primeᵀX_prime = Y. Here we only return one valid decomposition.

Parameters

Y	A symmetric positive semidefinite matrix.
zero_tol	We will need to check if some value (for example, the absolute value of Y's eigenvalues) is smaller than zero_tol. If it is, then we deem that value as 0.
return_empty_if_not_psd	If true, then return an empty matrix of size 0-by-Y.cols() if Y is not PSD (either the decomposition fails or the resulting eigenvalues are less that zero_tol). If false (the default), then throw an exception if Y is not PSD. This option is particularly useful because it is brittle/expensive to test the exact success criteria before calling this function.

Return values

X.	The matrix X satisfies XᵀX = Y and X.rows() = rank(Y).

Precondition

1. Y is positive semidefinite or return_empty_if_not_psd = true.

1. zero_tol is non-negative.

Exceptions

std::exception

when the pre-conditions are not satisfied.

Note

We only use the lower triangular part of Y.

DifferentiableNorm()

Derived::Scalar drake::math::DifferentiableNorm

(

const Eigen::MatrixBase< Derived > &

x

)

The 2-norm function |x| is not differentiable at x=0 (its gradient is x/|x|, which has a division-by-zero problem).

On the other hand, x=0 happens very often. Hence we return a subgradient x/(|x| + ε) when x is almost 0, and returns the original gradient, x/|x|, otherwise.

DiscardGradient()

decltype(auto) drake::math::DiscardGradient

(

const Eigen::MatrixBase< Derived > &

matrix

)

B = DiscardGradient(A) enables casting from a matrix of AutoDiffScalars to AutoDiffScalar::Scalar type, explicitly throwing away any gradient information.

For a matrix of type, e.g. MatrixX<AutoDiffXd> A, the comparable operation B = A.cast<double>() should (and does) fail to compile. Use DiscardGradient(A) if you want to force the cast (and explicitly declare that information is lost).

When called with a matrix that is already of type double, this function returns a reference to the argument without any copying. This efficiently avoids extra copying, but be careful about reference lifetimes!

See ExtractValue() for a note on similar Drake functions.

See also

ExtractValue(), DiscardZeroGradient()

DiscardZeroGradient()

decltype(auto) drake::math::DiscardZeroGradient	(	const Eigen::MatrixBase< Derived > &	auto_diff_matrix,
		double	precision = Eigen::NumTraits<double>::dummy_precision()
	)

B = DiscardZeroGradient(A, precision) enables casting from a matrix of AutoDiffScalars to AutoDiffScalar::Scalar type, but first checking that the gradient matrix is empty or zero.

For a matrix of type, e.g. MatrixX<AutoDiffXd> A, the comparable operation B = A.cast<double>() should (and does) fail to compile. Use DiscardZeroGradient(A) if you want to force the cast (and the check).

When called with a matrix that is already of type double, this function returns a reference to the argument without any copying. This efficiently avoids extra copying, but be careful about reference lifetimes!

See ExtractValue() for a note on similar Drake functions.

Parameters

precision

is passed to Eigen's isZero(precision) to evaluate whether the gradients are zero.

Exceptions

std::exception

if the gradients were not empty nor zero.

See also

DiscardGradient()

DiscreteAlgebraicRiccatiEquation() [1/2]

Eigen::MatrixXd drake::math::DiscreteAlgebraicRiccatiEquation	(	const Eigen::Ref< const Eigen::MatrixXd > &	A,
		const Eigen::Ref< const Eigen::MatrixXd > &	B,
		const Eigen::Ref< const Eigen::MatrixXd > &	Q,
		const Eigen::Ref< const Eigen::MatrixXd > &	R
	)

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

Exceptions

std::exception	if Q is not symmetric positive semidefinite.
std::exception	if R is not symmetric positive definite.
std::exception	if (A, B) isn't a stabilizable pair.
std::exception	if (A, C) isn't a detectable pair where Q = CᵀC.

DiscreteAlgebraicRiccatiEquation() [2/2]

Eigen::MatrixXd drake::math::DiscreteAlgebraicRiccatiEquation	(	const Eigen::Ref< const Eigen::MatrixXd > &	A,
		const Eigen::Ref< const Eigen::MatrixXd > &	B,
		const Eigen::Ref< const Eigen::MatrixXd > &	Q,
		const Eigen::Ref< const Eigen::MatrixXd > &	R,
		const Eigen::Ref< const Eigen::MatrixXd > &	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

Exceptions

std::exception	if Q₂ is not symmetric positive semidefinite.
std::exception	if R is not symmetric positive definite.
std::exception	if (A₂, B) isn't a stabilizable pair.
std::exception	if (A₂, C) isn't a detectable pair where Q₂ = CᵀC.

dquat2rotmat()

drake::math::Gradient<Eigen::Matrix<typename Derived::Scalar, 3, 3>, 4>::type drake::math::dquat2rotmat

(

const Eigen::MatrixBase< Derived > &

quaternion

)

Computes the gradient of the function that converts a unit length quaternion to a rotation matrix.

Parameters

quaternion

A unit length quaternion [w;x;y;z]

Returns

The gradient

drotmat2quat()

drake::math::Gradient<Eigen::Matrix<typename DerivedR::Scalar, 4, 1>, DerivedDR::ColsAtCompileTime>::type drake::math::drotmat2quat	(	const Eigen::MatrixBase< DerivedR > &	R,
		const Eigen::MatrixBase< DerivedDR > &	dR
	)

Computes the gradient of the function that converts rotation matrix to quaternion.

Parameters

R	A 3 x 3 rotation matrix
dR	A 9 x N matrix, dR(i,j) is the gradient of R(i) w.r.t x_var(j)

Returns

The gradient G. G is a 4 x N matrix G(0,j) is the gradient of w w.r.t x_var(j) G(1,j) is the gradient of x w.r.t x_var(j) G(2,j) is the gradient of y w.r.t x_var(j) G(3,j) is the gradient of z w.r.t x_var(j)

drotmat2rpy()

drake::math::Gradient<Eigen::Matrix<typename DerivedR::Scalar, 3, 1>, DerivedDR::ColsAtCompileTime>::type drake::math::drotmat2rpy	(	const Eigen::MatrixBase< DerivedR > &	R,
		const Eigen::MatrixBase< DerivedDR > &	dR
	)

Computes the gradient of the function that converts a rotation matrix to body-fixed z-y'-x'' Euler angles.

Parameters

R	A 3 x 3 rotation matrix
dR	A 9 x N matrix, dR(i,j) is the gradient of R(i) w.r.t x(j)

Returns

The gradient G. G is a 3 x N matrix. G(0,j) is the gradient of roll w.r.t x(j) G(1,j) is the gradient of pitch w.r.t x(j) G(2,j) is the gradient of yaw w.r.t x(j)

EigenToStdVector()

std::vector<MatrixX<T> > drake::math::EigenToStdVector

(

const Eigen::Ref< const MatrixX< T >> &

mat

)

Converts a MatrixX<T> into a std::vector<MatrixX<T>>, taking each column of the m-by-n matrix mat into an m-by-1 element of the returned std::vector.

ExtractGradient()

Eigen::Matrix<typename Derived::Scalar::Scalar, Derived::SizeAtCompileTime, Eigen::Dynamic> drake::math::ExtractGradient	(	const Eigen::MatrixBase< Derived > &	auto_diff_matrix,
		std::optional< int >	num_derivatives = {}
	)

Extracts the derivatives() portion from a matrix of AutoDiffScalar entries.

(Each entry contains a value and derivatives.)

Parameters

auto_diff_matrix	An object whose Eigen type represents a matrix of AutoDiffScalar entries.
num_derivatives	(Optional) The number of derivatives to return in case the input matrix has none, which we interpret as num_derivatives zeroes. If num_derivatives is supplied and the input matrix has derivatives, the sizes must match.

Return values

gradient_matrix

An Eigen::Matrix with number of rows equal to the total size (rows x cols) of the input matrix and number of columns equal to the number of derivatives. Each output row corresponds to one entry of the input matrix, using the input matrix storage order. For example, in the typical case of a ColMajor auto_diff_matrix, we have auto_diff_matrix(r, c).derivatives() == gradient_matrix.row(r + c * auto_diff_matrix.rows()).

Template Parameters

Derived

An Eigen type representing a matrix with AutoDiffScalar entries. The type will be inferred from the type of the auto_diff_matrix parameter at the call site.

Exceptions

std::exception	if the input matrix has elements with inconsistent, non-zero numbers of derivatives.
std::exception	if num_derivatives is specified but the input matrix has a different, non-zero number of derivatives.

ExtractPrincipalSubmatrix()

MatrixX<typename Derived::Scalar> drake::math::ExtractPrincipalSubmatrix	(	const Eigen::MatrixBase< Derived > &	mat,
		const std::set< int > &	indices
	)

Extracts the principal submatrix from the ordered set of indices.

The indices must be in monotonically increasing order and non-empty. This method makes no assumptions about the symmetry of the matrix, nor that the matrix is square. However, all indices must be valid for both rows and columns.

ExtractValue()

MatrixLikewise<typename Derived::Scalar::Scalar, Derived> drake::math::ExtractValue

(

const Eigen::MatrixBase< Derived > &

auto_diff_matrix

)

Extracts the value() portion from a matrix of AutoDiffScalar entries.

(Each entry contains a value and some derivatives.)

Parameters

auto_diff_matrix

An object whose Eigen type represents a matrix of AutoDiffScalar entries.

Return values

value

An Eigen::Matrix of the same dimensions as the input matrix, but containing only the value portion of each entry, without the derivatives.

Template Parameters

Derived

An Eigen type representing a matrix with AutoDiffScalar entries. The type will be inferred from the type of the auto_diff_matrix parameter at the call site.

Note

Drake provides several similar functions: DiscardGradient() is specialized so that it can be applied to a Matrix<T> where T could be an AutoDiffScalar or an ordinary double, in which case it returns the original matrix at no cost. DiscardZeroGradient() is similar but requires that the discarded gradient was zero. drake::ExtractDoubleOrThrow() has many specializations, including one for Matrix<AutoDiffScalar> that behaves identically to ExtractValue().

See also

DiscardGradient(), drake::ExtractDoubleOrThrow()

GetDerivativeSize()

std::enable_if<!std::is_same_v<typename Derived::Scalar, double>, int>::type drake::math::GetDerivativeSize

(

const Eigen::MatrixBase< Derived > &

A

)

Given a matrix of AutoDiffScalars, returns the size of the derivatives.

Exceptions

runtime_error

if some entry has different (non-zero) number of derivatives as the others.

GetLinearSolver()

internal::EigenLinearSolver<LinearSolverType, DerivedA> drake::math::GetLinearSolver

(

const Eigen::MatrixBase< DerivedA > &

A

)

Get the linear solver for a matrix A.

If A has scalar type of double or symbolic::Expressions, then the returned linear solver will have the same scalar type. If A has scalar type of Eigen::AutoDiffScalar, then the returned linear solver will have scalar type of double. See get_linear_solver for more details.

getSubMatrixGradient() [1/3]

Eigen::Matrix<typename Derived::Scalar, Eigen::Dynamic, Eigen::Dynamic> drake::math::getSubMatrixGradient	(	const Eigen::MatrixBase< Derived > &	dM,
		const std::vector< int > &	rows,
		const std::vector< int > &	cols,
		typename Derived::Index	M_rows,
		int	q_start = 0,
		typename Derived::Index	q_subvector_size = -1
	)

getSubMatrixGradient() [2/3]

GetSubMatrixGradientArray<QSubvectorSize, Derived, NRows, NCols>::type drake::math::getSubMatrixGradient	(	const Eigen::MatrixBase< Derived > &	dM,
		const std::array< int, NRows > &	rows,
		const std::array< int, NCols > &	cols,
		typename Derived::Index	M_rows,
		int	q_start = 0,
		typename Derived::Index	q_subvector_size = QSubvectorSize
	)

getSubMatrixGradient() [3/3]

GetSubMatrixGradientSingleElement<QSubvectorSize, Derived>::type drake::math::getSubMatrixGradient	(	const Eigen::MatrixBase< Derived > &	dM,
		int	row,
		int	col,
		typename Derived::Index	M_rows,
		typename Derived::Index	q_start = 0,
		typename Derived::Index	q_subvector_size = QSubvectorSize
	)

GrayCodeToInteger()

int drake::math::GrayCodeToInteger

(

const Eigen::Ref< const Eigen::VectorXi > &

gray_code

)

Converts the Gray code to an integer.

For example (0, 0) -> 0 (0, 1) -> 1 (1, 1) -> 2 (1, 0) -> 3

Parameters

gray_code

The N-digit Gray code, where N is gray_code.rows()

Returns

The integer represented by the Gray code gray_code.

hessian()

decltype(auto) drake::math::hessian	(	F &&	f,
		Arg &&	x
	)

Computes a matrix of AutoDiffScalars from which the value, Jacobian, and Hessian of a function

\[ f:\mathbb{R}^{n\times m}\rightarrow\mathbb{R}^{p\times q} \]

(f: R^n*m -> R^p*q) can be extracted.

The output is a matrix of nested AutoDiffScalars, being the result of calling jacobian on a function that returns the output of jacobian, called on f.

MaxChunkSizeOuter and MaxChunkSizeInner can be used to control chunk sizes (see jacobian).

See jacobian for requirements on the function f and the argument x.

Parameters

f	function
x	function argument value at which Hessian will be evaluated

Returns

AutoDiffScalar matrix corresponding to the Hessian of f evaluated at x

HopfCoordinateToQuaternion()

const Eigen::Quaternion<T> drake::math::HopfCoordinateToQuaternion	(	const T &	theta,
		const T &	phi,
		const T &	psi
	)

Transforms Hopf coordinates to a quaternion w, x, y, z as w = cos(θ/2)cos(ψ/2) x = cos(θ/2)sin(ψ/2) y = sin(θ/2)cos(φ+ψ/2) z = sin(θ/2)sin(φ+ψ/2) The user can refer to equation 5 of Generating Uniform Incremental Grids on SO(3) Using the Hopf Fibration by Anna Yershova, Steven LaValle and Julie Mitchell, 2008.

Parameters

theta	The θ angle.
phi	The φ angle.
psi	The ψ angle.

InitializeAutoDiff() [1/5]

void drake::math::InitializeAutoDiff	(	const Eigen::MatrixBase< Derived > &	value,
		std::optional< int >	num_derivatives,
		std::optional< int >	deriv_num_start,
		Eigen::MatrixBase< DerivedAutoDiff > *	auto_diff_matrix
	)

Initializes a single AutoDiff matrix given the corresponding value matrix.

Sets the values of auto_diff_matrix (after resizing if necessary) to be equal to value, and for each element i of auto_diff_matrix, resizes the derivatives vector to num_derivatives and sets derivative number deriv_num_start + i to one (all other elements of the derivative vector set to zero).

When value and auto_diff_matrix are matrices (rather than just vectors) note in particular that the derivative numbers count up using the storage order of value(i) and autodiff_matrix(i) and so the ColMajor vs RowMajor storage order of the two must match.

Parameters

[in]	value	a 'regular' matrix of values
[in]	num_derivatives	(Optional) size of the derivatives vector Default: total size of the value matrix
[in]	deriv_num_start	(Optional) starting index into derivative vector (i.e. element deriv_num_start in derivative vector corresponds to value(0, 0)). Default: 0
[out]	auto_diff_matrix	AutoDiff matrix set as described above

InitializeAutoDiff() [2/5]

void drake::math::InitializeAutoDiff	(	const Eigen::MatrixBase< DerivedValue > &	value,
		const Eigen::MatrixBase< DerivedGradient > &	gradient,
		Eigen::MatrixBase< DerivedAutoDiff > *	auto_diff_matrix
	)

Initializes an AutoDiff matrix given a matrix of values and a gradient matrix.

Parameters

[in]	value	The value matrix. Will be accessed with a single index.
[in]	gradient	The gradient matrix. The number of rows must match the total size (nrow x ncol) of the value matrix. Derivatives of value(j) should be stored in row j of the gradient matrix.
[out]	auto_diff_matrix	The matrix of AutoDiffScalars. Will be resized as needed to have the same dimensions as the value matrix. Must have the same storage order as value.

InitializeAutoDiff() [3/5]

void drake::math::InitializeAutoDiff	(	const Eigen::MatrixBase< Derived > &	value,
		Eigen::MatrixBase< DerivedAutoDiff > *	auto_diff_matrix
	)

Alternate signature provides default values for the number of derivatives (dynamic, determined at run time) and the starting index (0).

InitializeAutoDiff() [4/5]

AutoDiffMatrixType<DerivedValue, DerivedGradient::ColsAtCompileTime> drake::math::InitializeAutoDiff	(	const Eigen::MatrixBase< DerivedValue > &	value,
		const Eigen::MatrixBase< DerivedGradient > &	gradient
	)

Returns an AutoDiff matrix given a matrix of values and a gradient matrix.

Parameters

[in]	value	The value matrix. Will be accessed with a single index.
[in]	gradient	The gradient matrix. The number of rows must match the total size (nrow x ncol) of the value matrix. Derivatives of value(j) should be stored in row j of the gradient matrix.

Return values

auto_diff_matrix

The matrix of AutoDiffScalars. Will have the same dimensions and storage order as the value matrix.

InitializeAutoDiff() [5/5]

AutoDiffMatrixType<Derived, nq> drake::math::InitializeAutoDiff	(	const Eigen::MatrixBase< Derived > &	value,
		std::optional< int >	num_derivatives = {},
		std::optional< int >	deriv_num_start = {}
	)

Initializes a single AutoDiff matrix given the corresponding value matrix.

Creates an AutoDiff matrix that matches value in size with derivative of compile time size nq and runtime size num_derivatives. Sets its values to be equal to value, and for each element i of auto_diff_matrix, sets derivative number deriv_num_start + i to one (all other derivatives set to zero).

When value is a matrix (rather than just a vector) note in particular that the return value will use the same storage order (ColMajor vs RowMajor) and that the derivative numbers count up using the storage order of value(i).

Parameters

[in]	value	'regular' matrix of values
[in]	num_derivatives	(Optional) size of the derivatives vector Default: total size of the value matrix
[in]	deriv_num_start	(Optional) starting index into derivative vector (i.e. element deriv_num_start in derivative vector corresponds to matrix(0, 0)). Default: 0

Return values

auto_diff_matrix

The result as described above.

InitializeAutoDiffTuple()

auto drake::math::InitializeAutoDiffTuple

(

const Eigen::MatrixBase< Deriveds > &...

args

)

intRange()

std::array<int, Size> drake::math::intRange

(

int

start

)

is_quaternion_in_canonical_form()

boolean<T> drake::math::is_quaternion_in_canonical_form

(

const Eigen::Quaternion< T > &

quat

)

This function tests whether a quaternion is in "canonical form" meaning that it tests whether the quaternion [w, x, y, z] has a non-negative w value.

Example: [-0.3, +0.4, +0.5, +0.707] is not in canonical form. Example: [+0.3, -0.4, -0.5, -0.707] is in canonical form.

Parameters

quat	Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat is analogous to the rotation matrix R_AB.

Returns

true if quat.w() is nonnegative (in canonical form), else false.

IsBothQuaternionAndQuaternionDtOK()

boolean<T> drake::math::IsBothQuaternionAndQuaternionDtOK	(	const Eigen::Quaternion< T > &	quat,
		const Vector4< T > &	quatDt,
		const double	tolerance
	)

This function tests if a quaternion satisfies the time-derivative constraint specified in [Kane, 1983] Section 1.13, equation 13, page 59.

A quaternion [w, x, y, z] must satisfy w^2 + x^2 + y^2 + z^2 = 1, hence its time-derivative must satisfy 2*(w*ẇ + x*ẋ + y*ẏ + z*ż) = 0. Note: To accurately test whether the time-derivative quaternion constraint is satisfied, the quaternion constraint is also tested to be accurate.

• [Kane, 1983] "Spacecraft Dynamics," McGraw-Hill Book Co., New York, 1983. (with P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637

Parameters

quat	Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB.
quatDt	Time-derivative of quat, i.e., [ẇ, ẋ, ẏ, ż].
tolerance	Tolerance for quaternion constraints.

Returns

true if both of the two previous constraints are within tolerance.

IsPositiveDefinite()

bool drake::math::IsPositiveDefinite	(	const Eigen::MatrixBase< Derived > &	matrix,
		double	eigenvalue_tolerance = 0.0,
		double	symmetry_tolerance = 0.0
	)

Checks if a matrix is symmetric (with tolerance symmetry_tolerance – see IsSymmetric) and has all eigenvalues greater than eigenvalue_tolerance.

eigenvalue_tolerance must be >= 0 – where 0 implies positive semi-definite (but is of course subject to all of the pitfalls of floating point).

To consider the numerical robustness of the eigenvalue estimation, we specifically check that min_eigenvalue >= eigenvalue_tolerance * max(1, max_abs_eigenvalue).

IsQuaternionAndQuaternionDtEqualAngularVelocityExpressedInB()

bool drake::math::IsQuaternionAndQuaternionDtEqualAngularVelocityExpressedInB	(	const Eigen::Quaternion< T > &	quat,
		const Vector4< T > &	quatDt,
		const Vector3< T > &	w_B,
		const double	tolerance
	)

This function tests if a quaternion and a quaternion's time derivative can calculate and match an angular velocity to within a tolerance.

Note: This function first tests if the quaternion [w, x, y, z] satisfies w^2 + x^2 + y^2 + z^2 = 1 (to within tolerance) and if its time-derivative satisfies w*ẇ + x*ẋ + y*ẏ + z*ż = 0 (to within tolerance). Lastly, it tests if each element of the angular velocity calculated from quat and quatDt is within tolerance of w_B (described below).

Parameters

quat	Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB.
quatDt	Time-derivative of quat, i.e., [ẇ, ẋ, ẏ, ż].
w_B	Rigid body B's angular velocity in frame A, expressed in B.
tolerance	Tolerance for quaternion constraints.

Returns

true if all three of the previous constraints are within tolerance.

IsQuaternionValid()

boolean<T> drake::math::IsQuaternionValid	(	const Eigen::Quaternion< T > &	quat,
		const double	tolerance
	)

This function tests if a quaternion satisfies the quaternion constraint specified in [Kane, 1983] Section 1.3, equation 4, page 12, i.e., a quaternion [w, x, y, z] must satisfy: w^2 + x^2 + y^2 + z^2 = 1.

• [Kane, 1983] "Spacecraft Dynamics," McGraw-Hill Book Co., New York, 1983. (with P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637

Parameters

quat	Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB.
tolerance	Tolerance for quaternion constraint, i.e., how much is w^2 + x^2 + y^2 + z^2 allowed to differ from 1.

Returns

true if the quaternion constraint is satisfied within tolerance.

IsSymmetric() [1/2]

bool drake::math::IsSymmetric

(

const Eigen::MatrixBase< Derived > &

matrix

)

Determines if a matrix is symmetric.

If std::equal_to<>()(matrix(i, j), matrix(j, i)) is true for all i, j, then the matrix is symmetric.

IsSymmetric() [2/2]

bool drake::math::IsSymmetric	(	const Eigen::MatrixBase< Derived > &	matrix,
		const typename Derived::Scalar &	precision
	)

Determines if a matrix is symmetric based on whether the difference between matrix(i, j) and matrix(j, i) is smaller than precision for all i, j.

The precision is absolute. Matrix with nan or inf entries is not allowed.

jacobian()

decltype(auto) drake::math::jacobian	(	F &&	f,
		Arg &&	x
	)

Computes a matrix of AutoDiffScalars from which both the value and the Jacobian of a function

\[ f:\mathbb{R}^{n\times m}\rightarrow\mathbb{R}^{p\times q} \]

(f: R^n*m -> R^p*q) can be extracted.

The derivative vector for each AutoDiffScalar in the output contains the derivatives with respect to all components of the argument \( x \).

The return type of this function is a matrix with the ‘best’ possible AutoDiffScalar scalar type, in the following sense:

• If the number of derivatives can be determined at compile time, the AutoDiffScalar derivative vector will have that fixed size.
• If the maximum number of derivatives can be determined at compile time, the AutoDiffScalar derivative vector will have that maximum fixed size.
• If neither the number, nor the maximum number of derivatives can be determined at compile time, the output AutoDiffScalar derivative vector will be dynamically sized.

f should have a templated call operator that maps an Eigen matrix argument to another Eigen matrix. The scalar type of the output of \( f \) need not match the scalar type of the input (useful in recursive calls to the function to determine higher order derivatives). The easiest way to create an f is using a C++14 generic lambda.

The algorithm computes the Jacobian in chunks of up to MaxChunkSize derivatives at a time. This has three purposes:

• It makes it so that derivative vectors can be allocated on the stack, eliminating dynamic allocations and improving performance if the maximum number of derivatives cannot be determined at compile time.
• It gives control over, and limits the number of required instantiations of the call operator of f and all the functions it calls.
• Excessively large derivative vectors can result in CPU capacity cache misses; even if the number of derivatives is fixed at compile time, it may be better to break up into chunks if that means that capacity cache misses can be prevented.

Parameters

f	function
x	function argument value at which Jacobian will be evaluated

Returns

AutoDiffScalar matrix corresponding to the Jacobian of f evaluated at x.

matGradMult()

MatGradMult<DerivedDA, DerivedB>::type drake::math::matGradMult	(	const Eigen::MatrixBase< DerivedDA > &	dA,
		const Eigen::MatrixBase< DerivedB > &	B
	)

matGradMultMat()

MatGradMultMat<DerivedA, DerivedB, DerivedDA>::type drake::math::matGradMultMat	(	const Eigen::MatrixBase< DerivedA > &	A,
		const Eigen::MatrixBase< DerivedB > &	B,
		const Eigen::MatrixBase< DerivedDA > &	dA,
		const Eigen::MatrixBase< DerivedDB > &	dB
	)

NormalizeVector()

void drake::math::NormalizeVector	(	const Eigen::MatrixBase< Derived > &	x,
		typename Derived::PlainObject &	x_norm,
		typename drake::math::Gradient< Derived, Derived::RowsAtCompileTime, 1 >::type *	dx_norm = nullptr,
		typename drake::math::Gradient< Derived, Derived::RowsAtCompileTime, 2 >::type *	ddx_norm = nullptr
	)

Computes the normalized vector, optionally with its gradient and second derivative.

Parameters

[in]	x	An N x 1 vector to be normalized. Must not be zero.
[out]	x_norm	The normalized vector (N x 1).
[out]	dx_norm	If non-null, returned as an N x N matrix, where dx_norm(i,j) = D x_norm(i)/D x(j).
[out]	ddx_norm	If non-null, and dx_norm is non-null, returned as an N^2 x N matrix, where ddx_norm.col(j) = D dx_norm/D x(j), with dx_norm stacked columnwise.

(D x / D y above means partial derivative of x with respect to y.)

ProjectMatToRotMatWithAxis()

double drake::math::ProjectMatToRotMatWithAxis	(	const Eigen::Matrix3d &	M,
		const Eigen::Vector3d &	axis,
		double	angle_lb,
		double	angle_ub
	)

Projects an approximate 3 x 3 rotation matrix M onto an orthonormal matrix R so that R is a rotation matrix associated with a angle-axis rotation by an angle θ about a vector direction axis, with angle_lb <= θ <= angle_ub.

Parameters

[in]	M	the matrix to be projected.
[in]	axis	vector direction associated with angle-axis rotation for R. axis can be a non-unit vector, but cannot be the zero vector.
[in]	angle_lb	the lower bound of the rotation angle θ.
[in]	angle_ub	the upper bound of the rotation angle θ.

Returns

Rotation angle θ of the projected matrix, angle_lb <= θ <= angle_ub

Exceptions

std::exception

if axis is the zero vector or if angle_lb > angle_ub.

Note

This method is useful for reconstructing a rotation matrix for a revolute joint with joint limits.

This can be formulated as an optimization problem

  min_θ trace((R - M)ᵀ*(R - M))
  subject to R = I + sinθ * A + (1 - cosθ) * A²   (1)
             angle_lb <= θ <= angle_ub

where A is the cross product matrix of a = axis / axis.norm() = [a₁, a₂, a₃]

A = [ 0  -a₃  a₂]
    [ a₃  0  -a₁]
    [-a₂  a₁  0 ]

Equation (1) is the Rodriguez Formula that computes the rotation matrix R from the angle-axis rotation with angle θ and vector direction axis. For details, see http://mathworld.wolfram.com/RodriguesRotationFormula.html The objective function can be simplified as

   max_θ trace(Rᵀ * M + Mᵀ * R)

By substituting the matrix R with the angle-axis representation, the optimization problem is formulated as

   max_θ sinθ * trace(Aᵀ*M) - cosθ * trace(Mᵀ * A²)
   subject to angle_lb <= θ <= angle_ub

By introducing α = atan2(-trace(Mᵀ * A²), trace(Aᵀ*M)), we can compute the optimal θ as

   θ = π/2 + 2kπ - α, if angle_lb <= π/2 + 2kπ - α <= angle_ub, k ∈ integers
else
   θ = angle_lb, if sin(angle_lb + α) >= sin(angle_ub + α)
   θ = angle_ub, if sin(angle_lb + α) <  sin(angle_ub + α)

See also

GlobalInverseKinematics for an usage of this function.

quatConjugate()

Vector4<typename Derived::Scalar> drake::math::quatConjugate

(

const Eigen::MatrixBase< Derived > &

q

)

quatDiff()

Vector4<typename Derived1::Scalar> drake::math::quatDiff	(	const Eigen::MatrixBase< Derived1 > &	q1,
		const Eigen::MatrixBase< Derived2 > &	q2
	)

quatDiffAxisInvar()

Derived1::Scalar drake::math::quatDiffAxisInvar	(	const Eigen::MatrixBase< Derived1 > &	q1,
		const Eigen::MatrixBase< Derived2 > &	q2,
		const Eigen::MatrixBase< DerivedU > &	u
	)

QuaternionToCanonicalForm()

Eigen::Quaternion<T> drake::math::QuaternionToCanonicalForm

(

const Eigen::Quaternion< T > &

quat

)

This function returns a quaternion in its "canonical form" meaning that it returns a quaternion [w, x, y, z] with a non-negative w.

For example, if passed a quaternion [-0.3, +0.4, +0.5, +0.707], the function returns the quaternion's canonical form [+0.3, -0.4, -0.5, -0.707].

Parameters

quat	Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat is analogous to the rotation matrix R_AB.

Returns

Canonical form of quat, which means that either the original quat is returned or a quaternion representing the same orientation but with negated [w, x, y, z], to ensure a positive w in returned quaternion.

QuaternionToHopfCoordinate()

Vector3<T> drake::math::QuaternionToHopfCoordinate

(

const Eigen::Quaternion< T > &

quaternion

)

Convert a unit-length quaternion (w, x, y, z) (with the requirement w >= 0) to Hopf coordinate as ψ = 2*atan2(x, w) φ = mod(atan2(z, y) - ψ/2, 2pi) θ = 2*atan2(√(y²+z²), √(w²+x²)) ψ is in the range of [-pi, pi].

φ is in the range of [0, 2pi]. θ is in the range of [0, pi]. If the given quaternion has w < 0, then we reverse the signs of (w, x, y, z), and compute the Hopf coordinate of (-w, -x, -y, -z).

Parameters

quaternion

A unit length quaternion.

Returns

hopf_coordinate (θ, φ, ψ) as an Eigen vector.

quatProduct()

Vector4<typename Derived1::Scalar> drake::math::quatProduct	(	const Eigen::MatrixBase< Derived1 > &	q1,
		const Eigen::MatrixBase< Derived2 > &	q2
	)

quatRotateVec()

Vector3<typename DerivedV::Scalar> drake::math::quatRotateVec	(	const Eigen::MatrixBase< DerivedQ > &	q,
		const Eigen::MatrixBase< DerivedV > &	v
	)

RealContinuousLyapunovEquation()

Eigen::MatrixXd drake::math::RealContinuousLyapunovEquation	(	const Eigen::Ref< const Eigen::MatrixXd > &	A,
		const Eigen::Ref< const Eigen::MatrixXd > &	Q
	)

Parameters

A	A user defined real square matrix.
Q	A user defined real symmetric matrix.

Precondition

Q is a symmetric matrix.

Computes a unique solution X to the continuous Lyapunov equation: AᵀX + XA + Q = 0, where A is real and square, and Q is real, symmetric and of equal size as A.

Exceptions

std::exception

if A or Q are not square matrices or do not have the same size.

Limitations: Given the Eigenvalues of A as λ₁, ..., λₙ, there exists a unique solution if and only if λᵢ + λ̅ⱼ ≠ 0 ∀ i,j, where λ̅ⱼ is the complex conjugate of λⱼ.

Exceptions

std::exception

if the solution is not unique.

There are no further limitations on the eigenvalues of A. Further, if all λᵢ have negative real parts, and if Q is positive semi-definite, then X is also positive semi-definite [1]. Therefore, if one searches for a Lyapunov function V(z) = zᵀXz for the stable linear system ż = Az, then the solution of the Lyapunov Equation AᵀX + XA + Q = 0 only returns a valid Lyapunov function if Q is positive semi-definite.

The implementation is based on SLICOT routine SB03MD [2]. Note the transformation Q = -C. The complexity of this routine is O(n³). If A is larger than 2-by-2, then a Schur factorization is performed.

Exceptions

std::exception

if Schur factorization failed.

A tolerance of ε is used to check if a double variable is equal to zero, where the default value for ε is 1e-10. It has been used to check (1) if λᵢ + λ̅ⱼ = 0, ∀ i,j; (2) if A is a 1-by-1 zero matrix; (3) if A's trace or determinant is 0 when A is a 2-by-2 matrix.

[1] Bartels, R.H. and G.W. Stewart, "Solution of the Matrix Equation AX + XB = C," Comm. of the ACM, Vol. 15, No. 9, 1972.

[2] http://slicot.org/objects/software/shared/doc/SB03MD.html

RealDiscreteLyapunovEquation()

Eigen::MatrixXd drake::math::RealDiscreteLyapunovEquation	(	const Eigen::Ref< const Eigen::MatrixXd > &	A,
		const Eigen::Ref< const Eigen::MatrixXd > &	Q
	)

Parameters

A	A user defined real square matrix.
Q	A user defined real symmetric matrix.

Precondition

Q is a symmetric matrix.

Computes the unique solution X to the discrete Lyapunov equation: AᵀXA - X + Q = 0, where A is real and square, and Q is real, symmetric and of equal size as A.

Exceptions

std::exception

if A or Q are not square matrices or do not have the same size.

Limitations: Given the Eigenvalues of A as λ₁, ..., λₙ, there exists a unique solution if and only if λᵢ * λⱼ ≠ 1 ∀ i,j and λᵢ ≠ ±1, ∀ i [1].

Exceptions

std::exception

if the solution is not unique.[3]

There are no further limitations on the eigenvalues of A. Further, if |λᵢ|<1, ∀ i, and if Q is positive semi-definite, then X is also positive semi-definite [2]. Therefore, if one searches for a Lyapunov function V(z) = zᵀXz for the stable linear system zₙ₊₁ = Azₙ, then the solution of the Lyapunov Equation AᵀXA - X + Q = 0 only returns a valid Lyapunov function if Q is positive semi-definite.

The implementation is based on SLICOT routine SB03MD [2]. Note the transformation Q = -C. The complexity of this routine is O(n³). If A is larger than 2-by-2, then a Schur factorization is performed.

Exceptions

std::exception

if Schur factorization fails.

A tolerance of ε is used to check if a double variable is equal to zero, where the default value for ε is 1e-10. It has been used to check (1) if λᵢ = ±1 ∀ i; (2) if λᵢ * λⱼ = 1, i ≠ j.

[1] Barraud, A.Y., "A numerical algorithm to solve AᵀXA - X = Q," IEEE® Trans. Auto. Contr., AC-22, pp. 883-885, 1977.

[2] http://slicot.org/objects/software/shared/doc/SB03MD.html

[3] https://www.mathworks.com/help/control/ref/dlyap.html

setSubMatrixGradient() [1/3]

void drake::math::setSubMatrixGradient	(	Eigen::MatrixBase< DerivedA > &	dM,
		const Eigen::MatrixBase< DerivedB > &	dM_submatrix,
		const std::vector< int > &	rows,
		const std::vector< int > &	cols,
		typename DerivedA::Index	M_rows,
		typename DerivedA::Index	q_start = 0,
		typename DerivedA::Index	q_subvector_size = -1
	)

setSubMatrixGradient() [2/3]

void drake::math::setSubMatrixGradient	(	Eigen::MatrixBase< DerivedA > &	dM,
		const Eigen::MatrixBase< DerivedB > &	dM_submatrix,
		const std::array< int, NRows > &	rows,
		const std::array< int, NCols > &	cols,
		typename DerivedA::Index	M_rows,
		typename DerivedA::Index	q_start = 0,
		typename DerivedA::Index	q_subvector_size = QSubvectorSize
	)

setSubMatrixGradient() [3/3]

void drake::math::setSubMatrixGradient	(	Eigen::MatrixBase< DerivedDM > &	dM,
		const Eigen::MatrixBase< DerivedDMSub > &	dM_submatrix,
		int	row,
		int	col,
		typename DerivedDM::Index	M_rows,
		typename DerivedDM::Index	q_start = 0,
		typename DerivedDM::Index	q_subvector_size = QSubvectorSize
	)

SoftOverMax()

T drake::math::SoftOverMax	(	const std::vector< T > &	x,
		double	alpha = 1.0
	)

Computes a smooth over approximation of max function, namely SoftOverMax(x) >= max(x).

Mathematically we compute this as (log (∑ᵢ exp(αxᵢ))) / α.

Parameters

x	The vector for which we want to compute its soft max.
alpha	α in the documentation above. Larger α makes the soft max more similar to max, with a sharper corner. Must be strictly positive and finite. Default: is 1.

Exceptions

std::exception	if α <= 0.
std::exception	if α is non-finite.

Template Parameters

T	The scalar type, which must be one of the default nonsymbolic scalars.

SoftOverMin()

T drake::math::SoftOverMin	(	const std::vector< T > &	x,
		double	alpha = 1.0
	)

Computes a smooth over approximation of min function, namely SoftOverMin(x) >= min(x).

Mathematically we compute this as ∑ᵢ exp(-αxᵢ)*xᵢ / d, where d = ∑ⱼ exp(-αxⱼ)

Parameters

x	The vector for which we want to compute its soft min.
alpha	α in the documentation above. Larger α makes the soft min more similar to min, with a sharper corner. Must be strictly positive and finite. Default: is 1.

Exceptions

std::exception	if α <= 0.
std::exception	if α is non-finite.

Template Parameters

T	The scalar type, which must be one of the default nonsymbolic scalars.

SoftUnderMax()

T drake::math::SoftUnderMax	(	const std::vector< T > &	x,
		double	alpha = 1.0
	)

Computes a smooth under approximation of max function, namely SoftUnderMax(x) <= max(x).

Mathematically we compute this as ∑ᵢ exp(αxᵢ)*xᵢ / d, where d = ∑ⱼ exp(αxⱼ)

Parameters

x	The vector for which we want to compute its soft max.
alpha	α in the documentation above. Larger α makes the soft max more similar to max, with a sharper corner. Must be strictly positive and finite. Default: is 1.

Exceptions

std::exception	if α <= 0.
std::exception	if α is non-finite.

Template Parameters

T	The scalar type, which must be one of the default nonsymbolic scalars.

SoftUnderMin()

T drake::math::SoftUnderMin	(	const std::vector< T > &	x,
		double	alpha = 1.0
	)

Computes a smooth under approximation of min function, namely SoftUnderMin(x) <= min(x).

Mathematically we compute this as -(log (∑ᵢ exp(-αxᵢ))) / α

Parameters

x	The vector for which we want to compute its soft min.
alpha	α in the documentation above. Larger α makes the soft min more similar to min, with a sharper corner. Must be strictly positive and finite. Default: is 1.

Exceptions

std::exception	if α <= 0.
std::exception	if α is non-finite.

Template Parameters

T	The scalar type, which must be one of the default nonsymbolic scalars.

SolveLinearSystem() [1/6]

std::enable_if< internal::is_double_or_symbolic_v<typename DerivedA::Scalar> && internal::is_double_or_symbolic_v<typename DerivedB::Scalar> && std::is_same_v<typename DerivedA::Scalar, typename DerivedB::Scalar>, Eigen::Matrix<typename DerivedA::Scalar, DerivedA::RowsAtCompileTime, DerivedB::ColsAtCompileTime> >::type drake::math::SolveLinearSystem	(	const LinearSolver &	linear_solver,
		const Eigen::MatrixBase< DerivedA > &	A,
		const Eigen::MatrixBase< DerivedB > &	b
	)

Specialized when A and b are both double or symbolic::Expression matrices.

See linear_solve_given_solver for more details. Note that A is unused, as we already compute its factorization in linear_solver. But we keep it here for consistency with the overloaded function, where A is a matrix of AutoDiffScalar.

SolveLinearSystem() [2/6]

std::enable_if< internal::is_double_or_symbolic_v< typename LinearSolver::MatrixType::Scalar> && internal::is_double_or_symbolic_v<typename DerivedB::Scalar> && std::is_same_v<typename LinearSolver::MatrixType::Scalar, typename DerivedB::Scalar>, Eigen::Matrix<typename LinearSolver::MatrixType::Scalar, DerivedB::RowsAtCompileTime, DerivedB::ColsAtCompileTime> >::type drake::math::SolveLinearSystem	(	const LinearSolver &	linear_solver,
		const Eigen::MatrixBase< DerivedB > &	b
	)

Specialized when the matrix in linear_solver and b are both double or symbolic::Expression matrices.

See linear_solve_given_solver for more details.

SolveLinearSystem() [3/6]

std::enable_if< std::is_same_v<typename LinearSolver::MatrixType::Scalar, double> && internal::is_autodiff_v<typename DerivedB::Scalar>, Eigen::Matrix<typename DerivedB::Scalar, DerivedB::RowsAtCompileTime, DerivedB::ColsAtCompileTime> >::type drake::math::SolveLinearSystem	(	const LinearSolver &	linear_solver,
		const Eigen::MatrixBase< DerivedB > &	b
	)

Specialized the matrix in linear_solver is a double-valued matrix and b is an AutoDiffScalar-valued matrix.

See linear_solve_given_solver for more details.

SolveLinearSystem() [4/6]

std::enable_if< std::is_same_v<typename DerivedA::Scalar, double> && internal::is_autodiff_v<typename DerivedB::Scalar>, Eigen::Matrix<typename DerivedB::Scalar, DerivedA::RowsAtCompileTime, DerivedB::ColsAtCompileTime> >::type drake::math::SolveLinearSystem	(	const LinearSolver &	linear_solver,
		const Eigen::MatrixBase< DerivedA > &	A,
		const Eigen::MatrixBase< DerivedB > &	b
	)

Specialized when A is a double-valued matrix and b is an AutoDiffScalar-valued matrix.

See linear_solve_given_solver for more details. Note that A is unused, as we already compute its factorization in linear_solver. But we keep it here for consistency with the overloaded function, where A is a matrix of AutoDiffScalar.

SolveLinearSystem() [5/6]

std::enable_if< internal::is_autodiff_v<typename DerivedA::Scalar>, Eigen::Matrix<typename DerivedA::Scalar, DerivedA::RowsAtCompileTime, DerivedB::ColsAtCompileTime> >::type drake::math::SolveLinearSystem	(	const LinearSolver &	linear_solver,
		const Eigen::MatrixBase< DerivedA > &	A,
		const Eigen::MatrixBase< DerivedB > &	b
	)

Specialized when A is an AutoDiffScalar-valued matrix, and b can contain either AutoDiffScalar or double.

See linear_solve_given_solver for more details.

SolveLinearSystem() [6/6]

internal::Solution<DerivedA, DerivedB> drake::math::SolveLinearSystem	(	const Eigen::MatrixBase< DerivedA > &	A,
		const Eigen::MatrixBase< DerivedB > &	b
	)

Solves system A*x=b.

The supported combinations of scalar types are summarized in the table above. See linear_solve for more details.

SparseMatrixToRowColumnValueVectors()

void drake::math::SparseMatrixToRowColumnValueVectors	(	const Derived &	matrix,
		std::vector< Eigen::Index > &	row_indices,
		std::vector< Eigen::Index > &	col_indices,
		std::vector< typename Derived::Scalar > &	val
	)

For a sparse matrix, return the row indices, the column indices, and value of the non-zero entries.

For example, the matrix

\[ mat = \begin{bmatrix} 1 & 0 & 2\\ 0 & 3 & 4\end{bmatrix} \]

has

\[ row = \begin{bmatrix} 0 & 1 & 0 & 1\end{bmatrix}\\ col = \begin{bmatrix} 0 & 1 & 2 & 2\end{bmatrix}\\ val = \begin{bmatrix} 1 & 3 & 2 & 4\end{bmatrix} \]

Parameters

[in]	matrix	the input sparse matrix
[out]	row_indices	a vector containing the row indices of the non-zero entries
[out]	col_indices	a vector containing the column indices of the non-zero entries
[out]	val	a vector containing the values of the non-zero entries.

SparseMatrixToTriplets()

std::vector<Eigen::Triplet<Scalar> > drake::math::SparseMatrixToTriplets

(

const Eigen::SparseMatrix< Scalar, Options, StorageIndex > &

matrix

)

For a sparse matrix, return a vector of triplets, such that we can reconstruct the matrix using setFromTriplet function.

Parameters

matrix

A sparse matrix

Returns

A triplet with the row, column and value of the non-zero entries. See https://eigen.tuxfamily.org/dox/group__TutorialSparse.html for more information on the triplet

StdVectorToEigen()

MatrixX<T> drake::math::StdVectorToEigen

(

const std::vector< MatrixX< T >> &

vec

)

Converts a std::vector<MatrixX<T>> into a MatrixX<T>, composing each element of vec into a column of the returned matrix.

Precondition

all elements of vec must have one column and the same number of rows.

ToLowerTriangularColumnsFromMatrix()

drake::VectorX<typename Derived::Scalar> drake::math::ToLowerTriangularColumnsFromMatrix

(

const Eigen::MatrixBase< Derived > &

matrix

)

Given a square matrix, extract the lower triangular part as a stacked column vector.

This is a particularly useful operation when vectorizing symmetric matrices.

ToSymmetricMatrixFromLowerTriangularColumns() [1/2]

drake::MatrixX<typename Derived::Scalar> drake::math::ToSymmetricMatrixFromLowerTriangularColumns

(

const Eigen::MatrixBase< Derived > &

lower_triangular_columns

)

Given a column vector containing the stacked columns of the lower triangular part of a square matrix, returning a symmetric matrix whose lower triangular part is the same as the original matrix.

ToSymmetricMatrixFromLowerTriangularColumns() [2/2]

Eigen::Matrix<typename Derived::Scalar, rows, rows> drake::math::ToSymmetricMatrixFromLowerTriangularColumns

(

const Eigen::MatrixBase< Derived > &

lower_triangular_columns

)

Given a column vector containing the stacked columns of the lower triangular part of a square matrix, returning a symmetric matrix whose lower triangular part is the same as the original matrix.

Template Parameters

rows	The number of rows in the symmetric matrix.

transposeGrad()

Derived::PlainObject drake::math::transposeGrad	(	const Eigen::MatrixBase< Derived > &	dX,
		typename Derived::Index	rows_X
	)

UniformlyRandomAngleAxis()

Eigen::AngleAxis<T> drake::math::UniformlyRandomAngleAxis

(

Generator *

generator

)

Generates a rotation (in the axis-angle representation) that rotates a point on the unit sphere to another point on the unit sphere with a uniform distribution over the sphere.

UniformlyRandomQuaternion()

Eigen::Quaternion<T> drake::math::UniformlyRandomQuaternion

(

Generator *

generator

)

Generates a rotation (in the quaternion representation) that rotates a point on the unit sphere to another point on the unit sphere with a uniform distribution over the sphere.

This method is briefly explained in http://planning.cs.uiuc.edu/node198.html, a full explanation can be found in K. Shoemake. Uniform Random Rotations in D. Kirk, editor, Graphics Gems III, pages 124-132. Academic, New York, 1992.

UniformlyRandomRotationMatrix()

RotationMatrix<T> drake::math::UniformlyRandomRotationMatrix

(

Generator *

generator

)

Generates a rotation (in the rotation matrix representation) that rotates a point on the unit sphere to another point on the unit sphere with a uniform distribution over the sphere.

UniformlyRandomRPY()

Vector3<T> drake::math::UniformlyRandomRPY

(

Generator *

generator

)

Generates a rotation (in the roll-pitch-yaw representation) that rotates a point on the unit sphere to another point on the unit sphere with a uniform distribution over the sphere.

UniformPtsOnSphereFibonacci()

Eigen::Matrix3Xd drake::math::UniformPtsOnSphereFibonacci

(

int

num_points

)

Deterministically generates approximate evenly distributed points on a unit sphere.

This method uses Fibonacci number. For the detailed math, please refer to http://stackoverflow.com/questions/9600801/evenly-distributing-n-points-on-a-sphere This algorithm generates the points in O(n) time, where n is the number of points.

Parameters

num_points

The number of points we want on the unit sphere.

Returns

The generated points.

Precondition

num_samples >= 1. Throw std::exception if num_points < 1

VectorToSkewSymmetric()

drake::Matrix3<typename Derived::Scalar> drake::math::VectorToSkewSymmetric

(

const Eigen::MatrixBase< Derived > &

p

)

wrap_to()

T1 drake::math::wrap_to	(	const T1 &	value,
		const T2 &	low,
		const T2 &	high
	)

For variables that are meant to be periodic, (e.g.

over a 2π interval), wraps value into the interval [low, high). Precisely, wrap_to returns: value + k*(high-low) for the unique integer value k that lands the output in the desired interval. low and high must be finite, and low < high.