Drake
drake::math Namespace Reference

Namespaces

 internal
 
 quaternion_test
 
 test
 

Classes

struct  AutoDiffToGradientMatrix
 
struct  AutoDiffToValueMatrix
 
struct  GetSubMatrixGradientArray
 
struct  GetSubMatrixGradientSingleElement
 
struct  Gradient
 
struct  Gradient< Derived, Nq, 1 >
 
struct  GrayCodesMatrix
 GrayCodesMatrix::type returns an Eigen matrix of integers. More...
 
struct  GrayCodesMatrix< Eigen::Dynamic >
 
struct  MatGradMult
 
struct  MatGradMultMat
 

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

Functions

template<typename Derived >
AutoDiffToValueMatrix< Derived >::type autoDiffToValueMatrix (const Eigen::MatrixBase< Derived > &auto_diff_matrix)
 
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 >
AutoDiffToGradientMatrix< Derived >::type autoDiffToGradientMatrix (const Eigen::MatrixBase< Derived > &auto_diff_matrix, int num_variables=Eigen::Dynamic)
 
template<typename Derived , typename DerivedGradient , typename DerivedAutoDiff >
void 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. More...
 
template<typename Derived , typename DerivedGradient >
AutoDiffMatrixType< Derived, DerivedGradient::ColsAtCompileTime > initializeAutoDiffGivenGradientMatrix (const Eigen::MatrixBase< Derived > &val, const Eigen::MatrixBase< DerivedGradient > &gradient)
 Creates and initializes an autodiff matrix given a matrix of values and gradient matrix. More...
 
template<typename DerivedGradient , typename DerivedAutoDiff >
void gradientMatrixToAutoDiff (const Eigen::MatrixBase< DerivedGradient > &gradient, Eigen::MatrixBase< DerivedAutoDiff > &auto_diff_matrix)
 
template<typename Derived >
Eigen::AngleAxis< typename Derived::Scalar > axisToEigenAngleAxis (const Eigen::MatrixBase< Derived > &axis_angle)
 Converts Drake's axis-angle representation to Eigen's AngleAxis object. More...
 
template<typename Derived >
Vector4< typename Derived::Scalar > axis2quat (const Eigen::MatrixBase< Derived > &axis_angle)
 Converts Drake's axis-angle representation to quaternion representation. More...
 
template<typename Derived >
Matrix3< typename Derived::Scalar > axis2rotmat (const Eigen::MatrixBase< Derived > &axis_angle)
 Converts Drake's axis-angle representation to rotation matrix. More...
 
template<typename Derived >
Vector3< typename Derived::Scalar > axis2rpy (const Eigen::MatrixBase< Derived > &axis_angle)
 Converts Drake's axis-angle representation to body fixed z-y'-x'' Euler angles, or equivalently space fixed x-y-z Euler angles. 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 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 X to the continuous-time algebraic Riccati equation: More...
 
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)
 DiscreteAlgebraicRiccatiEquation function 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 \]

. More...

 
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...
 
template<typename Derived >
Eigen::Matrix< typename Derived::Scalar, 4, 1 > expmap2quat (const Eigen::MatrixBase< Derived > &v)
 
template<typename DerivedQ >
Eigen::Matrix< typename DerivedQ::Scalar, 3, 1 > quat2expmap (const Eigen::MatrixBase< DerivedQ > &q)
 
template<typename Derived1 , typename Derived2 >
Eigen::Matrix< typename Derived1::Scalar, 3, 1 > closestExpmap (const Eigen::MatrixBase< Derived1 > &expmap1, const Eigen::MatrixBase< Derived2 > &expmap2)
 
template<typename DerivedQ , typename DerivedE >
void quat2expmapSequence (const Eigen::MatrixBase< DerivedQ > &quat, const Eigen::MatrixBase< DerivedQ > &quat_dot, Eigen::MatrixBase< DerivedE > &expmap, Eigen::MatrixBase< DerivedE > &expmap_dot)
 
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)
 
int GrayCodeToInteger (const Eigen::Ref< const Eigen::VectorXi > &gray_code)
 Converts the Gray code to an integer. More...
 
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...
 
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 >
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<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 Derived >
Derived::Scalar quatNorm (const Eigen::MatrixBase< Derived > &q)
 
template<typename Derived1 , typename Derived2 , typename Scalar >
Vector4< Scalar > Slerp (const Eigen::MatrixBase< Derived1 > &q1, const Eigen::MatrixBase< Derived2 > &q2, const Scalar &interpolation_parameter)
 Q = Slerp(q1, q2, f) Spherical linear interpolation between two quaternions This function uses the implementation given in Algorithm 8 of [1]. More...
 
template<typename Scalar >
Eigen::AngleAxis< Scalar > QuaternionToAngleAxis (const Eigen::Quaternion< Scalar > &quaternion)
 Computes angle-axis orientation from a given quaternion. More...
 
template<typename Derived >
Vector4< typename Derived::Scalar > quat2axis (const Eigen::MatrixBase< Derived > &quaternion)
 (Deprecated) Computes axis-angle orientation from a given quaternion. More...
 
template<typename Derived >
Matrix3< typename Derived::Scalar > quat2rotmat (const Eigen::MatrixBase< Derived > &quaternion)
 Computes the rotation matrix from quaternion representation. More...
 
template<typename Derived >
Vector3< typename Derived::Scalar > QuaternionToSpaceXYZ (const Eigen::MatrixBase< Derived > &quaternion)
 Computes SpaceXYZ Euler angles from quaternion representation. More...
 
template<typename Derived >
Vector3< typename Derived::Scalar > quat2rpy (const Eigen::MatrixBase< Derived > &quaternion)
 (Deprecated) Computes SpaceXYZ Euler angles from quaternion. More...
 
template<typename Derived >
Eigen::Quaternion< typename Derived::Scalar > quat2eigenQuaternion (const Eigen::MatrixBase< Derived > &q)
 
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<class Generator >
Eigen::Vector4d UniformlyRandomAxisAngle (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<class Generator >
Eigen::Vector4d UniformlyRandomQuat (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<class Generator >
Eigen::Matrix3d UniformlyRandomRotmat (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<class Generator >
Eigen::Vector3d 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 >
Vector4< typename Derived::Scalar > rpy2quat (const Eigen::MatrixBase< Derived > &rpy)
 Computes the quaternion representation from Euler angles. More...
 
template<typename Derived >
Matrix3< typename Derived::Scalar > rpy2rotmat (const Eigen::MatrixBase< Derived > &rpy)
 We use an extrinsic rotation about Space-fixed x-y-z axes by angles [rpy(0), rpy(1), rpy(2)]. More...
 
template<typename Derived >
Eigen::Matrix< typename Derived::Scalar, 9, 3 > drpy2rotmat (const Eigen::MatrixBase< Derived > &rpy)
 
template<typename Derived >
Vector4< typename Derived::Scalar > rpy2axis (const Eigen::MatrixBase< Derived > &rpy)
 Computes angle-axis representation from Euler angles. More...
 
template<typename Derived >
Quaternion< typename Derived::Scalar > RollPitchYawToQuaternion (const Eigen::MatrixBase< Derived > &rpy)
 Computes the Quaternion representation of a rotation given the set of Euler angles describing this rotation. 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...
 
template<typename Derived >
Vector4< typename Derived::Scalar > rotmat2quat (const Eigen::MatrixBase< Derived > &M)
 Computes one of the quaternion from a rotation matrix. More...
 
template<typename Derived >
Vector4< typename Derived::Scalar > rotmat2axis (const Eigen::MatrixBase< Derived > &R)
 Computes the angle axis representation from a rotation matrix. More...
 
template<typename Derived >
Vector3< typename Derived::Scalar > rotmat2rpy (const Eigen::MatrixBase< Derived > &R)
 Computes SpaceXYZ Euler angles from rotation matrix. More...
 
template<typename Derived >
VectorX< typename Derived::Scalar > rotmat2Representation (const Eigen::MatrixBase< Derived > &R, int rotation_type)
 
template<typename T >
Matrix3< T > XRotation (const T &theta)
 Computes the rotation matrix for rotating by theta (radians) around the positive X axis. More...
 
template<typename T >
Matrix3< T > YRotation (const T &theta)
 Computes the rotation matrix for rotating by theta (radians) around the positive Y axis. More...
 
template<typename T >
Matrix3< T > ZRotation (const T &theta)
 Computes the rotation matrix for rotating by theta (radians) around the positive Z axis. More...
 
template<typename Derived >
Matrix3< typename Derived::Scalar > ProjectMatToRotMat (const Eigen::MatrixBase< Derived > &M)
 Projects a full-rank 3x3 matrix M onto O(3), defined as. More...
 
template<typename Derived >
double ProjectMatToRotMatWithAxis (const Eigen::MatrixBase< Derived > &M, const Eigen::Ref< const Eigen::Vector3d > &axis, double angle_lb, double angle_ub)
 Projects a 3 x 3 matrix M onto SO(3). 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...
 

Typedef Documentation

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.

Function Documentation

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
quat1Quaternion [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.
quat2Quaternion with a description analogous to quat1.
toleranceNonnegative 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.

Here is the call graph for this function:

Here is the caller graph for this function:

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

Here is the call graph for this function:

Here is the caller graph for this function:

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

Here is the caller graph for this function:

Vector4<typename Derived::Scalar> drake::math::axis2quat ( const Eigen::MatrixBase< Derived > &  axis_angle)

Converts Drake's axis-angle representation to quaternion representation.

Parameters
axis_angle.A 4 x 1 column vector [axis; angle]. axis is the unit length rotation axis, angle is within [-PI, PI].
Returns
A 4 x 1 column vector, the unit length quaternion [w; x; y; z].

Here is the call graph for this function:

Here is the caller graph for this function:

Matrix3<typename Derived::Scalar> drake::math::axis2rotmat ( const Eigen::MatrixBase< Derived > &  axis_angle)

Converts Drake's axis-angle representation to rotation matrix.

Parameters
axis_angle.A 4 x 1 column vector [axis; angle]. axis is the unit length rotation axis, angle is within [-PI, PI].
Returns
A 3 x 3 rotation matrix.

Here is the call graph for this function:

Here is the caller graph for this function:

Vector3<typename Derived::Scalar> drake::math::axis2rpy ( const Eigen::MatrixBase< Derived > &  axis_angle)

Converts Drake's axis-angle representation to body fixed z-y'-x'' Euler angles, or equivalently space fixed x-y-z Euler angles.

Parameters
axis_angle.A 4 x 1 column vector [axis; angle]. axis is the unit length rotation axis, angle is within [-PI, PI]
Returns
A 3 x 1 vector [roll, pitch, yaw]. Represents the body-fixed z-y'-x'' rotation with (yaw, pitch, roll) angles respectively.
See also
rpy2rotmat

Here is the call graph for this function:

Here is the caller graph for this function:

Eigen::AngleAxis<typename Derived::Scalar> drake::math::axisToEigenAngleAxis ( const Eigen::MatrixBase< Derived > &  axis_angle)

Converts Drake's axis-angle representation to Eigen's AngleAxis object.

Parameters
axis_angleA 4 x 1 column vector [axis; angle]. axis is the unit length rotation axis. angle is within [-PI, PI].
Returns
An Eigen::AngleAxis object.
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.

Parameters
quat_ABQuaternion [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.
quatDtTime-derivative of quat_AB, i.e. [ẇ, ẋ, ẏ, ż].
Return values
w_AB_BB's angular velocity in A, expressed in B.

Here is the caller graph for this function:

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.

Parameters
quatQuaternion [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.
quatDtTime-derivative of quat, i.e., [ẇ, ẋ, ẏ, ż].
Return values
quaternionDt_constraint_violationThe amount the time- derivative of the quaternion constraint has been violated, which may be positive or negative (0 means the constraint is perfectly satisfied).

Here is the caller graph for this function:

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.

Parameters
quat_ABQuaternion [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_BB's angular velocity in A, expressed in B.
Return values
quatDtTime-derivative of quat_AB, i.e., [ẇ, ẋ, ẏ, ż].

Here is the caller graph for this function:

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
NumDigitsThe number of digits in the Gray code.
Parameters
num_digitsThe 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.

Here is the call graph for this function:

Here is the caller graph for this function:

Eigen::Matrix<typename Derived1::Scalar, 3, 1> drake::math::closestExpmap ( const Eigen::MatrixBase< Derived1 > &  expmap1,
const Eigen::MatrixBase< Derived2 > &  expmap2 
)

Here is the call graph for this function:

Here is the caller graph for this function:

Eigen::Quaternion<Scalar> 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.

Here is the caller graph for this function:

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

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

Here is the call graph for this function:

Here is the caller graph for this function:

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 X to the continuous-time algebraic Riccati equation:

///  S'A + A'S - S B inv(R) B' S + Q = 0
/// 
@throws 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

Here is the call graph for this function:

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.

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 
)

DiscreteAlgebraicRiccatiEquation function 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 \]

.

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

Exceptions
std::runtime_errorif Q is not positive semi-definite.
std::runtime_errorif 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, in TAC, 1980, http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=1102434

Note: When, for example, n = 100, m = 80, and entries of A, B, Q_half, R_half are sampled from standard normal distributions, where Q = Q_half'*Q_half and similar for R, the absolute error of the solution is 10^{-6}, while the absolute error of the solution computed by Matlab is 10^{-8}.

TODO(weiqiao.han): I may overwrite the RealQZ function to improve the accuracy, together with more thorough tests.

\[ A'XA - X - A'XB(B'XB+R)^{-1}B'XA + Q = 0 \]

Exceptions
std::runtime_errorif Q is not positive semi-definite.
std::runtime_errorif 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

Here is the call graph for this function:

drake::math::Gradient<Eigen::Matrix<typename Derived::Scalar, 3, 3>, 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
quaternionA unit length quaternion [w;x;y;z]
Returns
The gradient

Here is the call graph for this function:

Here is the caller graph for this function:

drake::math::Gradient< Eigen::Matrix<typename DerivedR::Scalar, drake::kQuaternionSize, 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
RA 3 x 3 rotation matrix
dRA 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)

Here is the call graph for this function:

Here is the caller graph for this function:

drake::math::Gradient< Eigen::Matrix<typename DerivedR::Scalar, drake::kRpySize, 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
RA 3 x 3 rotation matrix
dRA 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)

Here is the call graph for this function:

Here is the caller graph for this function:

Eigen::Matrix<typename Derived::Scalar, 9, 3> drake::math::drpy2rotmat ( const Eigen::MatrixBase< Derived > &  rpy)

Here is the caller graph for this function:

Eigen::Matrix<typename Derived::Scalar, 4, 1> drake::math::expmap2quat ( const Eigen::MatrixBase< Derived > &  v)

Here is the call graph for this function:

Here is the caller graph for this function:

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 
)
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 
)
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 
)
void drake::math::gradientMatrixToAutoDiff ( const Eigen::MatrixBase< DerivedGradient > &  gradient,
Eigen::MatrixBase< DerivedAutoDiff > &  auto_diff_matrix 
)

Here is the caller graph for this function:

int 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_codeThe N-digit Gray code, where N is gray_code.rows()
Returns
The integer represented by the Gray code gray_code.

Here is the caller graph for this function:

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
ffunction
xfunction argument value at which Hessian will be evaluated
Returns
AutoDiffScalar matrix corresponding to the Hessian of f evaluated at x
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]retAutoDiff matrix
[in]num_derivativesthe size of the derivatives vector
Default: the size of mat
[in]deriv_num_startstarting index into derivative vector (i.e. element deriv_num_start in derivative vector corresponds to mat(0, 0)).
Default: 0

Here is the caller graph for this function:

AutoDiffMatrixType<Derived, Nq> 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_derivativesthe size of the derivatives vector
Default: the size of mat
[in]deriv_num_startstarting index into derivative vector (i.e. element deriv_num_start in derivative vector corresponds to mat(0, 0)).
Default: 0
Returns
AutoDiff matrix

Here is the call graph for this function:

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]valvalue matrix
[in]gradientgradient matrix; the derivatives of val(j) are stored in row j of the gradient matrix.
[out]autodiff_matrixmatrix of AutoDiffScalars with the same size as val

Here is the caller graph for this function:

AutoDiffMatrixType<Derived, DerivedGradient::ColsAtCompileTime> drake::math::initializeAutoDiffGivenGradientMatrix ( const Eigen::MatrixBase< Derived > &  val,
const Eigen::MatrixBase< DerivedGradient > &  gradient 
)

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

Parameters
[in]valvalue matrix
[in]gradientgradient 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

Here is the call graph for this function:

std::tuple<AutoDiffMatrixType< Deriveds, internal::totalSizeAtCompileTime<Deriveds...>)>...> 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
argsa series of Eigen matrices
Returns
a tuple of properly initialized AutoDiff matrices corresponding to args

Here is the call graph for this function:

Here is the caller graph for this function:

std::array<int, Size> drake::math::intRange ( int  start)
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
quatQuaternion [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.

Here is the caller graph for this function:

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*ẇ + 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.

Parameters
quatQuaternion [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.
quatDtTime-derivative of quat, i.e., [ẇ, ẋ, ẏ, ż].
toleranceTolerance for quaternion constraints.
Returns
true if both of the two previous constraints are within tolerance.

Here is the call graph for this function:

Here is the caller graph for this function:

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] satisifies 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
quatQuaternion [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.
quatDtTime-derivative of quat, i.e., [ẇ, ẋ, ẏ, ż].
w_BRigid body B's angular velocity in frame A, expressed in B.
toleranceTolerance for quaternion constraints.
Returns
true if all three of the previous constraints are within tolerance.

Here is the call graph for this function:

Here is the caller graph for this function:

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
quatQuaternion [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.
toleranceTolerance 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.

Here is the call graph for this function:

Here is the caller graph for this function:

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.

Here is the caller graph for this function:

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.

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
ffunction
xfunction argument value at which Jacobian will be evaluated
Returns
AutoDiffScalar matrix corresponding to the Jacobian of f evaluated at x.

Here is the call graph for this function:

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

Here is the caller graph for this function:

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 
)

Here is the caller graph for this function:

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]xAn N x 1 vector to be normalized. Must not be zero.
[out]x_normThe normalized vector (N x 1).
[out]dx_normIf non-null, returned as an N x N matrix, where dx_norm(i,j) = D x_norm(i)/D x(j).
[out]ddx_normIf 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.)

Here is the call graph for this function:

Here is the caller graph for this function:

Matrix3<typename Derived::Scalar> drake::math::ProjectMatToRotMat ( const Eigen::MatrixBase< Derived > &  M)

Projects a full-rank 3x3 matrix M onto O(3), defined as.

  min_R  ,j | R(i,j) - M(i,j) |^2
 subject to   R*R^T = I.

The algorithm (just SVD) can be derived as a small modification of section 3.2 in http://haralick.org/conferences/pose_estimation.pdf .

Note that it does not enforce det(R)=1; you could get det(R)=-1 if that solution is closer to the matrix M using the norm above.

Here is the caller graph for this function:

double drake::math::ProjectMatToRotMatWithAxis ( const Eigen::MatrixBase< Derived > &  M,
const Eigen::Ref< const Eigen::Vector3d > &  axis,
double  angle_lb,
double  angle_ub 
)

Projects a 3 x 3 matrix M onto SO(3).

The projected rotation matrix R has a given rotation axis a, and its rotation angle θ is bounded as angle_lb <= θ <= angle_ub. One use case for this function is to reconstruct the rotation matrix for a revolute joint with joint limits.

See also
GlobalInverseKinematics for an usage of this function. We can formulate this 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 the rotation axis a.
  A = [ 0  -a₃  a₂]
      [ a₃  0  -a₁]
      [-a₂  a₁  0 ]
Equation (1) is the Rodriguez Formula, to compute the rotation matrix from the rotation axis a and the rotation angle θ. For more details, refer to 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 axis-angle 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 ∈ ℤ
else
θ = angle_lb if sin(angle_lb + α) >= sin(angle_ub + α)
θ = angle_ub if sin(angle_lb + α) < sin(angle_ub + α)
Template Parameters
DerivedA 3 x 3 matrix
Parameters
MThe matrix to be projected.
axisThe axis of the rotation matrix. A unit length vector.
angle_lbThe lower bound of the rotation angle.
angle_ubThe upper bound of the rotation angle.
Returns
The rotation angle of the projected matrix.
Precondition
angle_ub >= angle_lb. Throw std::runtime_error if these bounds are violated.

Here is the call graph for this function:

Here is the caller graph for this function:

Vector4<typename Derived::Scalar> drake::math::quat2axis ( const Eigen::MatrixBase< Derived > &  quaternion)

(Deprecated) Computes axis-angle orientation from a given quaternion.

Template Parameters
DerivedAn Eigen derived type, e.g., an Eigen Vector3d.
Parameters
quaternion4 x 1 vector that may or may not be normalized.
Returns
axis-angle [x; y; z; angle] of quaternion with axis as a unit vector and 0 <= angle <= PI, Return is independent of quaternion normalization. (Deprecated) Use QuaternionToAngleAxis() instead.
See also
QuaternionToAngleAxis()

Here is the call graph for this function:

Here is the caller graph for this function:

Eigen::Quaternion<typename Derived::Scalar> drake::math::quat2eigenQuaternion ( const Eigen::MatrixBase< Derived > &  q)

Here is the caller graph for this function:

Eigen::Matrix<typename DerivedQ::Scalar, 3, 1> drake::math::quat2expmap ( const Eigen::MatrixBase< DerivedQ > &  q)

Here is the call graph for this function:

Here is the caller graph for this function:

void drake::math::quat2expmapSequence ( const Eigen::MatrixBase< DerivedQ > &  quat,
const Eigen::MatrixBase< DerivedQ > &  quat_dot,
Eigen::MatrixBase< DerivedE > &  expmap,
Eigen::MatrixBase< DerivedE > &  expmap_dot 
)

Here is the call graph for this function:

Matrix3<typename Derived::Scalar> drake::math::quat2rotmat ( const Eigen::MatrixBase< Derived > &  quaternion)

Computes the rotation matrix from quaternion representation.

Template Parameters
DerivedAn Eigen derived type, e.g., an Eigen Vector3d.
Parameters
quaternion4 x 1 unit length quaternion, q=[w;x;y;z]
Returns
3 x 3 rotation matrix

Here is the caller graph for this function:

Vector3<typename Derived::Scalar> drake::math::quat2rpy ( const Eigen::MatrixBase< Derived > &  quaternion)

(Deprecated) Computes SpaceXYZ Euler angles from quaternion.

Use QuaternionToSpaceXYZ() instead.

See also
QuaternionToSpaceXYZ()

Here is the call graph for this function:

Here is the caller graph for this function:

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

Here is the caller graph for this function:

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

Here is the call graph for this function:

Here is the caller graph for this function:

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

Here is the call graph for this function:

Here is the caller graph for this function:

Eigen::AngleAxis<Scalar> drake::math::QuaternionToAngleAxis ( const Eigen::Quaternion< Scalar > &  quaternion)

Computes angle-axis orientation from a given quaternion.

Template Parameters
ScalarThe element type which must be a valid Eigen scalar.
Parameters
quaternion4 x 1 non-zero vector that does not have to be normalized.
Returns
Angle-axis representation of quaternion with 0 <= angle <= PI. and axis as a unit vector. Return is independent of quaternion normalization.

Here is the call graph for this function:

Here is the caller graph for this function:

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

Here is the call graph for this function:

Here is the caller graph for this function:

Vector3<typename Derived::Scalar> drake::math::QuaternionToSpaceXYZ ( const Eigen::MatrixBase< Derived > &  quaternion)

Computes SpaceXYZ Euler angles from quaternion representation.

Template Parameters
DerivedAn Eigen derived type, e.g., an Eigen Vector3d.
Parameters
quaternion4x1 unit length vector with elements [ e0, e1, e2, e3 ].
Returns
3x1 SpaceXYZ Euler angles (called roll-pitch-yaw by ROS).

This accurate algorithm avoids numerical round-off issues encountered by some algorithms when pitch angle is within 1E-6 of PI/2 or -PI/2.

Note: SpaceXYZ roll-pitch-yaw is equivalent to BodyZYX yaw-pitch-roll. http://answers.ros.org/question/58863/incorrect-rollpitch-yaw-values-using-getrpy/

Theory

This algorithm was created October 2016 by Paul Mitiguy for TRI (Toyota). We believe this is a new algorithm (not previously published). Some of the theory/formulation of this algorithm are provided below.

Notation: Angles q1, q2, q3 designate SpaceXYZ "roll, pitch, yaw" angles.
          Symbols e0, e1, e2, e3 are elements of the passed-in quaternion.
          e0 = cos(theta/2), e1 = L1*sin(theta/2), e2 = L2*sin(theta/2), ...
Step 1.  Convert the quaternion to a 3x3 rotation matrix R.
         This is done solely to provide an accurate computation of pitch-
         angle q2, which is calculated with the atan2 function and only 5
         elements of what is interpretated as a SpaceXYZ rotation matrix.
         Since only 5 elements of R are used, perhaps the algorithm could
         be improved by only calculating those 5 elements -- or manipulating
         those 5 elements to reduce calculations involving e0, e1, e2, e3.
Step 2.  Realize the quaternion passed to the function can be regarded as
         resulting from multiplication of certain 4x4 and 4x1 matrices, or
         multiplying three rotation quaternions (Hamilton product), to give:
         e0 = sin(q1/2)*sin(q2/2)*sin(q3/2) + cos(q1/2)*cos(q2/2)*cos(q3/2)
         e1 = sin(q3/2)*cos(q1/2)*cos(q2/2) - sin(q1/2)*sin(q2/2)*cos(q3/2)
         e2 = sin(q1/2)*sin(q3/2)*cos(q2/2) + sin(q2/2)*cos(q1/2)*cos(q3/2)
         e3 = sin(q1/2)*cos(q2/2)*cos(q3/2) - sin(q2/2)*sin(q3/2)*cos(q1/2)
         Reference for step 2:
https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
Step 3.  Since q2 has already been calculated (in Step 1), substitute
         cos(q2/2) = A and sin(q2/2) = f*A.
         Note: The final results are independent of A and f = tan(q2/2).
         Note: -pi/2 <= q2 <= pi/2  so -0.707 <= [A = cos(q2/2)] <= 0.707...
         and  -1 <= [f = tan(q2/2)] <= 1.
Step 4.  Referring to Step 2 form: (1+f)*e1 + (1+f)*e3 and rearrange to:
         sin(q1/2+q3/2) = (e1+e3)/(A*(1-f))
         Referring to Step 2 form: (1+f)*e0 - (1+f)*e2 and rearrange to:
         cos(q1/2+q3/2) = (e0-e2)/(A*(1-f))
         Combine the two previous results to produce:
         1/2*( q1 + q3 ) = atan2( e1+e3, e0-e2 )
Step 5.  Referring to Step 2 form: (1-f)*e1 - (1-f)*e3 and rearrange to:
         sin(q1/5-q3/5) = -(e1-e3)/(A*(1+f))
         Referring to Step 2 form: (1-f)*e0 + (1-f)*e2 and rearrange to:
         cos(q1/2-q3/2) = (e0+e2)/(A*(1+f))
         Combine the two previous results to produce:
         1/2*( q1 - q3 ) = atan2( e3-e1, e0+e2 )
Step 6.  Combine Steps 4 and 5 and solve the linear equations for q1, q3.
         Use zA, zB to handle case in which both atan2 arguments are 0.
         zA = (e1+e3==0  &&  e0-e2==0) ? 0 : atan2( e1+e3, e0-e2 );
         zB = (e3-e1==0  &&  e0+e2==0) ? 0 : atan2( e3-e1, e0+e2 );
         Solve: 1/2*( q1 + q3 ) = zA     To produce:  q1 = zA + zB
                1/2*( q1 - q3 ) = zB                  q3 = zA - zB
Step 7.  As necessary, modify angles by 2*PI to return angles in range:
         -pi   <= q1 <= pi
         -pi/2 <= q2 <= pi/2
         -pi   <= q3 <= pi
Textbook reference: Mitiguy, Paul, Advanced Dynamics and Motion Simulation,
                    For professional engineers and scientists (2017).
                    Section 8.2, Euler rotation angles, pg 60.
                    Available at www.MotionGenesis.com
Author
Paul Mitiguy

Here is the call graph for this function:

Here is the caller graph for this function:

Derived::Scalar drake::math::quatNorm ( const Eigen::MatrixBase< Derived > &  q)

Here is the call graph for this function:

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

Here is the caller graph for this function:

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

Here is the call graph for this function:

Here is the caller graph for this function:

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
matmatrix, for which the derivative vectors of the elements will be resized
scalarscalar to match the derivative size vector against.

Here is the caller graph for this function:

Quaternion<typename Derived::Scalar> drake::math::RollPitchYawToQuaternion ( const Eigen::MatrixBase< Derived > &  rpy)

Computes the Quaternion representation of a rotation given the set of Euler angles describing this rotation.

These angles follow the Tait–Bryan formalism about body-fixed z-y'-x'' axes. This convention is equivalent to a space-fixed x-y-z sequence.

Parameters
rpyA vector conveniently packing the Euler angles as rpy = [roll, pitch, yaw]. These are defined such that they represent the rotations about the body-fixed z-y'-x'' axes.
Returns
A Quaternion representing the same rotation given by the input Euler angles rpy.

Here is the call graph for this function:

Here is the caller graph for this function:

Vector4<typename Derived::Scalar> drake::math::rotmat2axis ( const Eigen::MatrixBase< Derived > &  R)

Computes the angle axis representation from a rotation matrix.

Template Parameters
DerivedAn Eigen derived type, e.g., an Eigen Vector3d.
Parameters
Rthe 3 x 3 rotation matrix.
Returns
angle-axis representation, 4 x 1 vector as [x, y, z, angle]. [x, y, z] is a unit vector and 0 <= angle <= PI.

Here is the caller graph for this function:

Vector4<typename Derived::Scalar> drake::math::rotmat2quat ( const Eigen::MatrixBase< Derived > &  M)

Computes one of the quaternion from a rotation matrix.

This implementation is adapted from simbody https://github.com/simbody/simbody/blob/master/SimTKcommon/Mechanics/src/Rotation.cpp Notice that there are two quaternions corresponding to the same rotation, namely q and -q represent the same rotation.

Parameters
MA 3 x 3 rotation matrix.
Returns
a 4 x 1 unit length vector, the quaternion corresponding to the rotation matrix.

Here is the caller graph for this function:

VectorX<typename Derived::Scalar> drake::math::rotmat2Representation ( const Eigen::MatrixBase< Derived > &  R,
int  rotation_type 
)

Here is the call graph for this function:

Vector3<typename Derived::Scalar> drake::math::rotmat2rpy ( const Eigen::MatrixBase< Derived > &  R)

Computes SpaceXYZ Euler angles from rotation matrix.

Template Parameters
DerivedAn Eigen derived type, e.g., an Eigen Vector3d.
Parameters
R3x3 rotation matrix.
Returns
3x1 SpaceXYZ Euler angles (called roll-pitch-yaw by ROS). Note: SpaceXYZ roll-pitch-yaw is equivalent to BodyZYX yaw-pitch-roll. http://answers.ros.org/question/58863/incorrect-rollpitch-yaw-values-using-getrpy/
See also
rpy2rotmat

Here is the call graph for this function:

Here is the caller graph for this function:

Vector4<typename Derived::Scalar> drake::math::rpy2axis ( const Eigen::MatrixBase< Derived > &  rpy)

Computes angle-axis representation from Euler angles.

Parameters
rpyA 3 x 1 vector. The Euler angles about Body-fixed z-y'-x'' axes by angles [rpy(2), rpy(1), rpy(0)].
Returns
A 4 x 1 angle-axis representation a, with a.head<3>() being the rotation axis, a(3) being the rotation angle
See also
rpy2rotmat

Here is the call graph for this function:

Vector4<typename Derived::Scalar> drake::math::rpy2quat ( const Eigen::MatrixBase< Derived > &  rpy)

Computes the quaternion representation from Euler angles.

Parameters
rpy3 x 1 vector with SpaceXYZ Euler angles.
Returns
4 x 1 unit length quaternion quaternion = [w; x; y; z].
See also
rpy2rotmat

Here is the caller graph for this function:

Matrix3<typename Derived::Scalar> drake::math::rpy2rotmat ( const Eigen::MatrixBase< Derived > &  rpy)

We use an extrinsic rotation about Space-fixed x-y-z axes by angles [rpy(0), rpy(1), rpy(2)].

Or equivalently, we use an intrinsic rotation about Body-fixed z-y'-x'' axes by angles [rpy(2), rpy(1), rpy(0)]. The rotation matrix returned is equivalent to rotz(rpy(2)) * roty(rpy(1)) * rotx(rpy(0)), where

\[ rotz(a) = \begin{bmatrix} cos(a)& -sin(a) & 0\\ sin(a) & cos(a) & 0\\ 0 & 0 & 1 \end{bmatrix}\;, roty(b) = \begin{bmatrix} cos(b) & 0 & sin(b)\\ 0 & 1 & 0 \\ -sin(b) & 0 & cos(b)\end{bmatrix}\;, rotx(c) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(c) & -sin(c)\\ 0 & sin(c) & cos(c)\end{bmatrix} \]

Here is the caller graph for this function:

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.

Here is the call graph for this function:

Here is the caller graph for this function:

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 
)
Vector4<Scalar> drake::math::Slerp ( const Eigen::MatrixBase< Derived1 > &  q1,
const Eigen::MatrixBase< Derived2 > &  q2,
const Scalar &  interpolation_parameter 
)

Q = Slerp(q1, q2, f) Spherical linear interpolation between two quaternions This function uses the implementation given in Algorithm 8 of [1].

Parameters
q1Initial quaternion (w, x, y, z)
q2Final quaternion (w, x, y, z)
interpolation_parameterbetween 0 and 1 (inclusive)
Return values
QInterpolated quaternion(s). 4-by-1 vector.

[1] Kuffner, J.J., "Effective sampling and distance metrics for 3D rigid body path planning," Robotics and Automation, 2004. Proceedings. ICRA '04. 2004 IEEE International Conference on , vol.4, no., pp.3993, 3998 Vol.4, April 26-May 1, 2004 doi: 10.1109/ROBOT.2004.1308895

Here is the call graph for this function:

Here is the caller graph for this function:

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]matrixthe input sparse matrix
[out]row_indicesa vector containing the row indices of the non-zero entries
[out]col_indicesa vector containing the column indices of the non-zero entries
[out]vala vector containing the values of the non-zero entries.
std::vector<Eigen::Triplet<typename Derived::Scalar> > 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
matrixA 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
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.

Here is the call graph for this function:

Here is the caller graph for this function:

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
rowsThe number of rows in the symmetric matrix.

Here is the call graph for this function:

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

Here is the caller graph for this function:

Eigen::Vector4d drake::math::UniformlyRandomAxisAngle ( 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.

Justification for the algorithm can be found in, e.g.: Mervin E. Muller. 1959. A note on a method for generating points uniformly on n-dimensional spheres. Commun. ACM 2, 4 (April 1959), 19-20. DOI=http://dx.doi.org/10.1145/377939.377946

Here is the caller graph for this function:

Eigen::Vector4d drake::math::UniformlyRandomQuat ( 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.

Here is the call graph for this function:

Here is the caller graph for this function:

Eigen::Matrix3d drake::math::UniformlyRandomRotmat ( 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.

Here is the call graph for this function:

Eigen::Vector3d 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.

Here is the call graph for this function:

Here is the caller graph for this function:

Eigen::Matrix3Xd 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_pointsThe 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

Here is the call graph for this function:

drake::Matrix3<typename Derived::Scalar> drake::math::VectorToSkewSymmetric ( const Eigen::MatrixBase< Derived > &  p)

Here is the caller graph for this function:

Matrix3<T> drake::math::XRotation ( const T &  theta)

Computes the rotation matrix for rotating by theta (radians) around the positive X axis.

Here is the call graph for this function:

Matrix3<T> drake::math::YRotation ( const T &  theta)

Computes the rotation matrix for rotating by theta (radians) around the positive Y axis.

Here is the call graph for this function:

Matrix3<T> drake::math::ZRotation ( const T &  theta)

Computes the rotation matrix for rotating by theta (radians) around the positive Z axis.

Here is the call graph for this function: