Drake
drake::math Namespace Reference

## Classes

struct  AutoDiffToValueMatrix

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  Gradient< Derived, Nq, 1 >

struct  GrayCodesMatrix
GrayCodesMatrix::type returns an Eigen matrix of integers. More...

struct  GrayCodesMatrix< Eigen::Dynamic >

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 = Eigen::Matrix< Eigen::AutoDiffScalar< Eigen::Matrix< typename Derived::Scalar, Nq, 1 > >, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime, 0, Derived::MaxRowsAtCompileTime, Derived::MaxColsAtCompileTime >
The appropriate AutoDiffScalar gradient 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 >
AutoDiffToValueMatrix< Derived >::type autoDiffToValueMatrix (const Eigen::MatrixBase< Derived > &auto_diff_matrix)

template<typename Derived >
std::enable_if< !std::is_same< typename Derived::Scalar, double >::value, Eigen::Matrix< typename Derived::Scalar::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime, 0, Derived::MaxRowsAtCompileTime, Derived::MaxColsAtCompileTime > >::type DiscardGradient (const Eigen::MatrixBase< Derived > &auto_diff_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 >
std::enable_if< std::is_same< typename Derived::Scalar, double >::value, const Eigen::MatrixBase< Derived > & >::type DiscardGradient (const Eigen::MatrixBase< Derived > &matrix)

template<typename _Scalar , int _Dim, int _Mode, int _Options>
std::enable_if< !std::is_same< _Scalar, double >::value, Eigen::Transform< typename _Scalar::Scalar, _Dim, _Mode, _Options > >::type DiscardGradient (const Eigen::Transform< _Scalar, _Dim, _Mode, _Options > &auto_diff_transform)

template<typename _Scalar , int _Dim, int _Mode, int _Options>
std::enable_if< std::is_same< _Scalar, double >::value, const Eigen::Transform< _Scalar, _Dim, _Mode, _Options > & >::type DiscardGradient (const Eigen::Transform< _Scalar, _Dim, _Mode, _Options > &transform)

template<typename Derived , typename DerivedAutoDiff >
void initializeAutoDiff (const Eigen::MatrixBase< Derived > &val, Eigen::MatrixBase< DerivedAutoDiff > &auto_diff_matrix, Eigen::DenseIndex num_derivatives=Eigen::Dynamic, Eigen::DenseIndex deriv_num_start=0)
Initialize a single autodiff matrix given the corresponding value matrix. More...

template<int Nq = Eigen::Dynamic, typename Derived >
AutoDiffMatrixType< Derived, Nq > initializeAutoDiff (const Eigen::MatrixBase< Derived > &mat, Eigen::DenseIndex num_derivatives=-1, Eigen::DenseIndex deriv_num_start=0)
Initialize a single autodiff matrix given the corresponding value matrix. More...

template<typename Derived >
void resizeDerivativesToMatchScalar (Eigen::MatrixBase< Derived > &mat, const typename Derived::Scalar &scalar)
Resize derivatives vector of each element of a matrix to to match the size of the derivatives vector of a given scalar. More...

template<typename... Deriveds>
std::tuple< AutoDiffMatrixType< Deriveds, internal::totalSizeAtCompileTime< Deriveds... >)>... > initializeAutoDiffTuple (const Eigen::MatrixBase< Deriveds > &... args)
Given a series of Eigen matrices, create a tuple of corresponding AutoDiff matrices with values equal to the input matrices and properly initialized derivative vectors. More...

template<typename Derived >

template<typename Derived , typename DerivedGradient , typename DerivedAutoDiff >
Initializes an autodiff matrix given a matrix of values and gradient matrix. More...

template<typename Derived , typename DerivedGradient >
Creates and initializes an autodiff matrix given a matrix of values and gradient matrix. More...

template<typename DerivedGradient , typename DerivedAutoDiff >

template<typename Derived >
std::enable_if< !std::is_same< typename Derived::Scalar, double >::value, Eigen::Matrix< typename Derived::Scalar::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime, 0, Derived::MaxRowsAtCompileTime, Derived::MaxColsAtCompileTime > >::type DiscardZeroGradient (const Eigen::MatrixBase< Derived > &auto_diff_matrix, const typename Eigen::NumTraits< typename Derived::Scalar::Scalar >::Real &precision=Eigen::NumTraits< typename Derived::Scalar::Scalar >::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< typename Derived::Scalar, double >::value, const Eigen::MatrixBase< Derived > & >::type DiscardZeroGradient (const Eigen::MatrixBase< Derived > &matrix, double precision=0.)

template<typename _Scalar , int _Dim, int _Mode, int _Options>
std::enable_if< !std::is_same< _Scalar, double >::value, Eigen::Transform< typename _Scalar::Scalar, _Dim, _Mode, _Options > >::type DiscardZeroGradient (const Eigen::Transform< _Scalar, _Dim, _Mode, _Options > &auto_diff_transform, const typename Eigen::NumTraits< typename _Scalar::Scalar >::Real &precision=Eigen::NumTraits< typename _Scalar::Scalar >::dummy_precision())

template<typename _Scalar , int _Dim, int _Mode, int _Options>
std::enable_if< std::is_same< _Scalar, double >::value, const Eigen::Transform< _Scalar, _Dim, _Mode, _Options > & >::type DiscardZeroGradient (const Eigen::Transform< _Scalar, _Dim, _Mode, _Options > &transform, double precision=0.)

template<typename DerivedX , typename DerivedY , typename DerivedCalcX >
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)

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 RealDiscreteLyapunovEquation (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::MatrixXd > &Q)

template<typename Derived >
std::vector< Eigen::Triplet< typename Derived::Scalar > > SparseMatrixToTriplets (const Derived &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...

void ComposeRR (const double *R_AB, const double *R_BC, double *R_AC)
Composes two drake::math::RotationMatrix<double> objects as quickly as possible. More...

bool IsUsingPortableCompositionMethods ()
Returns true if we are using the portable fallback implementations for the above methods. 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) to Hopf coordinate as if w >= 0 ψ = 2*atan2(x, w) else ψ = 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 >
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 –. 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...

template<class T >
Matrix3< T > ComputeBasisFromAxis (int axis_index, const Vector3< T > &axis_W)
Creates a right-handed local basis from a given axis. More...

Eigen::MatrixXd DecomposePSDmatrixIntoXtransposeTimesX (const Eigen::Ref< const Eigen::MatrixXd > &Y, double zero_tol)
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 RᵀR = Q Rᵀd = b / 2 Notice that this decomposition is not unique. 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 > &q0, const Eigen::Quaternion< Scalar > &q1)
Returns a unit quaternion that represents the same orientation as q1, and has the "shortest" geodesic distance on the unit sphere to q0. 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 >
bool 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 >
bool 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 >
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 >
bool 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 >
bool 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 quaternions 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 >, drake::kQuaternionSize >::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, drake::kRpySize, 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, drake::kQuaternionSize, 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<class T1 , class T2 , class T3 >
T1 saturate (const T1 &value, const T2 &low, const T3 &high)
Saturates the input value between upper and lower bounds. 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...

## ◆ AutoDiffMatrixType

 using AutoDiffMatrixType = Eigen::Matrix< Eigen::AutoDiffScalar >, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime, 0, Derived::MaxRowsAtCompileTime, Derived::MaxColsAtCompileTime>

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

## ◆ 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.

Enumerator
kUniform
kClampedUniform

 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.

## ◆ AreQuaternionsEqualForOrientation()

 bool 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.

 AutoDiffToGradientMatrix::type drake::math::autoDiffToGradientMatrix ( const Eigen::MatrixBase< Derived > & auto_diff_matrix, int num_variables = Eigen::Dynamic )

## ◆ autoDiffToValueMatrix()

 AutoDiffToValueMatrix::type drake::math::autoDiffToValueMatrix ( const Eigen::MatrixBase< Derived > & auto_diff_matrix )

 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.

## ◆ CalculateAngularVelocityExpressedInBFromQuaternionDt()

 Vector3 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.

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. [ẇ, ẋ, ẏ, ż].
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*ẇ + x*ẋ + y*ẏ + z*ż) = 0.

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̇, ẋ, ẏ, ż].
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 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.

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., [ẇ, ẋ, ẏ, ż].

## ◆ CalculateReflectedGrayCodes()

 GrayCodesMatrix::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 drake::math::ClosestQuaternion ( const Eigen::Quaternion< Scalar > & q0, const Eigen::Quaternion< Scalar > & q1 )

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

## ◆ ComposeRR()

 void drake::math::ComposeRR ( const double * R_AB, const double * R_BC, double * R_AC )

Composes two drake::math::RotationMatrix<double> objects as quickly as possible.

Drake RotationMatrix objects are stored as 3x3 column-ordered matrices in nine consecutive doubles.

This method can also be used to form the product of two general 3x3 matrices.

Here we calculate R_AC = R_AB * R_BC. It is OK for R_AC to overlap with one or both inputs.

## ◆ ComputeBasisFromAxis()

 Matrix3 drake::math::ComputeBasisFromAxis ( int axis_index, const Vector3< T > & axis_W )

Creates a right-handed local basis from a given axis.

Defines two other arbitrary axes such that the basis is orthonormal. The basis is R_WL, where W is the frame in which the input axis is expressed and L is a local basis such that v_W = R_WL * v_L.

Parameters
 [in] axis_index The index of the axis (in the range [0,2]), with 0 corresponding to the x-axis, 1 corresponding to the y-axis, and z-corresponding to the z-axis. [in] axis_W The vector defining the basis's given axis expressed in frame W. The vector need not be a unit vector: this routine will normalize it.
Return values
 R_WL The computed basis.
Exceptions
 std::logic_error if the norm of axis_W is within 1e-10 to zero or axis_index does not lie in the range [0,2].

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

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

Parameters
 calc_fun calc_fun(x, &y) computes the value of f(x), and stores the value in y. calc_fun 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_fun. Must be an Eigen column vector. It is possible to have DerivedCalcX being different from DerivedX, for example, calc_fun could be solvers::EvaluatorBase(const Eigen::Ref&, 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(calc_fun, 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);
// 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; };

## ◆ 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::runtime_error if R is not positive definite.

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 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.

 std::pair 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 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 constatnt 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(). The matrix X = [R d] has the same number of rows as the rank of  [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::runtime_error if the precondition is not satisfied.

## ◆ DecomposePSDmatrixIntoXtransposeTimesX()

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

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 values
 X. The matrix X satisfies XᵀX = Y and X.rows() = rank(Y).
Precondition
1. Y is positive semidefinite.
1. zero_tol is non-negative.
Exceptions
 std::runtime_error when the pre-conditions are not satisfied.
Note
We only use the lower triangular part of Y.

 std::enable_if< !std::is_same::value, Eigen::Matrix >::type drake::math::DiscardGradient ( const Eigen::MatrixBase< Derived > & auto_diff_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).

This method is overloaded to permit the user to call it for double types and AutoDiffScalar types (to avoid the calling function having to handle the two cases differently).

 std::enable_if< std::is_same::value, const Eigen::MatrixBase&>::type drake::math::DiscardGradient ( const Eigen::MatrixBase< Derived > & matrix )

 std::enable_if< !std::is_same<_Scalar, double>::value, Eigen::Transform >::type drake::math::DiscardGradient ( const Eigen::Transform< _Scalar, _Dim, _Mode, _Options > & auto_diff_transform )

 std::enable_if::value, const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>&>::type drake::math::DiscardGradient ( const Eigen::Transform< _Scalar, _Dim, _Mode, _Options > & transform )

 std::enable_if< !std::is_same::value, Eigen::Matrix >::type drake::math::DiscardZeroGradient ( const Eigen::MatrixBase< Derived > & auto_diff_matrix, const typename Eigen::NumTraits< typename Derived::Scalar::Scalar >::Real & precision = Eigen::NumTraits::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).

This method is overloaded to permit the user to call it for double types and AutoDiffScalar types (to avoid the calling function having to handle the two cases differently).

Parameters
 precision is passed to Eigen's isZero(precision) to evaluate whether the gradients are zero.
Exceptions
 std::runtime_error if the gradients were not empty nor zero.

 std::enable_if::value, const Eigen::MatrixBase&>::type drake::math::DiscardZeroGradient ( const Eigen::MatrixBase< Derived > & matrix, double precision = 0. )

 std::enable_if< !std::is_same<_Scalar, double>::value, Eigen::Transform >::type drake::math::DiscardZeroGradient ( const Eigen::Transform< _Scalar, _Dim, _Mode, _Options > & auto_diff_transform, const typename Eigen::NumTraits< typename _Scalar::Scalar >::Real & precision = Eigen::NumTraits::dummy_precision() )

 std::enable_if< std::is_same<_Scalar, double>::value, const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>&>::type drake::math::DiscardZeroGradient ( const Eigen::Transform< _Scalar, _Dim, _Mode, _Options > & transform, double precision = 0. )

## ◆ DiscreteAlgebraicRiccatiEquation()

 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)^{-1}B'XA + Q = 0$

Exceptions
 std::runtime_error if Q is not positive semi-definite. std::runtime_error if R is not positive definite.

Based on the Schur Vector approach outlined in this paper: "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation" by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell

## ◆ dquat2rotmat()

 drake::math::Gradient, drake::kQuaternionSize>::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

## ◆ drotmat2quat()

 drake::math::Gradient< Eigen::Matrix, 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, 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)

 Eigen::Matrix 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 )

 GetSubMatrixGradientArray::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 )

 GetSubMatrixGradientSingleElement::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 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/2]

 void drake::math::initializeAutoDiff ( const Eigen::MatrixBase< Derived > & val, Eigen::MatrixBase< DerivedAutoDiff > & auto_diff_matrix, Eigen::DenseIndex num_derivatives = Eigen::Dynamic, Eigen::DenseIndex deriv_num_start = 0 )

Initialize a single autodiff matrix given the corresponding value matrix.

Set the values of auto_diff_matrix to be equal to val, and for each element i of auto_diff_matrix, resize the derivatives vector to num_derivatives, and set derivative number deriv_num_start + i to one (all other elements of the derivative vector set to zero).

Parameters
 [in] mat 'regular' matrix of values [out] ret AutoDiff matrix [in] num_derivatives the size of the derivatives vector Default: the size of mat [in] deriv_num_start starting index into derivative vector (i.e. element deriv_num_start in derivative vector corresponds to mat(0, 0)). Default: 0

## ◆ initializeAutoDiff() [2/2]

 AutoDiffMatrixType drake::math::initializeAutoDiff ( const Eigen::MatrixBase< Derived > & mat, Eigen::DenseIndex num_derivatives = -1, Eigen::DenseIndex deriv_num_start = 0 )

Initialize a single autodiff matrix given the corresponding value matrix.

Create autodiff matrix that matches mat in size with derivatives of compile time size Nq and runtime size num_derivatives. Set its values to be equal to val, and for each element i of auto_diff_matrix, set derivative number deriv_num_start + i to one (all other derivatives set to zero).

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

 void drake::math::initializeAutoDiffGivenGradientMatrix ( const Eigen::MatrixBase< Derived > & val, const Eigen::MatrixBase< DerivedGradient > & gradient, Eigen::MatrixBase< DerivedAutoDiff > & auto_diff_matrix )

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

Parameters
 [in] val value matrix [in] gradient gradient matrix; the derivatives of val(j) are stored in row j of the gradient matrix. [out] autodiff_matrix matrix of AutoDiffScalars with the same size as val

Creates and initializes an autodiff matrix given a matrix of values and gradient matrix.

Parameters
 [in] val value matrix [in] gradient gradient matrix; the derivatives of val(j) are stored in row j of the gradient matrix.
Returns
autodiff_matrix matrix of AutoDiffScalars with the same size as val

## ◆ initializeAutoDiffTuple()

 std::tuple)>...> drake::math::initializeAutoDiffTuple ( const Eigen::MatrixBase< Deriveds > &... args )

Given a series of Eigen matrices, create a tuple of corresponding AutoDiff matrices with values equal to the input matrices and properly initialized derivative vectors.

The size of the derivative vector of each element of the matrices in the output tuple will be the same, and will equal the sum of the number of elements of the matrices in args. If all of the matrices in args have fixed size, then the derivative vectors will also have fixed size (being the sum of the sizes at compile time of all of the input arguments), otherwise the derivative vectors will have dynamic size. The 0th element of the derivative vectors will correspond to the derivative with respect to the 0th element of the first argument. Subsequent derivative vector elements correspond first to subsequent elements of the first input argument (traversed first by row, then by column), and so on for subsequent arguments.

Parameters
 args a series of Eigen matrices
Returns
a tuple of properly initialized AutoDiff matrices corresponding to args

## ◆ intRange()

 std::array drake::math::intRange ( int start )

## ◆ is_quaternion_in_canonical_form()

 bool 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()

 bool 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*ẇ + x*ẋ + y*ẏ + z*ż) = 0. Note: To accurately test whether the time-derivative quaternion constraint is satisfied, the quaternion constraint is also tested to be accurate.

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̇, ẋ, ẏ, ż]. 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 –.

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 quaternions 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*ẇ + x*ẋ + y*ẏ + z*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̇, ẋ, ẏ, ż]. 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()

 bool 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.

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.

## ◆ IsUsingPortableCompositionMethods()

 bool drake::math::IsUsingPortableCompositionMethods ( )

Returns true if we are using the portable fallback implementations for the above methods.

## ◆ 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::type drake::math::matGradMult ( const Eigen::MatrixBase< DerivedDA > & dA, const Eigen::MatrixBase< DerivedB > & B )

 MatGradMultMat::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::runtime_error 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 + α)

GlobalInverseKinematics for an usage of this function.

## ◆ quatConjugate()

 Vector4 drake::math::quatConjugate ( const Eigen::MatrixBase< Derived > & q )

## ◆ quatDiff()

 Vector4 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 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 drake::math::QuaternionToHopfCoordinate ( const Eigen::Quaternion< T > & quaternion )

Convert a unit-length quaternion (w, x, y, z) to Hopf coordinate as if w >= 0 ψ = 2*atan2(x, w) else ψ = 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].

Parameters
 quaternion A unit length quaternion.
Returns
hopf_coordinate (θ, φ, ψ) as an Eigen vector.

## ◆ quatProduct()

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

## ◆ quatRotateVec()

 Vector3 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::runtime_error 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::runtime_error 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 ż = 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::runtime_error 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.

## ◆ 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::runtime_error 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::runtime_error 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::runtime_error 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.

## ◆ resizeDerivativesToMatchScalar()

 void drake::math::resizeDerivativesToMatchScalar ( Eigen::MatrixBase< Derived > & mat, const typename Derived::Scalar & scalar )

Resize derivatives vector of each element of a matrix to to match the size of the derivatives vector of a given scalar.

If the mat and scalar inputs are AutoDiffScalars, resize the derivatives vector of each element of the matrix mat to match the number of derivatives of the scalar. This is useful in functions that return matrices that do not depend on an AutoDiffScalar argument (e.g. a function with a constant output), while it is desired that information about the number of derivatives is preserved.

Parameters
 mat matrix, for which the derivative vectors of the elements will be resized scalar scalar to match the derivative size vector against.

## ◆ saturate()

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

Saturates the input value between upper and lower bounds.

If value is within [low, high] then return it; else return the boundary.

 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 )

 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 )

 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 )

## ◆ 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 > drake::math::SparseMatrixToTriplets ( const Derived & 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

## ◆ ToSymmetricMatrixFromLowerTriangularColumns() [1/2]

 drake::MatrixX 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 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.

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

## ◆ UniformlyRandomAngleAxis()

 Eigen::AngleAxis 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 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 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 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::runtime_error if num_points < 1

## ◆ VectorToSkewSymmetric()

 drake::Matrix3 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.